From a23c373f16e108d6fc30c1ef36cac40dd50e1a4d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 12 Oct 2016 09:29:37 +1100
Subject: [PATCH] Plane: fixed servo mixing for AFS and failsafe case

---
 ArduPlane/Plane.h       |  2 --
 ArduPlane/afs_plane.cpp |  8 ++------
 ArduPlane/failsafe.cpp  | 11 ++---------
 ArduPlane/radio.cpp     |  4 ++++
 ArduPlane/servos.cpp    |  5 +++++
 5 files changed, 13 insertions(+), 17 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index eb36fb0a10..417bf990d2 100644
--- a/ArduPlane/Plane.h
+++ b/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;
 
diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp
index 06592550c7..1d492a29a2 100644
--- a/ArduPlane/afs_plane.cpp
+++ b/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
diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp
index c1374b0f01..a38365983a 100644
--- a/ArduPlane/failsafe.cpp
+++ b/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);
     }
diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp
index ef84a887c0..23fbed58ab 100644
--- a/ArduPlane/radio.cpp
+++ b/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();  
 }
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index 1b75c86612..18f54d2422 100644
--- a/ArduPlane/servos.cpp
+++ b/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