Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsTri: stability patch in 0 to 1 range

Browse Source
This commit is contained in:
Leonard Hall 2016-01-20 12:14:20 +09:00 committed by Randy Mackay
parent 80f77bc30b
commit d0a7579fa0
2 changed files with 121 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

209
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -133,120 +133,141 @@ uint16_t AP_MotorsTri::get_motor_mask()
(1U << AP_MOTORS_CH_TRI_YAW); (1U << AP_MOTORS_CH_TRI_YAW);
} }
// sends commands to the motors // 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_MotorsTri::output_armed_stabilizing() void AP_MotorsTri::output_armed_stabilizing()
{ {
int16_t roll_pwm; // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400 float roll_thrust; // roll thrust input value, +/- 1.0
int16_t pitch_pwm; // pitch pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400 float pitch_thrust; // pitch thrust input value, +/- 1.0
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 float yaw_thrust; // yaw thrust input value, +/- 1.0
int16_t yaw_radio_output; // final yaw pwm value sent to motors, typically ~1100-1900 float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
int16_t out_min = _throttle_radio_min + _min_throttle; float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
int16_t out_max = _throttle_radio_max; float throttle_thrust_rpy_mix; // partial calculation of throttle_thrust_best_rpy
int16_t motor_out[AP_MOTORS_MOT_4+1]; 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
float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); // throttle hover thrust value, 0.0 - 1.0
// initialize limits flags // apply voltage and air pressure compensation
limit.roll_pitch = false; roll_thrust = get_roll_thrust() * get_compensation_gain();
limit.yaw = false; pitch_thrust = get_pitch_thrust() * get_compensation_gain();
limit.throttle_lower = false; yaw_thrust = get_yaw_thrust() * get_compensation_gain()*sinf(_pivot_angle_max); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
limit.throttle_upper = false; throttle_thrust = get_throttle() * get_compensation_gain();
float pivot_angle_request_max = asin(yaw_thrust);
float pivot_thrust_max = cosf(pivot_angle_request_max);
float thrust_max = 1.0f;
// Throttle is 0 to 1000 only // sanity check throttle is above zero and below current limited throttle
int16_t thr_in_min = rel_pwm_to_thr_range(_min_throttle); if (throttle_thrust <= 0.0f) {
if (_throttle_control_input <= thr_in_min) { throttle_thrust = 0.0f;
_throttle_control_input = thr_in_min;
limit.throttle_lower = true; limit.throttle_lower = true;
} }
if (_throttle_control_input >= _max_throttle) { // convert throttle_max from 0~1000 to 0~1 range
_throttle_control_input = _max_throttle; if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true; limit.throttle_upper = true;
} }
// tricopters limit throttle to 80% throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix);
// To-Do: implement improved stability patch and remove this limit
if (_throttle_control_input > 800) { // The following mix may be offer less coupling between axis but needs testing
_throttle_control_input = 800; //_thrust_right = roll_thrust * -0.5f + pitch_thrust * 1.0f;
limit.throttle_upper = true; //_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;
} }
roll_pwm = calc_roll_pwm(); // calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
pitch_pwm = calc_pitch_pwm(); // 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
throttle_radio_output = calc_throttle_radio_output(); // 2. the higher of:
yaw_radio_output = calc_yaw_radio_output(); // 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
// if we are not sending a throttle output, we cut the motors // check everything fits
if( is_zero(_throttle_control_input) ) { throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, throttle_thrust_rpy_mix);
// range check spin_when_armed if(is_zero(rpy_low)){
if (_spin_when_armed_ramped < 0) { rpy_scale = 1.0f;
_spin_when_armed_ramped = 0; } else {
} rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
if (_spin_when_armed_ramped > _min_throttle) { }
_spin_when_armed_ramped = _min_throttle;
}
motor_out[AP_MOTORS_MOT_1] = _throttle_radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_2] = _throttle_radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_4] = _throttle_radio_min + _spin_when_armed_ramped;
}else{ // calculate how close the motors can come to the desired throttle
int16_t roll_out = (float)(roll_pwm * 0.866f); thr_adj = throttle_thrust - throttle_thrust_best_rpy;
int16_t pitch_out = pitch_pwm / 2; if(rpy_scale < 1.0f){
// Full range is being used by roll, pitch, and yaw.
// check if throttle is below limit limit.roll_pitch = true;
if (_throttle_control_input <= _min_throttle) { if(thr_adj < 0.0f){
limit.throttle_lower = true; limit.throttle_lower = true;
_throttle_control_input = _min_throttle; }else if(thr_adj > 0.0f){
throttle_radio_output = calc_throttle_radio_output(); limit.throttle_upper = true;
} }
thr_adj = 0.0f;
// TODO: set limits.roll_pitch and limits.yaw }else{
if(thr_adj < -(throttle_thrust_best_rpy+rpy_low)){
//left front // Throttle can't be reduced to desired value
motor_out[AP_MOTORS_MOT_2] = throttle_radio_output + roll_out + pitch_out; thr_adj = -(throttle_thrust_best_rpy+rpy_low);
//right front limit.throttle_lower = true;
motor_out[AP_MOTORS_MOT_1] = throttle_radio_output - roll_out + pitch_out; }else if(thr_adj > thrust_max - (throttle_thrust_best_rpy+rpy_high)){
// rear // Throttle can't be increased to desired value
motor_out[AP_MOTORS_MOT_4] = throttle_radio_output - pitch_pwm; thr_adj = thrust_max - (throttle_thrust_best_rpy+rpy_high);
limit.throttle_upper = true;
// Tridge's stability patch
if(motor_out[AP_MOTORS_MOT_1] > out_max) {
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_1] - out_max);
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_1] - out_max);
motor_out[AP_MOTORS_MOT_1] = out_max;
} }
if(motor_out[AP_MOTORS_MOT_2] > out_max) {
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_2] - out_max);
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_2] - out_max);
motor_out[AP_MOTORS_MOT_2] = out_max;
}
if(motor_out[AP_MOTORS_MOT_4] > out_max) {
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_4] - out_max);
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_4] - out_max);
motor_out[AP_MOTORS_MOT_4] = out_max;
}
// adjust for thrust curve and voltage scaling
motor_out[AP_MOTORS_MOT_1] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_1], out_min, out_max);
motor_out[AP_MOTORS_MOT_2] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_2], out_min, out_max);
motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max);
// ensure motors don't drop below a minimum value and stop
motor_out[AP_MOTORS_MOT_1] = MAX(motor_out[AP_MOTORS_MOT_1], out_min);
motor_out[AP_MOTORS_MOT_2] = MAX(motor_out[AP_MOTORS_MOT_2], out_min);
motor_out[AP_MOTORS_MOT_4] = MAX(motor_out[AP_MOTORS_MOT_4], out_min);
} }
hal.rcout->cork(); // 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;
// send output to each motor // calculate angle of yaw pivot
rc_write(AP_MOTORS_MOT_1, motor_out[AP_MOTORS_MOT_1]); if(is_zero(_thrust_rear)){
rc_write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]); limit.yaw = true;
rc_write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]); if(yaw_thrust > 0.0f){
_pivot_angle = _pivot_angle_max;
}else if(yaw_thrust < 0.0f){
_pivot_angle = -_pivot_angle_max;
} else {
_pivot_angle = 0.0f;
}
} else {
_pivot_angle = atan(yaw_thrust/_thrust_rear);
if(fabsf(_pivot_angle)>_pivot_angle_max){
limit.yaw = true;
_pivot_angle = constrain_float(_pivot_angle, -_pivot_angle_max, _pivot_angle_max);
}
}
// scale pivot thrust to account for pivot angle
// we should not need to check for divide by zero as _pivot_angle should always be much less than 90 degrees
_thrust_rear = _thrust_rear/cosf(_pivot_angle);
// send out to yaw command to tail servo // constrain all outputs to 0.0f to 1.0f
rc_write(AP_MOTORS_CH_TRI_YAW, yaw_radio_output); // 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);
hal.rcout->push(); _thrust_left = constrain_float(_thrust_left, 0.0f, 1.0f);
_thrust_rear = constrain_float(_thrust_rear, 0.0f, 1.0f);
} }
// output_test - spin a motor at the pwm value specified // output_test - spin a motor at the pwm value specified

7
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -52,11 +52,16 @@ protected:
void output_armed_stabilizing(); void output_armed_stabilizing();
// calc_yaw_radio_output - calculate final radio output for yaw channel // calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t calc_yaw_radio_output(); // calculate radio output for yaw servo, typically in range of 1100-1900 int16_t calc_yaw_radio_output(float yaw_input, float yaw_input_max); // calculate radio output for yaw servo, typically in range of 1100-1900
// parameters // parameters
AP_Int8 _yaw_reverse; // Reverse yaw output AP_Int8 _yaw_reverse; // Reverse yaw output
AP_Int16 _yaw_servo_trim; // Trim or center position of yaw servo AP_Int16 _yaw_servo_trim; // Trim or center position of yaw servo
AP_Int16 _yaw_servo_min; // Minimum angle limit of yaw servo AP_Int16 _yaw_servo_min; // Minimum angle limit of yaw servo
AP_Int16 _yaw_servo_max; // Maximum angle limit of yaw servo AP_Int16 _yaw_servo_max; // Maximum angle limit of yaw servo
float _pivot_angle_max = radians(30.0f); // Maximum angle of yaw pivot
float _pivot_angle; // Angle of yaw pivot
float _thrust_right;
float _thrust_rear;
float _thrust_left;
}; };