diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 7b868d3aaf4bf3..43730335fd584b 100644 --- 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)