Plane: support twin-engine planes

with differential thrust for yaw

ArduPlane/Plane.h

@@ -1009,6 +1009,7 @@ private:
     void servo_output_mixers(void);
     void servos_output(void);
     void servos_auto_trim(void);
+    void servos_twin_engine_mix();
     void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
     bool allow_reverse_thrust(void);
     void update_aux();

ArduPlane/radio.cpp

@@ -29,6 +29,9 @@ void Plane::set_control_channels(void)
     } else {
         // reverse thrust
         channel_throttle->set_angle(100);
+        SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
+        SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 100);
+        SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 100);
     }
 
     if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) {

ArduPlane/servos.cpp

@@ -595,6 +595,28 @@ void Plane::servo_output_mixers(void)
     }
 }
 
+/*
+  support for twin-engine planes
+ */
+void Plane::servos_twin_engine_mix(void)
+{
+    float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
+    float rudder = SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / float(SERVO_MAX);
+    float throttle_left, throttle_right;
+    
+    if (throttle < 0 && aparm.throttle_min < 0) {
+        // doing reverse thrust
+        throttle_left  = constrain_float(throttle + 50 * rudder, -100, 0);
+        throttle_right = constrain_float(throttle - 50 * rudder, -100, 0);
+    } else {
+        // doing forward thrust
+        throttle_left  = constrain_float(throttle + 50 * rudder, 0, 100);
+        throttle_right = constrain_float(throttle - 50 * rudder, 0, 100);
+    }
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_left);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_right);
+}
+
 
 /*
   Set the flight control servos based on the current calculated values
@@ -733,6 +755,9 @@ void Plane::set_servos(void)
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, override_pct);
     }
 
+    // support twin-engine aircraft
+    servos_twin_engine_mix();
+    
     // run output mixer and send values to the hal for output
     servos_output();
 }