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

@@ -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;
 

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

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
 };
 

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;