Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : AP_AdvancedFailsafe.cpp
18 :
19 : This is an advanced failsafe module originally modelled on the
20 : failsafe rules of the Outback Challenge
21 : */
22 : #include "AP_AdvancedFailsafe.h"
23 :
24 : #if AP_ADVANCEDFAILSAFE_ENABLED
25 :
26 : #include <AP_HAL/AP_HAL.h>
27 : #include <RC_Channel/RC_Channel.h>
28 : #include <SRV_Channel/SRV_Channel.h>
29 : #include <GCS_MAVLink/GCS.h>
30 : #include <AP_GPS/AP_GPS.h>
31 : #include <AP_Baro/AP_Baro.h>
32 : #include <AP_Mission/AP_Mission.h>
33 : #include <AC_Fence/AC_Fence.h>
34 :
35 : AP_AdvancedFailsafe *AP_AdvancedFailsafe::_singleton;
36 :
37 : extern const AP_HAL::HAL& hal;
38 :
39 : // table of user settable parameters
40 : const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = {
41 : // @Param: ENABLE
42 : // @DisplayName: Enable Advanced Failsafe
43 : // @Description: This enables the advanced failsafe system. If this is set to zero (disable) then all the other AFS options have no effect
44 : // @User: Advanced
45 : AP_GROUPINFO_FLAGS("ENABLE", 11, AP_AdvancedFailsafe, _enable, 0, AP_PARAM_FLAG_ENABLE),
46 :
47 : // @Param: MAN_PIN
48 : // @DisplayName: Manual Pin
49 : // @Description: This sets a digital output pin to set high when in manual mode. See the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
50 : // @User: Advanced
51 : AP_GROUPINFO("MAN_PIN", 0, AP_AdvancedFailsafe, _manual_pin, -1),
52 :
53 : // @Param: HB_PIN
54 : // @DisplayName: Heartbeat Pin
55 : // @Description: This sets a digital output pin which is cycled at 10Hz when termination is not activated. Note that if a FS_TERM_PIN is set then the heartbeat pin will continue to cycle at 10Hz when termination is activated, to allow the termination board to distinguish between autopilot crash and termination. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
56 : // @User: Advanced
57 : // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5
58 : // @Range: -1 127
59 : AP_GROUPINFO("HB_PIN", 1, AP_AdvancedFailsafe, _heartbeat_pin, -1),
60 :
61 : // @Param: WP_COMMS
62 : // @DisplayName: Comms Waypoint
63 : // @Description: Waypoint number to navigate to on comms loss
64 : // @User: Advanced
65 : AP_GROUPINFO("WP_COMMS", 2, AP_AdvancedFailsafe, _wp_comms_hold, 0),
66 :
67 : // @Param: WP_GPS_LOSS
68 : // @DisplayName: GPS Loss Waypoint
69 : // @Description: Waypoint number to navigate to on GPS lock loss
70 : // @User: Advanced
71 : AP_GROUPINFO("WP_GPS_LOSS", 4, AP_AdvancedFailsafe, _wp_gps_loss, 0),
72 :
73 : // @Param: TERMINATE
74 : // @DisplayName: Force Terminate
75 : // @Description: Can be set in flight to force termination of the heartbeat signal
76 : // @User: Advanced
77 : AP_GROUPINFO("TERMINATE", 5, AP_AdvancedFailsafe, _terminate, 0),
78 :
79 : // @Param: TERM_ACTION
80 : // @DisplayName: Terminate action
81 : // @Description: This can be used to force an action on flight termination. Normally this is handled by an external failsafe board, but you can setup ArduPilot to handle it here. Please consult the wiki for more information on the possible values of the parameter
82 : // @User: Advanced
83 : AP_GROUPINFO("TERM_ACTION", 6, AP_AdvancedFailsafe, _terminate_action, 0),
84 :
85 : // @Param: TERM_PIN
86 : // @DisplayName: Terminate Pin
87 : // @Description: This sets a digital output pin to set high on flight termination. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
88 : // @User: Advanced
89 : // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5
90 : // @Range: -1 127
91 : AP_GROUPINFO("TERM_PIN", 7, AP_AdvancedFailsafe, _terminate_pin, -1),
92 :
93 : // @Param: AMSL_LIMIT
94 : // @DisplayName: AMSL limit
95 : // @Description: This sets the AMSL (above mean sea level) altitude limit. If the pressure altitude determined by QNH exceeds this limit then flight termination will be forced. Note that this limit is in meters, whereas pressure altitude limits are often quoted in feet. A value of zero disables the pressure altitude limit.
96 : // @User: Advanced
97 : // @Units: m
98 : AP_GROUPINFO("AMSL_LIMIT", 8, AP_AdvancedFailsafe, _amsl_limit, 0),
99 :
100 : // @Param: AMSL_ERR_GPS
101 : // @DisplayName: Error margin for GPS based AMSL limit
102 : // @Description: This sets margin for error in GPS derived altitude limit. This error margin is only used if the barometer has failed. If the barometer fails then the GPS will be used to enforce the AMSL_LIMIT, but this margin will be subtracted from the AMSL_LIMIT first, to ensure that even with the given amount of GPS altitude error the pressure altitude is not breached. OBC users should set this to comply with their D2 safety case. A value of -1 will mean that barometer failure will lead to immediate termination.
103 : // @User: Advanced
104 : // @Units: m
105 : AP_GROUPINFO("AMSL_ERR_GPS", 9, AP_AdvancedFailsafe, _amsl_margin_gps, -1),
106 :
107 : // @Param: QNH_PRESSURE
108 : // @DisplayName: QNH pressure
109 : // @Description: This sets the QNH pressure in millibars to be used for pressure altitude in the altitude limit. A value of zero disables the altitude limit.
110 : // @Units: hPa
111 : // @User: Advanced
112 : AP_GROUPINFO("QNH_PRESSURE", 10, AP_AdvancedFailsafe, _qnh_pressure, 0),
113 :
114 : // *NOTE* index 11 is "Enable" and is moved to the top to allow AP_PARAM_FLAG_ENABLE
115 :
116 : // *NOTE* index 12 of AP_Int16 RC_FAIL_MS was depreciated. Replaced by RC_FAIL_TIME.
117 :
118 : // @Param: MAX_GPS_LOSS
119 : // @DisplayName: Maximum number of GPS loss events
120 : // @Description: Maximum number of GPS loss events before the aircraft stops returning to mission on GPS recovery. Use zero to allow for any number of GPS loss events.
121 : // @User: Advanced
122 : AP_GROUPINFO("MAX_GPS_LOSS", 13, AP_AdvancedFailsafe, _max_gps_loss, 0),
123 :
124 : // @Param: MAX_COM_LOSS
125 : // @DisplayName: Maximum number of comms loss events
126 : // @Description: Maximum number of comms loss events before the aircraft stops returning to mission on comms recovery. Use zero to allow for any number of comms loss events.
127 : // @User: Advanced
128 : AP_GROUPINFO("MAX_COM_LOSS", 14, AP_AdvancedFailsafe, _max_comms_loss, 0),
129 :
130 : // @Param: GEOFENCE
131 : // @DisplayName: Enable geofence Advanced Failsafe
132 : // @Description: This enables the geofence part of the AFS. Will only be in effect if AFS_ENABLE is also 1
133 : // @User: Advanced
134 : AP_GROUPINFO("GEOFENCE", 15, AP_AdvancedFailsafe, _enable_geofence_fs, 1),
135 :
136 : // @Param: RC
137 : // @DisplayName: Enable RC Advanced Failsafe
138 : // @Description: This enables the RC part of the AFS. Will only be in effect if AFS_ENABLE is also 1
139 : // @User: Advanced
140 : AP_GROUPINFO("RC", 16, AP_AdvancedFailsafe, _enable_RC_fs, 1),
141 :
142 : // @Param: RC_MAN_ONLY
143 : // @DisplayName: Enable RC Termination only in manual control modes
144 : // @Description: If this parameter is set to 1, then an RC loss will only cause the plane to terminate in manual control modes. If it is 0, then the plane will terminate in any flight mode.
145 : // @User: Advanced
146 : AP_GROUPINFO("RC_MAN_ONLY", 17, AP_AdvancedFailsafe, _rc_term_manual_only, 1),
147 :
148 : // @Param: DUAL_LOSS
149 : // @DisplayName: Enable dual loss terminate due to failure of both GCS and GPS simultaneously
150 : // @Description: This enables the dual loss termination part of the AFS system. If this parameter is 1 and both GPS and the ground control station fail simultaneously, this will be considered a "dual loss" and cause termination.
151 : // @User: Advanced
152 : AP_GROUPINFO("DUAL_LOSS", 18, AP_AdvancedFailsafe, _enable_dual_loss, 1),
153 :
154 : // @Param: RC_FAIL_TIME
155 : // @DisplayName: RC failure time
156 : // @Description: This is the time in seconds in manual mode that failsafe termination will activate if RC input is lost. For the OBC rules this should be (1.5). Use 0 to disable.
157 : // @User: Advanced
158 : // @Units: s
159 : AP_GROUPINFO("RC_FAIL_TIME", 19, AP_AdvancedFailsafe, _rc_fail_time_seconds, 0),
160 :
161 : // @Param: MAX_RANGE
162 : // @DisplayName: Max allowed range
163 : // @Description: This is the maximum range of the vehicle in kilometers from first arming. If the vehicle goes beyond this range then the TERM_ACTION is performed. A value of zero disables this feature.
164 : // @User: Advanced
165 : // @Units: km
166 : AP_GROUPINFO("MAX_RANGE", 20, AP_AdvancedFailsafe, _max_range_km, 0),
167 :
168 : // @Param: OPTIONS
169 : // @DisplayName: AFS options
170 : // @Description: See description for each bitmask bit description
171 : // @Bitmask: 0: Continue the mission even after comms are recovered (does not go to the mission item at the time comms were lost)
172 : // @Bitmask: 1: Enable AFS for all autonomous modes (not just AUTO)
173 : // @Bitmask: 2: Option to not jump to AFS_WP_COMMS if already in the return path
174 : AP_GROUPINFO("OPTIONS", 21, AP_AdvancedFailsafe, options, 0),
175 :
176 : // @Param: GCS_TIMEOUT
177 : // @DisplayName: GCS timeout
178 : // @Description: The time (in seconds) of persistent data link loss before GCS failsafe occurs.
179 : // @User: Advanced
180 : // @Units: s
181 : AP_GROUPINFO("GCS_TIMEOUT", 22, AP_AdvancedFailsafe, _gcs_fail_time_seconds, 10),
182 :
183 : AP_GROUPEND
184 : };
185 :
186 : // check for Failsafe conditions. This is called at 10Hz by the main
187 : // ArduPlane code
188 : void
189 547097 : AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
190 : {
191 547097 : if (!_enable) {
192 545335 : return;
193 : }
194 :
195 : #if AP_FENCE_ENABLED
196 : // we always check for fence breach
197 1762 : if(_enable_geofence_fs) {
198 1762 : const AC_Fence *ap_fence = AP::fence();
199 1762 : if ((ap_fence != nullptr && ap_fence->get_breaches() != 0) || check_altlimit()) {
200 11 : if (!_terminate) {
201 1 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to fence breach");
202 1 : _terminate.set_and_notify(1);
203 : }
204 : }
205 : }
206 : #endif
207 :
208 : // update max range check
209 1762 : max_range_update();
210 :
211 1762 : enum control_mode mode = afs_mode();
212 :
213 : // check if RC termination is enabled
214 : // check for RC failure in manual mode or RC failure when AFS_RC_MANUAL is 0
215 1151 : if (_state != STATE_PREFLIGHT && !_terminate && _enable_RC_fs &&
216 970 : (mode == AFS_MANUAL || mode == AFS_STABILIZED || !_rc_term_manual_only) &&
217 2924 : _rc_fail_time_seconds > 0 &&
218 11 : (AP_HAL::millis() - last_valid_rc_ms) > (_rc_fail_time_seconds * 1000.0f)) {
219 1 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to RC failure");
220 1 : _terminate.set_and_notify(1);
221 : }
222 :
223 : // tell the failsafe board if we are in manual control
224 : // mode. This tells it to pass through controls from the
225 : // receiver
226 1762 : if (_manual_pin != -1) {
227 0 : hal.gpio->pinMode(_manual_pin, HAL_GPIO_OUTPUT);
228 0 : hal.gpio->write(_manual_pin, mode==AFS_MANUAL);
229 : }
230 :
231 1762 : const uint32_t last_heartbeat_ms = gcs().sysid_mygcs_last_seen_time_ms();
232 1762 : uint32_t now = AP_HAL::millis();
233 1762 : bool gcs_link_ok = ((now - last_heartbeat_ms) < (_gcs_fail_time_seconds*1000.0f));
234 1762 : bool gps_lock_ok = ((now - AP::gps().last_fix_time_ms()) < 3000);
235 :
236 1762 : AP_Mission &mission = AP::mission();
237 :
238 1762 : switch (_state) {
239 611 : case STATE_PREFLIGHT:
240 : // we startup in preflight mode. This mode ends when
241 : // we first enter auto.
242 611 : if (mode == AFS_AUTO) {
243 2 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO");
244 2 : _state = STATE_AUTO;
245 : }
246 611 : break;
247 :
248 1123 : case STATE_AUTO:
249 : // this is the normal mode.
250 1123 : if (!gcs_link_ok) {
251 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: DATA_LINK_LOSS");
252 0 : _state = STATE_DATA_LINK_LOSS;
253 :
254 0 : if (should_use_comms_hold()) {
255 0 : _saved_wp = mission.get_current_nav_cmd().index;
256 0 : mission.set_current_cmd(_wp_comms_hold);
257 :
258 0 : if (mode == AFS_AUTO && option_is_set(Option::GCS_FS_ALL_AUTONOMOUS_MODES)) {
259 0 : set_mode_auto();
260 : }
261 : }
262 :
263 : // if two events happen within 30s we consider it to be part of the same event
264 0 : if (now - _last_comms_loss_ms > 30*1000UL) {
265 0 : _comms_loss_count++;
266 0 : _last_comms_loss_ms = now;
267 : }
268 0 : break;
269 : }
270 1123 : if (!gps_lock_ok) {
271 3 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: GPS_LOSS");
272 3 : _state = STATE_GPS_LOSS;
273 3 : if (_wp_gps_loss) {
274 0 : _saved_wp = mission.get_current_nav_cmd().index;
275 0 : mission.set_current_cmd(_wp_gps_loss);
276 : }
277 : // if two events happen within 30s we consider it to be part of the same event
278 3 : if (now - _last_gps_loss_ms > 30*1000UL) {
279 0 : _gps_loss_count++;
280 0 : _last_gps_loss_ms = now;
281 : }
282 3 : break;
283 : }
284 1120 : break;
285 :
286 0 : case STATE_DATA_LINK_LOSS:
287 0 : if (!gps_lock_ok) {
288 : // losing GPS lock when data link is lost
289 : // leads to termination if AFS_DUAL_LOSS is 1
290 0 : if(_enable_dual_loss) {
291 0 : if (!_terminate) {
292 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to dual loss");
293 0 : _terminate.set_and_notify(1);
294 : }
295 : }
296 0 : } else if (gcs_link_ok) {
297 0 : _state = STATE_AUTO;
298 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO, GCS now OK");
299 :
300 0 : if (option_is_set(Option::CONTINUE_AFTER_RECOVERED)) {
301 0 : break;
302 : }
303 :
304 : // we only return to the mission if we have not exceeded AFS_MAX_COM_LOSS
305 0 : if (_saved_wp != 0 &&
306 0 : (_max_comms_loss <= 0 ||
307 0 : _comms_loss_count <= _max_comms_loss)) {
308 0 : mission.set_current_cmd(_saved_wp);
309 0 : _saved_wp = 0;
310 : }
311 : }
312 0 : break;
313 :
314 28 : case STATE_GPS_LOSS:
315 28 : if (!gcs_link_ok) {
316 : // losing GCS link when GPS lock lost
317 : // leads to termination if AFS_DUAL_LOSS is 1
318 0 : if (!_terminate && _enable_dual_loss) {
319 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to dual loss");
320 0 : _terminate.set_and_notify(1);
321 : }
322 28 : } else if (gps_lock_ok) {
323 3 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO, GPS now OK");
324 3 : _state = STATE_AUTO;
325 : // we only return to the mission if we have not exceeded AFS_MAX_GPS_LOSS
326 3 : if (_saved_wp != 0 &&
327 0 : (_max_gps_loss <= 0 || _gps_loss_count <= _max_gps_loss)) {
328 0 : mission.set_current_cmd(_saved_wp);
329 0 : _saved_wp = 0;
330 : }
331 : }
332 28 : break;
333 : }
334 :
335 : // if we are not terminating or if there is a separate terminate
336 : // pin configured then toggle the heartbeat pin at 10Hz
337 1762 : heartbeat();
338 :
339 : // set the terminate pin
340 1762 : if (_terminate_pin != -1) {
341 0 : hal.gpio->pinMode(_terminate_pin, HAL_GPIO_OUTPUT);
342 0 : hal.gpio->write(_terminate_pin, _terminate?1:0);
343 : }
344 : }
345 :
346 :
347 : // send heartbeat messages during sensor calibration
348 : void
349 5188 : AP_AdvancedFailsafe::heartbeat(void)
350 : {
351 5188 : if (!_enable) {
352 3426 : return;
353 : }
354 :
355 : // if we are not terminating or if there is a separate terminate
356 : // pin configured then toggle the heartbeat pin at 10Hz
357 1762 : if (_heartbeat_pin != -1 && (_terminate_pin != -1 || !_terminate)) {
358 0 : _heartbeat_pin_value = !_heartbeat_pin_value;
359 0 : hal.gpio->pinMode(_heartbeat_pin, HAL_GPIO_OUTPUT);
360 0 : hal.gpio->write(_heartbeat_pin, _heartbeat_pin_value);
361 : }
362 : }
363 :
364 : bool
365 66 : AP_AdvancedFailsafe::gps_altitude_ok() const
366 : {
367 66 : if (_amsl_margin_gps == -1) {
368 0 : return false;
369 : }
370 66 : const AP_GPS &gps = AP::gps();
371 66 : if (gps.status() < AP_GPS_FixType::FIX_3D) {
372 11 : return false;
373 : }
374 55 : const auto &location = gps.location();
375 : float alt;
376 55 : if (!location.get_alt_m(Location::AltFrame::ABSOLUTE, alt)) {
377 0 : return false;
378 : }
379 55 : if (alt > _amsl_limit - _amsl_margin_gps) {
380 0 : return false;
381 : }
382 55 : return true;
383 : }
384 :
385 : // check for altitude limit breach
386 : bool
387 1762 : AP_AdvancedFailsafe::check_altlimit(void)
388 : {
389 1762 : if (!_enable) {
390 0 : return false;
391 : }
392 1762 : if (_amsl_limit == 0 || _qnh_pressure <= 0) {
393 : // no limit set
394 1503 : return false;
395 : }
396 :
397 : // see if the barometer is dead
398 259 : const AP_Baro &baro = AP::baro();
399 259 : if (AP_HAL::millis() - baro.get_last_update() > 5000) {
400 : // the barometer has been unresponsive for 5 seconds. See if we can switch to GPS
401 66 : if (gps_altitude_ok()) {
402 : // GPS based altitude OK
403 55 : return false;
404 : }
405 : // no barometer - immediate termination
406 11 : return true;
407 : }
408 :
409 193 : float alt_amsl = baro.get_altitude_difference(_qnh_pressure*100, baro.get_pressure());
410 193 : if (alt_amsl > _amsl_limit) {
411 : // pressure altitude breach
412 0 : return true;
413 : }
414 :
415 : // all OK
416 193 : return false;
417 : }
418 :
419 : /*
420 : return true if we should jump to _wp_comms_hold
421 : */
422 0 : bool AP_AdvancedFailsafe::should_use_comms_hold(void){
423 :
424 0 : if (_wp_comms_hold <= 0) {
425 0 : return false;
426 : }
427 :
428 0 : const AP_Mission &_mission = AP::mission();
429 0 : if ((_mission.state() == AP_Mission::MISSION_RUNNING) && _mission.get_in_return_path_flag() && option_is_set(Option::CONTINUE_IF_ALREADY_IN_RETURN_PATH)) {
430 0 : return false;
431 : }
432 :
433 0 : return true;
434 : }
435 : /*
436 : return true if we should crash the vehicle
437 : */
438 26119455 : bool AP_AdvancedFailsafe::should_crash_vehicle(void)
439 : {
440 26119455 : if (!_enable) {
441 26101369 : return false;
442 : }
443 : // ensure failsafe values are setup for if FMU crashes on PX4/Pixhawk
444 18086 : if (!_failsafe_setup) {
445 7 : _failsafe_setup = true;
446 7 : setup_IO_failsafe();
447 : }
448 :
449 : // determine if the vehicle should be crashed
450 : // only possible if FS_TERM_ACTION is 42 and _terminate is non zero
451 : // _terminate may be set via parameters, or a mavlink command
452 18086 : if (_terminate &&
453 718 : (_terminate_action == TERMINATE_ACTION_TERMINATE ||
454 18194 : _terminate_action == TERMINATE_ACTION_LAND)) {
455 : // we are terminating
456 610 : return true;
457 : }
458 :
459 : // continue flying
460 17476 : return false;
461 : }
462 :
463 : // update GCS based termination
464 : // returns true if AFS is in the desired termination state
465 5 : bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) {
466 5 : if (!_enable) {
467 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AFS not enabled, can't terminate the vehicle");
468 0 : return false;
469 : }
470 :
471 5 : _terminate.set_and_notify(should_terminate ? 1 : 0);
472 :
473 : // evaluate if we will crash or not, as AFS must be enabled, and TERM_ACTION has to be correct
474 5 : bool is_terminating = should_crash_vehicle();
475 :
476 5 : if(should_terminate == is_terminating) {
477 5 : if (is_terminating) {
478 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Terminating due to %s", reason);
479 : } else {
480 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Aborting termination due to %s", reason);
481 : }
482 5 : return true;
483 0 : } else if (should_terminate && _terminate_action != TERMINATE_ACTION_TERMINATE) {
484 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unable to terminate, termination is not configured");
485 : }
486 0 : return false;
487 : }
488 :
489 : /*
490 : update check of maximum range
491 : */
492 1762 : void AP_AdvancedFailsafe::max_range_update(void)
493 : {
494 1762 : if (_max_range_km <= 0) {
495 1762 : return;
496 : }
497 :
498 0 : if (!_have_first_location) {
499 0 : if (AP::gps().status() < AP_GPS_FixType::FIX_3D) {
500 : // wait for 3D fix
501 0 : return;
502 : }
503 0 : if (!hal.util->get_soft_armed()) {
504 : // wait for arming
505 0 : return;
506 : }
507 0 : _first_location = AP::gps().location();
508 0 : _have_first_location = true;
509 : }
510 :
511 0 : if (AP::gps().status() < AP_GPS_FixType::FIX_2D) {
512 : // don't trigger when dead-reckoning
513 0 : return;
514 : }
515 :
516 : // check distance from first location
517 0 : float distance_km = _first_location.get_distance(AP::gps().location()) * 0.001;
518 0 : if (distance_km > _max_range_km) {
519 0 : uint32_t now = AP_HAL::millis();
520 0 : if (now - _term_range_notice_ms > 5000) {
521 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to range %.1fkm", distance_km);
522 0 : _term_range_notice_ms = now;
523 : }
524 0 : _terminate.set_and_notify(1);
525 : }
526 : }
527 :
528 : namespace AP {
529 :
530 29 : AP_AdvancedFailsafe *advancedfailsafe()
531 : {
532 29 : return AP_AdvancedFailsafe::get_singleton();
533 : }
534 :
535 : };
536 :
537 : #endif // AP_ADVANCEDFAILSAFE_ENABLED
|
