From c9b8afd7f54e528e8065729bd2c157e43868ede4 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 22 Feb 2024 17:57:27 +0000 Subject: [PATCH] SITL: FlightAxis: Fix helidemix --- libraries/SITL/SIM_FlightAxis.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 36d63e0d67cdc..d60793c006dc6 100644 --- 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;