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

AP_AdvancedFailsafe: re-work for use as vehicle derived class

Browse Source
This commit is contained in:
Andrew Tridgell 2016-07-22 18:14:30 +10:00
parent 5d6dfd927b
commit 838728348f
2 changed files with 28 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
View File

@ -152,7 +152,7 @@ const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = {
// check for Failsafe conditions. This is called at 10Hz by the main // check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code // ArduPlane code
void void
AP_AdvancedFailsafe::check(AP_AdvancedFailsafe::control_mode mode, uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms) AP_AdvancedFailsafe::check(uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms)
{ {
if (!_enable) { if (!_enable) {
return; return;
@ -167,10 +167,12 @@ AP_AdvancedFailsafe::check(AP_AdvancedFailsafe::control_mode mode, uint32_t last
} }
} }
enum control_mode mode = afs_mode();
// check if RC termination is enabled // check if RC termination is enabled
// check for RC failure in manual mode or RC failure when AFS_RC_MANUAL is 0 // check for RC failure in manual mode or RC failure when AFS_RC_MANUAL is 0
if (_state != STATE_PREFLIGHT && !_terminate && _enable_RC_fs && if (_state != STATE_PREFLIGHT && !_terminate && _enable_RC_fs &&
(mode == AFS_MANUAL || mode == AFS_FBW || !_rc_term_manual_only) && (mode == AFS_MANUAL || mode == AFS_STABILIZED || !_rc_term_manual_only) &&
_rc_fail_time_seconds > 0 && _rc_fail_time_seconds > 0 &&
(AP_HAL::millis() - last_valid_rc_ms) > (_rc_fail_time_seconds * 1000.0f)) { (AP_HAL::millis() - last_valid_rc_ms) > (_rc_fail_time_seconds * 1000.0f)) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "RC failure terminate"); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "RC failure terminate");
@ -344,75 +346,26 @@ AP_AdvancedFailsafe::check_altlimit(void)
} }
/* /*
setup the IO boards failsafe values for if the FMU firmware crashes return true if we should crash the vehicle
*/ */
void AP_AdvancedFailsafe::setup_failsafe(void) bool AP_AdvancedFailsafe::should_crash_vehicle(void)
{ {
if (!_enable) { if (!_enable) {
return; return false;
}
const RC_Channel *ch_roll = RC_Channel::rc_channel(rcmap.roll()-1);
const RC_Channel *ch_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
const RC_Channel *ch_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
const RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
// setup primary channel output values
hal.rcout->set_failsafe_pwm(1U<<(rcmap.roll()-1), ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.pitch()-1), ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.yaw()-1), ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.throttle()-1), ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
// and all aux channels
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
}
/*
setu radio_out values for all channels to termination values if we
are terminating
*/
void AP_AdvancedFailsafe::check_crash_plane(void)
{
if (!_enable) {
return;
} }
// ensure failsafe values are setup for if FMU crashes on PX4/Pixhawk // ensure failsafe values are setup for if FMU crashes on PX4/Pixhawk
if (!_failsafe_setup) { if (!_failsafe_setup) {
_failsafe_setup = true; _failsafe_setup = true;
setup_failsafe(); setup_IO_failsafe();
} }
// should we crash the plane? Only possible with // should we crash the plane? Only possible with
// FS_TERM_ACTTION set to 42 // FS_TERM_ACTTION set to 42
if (!_terminate || _terminate_action != 42) { if (!_terminate || _terminate_action != 42) {
// not terminating // not terminating
return; return false;
} }
// we are terminating. Setup primary output channels radio_out values // we are crashing
RC_Channel *ch_roll = RC_Channel::rc_channel(rcmap.roll()-1); return true;
RC_Channel *ch_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
RC_Channel *ch_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
ch_roll->set_radio_out(ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
ch_pitch->set_radio_out(ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
ch_yaw->set_radio_out(ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
ch_throttle->set_radio_out(ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
// and all aux channels
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
} }

29
libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h
View File

@ -34,9 +34,9 @@ class AP_AdvancedFailsafe
{ {
public: public:
enum control_mode { enum control_mode {
AFS_MANUAL = 0, AFS_MANUAL = 0,
AFS_FBW = 1, AFS_STABILIZED = 1,
AFS_AUTO = 2 AFS_AUTO = 2
}; };
enum state { enum state {
@ -64,19 +64,28 @@ public:
} }
// check that everything is OK // check that everything is OK
void check(enum control_mode control_mode, uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms); void check(uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms);
// generate heartbeat msgs, so external failsafe boards are happy // generate heartbeat msgs, so external failsafe boards are happy
// during sensor calibration // during sensor calibration
void heartbeat(void); void heartbeat(void);
// called in servo output code to set servos to crash position if needed // return true if we are terminating (deliberately crashing the vehicle)
void check_crash_plane(void); bool should_crash_vehicle(void);
// for holding parameters // for holding parameters
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: // called to set all outputs to termination state
virtual void terminate_vehicle(void) = 0;
protected:
// setup failsafe values for if FMU firmware stops running
virtual void setup_IO_failsafe(void) = 0;
// return the AFS mapped control mode
virtual enum control_mode afs_mode(void) = 0;
enum state _state; enum state _state;
AP_Mission &mission; AP_Mission &mission;
@ -127,11 +136,5 @@ private:
// have the failsafe values been setup? // have the failsafe values been setup?
bool _failsafe_setup:1; bool _failsafe_setup:1;
// setup failsafe values for if FMU firmware stops running
void setup_failsafe(void);
bool check_altlimit(void); bool check_altlimit(void);
}; };
// map from ArduPlane control_mode to AP_AdvancedFailsafe::control_mode
#define AFS_MODE_PLANE(control_mode) (auto_throttle_mode?AP_AdvancedFailsafe::AFS_AUTO:(control_mode==MANUAL?AP_AdvancedFailsafe::AFS_MANUAL:AP_AdvancedFailsafe::AFS_FBW))