Line data Source code
1 : #include "AC_PrecLand_config.h"
2 :
3 : #if AC_PRECLAND_ENABLED
4 :
5 : #include "AC_PrecLand.h"
6 : #include <AP_HAL/AP_HAL.h>
7 : #include <AP_AHRS/AP_AHRS.h>
8 :
9 : #include "AC_PrecLand_Backend.h"
10 : #include "AC_PrecLand_MAVLink.h"
11 : #include "AC_PrecLand_IRLock.h"
12 : #include "AC_PrecLand_SITL_Gazebo.h"
13 : #include "AC_PrecLand_SITL.h"
14 : #include <AP_Logger/AP_Logger.h>
15 : #include <GCS_MAVLink/GCS.h>
16 : #include <AP_Vehicle/AP_Vehicle_Type.h>
17 :
18 : extern const AP_HAL::HAL& hal;
19 :
20 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
21 : # define AC_PRECLAND_ORIENT_DEFAULT Rotation::ROTATION_NONE
22 : #else
23 : # define AC_PRECLAND_ORIENT_DEFAULT Rotation::ROTATION_PITCH_270
24 : #endif
25 :
26 : static const uint32_t EKF_INIT_TIME_MS = 2000; // EKF initialisation requires this many milliseconds of good sensor data
27 : static const uint32_t EKF_INIT_SENSOR_MIN_UPDATE_MS = 500; // Sensor must update within this many ms during EKF init, else init will fail
28 : static const uint32_t LANDING_TARGET_TIMEOUT_MS = 2000; // Sensor must update within this many ms, else prec landing will be switched off
29 : static const uint32_t LANDING_TARGET_LOST_TIMEOUT_MS = 180000; // Target will be considered as "lost" if the last known location of the target is more than this many ms ago
30 : static const float LANDING_TARGET_LOST_DIST_THRESH_M = 30; // If the last known location of the landing target is beyond this many meters, then we will consider it lost
31 :
32 : const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
33 : // @Param: ENABLED
34 : // @DisplayName: Precision Land enabled/disabled
35 : // @Description: Precision Land enabled/disabled
36 : // @Values: 0:Disabled, 1:Enabled
37 : // @RebootRequired: True
38 : // @User: Advanced
39 : AP_GROUPINFO_FLAGS("ENABLED", 0, AC_PrecLand, _enabled, 0, AP_PARAM_FLAG_ENABLE),
40 :
41 : // @Param: TYPE
42 : // @DisplayName: Precision Land Type
43 : // @Description: Precision Land Type
44 : // @Values: 0:None, 1:MAVLink, 2:IRLock, 3:SITL_Gazebo, 4:SITL
45 : // @RebootRequired: True
46 : // @User: Advanced
47 : AP_GROUPINFO("TYPE", 1, AC_PrecLand, _type, 0),
48 :
49 : // @Param: YAW_ALIGN
50 : // @DisplayName: Sensor yaw alignment
51 : // @Description: Yaw angle from body x-axis to sensor x-axis.
52 : // @Range: 0 36000
53 : // @Increment: 10
54 : // @User: Advanced
55 : // @Units: cdeg
56 : AP_GROUPINFO("YAW_ALIGN", 2, AC_PrecLand, _yaw_align_cd, 0),
57 :
58 : // @Param: LAND_OFS_X
59 : // @DisplayName: Land offset forward
60 : // @Description: Desired landing position of the camera forward of the target in vehicle body frame
61 : // @Range: -20 20
62 : // @Increment: 1
63 : // @User: Advanced
64 : // @Units: cm
65 : AP_GROUPINFO("LAND_OFS_X", 3, AC_PrecLand, _land_ofs_cm_x, 0),
66 :
67 : // @Param: LAND_OFS_Y
68 : // @DisplayName: Land offset right
69 : // @Description: desired landing position of the camera right of the target in vehicle body frame
70 : // @Range: -20 20
71 : // @Increment: 1
72 : // @User: Advanced
73 : // @Units: cm
74 : AP_GROUPINFO("LAND_OFS_Y", 4, AC_PrecLand, _land_ofs_cm_y, 0),
75 :
76 : // @Param: EST_TYPE
77 : // @DisplayName: Precision Land Estimator Type
78 : // @Description: Specifies the estimation method to be used
79 : // @Values: 0:RawSensor, 1:KalmanFilter
80 : // @User: Advanced
81 : AP_GROUPINFO("EST_TYPE", 5, AC_PrecLand, _estimator_type, 1),
82 :
83 : // @Param: ACC_P_NSE
84 : // @DisplayName: Kalman Filter Accelerometer Noise
85 : // @Description: Kalman Filter Accelerometer Noise, higher values weight the input from the camera more, accels less
86 : // @Range: 0.5 5
87 : // @User: Advanced
88 : AP_GROUPINFO("ACC_P_NSE", 6, AC_PrecLand, _accel_noise, 2.5f),
89 :
90 : // @Param: CAM_POS_X
91 : // @DisplayName: Camera X position offset
92 : // @Description: X position of the camera in body frame. Positive X is forward of the origin.
93 : // @Units: m
94 : // @Range: -5 5
95 : // @Increment: 0.01
96 : // @User: Advanced
97 :
98 : // @Param: CAM_POS_Y
99 : // @DisplayName: Camera Y position offset
100 : // @Description: Y position of the camera in body frame. Positive Y is to the right of the origin.
101 : // @Units: m
102 : // @Range: -5 5
103 : // @Increment: 0.01
104 : // @User: Advanced
105 :
106 : // @Param: CAM_POS_Z
107 : // @DisplayName: Camera Z position offset
108 : // @Description: Z position of the camera in body frame. Positive Z is down from the origin.
109 : // @Units: m
110 : // @Range: -5 5
111 : // @Increment: 0.01
112 : // @User: Advanced
113 : AP_GROUPINFO("CAM_POS", 7, AC_PrecLand, _cam_offset_m, 0.0f),
114 :
115 : // @Param: BUS
116 : // @DisplayName: Sensor Bus
117 : // @Description: Precland sensor bus for I2C sensors.
118 : // @Values: -1:DefaultBus,0:InternalI2C,1:ExternalI2C
119 : // @User: Advanced
120 : AP_GROUPINFO("BUS", 8, AC_PrecLand, _bus, -1),
121 :
122 : // @Param: LAG
123 : // @DisplayName: Precision Landing sensor lag
124 : // @Description: Precision Landing sensor lag, to cope with variable landing_target latency
125 : // @Range: 0.02 0.250
126 : // @Increment: 1
127 : // @Units: s
128 : // @User: Advanced
129 : // @RebootRequired: True
130 : AP_GROUPINFO("LAG", 9, AC_PrecLand, _lag_s, 0.02f), // 20ms is the old default buffer size (8 frames @ 400hz/2.5ms)
131 :
132 : // @Param: XY_DIST_MAX
133 : // @DisplayName: Precision Landing maximum distance to target before descending
134 : // @Description: The vehicle will not start descending if the landing target is detected and it is further than this many meters away. Set 0 to always descend.
135 : // @Range: 0 10
136 : // @Units: m
137 : // @User: Advanced
138 : AP_GROUPINFO("XY_DIST_MAX", 10, AC_PrecLand, _xy_max_dist_desc_m, 2.5f),
139 :
140 : // @Param: STRICT
141 : // @DisplayName: PrecLand strictness
142 : // @Description: How strictly should the vehicle land on the target if target is lost
143 : // @Values: 0: Land Vertically (Not strict), 1: Retry Landing(Normal Strictness), 2: Do not land (just Hover) (Very Strict)
144 : AP_GROUPINFO("STRICT", 11, AC_PrecLand, _strict, 1),
145 :
146 : // @Param: RET_MAX
147 : // @DisplayName: PrecLand Maximum number of retires for a failed landing
148 : // @Description: PrecLand Maximum number of retires for a failed landing. Set to zero to disable landing retry.
149 : // @Range: 0 10
150 : // @Increment: 1
151 : AP_GROUPINFO("RET_MAX", 12, AC_PrecLand, _retry_max, 4),
152 :
153 : // @Param: TIMEOUT
154 : // @DisplayName: PrecLand retry timeout
155 : // @Description: Time for which vehicle continues descend even if target is lost. After this time period, vehicle will attempt a landing retry depending on PLND_STRICT parameter.
156 : // @Range: 0 20
157 : // @Units: s
158 : AP_GROUPINFO("TIMEOUT", 13, AC_PrecLand, _retry_timeout_s, 4),
159 :
160 : // @Param: RET_BEHAVE
161 : // @DisplayName: PrecLand retry behaviour
162 : // @Description: Prec Land will do the action selected by this parameter if a retry to a landing is needed
163 : // @Values: 0: Go to the last location where landing target was detected, 1: Go towards the approximate location of the detected landing target
164 : AP_GROUPINFO("RET_BEHAVE", 14, AC_PrecLand, _retry_behave, 0),
165 :
166 : // @Param: ALT_MIN
167 : // @DisplayName: PrecLand minimum alt for retry
168 : // @Description: Vehicle will continue landing vertically even if target is lost below this height. This needs a rangefinder to work. Set to zero to disable this.
169 : // @Range: 0 5
170 : // @Units: m
171 : AP_GROUPINFO("ALT_MIN", 15, AC_PrecLand, _sensor_min_alt_m, 0.75),
172 :
173 : // @Param: ALT_MAX
174 : // @DisplayName: PrecLand maximum alt for retry
175 : // @Description: Vehicle will continue landing vertically until this height if target is not found. Below this height if landing target is not found, landing retry/failsafe might be attempted. This needs a rangefinder to work. Set to zero to disable this.
176 : // @Range: 0 50
177 : // @Units: m
178 : AP_GROUPINFO("ALT_MAX", 16, AC_PrecLand, _sensor_max_alt_m, 8),
179 :
180 : // @Param: OPTIONS
181 : // @DisplayName: Precision Landing Extra Options
182 : // @Description: Precision Landing Extra Options
183 : // @Bitmask: 0: Moving Landing Target, 1: Allow Precision Landing after manual reposition, 2: Maintain high speed in final descent
184 : // @User: Advanced
185 : AP_GROUPINFO("OPTIONS", 17, AC_PrecLand, _options, 0),
186 :
187 : // @Param{Rover}: ORIENT
188 : // @DisplayName: Camera Orientation
189 : // @Description: Orientation of camera/sensor on body
190 : // @Values: 0:Forward, 4:Back, 25:Down
191 : // @User: Advanced
192 : // @RebootRequired: True
193 : AP_GROUPINFO_FRAME("ORIENT", 18, AC_PrecLand, _orient, AC_PRECLAND_ORIENT_DEFAULT, AP_PARAM_FRAME_ROVER),
194 :
195 : AP_GROUPEND
196 : };
197 :
198 : // Default constructor.
199 1978 : AC_PrecLand::AC_PrecLand()
200 : {
201 1978 : if (_singleton != nullptr) {
202 0 : AP_HAL::panic("AC_PrecLand must be singleton");
203 : }
204 1978 : _singleton = this;
205 :
206 : // set parameters to defaults
207 1978 : AP_Param::setup_object_defaults(this, var_info);
208 1978 : }
209 :
210 : // perform any required initialisation of landing controllers
211 : // update_rate_hz should be the rate at which the update method will be called in hz
212 1975 : void AC_PrecLand::init(uint16_t update_rate_hz)
213 : {
214 : // exit immediately if init has already been run
215 1975 : if (_backend != nullptr) {
216 0 : return;
217 : }
218 :
219 : // init as target TARGET_NEVER_SEEN, we will update this later
220 1975 : _current_target_state = TargetState::TARGET_NEVER_SEEN;
221 :
222 : // default health to false
223 1975 : _backend = nullptr;
224 1975 : _backend_state.healthy = false;
225 :
226 : // create inertial history buffer
227 : // constrain lag parameter to be within bounds
228 1975 : _lag_s.set(constrain_float(_lag_s, 0.02f, 0.25f)); // must match LAG parameter range at line 124
229 :
230 : // calculate inertial buffer size from lag and minimum of main loop rate and update_rate_hz argument
231 1975 : const uint16_t inertial_buffer_size = MAX((uint16_t)roundf(_lag_s * update_rate_hz), 1);
232 :
233 : // instantiate ring buffer to hold inertial history, return on failure so no backends are created
234 1975 : _inertial_history = NEW_NOTHROW ObjectArray<inertial_data_frame_s>(inertial_buffer_size);
235 1975 : if (_inertial_history == nullptr) {
236 0 : return;
237 : }
238 :
239 : // instantiate backend based on type parameter
240 1975 : switch ((Type)(_type.get())) {
241 : // no type defined
242 1962 : case Type::NONE:
243 : default:
244 1962 : return;
245 : // companion computer
246 : #if AC_PRECLAND_MAVLINK_ENABLED
247 2 : case Type::MAVLINK:
248 2 : _backend = NEW_NOTHROW AC_PrecLand_MAVLink(*this, _backend_state);
249 2 : break;
250 : // IR Lock
251 : #endif
252 : #if AC_PRECLAND_IRLOCK_ENABLED
253 2 : case Type::IRLOCK:
254 2 : _backend = NEW_NOTHROW AC_PrecLand_IRLock(*this, _backend_state);
255 2 : break;
256 : #endif
257 : #if AC_PRECLAND_SITL_GAZEBO_ENABLED
258 0 : case Type::SITL_GAZEBO:
259 0 : _backend = NEW_NOTHROW AC_PrecLand_SITL_Gazebo(*this, _backend_state);
260 0 : break;
261 : #endif
262 : #if AC_PRECLAND_SITL_ENABLED
263 9 : case Type::SITL:
264 9 : _backend = NEW_NOTHROW AC_PrecLand_SITL(*this, _backend_state);
265 9 : break;
266 : #endif
267 : }
268 :
269 : // init backend
270 13 : if (_backend != nullptr) {
271 13 : _backend->init();
272 : }
273 :
274 13 : _approach_vector_body.x = 1;
275 13 : _approach_vector_body.rotate(_orient);
276 : }
277 :
278 : // update - give chance to driver to get updates from sensor
279 39418202 : void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
280 : {
281 : // exit immediately if not enabled
282 39418202 : if (_backend == nullptr || _inertial_history == nullptr) {
283 39077026 : return;
284 : }
285 :
286 : // append current velocity and attitude correction into history buffer
287 341176 : struct inertial_data_frame_s inertial_data_newest;
288 341176 : const auto &_ahrs = AP::ahrs();
289 :
290 341176 : Vector3f curr_vel;
291 : nav_filter_status status;
292 341176 : if (!_ahrs.getCorrectedDeltaVelocityNED(inertial_data_newest.correctedVehicleDeltaVelocityNED, inertial_data_newest.dt) ||
293 673742 : !_ahrs.get_velocity_NED(curr_vel) ||
294 332566 : !_ahrs.get_filter_status(status)) {
295 8610 : inertial_data_newest.inertialNavVelocityValid = false;
296 : } else {
297 332566 : inertial_data_newest.inertialNavVelocityValid = status.flags.horiz_vel;
298 : }
299 341176 : inertial_data_newest.Tbn = _ahrs.get_rotation_body_to_ned();
300 341176 : curr_vel.z = -curr_vel.z; // NED to NEU
301 341176 : inertial_data_newest.inertialNavVelocity = curr_vel;
302 :
303 341176 : inertial_data_newest.time_usec = AP_HAL::micros64();
304 341176 : _inertial_history->push_force(inertial_data_newest);
305 :
306 341176 : const float rangefinder_alt_m = rangefinder_alt_cm*0.01f; //cm to meter
307 :
308 : // update estimator of target position
309 341176 : if (_backend != nullptr && _enabled) {
310 337744 : _backend->update();
311 337744 : run_estimator(rangefinder_alt_m, rangefinder_alt_valid);
312 : }
313 :
314 : // check the status of the landing target location
315 341176 : check_target_status(rangefinder_alt_m, rangefinder_alt_valid);
316 :
317 : #if HAL_LOGGING_ENABLED
318 341176 : const uint32_t now = AP_HAL::millis();
319 341176 : if (now - _last_log_ms > 40) { // 25Hz
320 24903 : _last_log_ms = now;
321 24903 : Write_Precland();
322 : }
323 : #endif
324 : }
325 :
326 : // check the status of the target
327 341176 : void AC_PrecLand::check_target_status(float rangefinder_alt_m, bool rangefinder_alt_valid)
328 : {
329 341176 : if (target_acquired()) {
330 : // target in sight
331 100398 : _current_target_state = TargetState::TARGET_FOUND;
332 : // early return because we already know the status
333 100398 : return;
334 : }
335 :
336 : // target not in sight
337 240778 : if (_current_target_state == TargetState::TARGET_FOUND ||
338 240773 : _current_target_state == TargetState::TARGET_RECENTLY_LOST) {
339 : // we had target in sight, but not any more, i.e we have lost the target
340 1890 : _current_target_state = TargetState::TARGET_RECENTLY_LOST;
341 : } else {
342 : // we never had the target in sight
343 238888 : _current_target_state = TargetState::TARGET_NEVER_SEEN;
344 : }
345 :
346 : // We definitely do not have the target in sight
347 : // check if the precision landing sensor is supposed to be in range
348 : // this needs a valid rangefinder to work
349 240778 : if (!check_if_sensor_in_range(rangefinder_alt_m, rangefinder_alt_valid)) {
350 : // Target is not in range (vehicle is either too high or too low). Vehicle will not be attempting any sort of landing retries during this period
351 201076 : _current_target_state = TargetState::TARGET_OUT_OF_RANGE;
352 201076 : return;
353 : }
354 :
355 39702 : if (_current_target_state == TargetState::TARGET_RECENTLY_LOST) {
356 : // check if it's nearby/found recently, else the status will be demoted to "TARGET_LOST"
357 1885 : Vector2p curr_pos_ne_m;
358 1885 : if (AP::ahrs().get_relative_position_NE_origin(curr_pos_ne_m)) {
359 1885 : const float dist_to_last_target_loc_xy = (curr_pos_ne_m - _last_target_pos_rel_origin_ned_m.xy()).length();
360 1885 : const float dist_to_last_loc_ne_m = (curr_pos_ne_m - _last_vehicle_pos_ned_m.xy()).length();
361 1885 : if ((AP_HAL::millis() - _last_valid_target_ms) > LANDING_TARGET_LOST_TIMEOUT_MS) {
362 : // the target has not been seen for a long time
363 : // might as well consider it as "never seen"
364 0 : _current_target_state = TargetState::TARGET_NEVER_SEEN;
365 0 : return;
366 : }
367 :
368 1885 : if ((dist_to_last_target_loc_xy > LANDING_TARGET_LOST_DIST_THRESH_M) || (dist_to_last_loc_ne_m > LANDING_TARGET_LOST_DIST_THRESH_M)) {
369 : // the last known location of target is too far away
370 0 : _current_target_state = TargetState::TARGET_NEVER_SEEN;
371 0 : return;
372 : }
373 : }
374 : }
375 : }
376 :
377 : // Check if the landing target is supposed to be in sight based on the height of the vehicle from the ground
378 : // This needs a valid rangefinder to work, if the min/max parameters are non zero
379 240778 : bool AC_PrecLand::check_if_sensor_in_range(float rangefinder_alt_m, bool rangefinder_alt_valid) const
380 : {
381 240778 : if (is_zero(_sensor_max_alt_m) && is_zero(_sensor_min_alt_m)) {
382 : // no sensor limits have been specified, assume sensor is always in range
383 0 : return true;
384 : }
385 :
386 240778 : if (!rangefinder_alt_valid) {
387 : // rangefinder isn't healthy. We might be at a very high altitude
388 22268 : return false;
389 : }
390 :
391 218510 : if (rangefinder_alt_m > _sensor_max_alt_m && !is_zero(_sensor_max_alt_m)) {
392 : // this prevents triggering a retry when we are too far away from the target
393 70652 : return false;
394 : }
395 :
396 147858 : if (rangefinder_alt_m < _sensor_min_alt_m && !is_zero(_sensor_min_alt_m)) {
397 : // this prevents triggering a retry when we are very close to the target
398 108156 : return false;
399 : }
400 :
401 : // target should be in range
402 39702 : return true;
403 : }
404 :
405 : // returns true when the landing target has been detected
406 10623170 : bool AC_PrecLand::target_acquired()
407 : {
408 10623170 : if ((AP_HAL::millis()-_last_update_ms) > LANDING_TARGET_TIMEOUT_MS) {
409 9873882 : if (_target_acquired) {
410 : // just lost the landing target, inform the user. This message will only be sent once every time target is lost
411 5 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PrecLand: Target Lost");
412 : }
413 : // not had a sensor update since a long time
414 : // probably lost the target
415 9873882 : _estimator_initialized = false;
416 9873882 : _target_acquired = false;
417 : }
418 10623170 : return _target_acquired;
419 : }
420 :
421 : // returns target position relative to the EKF origin
422 200993 : bool AC_PrecLand::get_target_position_m(Vector2p& ret)
423 : {
424 200993 : if (!target_acquired()) {
425 0 : return false;
426 : }
427 200993 : Vector2p curr_pos_ne_m;
428 200993 : if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_ne_m)) {
429 0 : return false;
430 : }
431 200993 : ret.x = (_target_pos_rel_out_ne_m.x + curr_pos_ne_m.x);
432 200993 : ret.y = (_target_pos_rel_out_ne_m.y + curr_pos_ne_m.y);
433 200993 : return true;
434 : }
435 :
436 : // returns target relative position as 3D vector
437 58946 : void AC_PrecLand::get_target_position_measurement_NED_m(Vector3f& ret)
438 : {
439 58946 : ret = _target_pos_rel_meas_ned_m;
440 58946 : return;
441 : }
442 :
443 : // returns target position relative to vehicle
444 24679 : bool AC_PrecLand::get_target_position_relative_NE_m(Vector2f& ret)
445 : {
446 24679 : if (!target_acquired()) {
447 16485 : return false;
448 : }
449 8194 : ret = _target_pos_rel_out_ne_m;
450 8194 : return true;
451 : }
452 :
453 : // returns target velocity relative to vehicle
454 24679 : bool AC_PrecLand::get_target_velocity_relative_NE_ms(Vector2f& ret)
455 : {
456 24679 : if (!target_acquired()) {
457 16485 : return false;
458 : }
459 8194 : ret = _target_vel_rel_out_ne_ms;
460 8194 : return true;
461 : }
462 :
463 : // get the absolute velocity of the vehicle
464 59656 : void AC_PrecLand::get_target_velocity_ms(const Vector2f& vehicle_velocity_ne_ms, Vector2f& target_vel_ne_ms)
465 : {
466 59656 : if (!(_options & PLND_OPTION_MOVING_TARGET)) {
467 : // the target should not be moving
468 59656 : target_vel_ne_ms.zero();
469 59656 : return;
470 : }
471 0 : if ((EstimatorType)_estimator_type.get() == EstimatorType::RAW_SENSOR) {
472 : // We do not predict the velocity of the target in this case
473 : // assume velocity to be zero
474 0 : target_vel_ne_ms.zero();
475 0 : return;
476 : }
477 0 : Vector2f target_vel_rel_ne_ms;
478 0 : if (!get_target_velocity_relative_NE_ms(target_vel_rel_ne_ms)) {
479 : // Don't know where the target is
480 : // assume velocity to be zero
481 0 : target_vel_ne_ms.zero();
482 0 : return;
483 : }
484 : // return the absolute velocity
485 0 : target_vel_ne_ms = target_vel_rel_ne_ms + vehicle_velocity_ne_ms;
486 : }
487 :
488 : // handle_msg - Process a LANDING_TARGET mavlink message
489 671 : void AC_PrecLand::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
490 : {
491 : // run backend update
492 671 : if (_backend != nullptr) {
493 671 : _backend->handle_msg(packet, timestamp_ms);
494 : }
495 671 : }
496 :
497 : //
498 : // Private methods
499 : //
500 :
501 : // run target position estimator
502 337744 : void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
503 : {
504 337744 : _inertial_data_delayed = (*_inertial_history)[0];
505 :
506 337744 : switch ((EstimatorType)_estimator_type.get()) {
507 0 : case EstimatorType::RAW_SENSOR: {
508 : // Return if there's any invalid velocity data
509 0 : for (uint8_t i=0; i<_inertial_history->available(); i++) {
510 0 : const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i];
511 0 : if (!inertial_data->inertialNavVelocityValid) {
512 0 : _target_acquired = false;
513 0 : return;
514 : }
515 : }
516 :
517 : // Predict
518 0 : if (target_acquired()) {
519 0 : _target_pos_rel_est_ne_m.x -= _inertial_data_delayed->inertialNavVelocity.x * _inertial_data_delayed->dt;
520 0 : _target_pos_rel_est_ne_m.y -= _inertial_data_delayed->inertialNavVelocity.y * _inertial_data_delayed->dt;
521 0 : _target_vel_rel_est_ne_ms.x = -_inertial_data_delayed->inertialNavVelocity.x;
522 0 : _target_vel_rel_est_ne_ms.y = -_inertial_data_delayed->inertialNavVelocity.y;
523 : }
524 :
525 : // Update if a new Line-Of-Sight measurement is available
526 0 : if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
527 0 : if (!_estimator_initialized) {
528 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Target Found");
529 0 : _estimator_initialized = true;
530 : }
531 0 : _target_pos_rel_est_ne_m.x = _target_pos_rel_meas_ned_m.x;
532 0 : _target_pos_rel_est_ne_m.y = _target_pos_rel_meas_ned_m.y;
533 0 : _target_vel_rel_est_ne_ms.x = -_inertial_data_delayed->inertialNavVelocity.x;
534 0 : _target_vel_rel_est_ne_ms.y = -_inertial_data_delayed->inertialNavVelocity.y;
535 :
536 0 : _last_update_ms = AP_HAL::millis();
537 0 : _target_acquired = true;
538 : }
539 :
540 : // Output prediction
541 0 : if (target_acquired()) {
542 0 : run_output_prediction();
543 : }
544 0 : break;
545 : }
546 337744 : case EstimatorType::KALMAN_FILTER: {
547 : // Predict
548 337744 : if (target_acquired() || _estimator_initialized) {
549 106040 : const float& dt = _inertial_data_delayed->dt;
550 106040 : const Vector3f& vehicleDelVel = _inertial_data_delayed->correctedVehicleDeltaVelocityNED;
551 :
552 106040 : _ekf_x.predict(dt, -vehicleDelVel.x, _accel_noise*dt);
553 106040 : _ekf_y.predict(dt, -vehicleDelVel.y, _accel_noise*dt);
554 : }
555 :
556 : // Update if a new Line-Of-Sight measurement is available
557 337744 : if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
558 26620 : float xy_pos_var = sq(_target_pos_rel_meas_ned_m.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f);
559 26620 : if (!_estimator_initialized) {
560 : // Inform the user landing target has been found
561 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Target Found");
562 : // start init of EKF. We will let the filter consume the data for a while before it available for consumption
563 : // reset filter state
564 11 : if (_inertial_data_delayed->inertialNavVelocityValid) {
565 11 : _ekf_x.init(_target_pos_rel_meas_ned_m.x, xy_pos_var, -_inertial_data_delayed->inertialNavVelocity.x, sq(2.0f));
566 11 : _ekf_y.init(_target_pos_rel_meas_ned_m.y, xy_pos_var, -_inertial_data_delayed->inertialNavVelocity.y, sq(2.0f));
567 : } else {
568 0 : _ekf_x.init(_target_pos_rel_meas_ned_m.x, xy_pos_var, 0.0f, sq(10.0f));
569 0 : _ekf_y.init(_target_pos_rel_meas_ned_m.y, xy_pos_var, 0.0f, sq(10.0f));
570 : }
571 11 : _last_update_ms = AP_HAL::millis();
572 11 : _estimator_init_ms = AP_HAL::millis();
573 : // we have initialized the estimator but will not use the values for sometime so that EKF settles down
574 11 : _estimator_initialized = true;
575 : } else {
576 26609 : float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_ned_m.x, xy_pos_var);
577 26609 : float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_ned_m.y, xy_pos_var);
578 26609 : if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
579 25861 : _outlier_reject_count = 0;
580 25861 : _ekf_x.fusePos(_target_pos_rel_meas_ned_m.x, xy_pos_var);
581 25861 : _ekf_y.fusePos(_target_pos_rel_meas_ned_m.y, xy_pos_var);
582 25861 : _last_update_ms = AP_HAL::millis();
583 : } else {
584 748 : _outlier_reject_count++;
585 : }
586 : }
587 : }
588 :
589 : // check EKF was properly initialized when the sensor detected a landing target
590 337744 : check_ekf_init_timeout();
591 :
592 : // Output prediction
593 337744 : if (target_acquired()) {
594 100339 : _target_pos_rel_est_ne_m.x = _ekf_x.getPos();
595 100339 : _target_pos_rel_est_ne_m.y = _ekf_y.getPos();
596 100339 : _target_vel_rel_est_ne_ms.x = _ekf_x.getVel();
597 100339 : _target_vel_rel_est_ne_ms.y = _ekf_y.getVel();
598 :
599 100339 : run_output_prediction();
600 : }
601 337744 : break;
602 : }
603 : }
604 : }
605 :
606 :
607 : // check if EKF got the time to initialize when the landing target was first detected
608 : // Expects sensor to update within EKF_INIT_SENSOR_MIN_UPDATE_MS milliseconds till EKF_INIT_TIME_MS milliseconds have passed
609 : // after this period landing target estimates can be used by vehicle
610 337744 : void AC_PrecLand::check_ekf_init_timeout()
611 : {
612 337744 : if (!target_acquired() && _estimator_initialized) {
613 : // we have just got the target in sight
614 5722 : if (AP_HAL::millis()-_last_update_ms > EKF_INIT_SENSOR_MIN_UPDATE_MS) {
615 : // we have lost the target, not enough readings to initialize the EKF
616 1 : _estimator_initialized = false;
617 1 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PrecLand: Init Failed");
618 5721 : } else if (AP_HAL::millis()-_estimator_init_ms > EKF_INIT_TIME_MS) {
619 : // the target has been visible for a while, EKF should now have initialized to a good value
620 10 : _target_acquired = true;
621 10 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Init Complete");
622 : }
623 : }
624 337744 : }
625 :
626 : // get 3D vector from vehicle to target and frame. returns true on success, false on failure
627 337744 : bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit, VectorFrame& frame)
628 : {
629 337744 : const uint32_t los_meas_time_ms = _backend->los_meas_time_ms();
630 337744 : if ((los_meas_time_ms != _last_backend_los_meas_ms) && _backend->get_los_meas(target_vec_unit, frame)) {
631 26879 : _last_backend_los_meas_ms = los_meas_time_ms;
632 26879 : if (!is_zero(_yaw_align_cd)) {
633 : // Apply sensor yaw alignment rotation
634 0 : target_vec_unit.rotate_xy(cd_to_rad(_yaw_align_cd));
635 : }
636 :
637 : // rotate vector based on sensor orientation to get correct body frame vector
638 26879 : if (_orient != ROTATION_PITCH_270) {
639 : // by default, the vector is constructed downwards in body frame
640 : // hence, we do not do any rotation if the orientation is downwards
641 : // if it is some other orientation, we first bring the vector to forward
642 : // and then we rotate it to desired orientation
643 : // because the rotations are measured with respect to a vector pointing towards front in body frame
644 : // for eg, if orientation is back, i.e., ROTATION_YAW_180,
645 : // the vector is first brought to front and then rotation by YAW 180 to take it to the back of vehicle
646 7252 : target_vec_unit.rotate(ROTATION_PITCH_90); // bring vector to front
647 7252 : target_vec_unit.rotate(_orient); // rotate it to desired orientation
648 : }
649 :
650 26879 : return true;
651 : }
652 310865 : return false;
653 : }
654 :
655 : // If a new measurement was retrieved, sets _target_pos_rel_meas_ned_m and returns true
656 337744 : bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool rangefinder_alt_valid)
657 : {
658 337744 : Vector3f target_vec_unit;
659 : VectorFrame target_vec_frame;
660 337744 : if (retrieve_los_meas(target_vec_unit, target_vec_frame)) {
661 26879 : _inertial_data_delayed = (*_inertial_history)[0];
662 :
663 : // sanity check vector is pointing in the right direction
664 26879 : const bool target_vec_valid = target_vec_unit.projected(_approach_vector_body).dot(_approach_vector_body) > 0.0f;
665 :
666 : // calculate 3D vector to target in NED frame
667 26879 : Vector3f target_vec_unit_ned;
668 26879 : switch (target_vec_frame) {
669 26776 : case VectorFrame::BODY_FRD:
670 : // convert to NED
671 26776 : target_vec_unit_ned = _inertial_data_delayed->Tbn * target_vec_unit;
672 26776 : break;
673 103 : case VectorFrame::LOCAL_FRD:
674 : // rotate vector using delayed yaw
675 : float roll_rad, pitch_rad, yaw_rad;
676 103 : _inertial_data_delayed->Tbn.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
677 103 : target_vec_unit_ned = target_vec_unit;
678 103 : target_vec_unit_ned.rotate_xy(yaw_rad);
679 103 : break;
680 : }
681 :
682 26879 : const Vector3f approach_vector_NED_m = _inertial_data_delayed->Tbn * _approach_vector_body;
683 26879 : const bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
684 26879 : if (target_vec_valid && alt_valid) {
685 : // distance to target and distance to target along approach vector
686 : float dist_to_target_m, dist_to_target_along_av_m;
687 : // figure out ned camera orientation w.r.t its offset
688 26620 : Vector3f cam_pos_ned_m;
689 26620 : if (!_cam_offset_m.get().is_zero()) {
690 : // user has specifed offset for camera
691 : // take its height into account while calculating distance
692 0 : cam_pos_ned_m = _inertial_data_delayed->Tbn * _cam_offset_m;
693 : }
694 26620 : if (_backend->distance_to_target() > 0.0f) {
695 : // sensor has provided distance to landing target
696 7913 : dist_to_target_m = _backend->distance_to_target();
697 : } else {
698 : // sensor only knows the horizontal location of the landing target
699 : // rely on rangefinder for the vertical target
700 18707 : dist_to_target_along_av_m = MAX(rangefinder_alt_m - cam_pos_ned_m.projected(approach_vector_NED_m).length(), 0.0f);
701 18707 : dist_to_target_m = dist_to_target_along_av_m / target_vec_unit_ned.projected(approach_vector_NED_m).length();
702 : }
703 :
704 : // Compute camera position relative to IMU
705 26620 : const Vector3f accel_pos_ned_m = _inertial_data_delayed->Tbn * AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index());
706 26620 : const Vector3f cam_pos_ned_rel_imu_ned_m = cam_pos_ned_m - accel_pos_ned_m;
707 :
708 : // Compute target position relative to IMU
709 26620 : _target_pos_rel_meas_ned_m = (target_vec_unit_ned * dist_to_target_m) + cam_pos_ned_rel_imu_ned_m;
710 :
711 : // store the current relative down position so that if we need to retry landing, we know at this height landing target can be found
712 26620 : const AP_AHRS &_ahrs = AP::ahrs();
713 26620 : Vector3p pos_NED;
714 26620 : if (_ahrs.get_relative_position_NED_origin(pos_NED)) {
715 26620 : _last_target_pos_rel_origin_ned_m.z = pos_NED.z;
716 26620 : _last_vehicle_pos_ned_m = pos_NED;
717 : }
718 26620 : return true;
719 : }
720 : }
721 311124 : return false;
722 : }
723 :
724 : // calculate target's position and velocity relative to the vehicle (used as input to position controller)
725 : // results are stored in_target_pos_rel_out_NE, _target_vel_rel_out_ne_ms
726 100339 : void AC_PrecLand::run_output_prediction()
727 : {
728 100339 : _target_pos_rel_out_ne_m = _target_pos_rel_est_ne_m;
729 100339 : _target_vel_rel_out_ne_ms = _target_vel_rel_est_ne_ms;
730 :
731 : // Predict forward from delayed time horizon
732 714063 : for (uint8_t i=1; i<_inertial_history->available(); i++) {
733 613724 : const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i];
734 613724 : _target_vel_rel_out_ne_ms.x -= inertial_data->correctedVehicleDeltaVelocityNED.x;
735 613724 : _target_vel_rel_out_ne_ms.y -= inertial_data->correctedVehicleDeltaVelocityNED.y;
736 613724 : _target_pos_rel_out_ne_m.x += _target_vel_rel_out_ne_ms.x * inertial_data->dt;
737 613724 : _target_pos_rel_out_ne_m.y += _target_vel_rel_out_ne_ms.y * inertial_data->dt;
738 : }
739 :
740 100339 : const AP_AHRS &_ahrs = AP::ahrs();
741 :
742 100339 : const Matrix3f& Tbn = (*_inertial_history)[_inertial_history->available()-1]->Tbn;
743 100339 : Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(_ahrs.get_primary_accel_index());
744 :
745 : // Apply position correction for CG offset from IMU
746 100339 : Vector3f imu_pos_ned = Tbn * accel_body_offset;
747 100339 : _target_pos_rel_out_ne_m.x += imu_pos_ned.x;
748 100339 : _target_pos_rel_out_ne_m.y += imu_pos_ned.y;
749 :
750 : // Apply position correction for body-frame horizontal camera offset from CG, so that vehicle lands lens-to-target
751 100339 : Vector3f cam_pos_horizontal_ned = Tbn * Vector3f{_cam_offset_m.get().x, _cam_offset_m.get().y, 0};
752 100339 : _target_pos_rel_out_ne_m.x -= cam_pos_horizontal_ned.x;
753 100339 : _target_pos_rel_out_ne_m.y -= cam_pos_horizontal_ned.y;
754 :
755 : // Apply velocity correction for IMU offset from CG
756 100339 : Vector3f vel_ned_rel_imu = Tbn * (_ahrs.get_gyro() % (-accel_body_offset));
757 100339 : _target_vel_rel_out_ne_ms.x -= vel_ned_rel_imu.x;
758 100339 : _target_vel_rel_out_ne_ms.y -= vel_ned_rel_imu.y;
759 :
760 : // remember vehicle velocity
761 100339 : UNUSED_RESULT(_ahrs.get_velocity_NED(_last_veh_velocity_NED_ms));
762 :
763 : // Apply land offset
764 100339 : Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f{_land_ofs_cm_x, _land_ofs_cm_y, 0} * 0.01f;
765 100339 : _target_pos_rel_out_ne_m.x += land_ofs_ned_m.x;
766 100339 : _target_pos_rel_out_ne_m.y += land_ofs_ned_m.y;
767 :
768 : // store the landing target as a offset from current position. This is used in landing retry
769 100339 : Vector2p last_target_loc_rel_origin_ne_m;
770 100339 : get_target_position_m(last_target_loc_rel_origin_ne_m);
771 100339 : _last_target_pos_rel_origin_ned_m.x = last_target_loc_rel_origin_ne_m.x;
772 100339 : _last_target_pos_rel_origin_ned_m.y = last_target_loc_rel_origin_ne_m.y;
773 :
774 : // record the last time there was a target output
775 100339 : _last_valid_target_ms = AP_HAL::millis();
776 100339 : }
777 :
778 : /*
779 : get target location lat/lon. Note that altitude in returned
780 : location is not reliable
781 : */
782 463 : bool AC_PrecLand::get_target_location(Location &loc)
783 : {
784 463 : if (!target_acquired()) {
785 0 : return false;
786 : }
787 463 : if (!AP::ahrs().get_origin(loc)) {
788 0 : return false;
789 : }
790 463 : loc.offset(_last_target_pos_rel_origin_ned_m.x, _last_target_pos_rel_origin_ned_m.y);
791 463 : loc.offset_up_m(-_last_target_pos_rel_origin_ned_m.z);
792 463 : return true;
793 : }
794 :
795 : /*
796 : get the absolute velocity of the target in m/s.
797 : return false if we cannot estimate target velocity or if the target is not acquired
798 : */
799 386 : bool AC_PrecLand::get_target_velocity(Vector2f& target_vel)
800 : {
801 386 : if (!(_options & PLND_OPTION_MOVING_TARGET)) {
802 : // the target should not be moving
803 386 : return false;
804 : }
805 0 : if ((EstimatorType)_estimator_type.get() == EstimatorType::RAW_SENSOR) {
806 0 : return false;
807 : }
808 0 : Vector2f target_vel_rel_ne_ms;
809 0 : if (!get_target_velocity_relative_NE_ms(target_vel_rel_ne_ms)) {
810 0 : return false;
811 : }
812 : // return the absolute velocity
813 0 : target_vel = (target_vel_rel_ne_ms) + _last_veh_velocity_NED_ms.xy();
814 0 : return true;
815 : }
816 :
817 : #if HAL_LOGGING_ENABLED
818 : // Write a precision landing entry
819 24903 : void AC_PrecLand::Write_Precland()
820 : {
821 : // exit immediately if not enabled
822 24903 : if (!enabled()) {
823 224 : return;
824 : }
825 :
826 24679 : Vector2f target_pos_rel_ne_m;
827 24679 : Vector2f target_vel_rel_ne_ms;
828 24679 : Vector3f target_pos_meas_ned_m;
829 24679 : get_target_position_relative_NE_m(target_pos_rel_ne_m);
830 24679 : get_target_velocity_relative_NE_ms(target_vel_rel_ne_ms);
831 24679 : get_target_position_measurement_NED_m(target_pos_meas_ned_m);
832 :
833 24679 : const struct log_Precland pkt {
834 : LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG),
835 24679 : time_us : AP_HAL::micros64(),
836 49358 : healthy : healthy(),
837 49358 : target_acquired : target_acquired(),
838 24679 : pos_x : target_pos_rel_ne_m.x,
839 24679 : pos_y : target_pos_rel_ne_m.y,
840 24679 : vel_x : target_vel_rel_ne_ms.x,
841 24679 : vel_y : target_vel_rel_ne_ms.y,
842 24679 : meas_x : target_pos_meas_ned_m.x,
843 24679 : meas_y : target_pos_meas_ned_m.y,
844 24679 : meas_z : target_pos_meas_ned_m.z,
845 24679 : last_meas : last_backend_los_meas_ms(),
846 24679 : ekf_outcount : ekf_outlier_count(),
847 49358 : estimator : (uint8_t)_estimator_type
848 24679 : };
849 24679 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
850 : }
851 : #endif
852 :
853 : // singleton instance
854 : AC_PrecLand *AC_PrecLand::_singleton;
855 :
856 : namespace AP {
857 :
858 82533 : AC_PrecLand *ac_precland()
859 : {
860 82533 : return AC_PrecLand::get_singleton();
861 : }
862 :
863 : }
864 :
865 : #endif // AC_PRECLAND_ENABLED
|
