Plane: tailsitter: always give pitch prority elevon mixing in VTOL modes

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);

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();

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);
     }
 }