AP_Motors: allow output_motor_mask() to work properly with slew limits

this fixes tilt quadplanes with slew limits when we set motors state to SHUT_DOWN
tridge · Mar 4, 2024 · 583dcb3 · 583dcb3
1 parent f6d3963
commit 583dcb3
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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: