AP_Motors: Tricopter: fix actuator indexing

ArduPilot · Feb 27, 2024 · 744df1c · 744df1c
1 parent fee5374
commit 744df1c
Showing 1 changed file with 13 additions and 15 deletions.
diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp
@@ -90,34 +90,32 @@ void AP_MotorsTri::output_to_motors()
     switch (_spool_state) {
         case SpoolState::SHUT_DOWN:
             // sends minimum values out to the motors
-            rc_write(AP_MOTORS_MOT_1, output_to_pwm(0));
-            rc_write(AP_MOTORS_MOT_2, output_to_pwm(0));
-            rc_write(AP_MOTORS_MOT_4, output_to_pwm(0));
+            _actuator[AP_MOTORS_MOT_1] = 0.0;
+            _actuator[AP_MOTORS_MOT_2] = 0.0;
+            _actuator[AP_MOTORS_MOT_4] = 0.0;
             rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0);
             break;
         case SpoolState::GROUND_IDLE:
             // sends output to motors when armed but not flying
-            set_actuator_with_slew(_actuator[1], actuator_spin_up_to_ground_idle());
-            set_actuator_with_slew(_actuator[2], actuator_spin_up_to_ground_idle());
-            set_actuator_with_slew(_actuator[4], actuator_spin_up_to_ground_idle());
-            rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1]));
-            rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2]));
-            rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4]));
+            set_actuator_with_slew(_actuator[AP_MOTORS_MOT_1], actuator_spin_up_to_ground_idle());
+            set_actuator_with_slew(_actuator[AP_MOTORS_MOT_2], actuator_spin_up_to_ground_idle());
+            set_actuator_with_slew(_actuator[AP_MOTORS_MOT_4], actuator_spin_up_to_ground_idle());
             rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0);
             break;
         case SpoolState::SPOOLING_UP:
         case SpoolState::THROTTLE_UNLIMITED:
         case SpoolState::SPOOLING_DOWN:
             // set motor output based on thrust requests
-            set_actuator_with_slew(_actuator[1], thr_lin.thrust_to_actuator(_thrust_right));
-            set_actuator_with_slew(_actuator[2], thr_lin.thrust_to_actuator(_thrust_left));
-            set_actuator_with_slew(_actuator[4], thr_lin.thrust_to_actuator(_thrust_rear));
-            rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1]));
-            rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2]));
-            rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4]));
+            set_actuator_with_slew(_actuator[AP_MOTORS_MOT_1], thr_lin.thrust_to_actuator(_thrust_right));
+            set_actuator_with_slew(_actuator[AP_MOTORS_MOT_2], thr_lin.thrust_to_actuator(_thrust_left));
+            set_actuator_with_slew(_actuator[AP_MOTORS_MOT_4], thr_lin.thrust_to_actuator(_thrust_rear));
             rc_write_angle(AP_MOTORS_CH_TRI_YAW, degrees(_pivot_angle)*100);
             break;
     }
+
+    rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[AP_MOTORS_MOT_1]));
+    rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[AP_MOTORS_MOT_2]));
+    rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[AP_MOTORS_MOT_4]));
 }
 
 // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)