APM_OBC: setup termination values in PX4IO

this sets up the PX4IO board with failsafe values in case the FMU is
not running

libraries/APM_OBC/APM_OBC.cpp

@@ -22,6 +22,8 @@
 */
 #include <AP_HAL.h>
 #include <APM_OBC.h>
+#include <RC_Channel.h>
+#include <RC_Channel_aux.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -241,3 +243,67 @@ APM_OBC::check_altlimit(void)
     // all OK
     return false;
 }
+
+/*
+  setup the IO boards failsafe values for if the FMU firmware crashes
+ */
+void APM_OBC::setup_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_MAX));
+    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_MAX);
+    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);
+}
+
+/*
+  setu radio_out values for all channels to termination values if we
+  are terminating
+ */
+void APM_OBC::check_crash_plane(void)
+{
+    // ensure failsafe values are setup for if FMU crashes on PX4/Pixhawk
+    if (!_failsafe_setup) {
+        _failsafe_setup = true;
+        setup_failsafe();
+    }
+
+	// should we crash the plane? Only possible with
+	// FS_TERM_ACTTION set to 42
+    if (!_terminate || _terminate_action != 42) {
+        // not terminating
+        return;
+	}
+
+    // 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->radio_out     = ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    ch_pitch->radio_out    = ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    ch_yaw->radio_out      = ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    ch_throttle->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_MAX);
+    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);
+}

libraries/APM_OBC/APM_OBC.h

@@ -27,6 +27,7 @@
 #include <AP_Mission.h>
 #include <AP_Baro.h>
 #include <AP_GPS.h>
+#include <AP_RCMapper.h>
 #include <inttypes.h>
 
 
@@ -47,10 +48,11 @@ public:
 	};
 
 	// Constructor
-    APM_OBC(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps) :
+    APM_OBC(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap) :
         mission(_mission),
         baro(_baro),
-        gps(_gps)
+        gps(_gps),
+        rcmap(_rcmap)
         {
             AP_Param::setup_object_defaults(this, var_info);
             
@@ -64,11 +66,8 @@ public:
 
 	void check(enum control_mode control_mode, uint32_t last_heartbeat_ms);
 
-	// should we crash the plane? Only possible with
-	// FS_TERM_ACTTION set to 42
-	bool crash_plane(void) {
-		return _terminate && _terminate_action == 42;
-	}
+    // called in servo output code to set servos to crash position if needed
+	void check_crash_plane(void);
 
 	// for holding parameters
 	static const struct AP_Param::GroupInfo var_info[];
@@ -79,6 +78,7 @@ private:
     AP_Mission &mission;
     AP_Baro &baro;
     const AP_GPS &gps;
+    const RCMapper &rcmap;
 
 	// digital output pins for communicating with the failsafe board
 	AP_Int8 _heartbeat_pin;
@@ -105,6 +105,12 @@ private:
 	// saved waypoint for resuming mission
 	uint8_t _saved_wp;
 
+    // have the failsafe values been setup?
+    bool _failsafe_setup:1;
+
+    // setup failsafe values for if FMU firmware stops running
+    void setup_failsafe(void);
+
 	bool check_altlimit(void);
 };