PX4 Firmware
PX4 Autopilot Software http://px4.io
LandingTargetEstimator.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /*
35  * @file LandingTargetEstimator.cpp
36  *
37  * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
38  * @author Mohammed Kabir <kabir@uasys.io>
39  *
40  */
41 
42 #include <px4_platform_common/px4_config.h>
43 #include <px4_platform_common/defines.h>
44 #include <drivers/drv_hrt.h>
45 
46 #include "LandingTargetEstimator.h"
47 
48 #define SEC2USEC 1000000.0f
49 
50 
52 {
53 
55 {
56  _paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
57  _paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
58  _paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN");
59  _paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN");
60  _paramHandle.mode = param_find("LTEST_MODE");
61  _paramHandle.scale_x = param_find("LTEST_SCALE_X");
62  _paramHandle.scale_y = param_find("LTEST_SCALE_Y");
63 
64  _check_params(true);
65 }
66 
68 {
69  _check_params(false);
70 
72 
73  /* predict */
76  PX4_WARN("Timeout");
77  _estimator_initialized = false;
78 
79  } else {
81 
82  // predict target position with the help of accel data
84 
87  _R_att = matrix::Dcm<float>(q_att);
88  a = _R_att * a;
89 
90  } else {
91  a.zero();
92  }
93 
94  _kalman_filter_x.predict(dt, -a(0), _params.acc_unc);
95  _kalman_filter_y.predict(dt, -a(1), _params.acc_unc);
96 
98  }
99  }
100 
101  if (!_new_irlockReport) {
102  // nothing to do
103  return;
104  }
105 
106  // mark this sensor measurement as consumed
107  _new_irlockReport = false;
108 
110  // don't have the data needed for an update
111  return;
112  }
113 
114  if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
115  return;
116  }
117 
118  // TODO account for sensor orientation as set by parameter
119  // default orientation has camera x pointing in body y, camera y in body -x
120 
121  matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
122  sensor_ray(0) = -_irlockReport.pos_y * _params.scale_y; // forward
123  sensor_ray(1) = _irlockReport.pos_x * _params.scale_x; // right
124  sensor_ray(2) = 1.0f;
125 
126  // rotate the unit ray into the navigation frame, assume sensor frame = body frame
128  _R_att = matrix::Dcm<float>(q_att);
129  sensor_ray = _R_att * sensor_ray;
130 
131  if (fabsf(sensor_ray(2)) < 1e-6f) {
132  // z component of measurement unsafe, don't use this measurement
133  return;
134  }
135 
136  float dist = _vehicleLocalPosition.dist_bottom;
137 
138  // scale the ray s.t. the z component has length of dist
139  _rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist;
140  _rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist;
141 
142  if (!_estimator_initialized) {
143  PX4_INFO("Init");
144  float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
145  float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
146  _kalman_filter_x.init(_rel_pos(0), vx_init, _params.pos_unc_init, _params.vel_unc_init);
147  _kalman_filter_y.init(_rel_pos(1), vy_init, _params.pos_unc_init, _params.vel_unc_init);
148 
149  _estimator_initialized = true;
152 
153  } else {
154  // update
155  bool update_x = _kalman_filter_x.update(_rel_pos(0), _params.meas_unc * dist * dist);
156  bool update_y = _kalman_filter_y.update(_rel_pos(1), _params.meas_unc * dist * dist);
157 
158  if (!update_x || !update_y) {
159  if (!_faulty) {
160  _faulty = true;
161  PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
162  }
163 
164  } else {
165  _faulty = false;
166  }
167 
168  if (!_faulty) {
169  // only publish if both measurements were good
170 
172 
173  float x, xvel, y, yvel, covx, covx_v, covy, covy_v;
174  _kalman_filter_x.getState(x, xvel);
175  _kalman_filter_x.getCovariance(covx, covx_v);
176 
177  _kalman_filter_y.getState(y, yvel);
178  _kalman_filter_y.getCovariance(covy, covy_v);
179 
181 
184  _target_pose.x_rel = x;
185  _target_pose.y_rel = y;
186  _target_pose.z_rel = dist;
187  _target_pose.vx_rel = xvel;
188  _target_pose.vy_rel = yvel;
189 
190  _target_pose.cov_x_rel = covx;
191  _target_pose.cov_y_rel = covy;
192 
193  _target_pose.cov_vx_rel = covx_v;
194  _target_pose.cov_vy_rel = covy_v;
195 
201 
202  } else {
203  _target_pose.abs_pos_valid = false;
204  }
205 
207 
210  }
211 
212  float innov_x, innov_cov_x, innov_y, innov_cov_y;
213  _kalman_filter_x.getInnovations(innov_x, innov_cov_x);
214  _kalman_filter_y.getInnovations(innov_y, innov_cov_y);
215 
217  _target_innovations.innov_x = innov_x;
218  _target_innovations.innov_cov_x = innov_cov_x;
219  _target_innovations.innov_y = innov_y;
220  _target_innovations.innov_cov_y = innov_cov_y;
221 
223  }
224 }
225 
227 {
228  bool updated = _parameterSub.updated();
229 
230  if (updated) {
231  parameter_update_s paramUpdate;
232  _parameterSub.copy(&paramUpdate);
233  }
234 
235  if (updated || force) {
236  _update_params();
237  }
238 }
239 
241 {
245 
247 }
248 
250 {
251  param_get(_paramHandle.acc_unc, &_params.acc_unc);
252  param_get(_paramHandle.meas_unc, &_params.meas_unc);
253  param_get(_paramHandle.pos_unc_init, &_params.pos_unc_init);
254  param_get(_paramHandle.vel_unc_init, &_params.vel_unc_init);
255 
256  int32_t mode = 0;
257  param_get(_paramHandle.mode, &mode);
258  _params.mode = (TargetMode)mode;
259 
260  param_get(_paramHandle.scale_x, &_params.scale_x);
261  param_get(_paramHandle.scale_y, &_params.scale_y);
262 }
263 
264 
265 } // namespace landing_target_estimator
uORB::Publication< landing_target_pose_s > _targetPosePub
void predict(float dt, float acc, float acc_unc)
Predict the state with an external acceleration estimate.
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
void init(matrix::Vector< float, 2 > &initial, matrix::Matrix< float, 2, 2 > &covInit)
Initialize filter state.
bool update(float meas, float measUnc)
Update the state estimate with a measurement.
#define SEC2USEC
Quaternion class.
Definition: Dcm.hpp:24
High-resolution timer with callouts and timekeeping.
void getInnovations(float &innov, float &innovCov)
Get measurement innovation and covariance of last update call.
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
struct landing_target_estimator::LandingTargetEstimator::@100 _paramHandle
Handles for parameters.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void getState(matrix::Vector< float, 2 > &state)
Get the current filter state.
void zero()
Definition: Matrix.hpp:421
bool updated()
Check if there is a new update.
uint64_t timestamp
Definition: irlock_report.h:53
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
struct landing_target_estimator::LandingTargetEstimator::@101 _params
void getCovariance(matrix::Matrix< float, 2, 2 > &covariance)
Get state covariance.
float dt
Definition: px4io.c:73
bool update(void *dst)
Update the struct.
uORB::Publication< landing_target_innovations_s > _targetInnovationsPub
mode
Definition: vtol_type.h:76
bool copy(void *dst)
Copy the struct.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).