From 0fa1ddf7c2999e8042c20c035fafa74cfeed969b Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 13 Nov 2021 23:57:05 +0000
Subject: [PATCH] Plane: tailsitter: always give pitch prority elevon mixing in
 VTOL modes

---
 ArduPlane/Plane.h        |  1 -
 ArduPlane/servos.cpp     | 23 +++++++----------------
 ArduPlane/tailsitter.cpp | 28 +++++++++++++++++++++++++++-
 3 files changed, 34 insertions(+), 18 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index ae02b3ec19..27ce3cdd55 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -1086,7 +1086,6 @@ private:
     void set_landing_gear(void);
     void dspoiler_update(void);
     void airbrake_update(void);
-    void servo_output_mixers(void);
     void landing_neutral_control_surface_servos(void);
     void servos_output(void);
     void servos_auto_trim(void);
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index 787e94d4d5..5e895cdfd5 100644
--- a/ArduPlane/servos.cpp
+++ b/ArduPlane/servos.cpp
@@ -702,19 +702,6 @@ void Plane::set_landing_gear(void)
 }
 #endif // LANDING_GEAR_ENABLED
 
-/*
-  apply vtail and elevon mixers
-  the rewrites radio_out for the corresponding channels
- */
-void Plane::servo_output_mixers(void)
-{
-    // mix elevons and vtail channels
-    channel_function_mixer(SRV_Channel::k_aileron, SRV_Channel::k_elevator, SRV_Channel::k_elevon_left, SRV_Channel::k_elevon_right);
-    channel_function_mixer(SRV_Channel::k_rudder,  SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left);
-
-    // implement differential spoilers
-    dspoiler_update();
-}
 
 /*
   support for twin-engine planes
@@ -970,6 +957,10 @@ void Plane::servos_output(void)
     // support twin-engine aircraft
     servos_twin_engine_mix();
 
+    // run vtail and elevon mixers
+    channel_function_mixer(SRV_Channel::k_aileron, SRV_Channel::k_elevator, SRV_Channel::k_elevon_left, SRV_Channel::k_elevon_right);
+    channel_function_mixer(SRV_Channel::k_rudder,  SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left);
+
 #if HAL_QUADPLANE_ENABLED
     // cope with tailsitters and bicopters
     quadplane.tailsitter.output();
@@ -978,9 +969,9 @@ void Plane::servos_output(void)
 
     // support forced flare option
     force_flare();
- 
-    // run vtail and elevon mixers
-    servo_output_mixers();
+
+    // implement differential spoilers
+    dspoiler_update();
 
     //  set control surface servos to neutral
     landing_neutral_control_surface_servos();
diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp
index fbe195e490..0b95bafc28 100644
--- a/ArduPlane/tailsitter.cpp
+++ b/ArduPlane/tailsitter.cpp
@@ -421,6 +421,33 @@ void Tailsitter::output(void)
     bool pitch_lim = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_elevator)) >= SERVO_MAX;
     bool yaw_lim = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_aileron)) >= SERVO_MAX;
 
+    // Mix elevons and V-tail, always giving full priority to pitch
+    float elevator_mix = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * (100.0 - plane.g.mixing_offset) * 0.01 * plane.g.mixing_gain;
+    float aileron_mix = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * (100.0 + plane.g.mixing_offset) * 0.01 * plane.g.mixing_gain;
+    float rudder_mix = SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) * (100.0 + plane.g.mixing_offset) * 0.01 * plane.g.mixing_gain;
+
+    const float headroom = SERVO_MAX - fabsf(elevator_mix);
+    if (is_positive(headroom)) {
+        if (fabsf(aileron_mix) > headroom) {
+            aileron_mix *= headroom / fabsf(aileron_mix);
+            yaw_lim = true;
+        }
+        if (fabsf(rudder_mix) > headroom) {
+            rudder_mix *= headroom / fabsf(rudder_mix);
+            roll_lim = true;
+        }
+    } else {
+        aileron_mix = 0.0;
+        rudder_mix = 0.0;
+        roll_lim = true;
+        pitch_lim = true;
+        yaw_lim = true;
+    }
+    SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, elevator_mix - aileron_mix);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, elevator_mix + aileron_mix);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_vtail_right, elevator_mix - rudder_mix);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_vtail_left, elevator_mix + rudder_mix);
+
     if (roll_lim) {
         motors->limit.roll = true;
     }
@@ -680,7 +707,6 @@ void Tailsitter::speed_scaling(void)
         } else {
             v *= spd_scaler;
         }
-        v = constrain_int32(v, -SERVO_MAX, SERVO_MAX);
         SRV_Channels::set_output_scaled(functions[i], v);
     }
 }