Plane: fixed servo mixing for AFS and failsafe case

ArduPlane/Plane.h

@@ -788,8 +788,6 @@ private:
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
 
-    bool demoing_servos = false;
-
     // use this to prevent recursion during sensor init
     bool in_mavlink_delay = false;
 

ArduPlane/afs_plane.cpp

@@ -28,6 +28,8 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
     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));
 
+    plane.servos_output();
+
     // 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);
@@ -38,12 +40,6 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
     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);
 
-    ch_roll->output();
-    ch_pitch->output();
-    ch_yaw->output();
-    ch_throttle->output();
-    RC_Channel_aux::output_ch_all();
-
     plane.quadplane.afs_terminate();
     
     // also disarm to ensure that ignition is cut

ArduPlane/failsafe.cpp

@@ -87,15 +87,6 @@ void Plane::failsafe_check(void)
             return;
         }
 
-        if (!demoing_servos) {
-            channel_roll->output();
-            channel_pitch->output();
-        }
-        channel_throttle->output();
-        if (g.rudder_only == 0) {
-            channel_rudder->output();
-        }
-
         // setup secondary output channels that do have
         // corresponding input channels
         RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true);
@@ -104,6 +95,8 @@ void Plane::failsafe_check(void)
         RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, 0);
         RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, 0);
 
+        servos_output();
+        
         // setup flaperons
         flaperon_update(0);
     }

ArduPlane/radio.cpp

@@ -94,9 +94,13 @@ void Plane::init_rc_out_aux()
     update_aux();
     RC_Channel_aux::enable_aux_servos();
 
+    hal.rcout->cork();
+    
     // Initialization of servo outputs
     RC_Channel::output_trim_all();
 
+    servos_output();
+    
     // setup PWM values to send if the FMU firmware dies
     RC_Channel::setup_failsafe_trim_all();  
 }

ArduPlane/servos.cpp

@@ -616,6 +616,11 @@ void Plane::set_servos_flaps(void)
 */
 void Plane::set_servos(void)
 {
+    // start with output corked. the cork is released when we run
+    // servos_output(), which is run from all code paths in this
+    // function
+    hal.rcout->cork();
+    
     // this is to allow the failsafe module to deliberately crash 
     // the plane. Only used in extreme circumstances to meet the
     // OBC rules