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