From 8df1b8119ab58cb99aab6f0bd1ab5818a2b9f6f0 Mon Sep 17 00:00:00 2001 From: MattKear Date: Tue, 4 Jun 2024 00:10:44 +0100 Subject: [PATCH] AP_Motors: Heli: Fix SWSH logging for reversed collectives --- libraries/AP_Motors/AP_MotorsHeli_Swash.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp index 5a93212739d91..d04312752a8c9 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp @@ -211,16 +211,17 @@ void AP_MotorsHeli_Swash::add_servo_raw(uint8_t num, float roll, float pitch, fl // calculates servo output void AP_MotorsHeli_Swash::calculate(float roll, float pitch, float collective) { - // Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch - if (_collective_direction == COLLECTIVE_DIRECTION_REVERSED){ - collective = 1 - collective; - } - // Store inputs for logging + // Store inputs for logging, store col before col reversal to ensure logging comes out with the correct sign (+/-) _roll_input = roll; _pitch_input = pitch; _collective_input_scaled = collective; + // Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch + if (_collective_direction == COLLECTIVE_DIRECTION_REVERSED){ + collective = 1 - collective; + } + for (uint8_t i = 0; i < _max_num_servos; i++) { if (!_enabled[i]) { // This servo is not enabled