From adb5a3ee1f8079438fffaef8695bcbbe5d256ce6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 22 Jul 2016 18:14:53 +1000 Subject: [PATCH] Plane: re-work AFS for new AP_AdvancedFailsafe API --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/Attitude.cpp | 13 ++++--- ArduPlane/Plane.h | 23 +++++++++++- ArduPlane/afs_plane.cpp | 78 +++++++++++++++++++++++++++++++++++++++++ ArduPlane/failsafe.cpp | 5 ++- 5 files changed, 113 insertions(+), 8 deletions(-) create mode 100644 ArduPlane/afs_plane.cpp diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index ed1dea3e0c..c3b934dedb 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -291,7 +291,7 @@ void Plane::update_logging2(void) void Plane::afs_fs_check(void) { // perform AFS failsafe checks - afs.check(AFS_MODE_PLANE(control_mode), failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms); + afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms); } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index cbb6251e02..3763e9133f 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -854,6 +854,14 @@ uint16_t Plane::throttle_min(void) const *****************************************/ void Plane::set_servos(void) { + // this is to allow the failsafe module to deliberately crash + // the plane. Only used in extreme circumstances to meet the + // OBC rules + if (afs.should_crash_vehicle()) { + afs.terminate_vehicle(); + return; + } + int16_t last_throttle = channel_throttle->get_radio_out(); // do any transition updates for quadplane @@ -1219,11 +1227,6 @@ void Plane::set_servos(void) } } - // this is to allow the failsafe module to deliberately crash - // the plane. Only used in extreme circumstances to meet the - // OBC rules - afs.check_crash_plane(); - #if HIL_SUPPORT if (g.hil_mode == 1) { // get the servos to the GCS immediately for HIL diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index e6d8559557..00f3c65ff6 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -126,6 +126,26 @@ protected: bool ins_checks(bool report); }; + +/* + a plane specific AP_AdvancedFailsafe class + */ +class AP_AdvancedFailsafe_Plane : public AP_AdvancedFailsafe +{ +public: + AP_AdvancedFailsafe_Plane(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap); + + // called to set all outputs to termination state + void terminate_vehicle(void); + +protected: + // setup failsafe values for if FMU firmware stops running + void setup_IO_failsafe(void); + + // return the AFS mapped control mode + enum control_mode afs_mode(void); +}; + /* main APM:Plane class */ @@ -137,6 +157,7 @@ public: friend class AP_Arming_Plane; friend class QuadPlane; friend class AP_Tuning_Plane; + friend class AP_AdvancedFailsafe_Plane; Plane(void); @@ -632,7 +653,7 @@ private: AP_ADSB adsb {ahrs}; // Outback Challenge Failsafe Support - AP_AdvancedFailsafe afs {mission, barometer, gps, rcmap}; + AP_AdvancedFailsafe_Plane afs {mission, barometer, gps, rcmap}; /* meta data to support counting the number of circles in a loiter diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp new file mode 100644 index 0000000000..aee94667db --- /dev/null +++ b/ArduPlane/afs_plane.cpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + plane specific AP_AdvancedFailsafe class + */ + +#include "Plane.h" + +// Constructor +AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap) : + AP_AdvancedFailsafe(_mission, _baro, _gps, _rcmap) +{} + + +/* + setup radio_out values for all channels to termination values + */ +void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) +{ + // we are terminating. Setup primary output channels radio_out values + RC_Channel *ch_roll = RC_Channel::rc_channel(rcmap.roll()-1); + 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); +} + +void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void) +{ + 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); +} + +/* + return an AFS_MODE for current control mode + */ +AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void) +{ + if (plane.auto_throttle_mode) { + return AP_AdvancedFailsafe::AFS_AUTO; + } + if (plane.control_mode == MANUAL) { + return AP_AdvancedFailsafe::AFS_MANUAL; + } + return AP_AdvancedFailsafe::AFS_STABILIZED; +} diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 12ad42661e..c1374b0f01 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -82,7 +82,10 @@ void Plane::failsafe_check(void) // this is to allow the failsafe module to deliberately crash // the plane. Only used in extreme circumstances to meet the // OBC rules - afs.check_crash_plane(); + if (afs.should_crash_vehicle()) { + afs.terminate_vehicle(); + return; + } if (!demoing_servos) { channel_roll->output();