AP_Motors: Heli: Fix SWSH logging for reversed collectives

MattKear · Jun 3, 2024 · 8df1b81 · 8df1b81
1 parent f345ae7
commit 8df1b81
Showing 1 changed file with 6 additions and 5 deletions.
diff --git 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