AP_MotorsTri: change test sequence for rev frame

ArduPilot · Dec 11, 2023 · db4d1f9 · db4d1f9
1 parent 0d110a6
commit db4d1f9
Showing 1 changed file with 78 additions and 35 deletions.
diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp
@@ -286,26 +286,50 @@ void AP_MotorsTri::output_armed_stabilizing()
 void AP_MotorsTri::_output_test_seq(uint8_t motor_seq, int16_t pwm)
 {
     // output to motors and servos
-    switch (motor_seq) {
-        case 1:
-            // front right motor
-            rc_write(AP_MOTORS_MOT_1, pwm);
-            break;
-        case 2:
-            // back motor
-            rc_write(AP_MOTORS_MOT_4, pwm);
-            break;
-        case 3:
-            // back servo
-            rc_write(AP_MOTORS_CH_TRI_YAW, pwm);
-            break;
-        case 4:
-            // front left motor
-            rc_write(AP_MOTORS_MOT_2, pwm);
-            break;
-        default:
-            // do nothing
-            break;
+    if(!_pitch_reversed) {
+        switch (motor_seq) {
+            case 1:
+                // front right motor
+                rc_write(AP_MOTORS_MOT_1, pwm);
+                break;
+            case 2:
+                // back motor
+                rc_write(AP_MOTORS_MOT_4, pwm);
+                break;
+            case 3:
+                // back servo
+                rc_write(AP_MOTORS_CH_TRI_YAW, pwm);
+                break;
+            case 4:
+                // front left motor
+                rc_write(AP_MOTORS_MOT_2, pwm);
+                break;
+            default:
+                // do nothing
+                break;
+        }
+    } else {
+        switch (motor_seq) {
+            case 1:
+                // front motor
+                rc_write(AP_MOTORS_MOT_4, pwm);
+                break;
+            case 2:
+                // front servo
+                rc_write(AP_MOTORS_CH_TRI_YAW, pwm);
+                break;
+            case 3:
+                // back right motor
+                rc_write(AP_MOTORS_MOT_1, pwm);
+                break;
+            case 4:
+                // back left motor
+                rc_write(AP_MOTORS_MOT_2, pwm);
+                break;
+            default:
+                // do nothing
+                break;
+        }
     }
 }
 
@@ -401,20 +425,39 @@ bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const
 // Get the testing order for the motors
 uint8_t AP_MotorsTri::get_motor_test_order(uint8_t i)
 {
-    switch (i) {
-        // front right motor
-        case AP_MOTORS_MOT_1:
-            return 1;
-        // back motor
-        case AP_MOTORS_MOT_4:
-            return 2;
-        // back servo
-        case AP_MOTORS_CH_TRI_YAW:
-            return 3;
-        // front left motor
-        case AP_MOTORS_MOT_2:
-            return 4;
-        default:
-            return 0;
+    if(!_pitch_reversed) {
+        switch (i) {
+            // front right motor
+            case AP_MOTORS_MOT_1:
+                return 1;
+            // back motor
+            case AP_MOTORS_MOT_4:
+                return 2;
+            // back servo
+            case AP_MOTORS_CH_TRI_YAW:
+                return 3;
+            // front left motor
+            case AP_MOTORS_MOT_2:
+                return 4;
+            default:
+                return 0;
+        }
+    } else {
+        switch (i) {
+            // front motor
+            case AP_MOTORS_MOT_4:
+                return 1;
+            // front servo
+            case AP_MOTORS_CH_TRI_YAW:
+                return 2;
+            // back right motor
+            case AP_MOTORS_MOT_1:
+                return 3;
+            // back left motor
+            case AP_MOTORS_MOT_2:
+                return 4;
+            default:
+                return 0;
+        }
     }
 }