diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index e2e71392621cf..70e07103be6ff 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -148,7 +148,7 @@ void AP_MotorsMatrix::output_to_motors() case SpoolState::SHUT_DOWN: { // no output for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { - if (motor_enabled[i]) { + if (motor_enabled_mask(i)) { _actuator[i] = 0.0f; } } diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index e68cf85c8208a..e4a75ef3303ee 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -272,6 +272,8 @@ void AP_MotorsMulticopter::output() // check for any external limit flags update_external_limits(); + // clear mask of overridden motors + _motor_mask_override = 0; }; void AP_MotorsMulticopter::update_external_limits() @@ -728,6 +730,8 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint16_t mask, float const int16_t pwm_min = get_pwm_output_min(); const int16_t pwm_range = get_pwm_output_max() - pwm_min; + _motor_mask_override = mask; + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { if ((mask & (1U << i)) && armed() && get_interlock()) { diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 3f97e5105a3bd..d6fdf6a3092b5 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -192,4 +192,14 @@ class AP_MotorsMulticopter : public AP_Motors { // array of motor output values float _actuator[AP_MOTORS_MAX_NUM_MOTORS]; + + /* motor enabled, checking the override mask + _motor_mask_override is only set for tilt quadplanes + */ + bool motor_enabled_mask(uint8_t i) const { + return motor_enabled[i] && (_motor_mask_override & (1U << i)) == 0; + } + + // mask of overridden motors (used by quadplane tiltrotors) + uint16_t _motor_mask_override; }; diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 43730335fd584..c109ade093616 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -90,9 +90,11 @@ void AP_MotorsTri::output_to_motors() switch (_spool_state) { case SpoolState::SHUT_DOWN: // sends minimum values out to the motors - _actuator[AP_MOTORS_MOT_1] = 0.0; - _actuator[AP_MOTORS_MOT_2] = 0.0; - _actuator[AP_MOTORS_MOT_4] = 0.0; + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + if (motor_enabled_mask(i)) { + rc_write(AP_MOTORS_MOT_1+i, output_to_pwm(0)); + } + } rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0); break; case SpoolState::GROUND_IDLE: