AP_Motors: improved motor loss redundancy

This detects a failed motor on copters with at least 6 motors
and recalculates the mixer to compensate

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -146,33 +146,33 @@ void AP_MotorsMatrix::output_armed_stabilizing()
     float   yaw_thrust;                 // yaw thrust input value, +/- 1.0
     float   throttle_thrust;            // throttle thrust input value, 0.0 - 1.0
     float   throttle_avg_max;           // throttle thrust average maximum value, 0.0 - 1.0
+    float   throttle_thrust_max;        // throttle thrust maximum value, 0.0 - 1.0
     float   throttle_thrust_best_rpy;   // throttle providing maximum roll, pitch and yaw range without climbing
     float   rpy_scale = 1.0f;           // this is used to scale the roll, pitch and yaw to fit within the motor limits
-    float   rpy_low = 0.0f;             // lowest motor value
-    float   rpy_high = 0.0f;            // highest motor value
     float   yaw_allowed = 1.0f;         // amount of yaw we can fit in
-    float   unused_range;               // amount of yaw we can fit in the current channel
     float   thr_adj;                    // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
 
     // apply voltage and air pressure compensation
-    const float compensation_gain = get_compensation_gain();
+    const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
     roll_thrust = _roll_in * compensation_gain;
     pitch_thrust = _pitch_in * compensation_gain;
     yaw_thrust = _yaw_in * compensation_gain;
     throttle_thrust = get_throttle() * compensation_gain;
     throttle_avg_max = _throttle_avg_max * compensation_gain;
+    throttle_thrust_max = _thrust_boost_ratio + (1.0f-_thrust_boost_ratio)*_throttle_thrust_max;
 
     // sanity check throttle is above zero and below current limited throttle
     if (throttle_thrust <= 0.0f) {
         throttle_thrust = 0.0f;
         limit.throttle_lower = true;
     }
-    if (throttle_thrust >= _throttle_thrust_max) {
+    if (throttle_thrust >= throttle_thrust_max) {
-        throttle_thrust = _throttle_thrust_max;
+        throttle_thrust = throttle_thrust_max;
         limit.throttle_upper = true;
     }
 
-    throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_max);
+    // ensure that throttle_avg_max is between the input throttle and the maximum throttle
+    throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max);
 
     // calculate throttle that gives most possible room for yaw which is the lower of:
     //      1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
@@ -184,43 +184,68 @@ void AP_MotorsMatrix::output_armed_stabilizing()
     //      We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
     //      We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low.  We favor reducing throttle instead of better yaw control because the pilot has commanded it
 
+    // Under the motor lost condition we remove the highest motor output from our calculations and let that motor go greater than 1.0
+    // To ensure control and maximum righting performance Hex and Octo have some optimal settings that should be used
+    // Y6               : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
+    // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
+    // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75,  ATC_RAT_PIT_IMAX = 0.75,  ATC_RAT_YAW_IMAX = 0.375
+    // Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
+    // Hex              : MOT_YAW_HEADROOM = 0,   ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
+    // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25,  ATC_RAT_PIT_IMAX = 0.25,  ATC_RAT_YAW_IMAX = 0.25
+    // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5,   ATC_RAT_PIT_IMAX = 0.5,   ATC_RAT_YAW_IMAX = 0.25
+    // Quads cannot make use of motor loss handling because it doesn't have enough degrees of freedom.
+
     // calculate amount of yaw we can fit into the throttle range
     // this is always equal to or less than the requested yaw from the pilot or rate controller
-
+    float rp_low = 1.0f;    // lowest thrust value
-    throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
+    float rp_high = -1.0f;  // highest thrust value
-
-    // calculate roll and pitch for each motor
-    // calculate the amount of yaw input that each motor can accept
     for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
+            // calculate the thrust outputs for roll and pitch
             _thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
-            if (!is_zero(_yaw_factor[i])){
+            // record lowest roll+pitch command
-                if (yaw_thrust * _yaw_factor[i] > 0.0f) {
+            if (_thrust_rpyt_out[i] < rp_low) {
-                    unused_range = fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]);
+                rp_low = _thrust_rpyt_out[i];
-                    if (yaw_allowed > unused_range) {
+            }
-                        yaw_allowed = unused_range;
+            // record highest roll+pitch command
-                    }
+            if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) {
-                } else {
+                rp_high = _thrust_rpyt_out[i];
-                    unused_range = fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i]);
-                    if (yaw_allowed > unused_range) {
-                        yaw_allowed = unused_range;
-                    }
-                }
             }
         }
     }
 
-    // todo: make _yaw_headroom 0 to 1
+    // include the lost motor scaled by _thrust_boost_ratio
-    yaw_allowed = MAX(yaw_allowed, (float)_yaw_headroom/1000.0f);
+    if (_thrust_boost && motor_enabled[_motor_lost_index]){
+        // record highest roll+pitch command
+        if (_thrust_rpyt_out[_motor_lost_index] > rp_high) {
+            rp_high = _thrust_boost_ratio*rp_high + (1.0f-_thrust_boost_ratio)*_thrust_rpyt_out[_motor_lost_index];
+        }
+    }
 
+    // check for roll and pitch saturation
+    rpy_scale = 1.0f;
+    if (rp_high-rp_low > 1.0f || throttle_avg_max < -rp_low) {
+        // Full range is being used by roll and pitch.
+        limit.roll_pitch = true;
+    }
+
+    // calculate the highest allowed average thrust that will provide maximum control range
+    throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
+
+    // calculate the maximum yaw control that can be used
+    // todo: make _yaw_headroom 0 to 1
+    yaw_allowed = (float)_yaw_headroom/1000.0f;
+    yaw_allowed = _thrust_boost_ratio*0.5f + (1.0f-_thrust_boost_ratio)*yaw_allowed;
+    yaw_allowed = MAX(MIN(throttle_thrust_best_rpy+rp_low, 1.0f-(throttle_thrust_best_rpy+rp_high)), yaw_allowed);
     if (fabsf(yaw_thrust) > yaw_allowed) {
+        // not all commanded yaw can be used
         yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
         limit.yaw = true;
     }
 
-    // add yaw to intermediate numbers for each motor
+    // add yaw control to thrust outputs
-    rpy_low = 0.0f;
+    float rpy_low = 1.0f;   // lowest thrust value
-    rpy_high = 0.0f;
+    float rpy_high = -1.0f; // highest thrust value
     for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
             _thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
@@ -230,27 +255,34 @@ void AP_MotorsMatrix::output_armed_stabilizing()
                 rpy_low = _thrust_rpyt_out[i];
             }
             // record highest roll+pitch+yaw command
-            if (_thrust_rpyt_out[i] > rpy_high) {
+            if (_thrust_rpyt_out[i] > rpy_high && (!_thrust_boost || i != _motor_lost_index)) {
                 rpy_high = _thrust_rpyt_out[i];
             }
         }
     }
+    // include the lost motor scaled by _thrust_boost_ratio
+    if (_thrust_boost ){
+        // record highest roll+pitch+yaw command
+        if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) {
+            rpy_high = _thrust_boost_ratio*rpy_high + (1.0f-_thrust_boost_ratio)*_thrust_rpyt_out[_motor_lost_index];
+        }
+    }
 
-    // check everything fits
+    // calculate any scaling needed to make the combined thrust outputs fit within the output range
-    throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, throttle_avg_max);
+    rpy_scale = 1.0f;
-    if (is_zero(rpy_low) && is_zero(rpy_high)){
+    if (rpy_high-rpy_low > 1.0f) {
-        rpy_scale = 1.0f;
+        rpy_scale = 1.0f/(rpy_high-rpy_low);
-    } else if (is_zero(rpy_low)) {
+    }
-        rpy_scale = constrain_float((1.0f-throttle_thrust_best_rpy)/rpy_high, 0.0f, 1.0f);
+    if (rpy_low < 0.0f) {
-    } else if (is_zero(rpy_high)) {
+        rpy_scale = MIN(rpy_scale, -throttle_avg_max/rpy_low);
-        rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
-    } else {
-        rpy_scale = constrain_float(MIN(-throttle_thrust_best_rpy/rpy_low, (1.0f-throttle_thrust_best_rpy)/rpy_high), 0.0f, 1.0f);
     }
 
     // calculate how close the motors can come to the desired throttle
+    rpy_high *= rpy_scale;
+    rpy_low *= rpy_scale;
+    throttle_thrust_best_rpy = -rpy_low;
     thr_adj = throttle_thrust - throttle_thrust_best_rpy;
-    if (rpy_scale < 1.0f){
+    if (rpy_scale < 1.0f) {
         // Full range is being used by roll, pitch, and yaw.
         limit.roll_pitch = true;
         limit.yaw = true;
@@ -259,10 +291,11 @@ void AP_MotorsMatrix::output_armed_stabilizing()
         }
         thr_adj = 0.0f;
     } else {
-        if (thr_adj < -(throttle_thrust_best_rpy+rpy_low)){
+        if (thr_adj < 0.0f) {
             // Throttle can't be reduced to desired value
-            thr_adj = -(throttle_thrust_best_rpy+rpy_low);
+            // todo: add lower limit flag and ensure it is handled correctly in altitude controller
-        } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy+rpy_high)){
+            thr_adj = 0.0f;
+        } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy+rpy_high)) {
             // Throttle can't be increased to desired value
             thr_adj = 1.0f - (throttle_thrust_best_rpy+rpy_high);
             limit.throttle_upper = true;
@@ -276,13 +309,62 @@ void AP_MotorsMatrix::output_armed_stabilizing()
         }
     }
 
-    // constrain all outputs to 0.0f to 1.0f
+    // check for failed motor
-    // test code should be run with these lines commented out as they should not do anything
+    check_for_failed_motor(throttle_thrust_best_rpy + thr_adj);
-    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+}
+
+// check for failed motor
+//   should be run immediately after output_armed_stabilizing
+//   first argument is the sum of:
+//      a) throttle_thrust_best_rpy : throttle level (from 0 to 1) providing maximum roll, pitch and yaw range without climbing
+//      b) thr_adj: the difference between the pilot's desired throttle and throttle_thrust_best_rpy
+//   records filtered motor output values in _thrust_rpyt_out_filt array
+//   sets thrust_balanced to true if motors are balanced, false if a motor failure is detected
+//   sets _motor_lost_index to index of failed motor
+void AP_MotorsMatrix::check_for_failed_motor(float throttle_thrust_best_plus_adj)
+{
+    // record filtered and scaled thrust output for motor loss monitoring purposes
+    float alpha = 1.0f / (1.0f + _loop_rate * 0.5f);
+    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
-            _thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i], 0.0f, 1.0f);
+            _thrust_rpyt_out_filt[i] += alpha * (_thrust_rpyt_out[i] - _thrust_rpyt_out_filt[i]);
         }
     }
+
+    float rpyt_high = 0.0f;
+    float rpyt_sum = 0.0f;
+    float number_motors = 0.0f;
+    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            number_motors += 1;
+            rpyt_sum += _thrust_rpyt_out_filt[i];
+            // record highest thrust command
+            if (_thrust_rpyt_out_filt[i] > rpyt_high) {
+                rpyt_high = _thrust_rpyt_out_filt[i];
+                // hold motor lost pointer constant while thrust balance is true
+                if (_thrust_balanced) {
+                    _motor_lost_index = i;
+                }
+            }
+        }
+    }
+
+    float thrust_balance = 1.0f;
+    if (rpyt_sum > 0.1f) {
+        thrust_balance = rpyt_high*number_motors/rpyt_sum;
+    }
+    // ensure thrust balance does not activate for multirotors with less than 6 motors
+    if (number_motors >= 6 && thrust_balance >= 1.5f && _thrust_balanced) {
+        _thrust_balanced = false;
+    }
+    if (thrust_balance <= 1.25f && !_thrust_balanced) {
+        _thrust_balanced = true;
+    }
+
+    // check to see if thrust boost is using more throttle than _throttle_thrust_max
+    if (_throttle_thrust_max > throttle_thrust_best_plus_adj && rpyt_high < 0.9f && _thrust_balanced) {
+        _thrust_boost = false;
+    }
 }
 
 // output_test_seq - spin a motor at the pwm value specified

libraries/AP_Motors/AP_MotorsMatrix.h

@@ -48,10 +48,16 @@ public:
     //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
     uint16_t            get_motor_mask() override;
 
+    // return number of motor that has failed.  Should only be called if get_thrust_boost() returns true
+    uint8_t             get_lost_motor() const { return _motor_lost_index; }
+
 protected:
     // output - sends commands to the motors
     void                output_armed_stabilizing();
 
+    // check for failed motor
+    void                check_for_failed_motor(float throttle_thrust_best);
+
     // add_motor using raw roll, pitch, throttle and yaw factors
     void                add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order);
 
@@ -80,4 +86,8 @@ protected:
     uint8_t             _test_order[AP_MOTORS_MAX_NUM_MOTORS];  // order of the motors in the test sequence
     motor_frame_class   _last_frame_class; // most recently requested frame class (i.e. quad, hexa, octa, etc)
     motor_frame_type    _last_frame_type; // most recently requested frame type (i.e. plus, x, v, etc)
+
+    // motor failure handling
+    float               _thrust_rpyt_out_filt[AP_MOTORS_MAX_NUM_MOTORS];    // filtered thrust outputs with 1 second time constant
+    uint8_t             _motor_lost_index;  // index number of the lost motor
 };

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -456,6 +456,10 @@ void AP_MotorsMulticopter::output_logic()
             // set and increment ramp variables
             _spin_up_ratio = 0.0f;
             _throttle_thrust_max = 0.0f;
+
+            // initialise motor failure variables
+            _thrust_boost = false;
+            _thrust_boost_ratio = 0.0f;
             break;
 
         case SPIN_WHEN_ARMED: {
@@ -492,6 +496,10 @@ void AP_MotorsMulticopter::output_logic()
                 _spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step);
             }
             _throttle_thrust_max = 0.0f;
+
+            // initialise motor failure variables
+            _thrust_boost = false;
+            _thrust_boost_ratio = 0.0f;
             break;
         }
         case SPOOL_UP:
@@ -521,6 +529,10 @@ void AP_MotorsMulticopter::output_logic()
             } else if (_throttle_thrust_max < 0.0f) {
                 _throttle_thrust_max = 0.0f;
             }
+
+            // initialise motor failure variables
+            _thrust_boost = false;
+            _thrust_boost_ratio = 0.0f;
             break;
 
         case THROTTLE_UNLIMITED:
@@ -542,6 +554,12 @@ void AP_MotorsMulticopter::output_logic()
             // set and increment ramp variables
             _spin_up_ratio = 1.0f;
             _throttle_thrust_max = get_current_limit_max_throttle();
+
+            if (_thrust_boost && !_thrust_balanced) {
+                _thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio+1.0f/(_spool_up_time*_loop_rate));
+            } else {
+                _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio-1.0f/(_spool_up_time*_loop_rate));
+            }
             break;
 
         case SPOOL_DOWN:
@@ -573,6 +591,8 @@ void AP_MotorsMulticopter::output_logic()
             } else if (is_zero(_throttle_thrust_max)) {
                 _spool_mode = SPIN_WHEN_ARMED;
             }
+
+            _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio-1.0f/(_spool_up_time*_loop_rate));
             break;
     }
 }

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -48,6 +48,8 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
     limit.yaw = true;
     limit.throttle_lower = true;
     limit.throttle_upper = true;
+    _thrust_boost = false;
+    _thrust_balanced = true;
 };
 
 void AP_Motors::armed(bool arm)

libraries/AP_Motors/AP_Motors_Class.h

@@ -95,6 +95,11 @@ public:
     float               get_lateral() const { return _lateral_in; }
     virtual float       get_throttle_hover() const = 0;
 
+    // motor failure handling
+    void                set_thrust_boost(bool enable) { _thrust_boost = enable; }
+    bool                get_thrust_boost() const { return _thrust_boost; }
+    virtual uint8_t     get_lost_motor() const { return 0; }
+
     // spool up states
     enum spool_up_down_desired {
         DESIRED_SHUT_DOWN = 0,              // all motors stop
@@ -212,6 +217,11 @@ protected:
 
     AP_Int8             _pwm_type;            // PWM output type
 
+    // motor failure handling
+    bool                _thrust_boost;          // true if thrust boost is enabled to handle motor failure
+    bool                _thrust_balanced;       // true when output thrust is well balanced
+    float               _thrust_boost_ratio;    // choice between highest and second highest motor output for output mixing (0 ~ 1). Zero is normal operation
+
 private:
     static AP_Motors *_instance;
 };