// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrixTS::output_armed_stabilizing()
{
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float thrust_max = 0.0f; // highest motor value
float thr_adj = 0.0f; // 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(); // compensation for battery voltage and altitude
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
// 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;
limit.throttle_upper = true;
}
thrust_max = 0.0f;
for (int 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] = throttle_thrust + roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
if (thrust_max < _thrust_rpyt_out[i]) {
thrust_max = _thrust_rpyt_out[i];
}
}
}
// if max thrust is more than one reduce average throttle
if (thrust_max > 1.0f) {
thr_adj = 1.0f - thrust_max;
limit.throttle_upper = true;
limit.roll_pitch = true;
for (int 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] += thr_adj;
}
}
}
}
// sends commands to the motors
void AP_MotorsSingle::output_armed_stabilizing()
{
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_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
float thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float actuator_allowed = 0.0f; // amount of yaw we can fit in
float actuator[NUM_ACTUATORS]; // combined roll, pitch and yaw thrusts for each actuator
float actuator_max = 0.0f; // maximum actuator value
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
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;
// 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;
limit.throttle_upper = true;
}
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
// calculate how much roll and pitch must be scaled to leave enough range for the minimum yaw
if (is_zero(rp_thrust_max)) {
rp_scale = 1.0f;
} else {
rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), (float)_yaw_headroom/1000.0f)) / rp_thrust_max, 0.0f, 1.0f);
if (rp_scale < 1.0f) {
limit.roll_pitch = true;
}
}
actuator_allowed = 1.0f - rp_scale * rp_thrust_max;
if (fabsf(yaw_thrust) > actuator_allowed) {
yaw_thrust = constrain_float(yaw_thrust, -actuator_allowed, actuator_allowed);
limit.yaw = true;
}
// combine roll, pitch and yaw on each actuator
// front servo
actuator[0] = rp_scale * roll_thrust - yaw_thrust;
// right servo
actuator[1] = rp_scale * pitch_thrust - yaw_thrust;
// rear servo
actuator[2] = -rp_scale * roll_thrust - yaw_thrust;
// left servo
actuator[3] = -rp_scale * pitch_thrust - yaw_thrust;
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rpy = MAX(MAX(fabsf(actuator[0]), fabsf(actuator[1])), MAX(fabsf(actuator[2]), fabsf(actuator[3])));
thr_adj = throttle_thrust - throttle_avg_max;
if (thr_adj < (thrust_min_rpy - throttle_avg_max)) {
// Throttle can't be reduced to the desired level because this would mean roll or pitch control
// would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(thrust_min_rpy, throttle_avg_max) - throttle_avg_max;
}
// calculate the throttle setting for the lift fan
_thrust_out = throttle_avg_max + thr_adj;
if (is_zero(_thrust_out)) {
limit.roll_pitch = true;
limit.yaw = true;
}
// limit thrust out for calculation of actuator gains
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,_thrust_out), 0.5f, 1.0f);
// calculate the maximum allowed actuator output and maximum requested actuator output
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
if (actuator_max > fabsf(actuator[i])) {
actuator_max = fabsf(actuator[i]);
}
}
if (actuator_max > thrust_out_actuator && !is_zero(actuator_max)) {
// roll, pitch and yaw request can not be achieved at full servo defection
// reduce roll, pitch and yaw to reduce the requested defection to maximum
limit.roll_pitch = true;
limit.yaw = true;
rp_scale = thrust_out_actuator/actuator_max;
} else {
rp_scale = 1.0f;
}
// force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
_actuator_out[i] = constrain_float(rp_scale*actuator[i]/thrust_out_actuator, -1.0f, 1.0f);
}
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsTri::output_armed_stabilizing()
{
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 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 thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
// sanity check YAW_SV_ANGLE parameter value to avoid divide by zero
_yaw_servo_angle_max_deg = constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX);
// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain()*sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
throttle_thrust = get_throttle() * get_compensation_gain();
// calculate angle of yaw pivot
_pivot_angle = safe_asin(yaw_thrust);
if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) {
limit.yaw = true;
_pivot_angle = constrain_float(_pivot_angle, -radians(_yaw_servo_angle_max_deg), radians(_yaw_servo_angle_max_deg));
}
float pivot_thrust_max = cosf(_pivot_angle);
float thrust_max = 1.0f;
// 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;
limit.throttle_upper = true;
}
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
// The following mix may be offer less coupling between axis but needs testing
//_thrust_right = roll_thrust * -0.5f + pitch_thrust * 1.0f;
//_thrust_left = roll_thrust * 0.5f + pitch_thrust * 1.0f;
//_thrust_rear = 0;
_thrust_right = roll_thrust * -0.5f + pitch_thrust * 0.5f;
_thrust_left = roll_thrust * 0.5f + pitch_thrust * 0.5f;
_thrust_rear = pitch_thrust * -0.5f;
// calculate roll and pitch for each motor
// set rpy_low and rpy_high to the lowest and highest values of the motors
// record lowest roll pitch command
rpy_low = MIN(_thrust_right,_thrust_left);
rpy_high = MAX(_thrust_right,_thrust_left);
if (rpy_low > _thrust_rear){
rpy_low = _thrust_rear;
}
// check to see if the rear motor will reach maximum thrust before the front two motors
if ((1.0f - rpy_high) > (pivot_thrust_max - _thrust_rear)){
thrust_max = pivot_thrust_max;
rpy_high = _thrust_rear;
}
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible room margin above the highest motor and below the lowest
// 2. the higher of:
// a) the pilot's throttle input
// 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 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
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, _throttle_avg_max);
if(is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
}
// 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;
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 > thrust_max - (throttle_thrust_best_rpy+rpy_high)){
// Throttle can't be increased to desired value
thr_adj = thrust_max - (throttle_thrust_best_rpy+rpy_high);
limit.throttle_upper = true;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
_thrust_right = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_right;
_thrust_left = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_left;
_thrust_rear = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_rear;
// scale pivot thrust to account for pivot angle
// we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range
_thrust_rear = _thrust_rear/cosf(_pivot_angle);
// 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
_thrust_right = constrain_float(_thrust_right, 0.0f, 1.0f);
_thrust_left = constrain_float(_thrust_left, 0.0f, 1.0f);
_thrust_rear = constrain_float(_thrust_rear, 0.0f, 1.0f);
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed_stabilizing()
{
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
// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain();
throttle_thrust = get_throttle() * get_compensation_gain();
// 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;
limit.throttle_upper = true;
}
throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix);
// 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 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 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
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<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
if (!is_zero(_yaw_factor[i])){
if (yaw_thrust * _yaw_factor[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.0f;
rpy_high = 0.0f;
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];
// record lowest roll+pitch+yaw command
if (_thrust_rpyt_out[i] < rpy_low) {
rpy_low = _thrust_rpyt_out[i];
}
// record highest roll+pitch+yaw command
if (_thrust_rpyt_out[i] > rpy_high) {
rpy_high = _thrust_rpyt_out[i];
}
}
}
// check everything fits
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);
}
// 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;
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;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + rpy_scale*_thrust_rpyt_out[i];
}
}
// 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<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i], 0.0f, 1.0f);
}
}
}
// 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
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
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;
limit.throttle_lower = true;
}
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
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();
// 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<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = roll_pwm * _roll_factor[i] * get_compensation_gain() +
pitch_pwm * _pitch_factor[i] * get_compensation_gain()+yaw_pwm*_yaw_factor[i]*(1000-m_Conv)/1000;
// record lowest roll pitch command
if (rpy_out[i] < rpy_low) {
rpy_low = rpy_out[i];
}
// record highest roll pich command
if (rpy_out[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)
// 2. the higher of:
// a) the pilot's throttle input
// b) the mid point 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));
// 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;
}
}
// add yaw to intermediate numbers for each motor
rpy_low = 0;
rpy_high = 0;
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
//rpy_out[i] = rpy_out[i] +
// yaw_allowed * _yaw_factor[i];
// record lowest roll+pitch+yaw command
if( rpy_out[i] < rpy_low ) {
rpy_low = rpy_out[i];
}
// record highest roll+pitch+yaw command
if( rpy_out[i] > rpy_high) {
rpy_high = rpy_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;
}
}
// 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
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;
}
// 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; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = out_best_thr_pwm+thr_adj +
rpy_scale*rpy_out[i];
}
}
// apply thrust curve and voltage scaling
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = apply_thrust_curve_and_volt_scaling(motor_out[i], out_min_pwm, out_max_pwm);
}
}
// clip motor output if required (shouldn't be)
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(motor_out[i], out_min_pwm, out_max_pwm);
}
}
// send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), motor_out[i]);
}
}
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed_stabilizing()
{
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_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 yaw_allowed = 1.0f; // amount of yaw we can fit in
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(); // 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;
limit.throttle_upper = true;
}
// 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
// 2. the higher of:
// a) the pilot's throttle input
// 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 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
float rp_high = -1.0f; // highest thrust value
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];
// 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];
}
}
}
// 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
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 control to thrust outputs
float rpy_low = 1.0f; // lowest thrust value
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];
// record lowest roll+pitch+yaw command
if (_thrust_rpyt_out[i] < rpy_low) {
rpy_low = _thrust_rpyt_out[i];
}
// record highest roll+pitch+yaw command
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];
}
}
// calculate any scaling needed to make the combined thrust outputs fit within the output range
if (rpy_high-rpy_low > 1.0f) {
rpy_scale = 1.0f / (rpy_high-rpy_low);
}
if (is_negative(rpy_low)) {
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) {
// Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true;
limit.yaw = true;
if (thr_adj > 0.0f) {
limit.throttle_upper = true;
}
thr_adj = 0.0f;
} else {
if (thr_adj < 0.0f) {
// Throttle can't be reduced to desired value
// 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;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * _thrust_rpyt_out[i]);
}
}
// check for failed motor
check_for_failed_motor(throttle_thrust_best_rpy + thr_adj);
}
// sends commands to the motors
void AP_MotorsCoax::output_armed_stabilizing()
{
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 thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float thrust_out; //
float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float actuator_allowed = 0.0f; // amount of yaw we can fit in
// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain();
throttle_thrust = get_throttle() * get_compensation_gain();
// 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;
limit.throttle_upper = true;
}
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
// calculate how much roll and pitch must be scaled to leave enough range for the minimum yaw
if (is_zero(rp_thrust_max)) {
rp_scale = 1.0f;
} else {
rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f*(float)_yaw_headroom/1000.0f)) / rp_thrust_max, 0.0f, 1.0f);
if (rp_scale < 1.0f) {
limit.roll_pitch = true;
}
}
actuator_allowed = 2.0f * (1.0f - rp_scale * rp_thrust_max);
if (fabsf(yaw_thrust) > actuator_allowed) {
yaw_thrust = constrain_float(yaw_thrust, -actuator_allowed, actuator_allowed);
limit.yaw = true;
}
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rpy = MAX(fabsf(rp_scale * rp_thrust_max), fabsf(yaw_thrust));
thr_adj = throttle_thrust - _throttle_avg_max;
if (thr_adj < (thrust_min_rpy - _throttle_avg_max)) {
// Throttle can't be reduced to the desired level because this would mean roll or pitch control
// would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(thrust_min_rpy, _throttle_avg_max) - _throttle_avg_max;
}
// calculate the throttle setting for the lift fan
thrust_out = _throttle_avg_max + thr_adj;
if (fabsf(yaw_thrust) > thrust_out) {
yaw_thrust = constrain_float(yaw_thrust, -thrust_out, thrust_out);
limit.yaw = true;
}
_thrust_yt_ccw = thrust_out + 0.5f * yaw_thrust;
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;
// limit thrust out for calculation of actuator gains
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,thrust_out), 0.1f, 1.0f);
if (is_zero(thrust_out)) {
limit.roll_pitch = true;
}
// force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
_actuator_out[0] = roll_thrust/thrust_out_actuator;
_actuator_out[1] = pitch_thrust/thrust_out_actuator;
if (fabsf(_actuator_out[0]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f);
}
if (fabsf(_actuator_out[1]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f);
}
_actuator_out[2] = -_actuator_out[0];
_actuator_out[3] = -_actuator_out[1];
}
