From 7ab7770c3d74538e1ebf85e18d026f2e700d488c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 28 Aug 2012 15:51:32 +1000 Subject: [PATCH] APM: added the ability to forcibly crash the plane on OBC failsafe breach this allows a user to setup the OBC failsafe system to forcibly crash the plane (surfaces at limits, zero throttle) when the failsafe system triggers. This is to allow APM to be used in the Outback Challenge. In the OBC an external failsafe board also does this using the heartbeat control pin, so this is an extra safety mechanism. To prevent users accidentially triggering a crash, this code only activates if FS_TERM_ACTION is set to to the magic value 42. --- ArduPlane/ArduPlane.pde | 9 +++++++++ ArduPlane/Attitude.pde | 24 +++++++++++++++++++----- libraries/APM_OBC/APM_OBC.cpp | 6 ++++++ libraries/APM_OBC/APM_OBC.h | 7 +++++++ 4 files changed, 41 insertions(+), 5 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 9072199d6d..ced391f05d 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1082,6 +1082,15 @@ static void update_current_flight_mode(void) nav_pitch_cd = constrain(nav_pitch_cd, 500, takeoff_pitch_cd); } + // don't use a pitch/roll integrators during takeoff if we are + // below minimum speed +#if APM_CONTROL == DISABLED + if (airspeed.use() && airspeed.get_airspeed() < g.flybywire_airspeed_min) { + g.pidServoPitch.reset_I(); + g.pidServoRoll.reset_I(); + } +#endif + // max throttle for takeoff g.channel_throttle.servo_out = g.throttle_max; diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index f219691349..5c290d973f 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -366,8 +366,6 @@ static void set_servos(void) } } else { if (g.mix_mode == 0) { - g.channel_roll.calc_pwm(); - g.channel_pitch.calc_pwm(); if (g_rc_function[RC_Channel_aux::k_aileron]) { g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out; g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm(); @@ -382,7 +380,6 @@ static void set_servos(void) g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); } - g.channel_rudder.calc_pwm(); #if THROTTLE_OUT == 0 g.channel_throttle.servo_out = 0; @@ -392,18 +389,35 @@ static void set_servos(void) if (suppress_throttle()) { g.channel_throttle.servo_out = 0; - g.channel_throttle.calc_pwm(); } #endif - g.channel_throttle.calc_pwm(); if (control_mode >= FLY_BY_WIRE_B) { /* only do throttle slew limiting in modes where throttle * control is automatic */ throttle_slew_limit(); } + +#if OBC_FAILSAFE == ENABLED + // this is to allow the failsafe module to deliberately crash + // the plane. Only used in extreme circumstances to meet the + // OBC rules + if (obc.crash_plane()) { + g.channel_roll.servo_out = -4500; + g.channel_pitch.servo_out = -4500; + g.channel_rudder.servo_out = -4500; + g.channel_throttle.servo_out = 0; + } +#endif + + + // push out the PWM values + g.channel_roll.calc_pwm(); + g.channel_pitch.calc_pwm(); + g.channel_throttle.calc_pwm(); + g.channel_rudder.calc_pwm(); } // Auto flap deployment diff --git a/libraries/APM_OBC/APM_OBC.cpp b/libraries/APM_OBC/APM_OBC.cpp index 96841b951f..6926a2d9f1 100644 --- a/libraries/APM_OBC/APM_OBC.cpp +++ b/libraries/APM_OBC/APM_OBC.cpp @@ -43,6 +43,12 @@ const AP_Param::GroupInfo APM_OBC::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("TERMINATE", 5, APM_OBC, _terminate, 0), + // @Param: TERM_ACTION + // @DisplayName: Terminate action + // @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 APM to handle it here. If set to 0 (which is the default) then no extra action is taken. If set to the magic value 42 then the plane will deliberately crash itself by setting maximum throws on all surfaces, and zero throttle + // @User: Advanced + AP_GROUPINFO("TERM_ACTION", 6, APM_OBC, _terminate_action, 0), + AP_GROUPEND }; diff --git a/libraries/APM_OBC/APM_OBC.h b/libraries/APM_OBC/APM_OBC.h index 67c7b664c4..6bdcd71d8e 100644 --- a/libraries/APM_OBC/APM_OBC.h +++ b/libraries/APM_OBC/APM_OBC.h @@ -50,6 +50,12 @@ public: uint32_t last_heartbeat_ms, uint32_t last_gps_fix_ms); + // should we crash the plane? Only possible with + // FS_TERM_ACTTION set to 43 + bool crash_plane(void) { + return _terminate && _terminate_action == 42; + } + // for holding parameters static const struct AP_Param::GroupInfo var_info[]; @@ -60,6 +66,7 @@ private: AP_Int8 _heartbeat_pin; AP_Int8 _manual_pin; AP_Int8 _terminate; + AP_Int8 _terminate_action; // last pins to cope with changing at runtime int8_t _last_heartbeat_pin;