Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AdvancedFailsafe: Allow the GCS to request terminations

Browse Source
This commit is contained in:
Michael du Breuil 2017-07-24 23:34:09 -07:00 committed by Francisco Ferreira
parent fc155eac7e
commit 97c1785bef
2 changed files with 38 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
View File

@ -25,6 +25,8 @@
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#define TERMINATE 42
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// table of user settable parameters // table of user settable parameters
@ -358,13 +360,40 @@ bool AP_AdvancedFailsafe::should_crash_vehicle(void)
setup_IO_failsafe(); setup_IO_failsafe();
} }
// should we crash the plane? Only possible with // determine if the vehicle should be crashed
// FS_TERM_ACTTION set to 42 // only possible if FS_TERM_ACTION is 42 and _terminate is non zero
if (!_terminate || _terminate_action != 42) { // _terminate may be set via parameters, or a mavlink command
// not terminating if (_terminate_action == TERMINATE && _terminate) {
// we are crashing
return true;
}
// continue flying
return false;
}
// update GCS based termination
// returns true if AFS is in the desired termination state
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate) {
if (!_enable) {
gcs().send_text(MAV_SEVERITY_INFO, "AFS not enabled, can't terminate the vehicle");
return false; return false;
} }
// we are crashing _terminate.set_and_notify(should_terminate ? 1 : 0);
// evaluate if we will crash or not, as AFS must be enabled, and TERM_ACTION has to be correct
bool is_terminating = should_crash_vehicle();
if(should_terminate == is_terminating) {
if (is_terminating) {
gcs().send_text(MAV_SEVERITY_INFO, "Terminating due to GCS request");
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Aborting termination due to GCS request");
}
return true; return true;
} else if (should_terminate && _terminate_action != TERMINATE) {
gcs().send_text(MAV_SEVERITY_INFO, "Unable to terminate, termination is not configured");
}
return false;
} }

3
libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h
View File

@ -72,6 +72,9 @@ public:
// return true if we are terminating (deliberately crashing the vehicle) // return true if we are terminating (deliberately crashing the vehicle)
bool should_crash_vehicle(void); bool should_crash_vehicle(void);
// enables or disables a GCS based termination, returns true if AFS is in the desired termination state
bool gcs_terminate(bool should_terminate);
// called to set all outputs to termination state // called to set all outputs to termination state
virtual void terminate_vehicle(void) = 0; virtual void terminate_vehicle(void) = 0;