diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index f8ef191dc4..aa1b637b43 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -172,200 +172,146 @@ void AP_MotorsMatrix::output_armed_not_stabilizing() // output_armed - sends commands to the motors // includes new scaling stability patch -// TODO pull code that is common to output_armed_not_stabilizing into helper functions void AP_MotorsMatrix::output_armed_stabilizing() { - int8_t i; - int16_t roll_pwm; // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400 - int16_t pitch_pwm; // pitch pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400 - int16_t yaw_pwm; // yaw pwm value, initially calculated by calc_yaw_pwm() but may be modified after, +/- 400 - int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 - int16_t out_min_pwm = _throttle_radio_min + _min_throttle; // minimum pwm value we can send to the motors - int16_t out_max_pwm = _throttle_radio_max; // maximum pwm value we can send to the motors - int16_t out_mid_pwm = (out_min_pwm+out_max_pwm)/2; // mid pwm value we can send to the motors - int16_t out_best_thr_pwm; // the is the best throttle we can come up which provides good control without climbing - float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits + uint8_t i; // general purpose counter + float roll_thrust; // roll thrust input value, +/- 1.0 + float pitch_thrust; // pitch thrust input value, +/- 1.0 + float yaw_thrust; // yaw thrust input value, +/- 1.0 + float throttle_thrust; // throttle thrust input value, 0.0 - 1.0 + float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing + float throttle_thrust_rpy_mix; // partial calculation of throttle_thrust_best_rpy + 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 + float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); // throttle hover thrust value, 0.0 - 1.0 - int16_t rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times. - int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors + // apply voltage and air pressure compensation + roll_thrust = get_roll_thrust() * get_compensation_gain(); + pitch_thrust = get_pitch_thrust() * get_compensation_gain(); + yaw_thrust = get_yaw_thrust() * get_compensation_gain(); + throttle_thrust = get_throttle() * get_compensation_gain(); - int16_t rpy_low = 0; // lowest motor value - int16_t rpy_high = 0; // highest motor value - int16_t yaw_allowed; // amount of yaw we can fit in - int16_t thr_adj; // the difference between the pilot's desired throttle and out_best_thr_pwm (the throttle that is actually provided) - - // initialize limits flags - limit.roll_pitch = false; - limit.yaw = false; - limit.throttle_lower = false; - limit.throttle_upper = false; - - // Ensure throttle is within bounds of 0 to 1000 - int16_t thr_in_min = rel_pwm_to_thr_range(_min_throttle); - if (_throttle_control_input <= thr_in_min) { - _throttle_control_input = thr_in_min; + // 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_control_input >= _max_throttle) { - _throttle_control_input = _max_throttle; + if (throttle_thrust >= _throttle_thrust_max) { + throttle_thrust = _throttle_thrust_max; limit.throttle_upper = true; } - roll_pwm = calc_roll_pwm(); - pitch_pwm = calc_pitch_pwm(); - yaw_pwm = calc_yaw_pwm(); - throttle_radio_output = calc_throttle_radio_output(); + throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix); - // calculate roll and pitch for each motor - // set rpy_low and rpy_high to the lowest and highest values of the motors - for (i=0; i rpy_high) { - rpy_high = rpy_out[i]; - } - } - } - - // calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of: - // 1. mid throttle - average of highest and lowest motor (this would give the maximum possible room margin above the highest motor and below the lowest) + // 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 // 2. the higher of: // a) the pilot's throttle input - // b) the mid point between the pilot's input throttle and hover-throttle + // b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle // Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this. // Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise. - // We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour 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 favour reducing throttle instead of better yaw control because the pilot has commanded it - int16_t motor_mid = (rpy_low+rpy_high)/2; - out_best_thr_pwm = MIN(out_mid_pwm - motor_mid, MAX(throttle_radio_output, throttle_radio_output*MAX(0,1.0f-_throttle_thr_mix)+get_hover_throttle_as_pwm()*_throttle_thr_mix)); + // 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 // 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 - yaw_allowed = MIN(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2; - yaw_allowed = MAX(yaw_allowed, _yaw_headroom); - if (yaw_pwm >= 0) { - // if yawing right - if (yaw_allowed > yaw_pwm * get_compensation_gain()) { - yaw_allowed = yaw_pwm * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output - }else{ - limit.yaw = true; - } - }else{ - // if yawing left - yaw_allowed = -yaw_allowed; - if (yaw_allowed < yaw_pwm * get_compensation_gain()) { - yaw_allowed = yaw_pwm * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output - }else{ - limit.yaw = true; + throttle_thrust_best_rpy = MIN(0.5f, throttle_thrust_rpy_mix); + + // calculate roll and pitch for each motor + // calculate the amount of yaw input that each motor can accept + for (i=0; i 0.0f) { + unused_range = fabsf((1.0 - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]))/_yaw_factor[i]); + if (yaw_allowed > unused_range) { + yaw_allowed = unused_range; + } + } else { + unused_range = fabsf((throttle_thrust_best_rpy + _thrust_rpyt_out[i])/_yaw_factor[i]); + if (yaw_allowed > unused_range) { + yaw_allowed = unused_range; + } + } + } } } + // todo: make _yaw_headroom 0 to 1 + yaw_allowed = MAX(yaw_allowed, (float)_yaw_headroom/1000.0f); + + if (fabsf(yaw_thrust) > yaw_allowed) { + yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed); + limit.yaw = true; + } + // add yaw to intermediate numbers for each motor - rpy_low = 0; - rpy_high = 0; + rpy_low = 0.0f; + rpy_high = 0.0f; for (i=0; i rpy_high) { - rpy_high = rpy_out[i]; + if (_thrust_rpyt_out[i] > rpy_high) { + rpy_high = _thrust_rpyt_out[i]; } } } // check everything fits - thr_adj = throttle_radio_output - out_best_thr_pwm; - - // calculate upper and lower limits of thr_adj - int16_t thr_adj_max = MAX(out_max_pwm-(out_best_thr_pwm+rpy_high),0); - - // if we are increasing the throttle (situation #2 above).. - if (thr_adj > 0) { - // increase throttle as close as possible to requested throttle - // without going over out_max_pwm - if (thr_adj > thr_adj_max){ - thr_adj = thr_adj_max; - // we haven't even been able to apply full throttle command - limit.throttle_upper = true; - } - }else if(thr_adj < 0){ - // decrease throttle as close as possible to requested throttle - // without going under out_min_pwm or over out_max_pwm - // earlier code ensures we can't break both boundaries - int16_t thr_adj_min = MIN(out_min_pwm-(out_best_thr_pwm+rpy_low),0); - if (thr_adj > thr_adj_max) { - thr_adj = thr_adj_max; - limit.throttle_upper = true; - } - if (thr_adj < thr_adj_min) { - thr_adj = thr_adj_min; - } + throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, throttle_thrust_rpy_mix); + if (is_zero(rpy_low)){ + rpy_scale = 1.0f; + } else { + rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f); } - // do we need to reduce roll, pitch, yaw command - // earlier code does not allow both limit's to be passed simultaneously with abs(_yaw_factor)<1 - if ((rpy_low+out_best_thr_pwm)+thr_adj < out_min_pwm){ - // protect against divide by zero - if (rpy_low != 0) { - rpy_scale = (float)(out_min_pwm-thr_adj-out_best_thr_pwm)/rpy_low; - } - // we haven't even been able to apply full roll, pitch and minimal yaw without scaling + // calculate how close the motors can come to the desired throttle + thr_adj = throttle_thrust - throttle_thrust_best_rpy; + if (rpy_scale < 1.0f){ + // Full range is being used by roll, pitch, and yaw. limit.roll_pitch = true; limit.yaw = true; - }else if((rpy_high+out_best_thr_pwm)+thr_adj > out_max_pwm){ - // protect against divide by zero - if (rpy_high != 0) { - rpy_scale = (float)(out_max_pwm-thr_adj-out_best_thr_pwm)/rpy_high; + if (thr_adj > 0.0f) { + limit.throttle_upper = true; + } + thr_adj = 0.0f; + } else { + if (thr_adj < -(throttle_thrust_best_rpy+rpy_low)){ + // 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)){ + // Throttle can't be increased to desired value + thr_adj = 1.0f - (throttle_thrust_best_rpy+rpy_high); + limit.throttle_upper = true; } - // we haven't even been able to apply full roll, pitch and minimal yaw without scaling - limit.roll_pitch = true; - limit.yaw = true; } // add scaled roll, pitch, constrained yaw and throttle for each motor for (i=0; icork(); - for( i=0; ipush(); } // output_disarmed - sends commands to the motors diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 62933ef498..832a56e70f 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -75,5 +75,6 @@ protected: float _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll float _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to yaw (normally 1 or -1) + float _thrust_rpyt_out[AP_MOTORS_MAX_NUM_MOTORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range uint8_t _test_order[AP_MOTORS_MAX_NUM_MOTORS]; // order of the motors in the test sequence };