APM:Libraries
AC_PrecLand.cpp
Go to the documentation of this file.
1 #include <AP_HAL/AP_HAL.h>
2 #include "AC_PrecLand.h"
3 #include "AC_PrecLand_Backend.h"
5 #include "AC_PrecLand_IRLock.h"
7 #include "AC_PrecLand_SITL.h"
8 
9 extern const AP_HAL::HAL& hal;
10 
12  // @Param: ENABLED
13  // @DisplayName: Precision Land enabled/disabled and behaviour
14  // @Description: Precision Land enabled/disabled and behaviour
15  // @Values: 0:Disabled, 1:Enabled Always Land, 2:Enabled Strict
16  // @User: Advanced
17  AP_GROUPINFO_FLAGS("ENABLED", 0, AC_PrecLand, _enabled, 0, AP_PARAM_FLAG_ENABLE),
18 
19  // @Param: TYPE
20  // @DisplayName: Precision Land Type
21  // @Description: Precision Land Type
22  // @Values: 0:None, 1:CompanionComputer, 2:IRLock, 3:SITL_Gazebo, 4:SITL
23  // @User: Advanced
24  AP_GROUPINFO("TYPE", 1, AC_PrecLand, _type, 0),
25 
26  // @Param: YAW_ALIGN
27  // @DisplayName: Sensor yaw alignment
28  // @Description: Yaw angle from body x-axis to sensor x-axis.
29  // @Range: 0 360
30  // @Increment: 1
31  // @User: Advanced
32  // @Units: cdeg
33  AP_GROUPINFO("YAW_ALIGN", 2, AC_PrecLand, _yaw_align, 0),
34 
35  // @Param: LAND_OFS_X
36  // @DisplayName: Land offset forward
37  // @Description: Desired landing position of the camera forward of the target in vehicle body frame
38  // @Range: -20 20
39  // @Increment: 1
40  // @User: Advanced
41  // @Units: cm
42  AP_GROUPINFO("LAND_OFS_X", 3, AC_PrecLand, _land_ofs_cm_x, 0),
43 
44  // @Param: LAND_OFS_Y
45  // @DisplayName: Land offset right
46  // @Description: desired landing position of the camera right of the target in vehicle body frame
47  // @Range: -20 20
48  // @Increment: 1
49  // @User: Advanced
50  // @Units: cm
51  AP_GROUPINFO("LAND_OFS_Y", 4, AC_PrecLand, _land_ofs_cm_y, 0),
52 
53  // @Param: EST_TYPE
54  // @DisplayName: Precision Land Estimator Type
55  // @Description: Specifies the estimation method to be used
56  // @Values: 0:RawSensor, 1:KalmanFilter
57  // @User: Advanced
58  AP_GROUPINFO("EST_TYPE", 5, AC_PrecLand, _estimator_type, 1),
59 
60  // @Param: ACC_P_NSE
61  // @DisplayName: Kalman Filter Accelerometer Noise
62  // @Description: Kalman Filter Accelerometer Noise, higher values weight the input from the camera more, accels less
63  // @Range: 0.5 5
64  // @User: Advanceds
65  AP_GROUPINFO("ACC_P_NSE", 6, AC_PrecLand, _accel_noise, 2.5f),
66 
67  // @Param: CAM_POS_X
68  // @DisplayName: Camera X position offset
69  // @Description: X position of the camera in body frame. Positive X is forward of the origin.
70  // @Units: m
71  // @User: Advanced
72 
73  // @Param: CAM_POS_Y
74  // @DisplayName: Camera Y position offset
75  // @Description: Y position of the camera in body frame. Positive Y is to the right of the origin.
76  // @Units: m
77  // @User: Advanced
78 
79  // @Param: CAM_POS_Z
80  // @DisplayName: Camera Z position offset
81  // @Description: Z position of the camera in body frame. Positive Z is down from the origin.
82  // @Units: m
83  // @User: Advanced
84  AP_GROUPINFO("CAM_POS", 7, AC_PrecLand, _cam_offset, 0.0f),
85 
86  // @Param: BUS
87  // @DisplayName: Sensor Bus
88  // @Description: Precland sensor bus for I2C sensors.
89  // @Values: -1:DefaultBus,0:InternalI2C,1:ExternalI2C
90  // @User: Advanced
91  AP_GROUPINFO("BUS", 8, AC_PrecLand, _bus, -1),
92 
94 };
95 
96 // Default constructor.
97 // Note that the Vector/Matrix constructors already implicitly zero
98 // their values.
99 //
101  _ahrs(ahrs),
102  _last_update_ms(0),
103  _target_acquired(false),
104  _last_backend_los_meas_ms(0),
105  _backend(nullptr)
106 {
107  // set parameters to defaults
109 
110  // other initialisation
111  _backend_state.healthy = false;
112 }
113 
114 // init - perform any required initialisation of backends
116 {
117  // exit immediately if init has already been run
118  if (_backend != nullptr) {
119  return;
120  }
121 
122  // default health to false
123  _backend = nullptr;
124  _backend_state.healthy = false;
125 
126  // instantiate backend based on type parameter
127  switch ((enum PrecLandType)(_type.get())) {
128  // no type defined
129  case PRECLAND_TYPE_NONE:
130  default:
131  return;
132  // companion computer
135  break;
136  // IR Lock
139  break;
140 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
143  break;
144  case PRECLAND_TYPE_SITL:
146  break;
147 #endif
148  }
149 
150  // init backend
151  if (_backend != nullptr) {
152  _backend->init();
153  }
154 }
155 
156 // update - give chance to driver to get updates from sensor
157 void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
158 {
159  // append current velocity and attitude correction into history buffer
160  struct inertial_data_frame_s inertial_data_newest;
161  _ahrs.getCorrectedDeltaVelocityNED(inertial_data_newest.correctedVehicleDeltaVelocityNED, inertial_data_newest.dt);
162  inertial_data_newest.Tbn = _ahrs.get_rotation_body_to_ned();
163  Vector3f curr_vel;
164  nav_filter_status status;
165  if (!_ahrs.get_velocity_NED(curr_vel) || !_ahrs.get_filter_status(status)) {
166  inertial_data_newest.inertialNavVelocityValid = false;
167  } else {
168  inertial_data_newest.inertialNavVelocityValid = status.flags.horiz_vel;
169  }
170  curr_vel.z = -curr_vel.z; // NED to NEU
171  inertial_data_newest.inertialNavVelocity = curr_vel;
172 
173  _inertial_history.push_back(inertial_data_newest);
174 
175  // update estimator of target position
176  if (_backend != nullptr && _enabled) {
177  _backend->update();
178  run_estimator(rangefinder_alt_cm*0.01f, rangefinder_alt_valid);
179  }
180 }
181 
183 {
185  return _target_acquired;
186 }
187 
189 {
190  if (!target_acquired()) {
191  return false;
192  }
193  Vector2f curr_pos;
194  if (!_ahrs.get_relative_position_NE_origin(curr_pos)) {
195  return false;
196  }
197  ret.x = (_target_pos_rel_out_NE.x + curr_pos.x) * 100.0f; // m to cm
198  ret.y = (_target_pos_rel_out_NE.y + curr_pos.y) * 100.0f; // m to cm
199  return true;
200 }
201 
203 {
204  if (!target_acquired()) {
205  return false;
206  }
207  ret = _target_pos_rel_out_NE*100.0f;
208  return true;
209 }
210 
212 {
213  if (!target_acquired()) {
214  return false;
215  }
216  ret = _target_vel_rel_out_NE*100.0f;
217  return true;
218 }
219 
220 // handle_msg - Process a LANDING_TARGET mavlink message
221 void AC_PrecLand::handle_msg(mavlink_message_t* msg)
222 {
223  // run backend update
224  if (_backend != nullptr) {
225  _backend->handle_msg(msg);
226  }
227 }
228 
229 //
230 // Private methods
231 //
232 
233 void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
234 {
235  const struct inertial_data_frame_s& inertial_data_delayed = _inertial_history.front();
236 
237  switch (_estimator_type) {
239  // Return if there's any invalid velocity data
240  for (uint8_t i=0; i<_inertial_history.size(); i++) {
241  const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i);
242  if (!inertial_data.inertialNavVelocityValid) {
243  _target_acquired = false;
244  return;
245  }
246  }
247 
248  // Predict
249  if (target_acquired()) {
250  _target_pos_rel_est_NE.x -= inertial_data_delayed.inertialNavVelocity.x * inertial_data_delayed.dt;
251  _target_pos_rel_est_NE.y -= inertial_data_delayed.inertialNavVelocity.y * inertial_data_delayed.dt;
252  _target_vel_rel_est_NE.x = -inertial_data_delayed.inertialNavVelocity.x;
253  _target_vel_rel_est_NE.y = -inertial_data_delayed.inertialNavVelocity.y;
254  }
255 
256  // Update if a new LOS measurement is available
257  if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
260  _target_vel_rel_est_NE.x = -inertial_data_delayed.inertialNavVelocity.x;
261  _target_vel_rel_est_NE.y = -inertial_data_delayed.inertialNavVelocity.y;
262 
264  _target_acquired = true;
265  }
266 
267  // Output prediction
268  if (target_acquired()) {
270  }
271  break;
272  }
274  // Predict
275  if (target_acquired()) {
276  const float& dt = inertial_data_delayed.dt;
277  const Vector3f& vehicleDelVel = inertial_data_delayed.correctedVehicleDeltaVelocityNED;
278 
279  _ekf_x.predict(dt, -vehicleDelVel.x, _accel_noise*dt);
280  _ekf_y.predict(dt, -vehicleDelVel.y, _accel_noise*dt);
281  }
282 
283  // Update if a new LOS measurement is available
284  if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
285  float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*_ahrs.get_gyro().length()) + 0.02f);
286  if (!target_acquired()) {
287  // reset filter state
288  if (inertial_data_delayed.inertialNavVelocityValid) {
289  _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed.inertialNavVelocity.x, sq(2.0f));
290  _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -inertial_data_delayed.inertialNavVelocity.y, sq(2.0f));
291  } else {
292  _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, 0.0f, sq(10.0f));
293  _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f));
294  }
296  _target_acquired = true;
297  } else {
298  float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_NED.x, xy_pos_var);
299  float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_NED.y, xy_pos_var);
300  if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
305  _target_acquired = true;
306  } else {
308  }
309  }
310  }
311 
312  // Output prediction
313  if (target_acquired()) {
318 
320  }
321  break;
322  }
323  }
324 }
325 
326 bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body)
327 {
330  _backend->get_los_body(target_vec_unit_body);
331 
332  // Apply sensor yaw alignment rotation
333  float sin_yaw_align = sinf(radians(_yaw_align*0.01f));
334  float cos_yaw_align = cosf(radians(_yaw_align*0.01f));
335  Matrix3f Rz = Matrix3f(
336  cos_yaw_align, -sin_yaw_align, 0,
337  sin_yaw_align, cos_yaw_align, 0,
338  0, 0, 1
339  );
340 
341  target_vec_unit_body = Rz*target_vec_unit_body;
342  return true;
343  } else {
344  return false;
345  }
346 }
347 
348 bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool rangefinder_alt_valid)
349 {
350  Vector3f target_vec_unit_body;
351  if (retrieve_los_meas(target_vec_unit_body)) {
352  const struct inertial_data_frame_s& inertial_data_delayed = _inertial_history.front();
353 
354  Vector3f target_vec_unit_ned = inertial_data_delayed.Tbn * target_vec_unit_body;
355  bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
356  bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
357  if (target_vec_valid && alt_valid) {
358  float dist, alt;
359  if (_backend->distance_to_target() > 0.0f) {
360  dist = _backend->distance_to_target();
361  alt = dist * target_vec_unit_ned.z;
362  } else {
363  alt = MAX(rangefinder_alt_m, 0.0f);
364  dist = alt / target_vec_unit_ned.z;
365  }
366 
367  // Compute camera position relative to IMU
369  Vector3f cam_pos_ned = inertial_data_delayed.Tbn * (_cam_offset.get() - accel_body_offset);
370 
371  // Compute target position relative to IMU
372  _target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt) + cam_pos_ned;
373  return true;
374  }
375  }
376  return false;
377 }
378 
380 {
383 
384  // Predict forward from delayed time horizon
385  for (uint8_t i=1; i<_inertial_history.size(); i++) {
386  const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i);
391  }
392 
393  const Matrix3f& Tbn = _inertial_history.peek(_inertial_history.size()-1).Tbn;
395 
396  // Apply position correction for CG offset from IMU
397  Vector3f imu_pos_ned = Tbn * accel_body_offset;
398  _target_pos_rel_out_NE.x += imu_pos_ned.x;
399  _target_pos_rel_out_NE.y += imu_pos_ned.y;
400 
401  // Apply position correction for body-frame horizontal camera offset from CG, so that vehicle lands lens-to-target
402  Vector3f cam_pos_horizontal_ned = Tbn * Vector3f(_cam_offset.get().x, _cam_offset.get().y, 0);
403  _target_pos_rel_out_NE.x -= cam_pos_horizontal_ned.x;
404  _target_pos_rel_out_NE.y -= cam_pos_horizontal_ned.y;
405 
406  // Apply velocity correction for IMU offset from CG
407  Vector3f vel_ned_rel_imu = Tbn * (_ahrs.get_gyro() % (-accel_body_offset));
408  _target_vel_rel_out_NE.x -= vel_ned_rel_imu.x;
409  _target_vel_rel_out_NE.y -= vel_ned_rel_imu.y;
410 
411  // Apply land offset
413  _target_pos_rel_out_NE.x += land_ofs_ned_m.x;
414  _target_pos_rel_out_NE.y += land_ofs_ned_m.y;
415 }
struct AC_PrecLand::precland_state _backend_state
AP_Float _land_ofs_cm_y
Definition: AC_PrecLand.h:115
Vector3< float > Vector3f
Definition: vector3.h:246
#define AP_PARAM_FLAG_ENABLE
Definition: AP_Param.h:56
AP_Buffer< inertial_data_frame_s, 8 > _inertial_history
Definition: AC_PrecLand.h:142
bool get_target_position_cm(Vector2f &ret)
virtual bool have_los_meas()=0
const Vector3f & get_gyro(void) const override
AP_AHRS_NavEKF & ahrs
Definition: AHRS_Test.cpp:39
virtual void handle_msg(mavlink_message_t *msg)
Vector2f _target_vel_rel_est_NE
Definition: AC_PrecLand.h:129
bool get_filter_status(nav_filter_status &status) const
AP_Int8 _estimator_type
Definition: AC_PrecLand.h:112
virtual void init()=0
bool construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool rangefinder_alt_valid)
#define AP_GROUPINFO(name, idx, clazz, element, def)
Definition: AP_Param.h:102
void init(float pos, float posVar, float vel, float velVar)
Definition: PosVelEKF.cpp:41
virtual float distance_to_target()
virtual uint32_t los_meas_time_ms()=0
AC_PrecLand(const AP_AHRS_NavEKF &ahrs)
AP_Float _yaw_align
Definition: AC_PrecLand.h:113
float getVel() const
Definition: PosVelEKF.h:11
bool get_target_velocity_relative_cms(Vector2f &ret)
Vector2f _target_pos_rel_est_NE
Definition: AC_PrecLand.h:128
Matrix3< float > Matrix3f
Definition: matrix3.h:270
virtual void update()=0
bool target_acquired()
AP_Float _land_ofs_cm_x
Definition: AC_PrecLand.h:114
static auto MAX(const A &one, const B &two) -> decltype(one > two ? one :two)
Definition: AP_Math.h:202
void fusePos(float pos, float posVar)
Definition: PosVelEKF.cpp:62
void run_output_prediction()
void predict(float dt, float dVel, float dVelNoise)
Definition: PosVelEKF.cpp:50
T y
Definition: vector2.h:37
#define AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, flags)
Definition: AP_Param.h:93
uint32_t _last_update_ms
Definition: AC_PrecLand.h:119
static const struct AP_Param::GroupInfo var_info[]
Definition: AC_PrecLand.h:81
Vector3f _target_pos_rel_meas_NED
Definition: AC_PrecLand.h:126
virtual bool get_los_body(Vector3f &dir_body)=0
#define f(i)
bool retrieve_los_meas(Vector3f &target_vec_unit_body)
void getCorrectedDeltaVelocityNED(Vector3f &ret, float &dt) const override
T y
Definition: vector3.h:67
PosVelEKF _ekf_y
Definition: AC_PrecLand.h:123
uint32_t millis()
Definition: system.cpp:157
bool get_target_position_relative_cm(Vector2f &ret)
PosVelEKF _ekf_x
Definition: AC_PrecLand.h:123
AP_Int8 _enabled
Definition: AC_PrecLand.h:109
bool _target_acquired
Definition: AC_PrecLand.h:120
uint8_t get_primary_accel_index(void) const override
T z
Definition: vector3.h:67
AP_Vector3f _cam_offset
Definition: AC_PrecLand.h:117
void update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
bool get_relative_position_NE_origin(Vector2f &posNE) const override
const Vector3f & get_imu_pos_offset(uint8_t instance) const
friend class AC_PrecLand_SITL
Definition: AC_PrecLand.h:25
uint32_t _last_backend_los_meas_ms
Definition: AC_PrecLand.h:121
friend class AC_PrecLand_Companion
Definition: AC_PrecLand.h:22
const AP_HAL::HAL & hal
Definition: AC_PID_test.cpp:14
float length(void) const
Definition: vector3.cpp:288
struct nav_filter_status::@138 flags
friend class AC_PrecLand_IRLock
Definition: AC_PrecLand.h:23
AP_Int8 _type
Definition: AC_PrecLand.h:110
T x
Definition: vector2.h:37
const AP_AHRS_NavEKF & _ahrs
Definition: AC_PrecLand.h:106
Vector2f _target_vel_rel_out_NE
Definition: AC_PrecLand.h:132
static constexpr float radians(float deg)
Definition: AP_Math.h:158
void handle_msg(mavlink_message_t *msg)
const Matrix3f & get_rotation_body_to_ned(void) const override
float sq(const T val)
Definition: AP_Math.h:170
void run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
AP_InertialSensor & ins()
friend class AC_PrecLand_SITL_Gazebo
Definition: AC_PrecLand.h:24
float getPos() const
Definition: PosVelEKF.h:10
bool get_velocity_NED(Vector3f &vec) const override
float getPosNIS(float pos, float posVar)
Definition: PosVelEKF.cpp:86
Vector2f _target_pos_rel_out_NE
Definition: AC_PrecLand.h:131
#define AP_GROUPEND
Definition: AP_Param.h:121
T x
Definition: vector3.h:67
AC_PrecLand_Backend * _backend
Definition: AC_PrecLand.h:148
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
Definition: AP_Param.cpp:1214
uint32_t _outlier_reject_count
Definition: AC_PrecLand.h:124
AP_Float _accel_noise
Definition: AC_PrecLand.h:116