SITL: FlightAxis: Fix helidemix

ArduPilot · Feb 22, 2024 · c9b8afd · c9b8afd
1 parent b19f8ed
commit c9b8afd
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp
@@ -316,10 +316,12 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
         float swash3 = scaled_servos[2];
 
         float roll_rate = swash1 - swash2;
-        float pitch_rate = -((swash1+swash2) / 2.0f - swash3);
+        float pitch_rate = ((swash1+swash2) / 2.0f - swash3);
+        float col = (swash1 + swash2 + swash3) / 3.0;
 
         scaled_servos[0] = constrain_float(roll_rate + 0.5, 0, 1);
         scaled_servos[1] = constrain_float(pitch_rate + 0.5, 0, 1);
+        scaled_servos[2] = constrain_float(col, 0, 1);
     }
 
     const uint16_t channels = hal.scheduler->is_system_initialized()?4095:0;