From 4774cb8dafe295d0b9ba16ffafeeb1f93ae746be Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 12 Aug 2018 23:49:20 +0930 Subject: [PATCH] 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 | 178 ++++++++++++++----- libraries/AP_Motors/AP_MotorsMatrix.h | 10 ++ libraries/AP_Motors/AP_MotorsMulticopter.cpp | 20 +++ libraries/AP_Motors/AP_Motors_Class.cpp | 2 + libraries/AP_Motors/AP_Motors_Class.h | 10 ++ 5 files changed, 172 insertions(+), 48 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 73d7c42774..a9a6750409 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/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) { - throttle_thrust = _throttle_thrust_max; + if (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 - - throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max); - - // calculate roll and pitch for each motor - // calculate the amount of yaw input that each motor can accept + float rp_low = 1.0f; // lowest thrust value + float rp_high = -1.0f; // highest thrust value for (i=0; i 0.0f) { - unused_range = fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]); - if (yaw_allowed > unused_range) { - yaw_allowed = unused_range; - } - } else { - 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; - } - } + // record lowest roll+pitch command + if (_thrust_rpyt_out[i] < rp_low) { + rp_low = _thrust_rpyt_out[i]; + } + // record highest roll+pitch command + if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) { + rp_high = _thrust_rpyt_out[i]; } } } - // todo: make _yaw_headroom 0 to 1 - yaw_allowed = MAX(yaw_allowed, (float)_yaw_headroom/1000.0f); + // include the lost motor scaled by _thrust_boost_ratio + 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 - rpy_low = 0.0f; - rpy_high = 0.0f; + // add yaw control to thrust outputs + float rpy_low = 1.0f; // lowest thrust value + float rpy_high = -1.0f; // highest thrust value for (i=0; 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 - throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, throttle_avg_max); - if (is_zero(rpy_low) && is_zero(rpy_high)){ - rpy_scale = 1.0f; - } else if (is_zero(rpy_low)) { - rpy_scale = constrain_float((1.0f-throttle_thrust_best_rpy)/rpy_high, 0.0f, 1.0f); - } else if (is_zero(rpy_high)) { - 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 any scaling needed to make the combined thrust outputs fit within the output range + rpy_scale = 1.0f; + if (rpy_high-rpy_low > 1.0f) { + rpy_scale = 1.0f/(rpy_high-rpy_low); + } + if (rpy_low < 0.0f) { + rpy_scale = MIN(rpy_scale, -throttle_avg_max/rpy_low); } // 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); - } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy+rpy_high)){ + // todo: add lower limit flag and ensure it is handled correctly in altitude controller + 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 - // test code should be run with these lines commented out as they should not do anything - for (i=0; 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 diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index bade96af61..d78a023fa7 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/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 }; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 5acc99d021..69aedb40aa 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/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; } } diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 5a55af40a8..f198fa2eb7 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/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) diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 119f5b3cb1..c0b10dcd33 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/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; };