Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: add flags for upper and lower throttle limits 

Freeze desired altitude when motors hit a limit
This commit is contained in:
Randy Mackay 2013-07-26 00:45:59 +09:00
parent de59ea3ddc
commit fe412437c2
3 changed files with 78 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/Attitude.pde
View File

@ -886,12 +886,15 @@ get_throttle_accel(int16_t z_target_accel)
// separately calculate p, i, d values for logging
p = g.pid_throttle_accel.get_p(z_accel_error);
// freeze I term if we've breached throttle limits
if (motors.limit.throttle) {
i = g.pid_throttle_accel.get_integrator();
}else{
// get i term
i = g.pid_throttle_accel.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if ((!motors.limit.throttle_lower && !motors.limit.throttle_lower) || (i>0&&z_accel_error<0) || (i<0&&z_accel_error>0)) {
i = g.pid_throttle_accel.get_i(z_accel_error, .01f);
}
d = g.pid_throttle_accel.get_d(z_accel_error, .01f);
//
@ -1103,8 +1106,11 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
static void
get_throttle_althold_with_slew(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate)
{
// limit target altitude change
controller_desired_alt += constrain_float(target_alt-controller_desired_alt, min_climb_rate*0.02f, max_climb_rate*0.02f);
float alt_change = target_alt-controller_desired_alt;
// adjust desired alt if motors have not hit their limits
if ((alt_change<0 && !motors.limit.throttle_lower) || (alt_change>0 && !motors.limit.throttle_upper)) {
controller_desired_alt += constrain_float(alt_change, min_climb_rate*0.02f, max_climb_rate*0.02f);
}
// do not let target altitude get too far from current altitude
controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
@ -1118,7 +1124,10 @@ get_throttle_althold_with_slew(int32_t target_alt, int16_t min_climb_rate, int16
static void
get_throttle_rate_stabilized(int16_t target_rate)
{
controller_desired_alt += target_rate * 0.02f;
// adjust desired alt if motors have not hit their limits
if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
controller_desired_alt += target_rate * 0.02f;
}
// do not let target altitude get too far from current altitude
controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
@ -1178,7 +1187,10 @@ get_throttle_surface_tracking(int16_t target_rate)
}
last_call_ms = now;
target_sonar_alt += target_rate * 0.02f;
// adjust target alt if motors have not hit their limits
if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
target_sonar_alt += target_rate * 0.02f;
}
distance_error = (target_sonar_alt-sonar_alt);
sonar_induced_slew_rate = constrain_float(fabsf(g.sonar_gain * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX);

85
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -80,6 +80,12 @@ void AP_MotorsMatrix::output_min()
{
int8_t i;
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = false;
// fill the motor_out[] array for HIL use and send minimum value to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
@ -94,10 +100,10 @@ void AP_MotorsMatrix::output_min()
void AP_MotorsMatrix::output_armed()
{
int8_t i;
int16_t out_min = _rc_throttle->radio_min + _min_throttle;
int16_t out_max = _rc_throttle->radio_max;
int16_t out_mid = (out_min+out_max)/2;
int16_t out_max_range; // the is the allowable throttle out setting that allowes maximum roll, pitch and yaw range
int16_t out_min_pwm = _rc_throttle->radio_min + _min_throttle; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _rc_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.
@ -105,12 +111,13 @@ void AP_MotorsMatrix::output_armed()
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; // how far we move the throttle point from out_max_range
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 flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// Throttle is 0 to 1000 only
// To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object
@ -141,16 +148,17 @@ void AP_MotorsMatrix::output_armed()
// Every thing is limited
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle = true;
limit.throttle_lower = true;
} else {
// check if throttle is below limit
if (_rc_throttle->radio_out < out_min) {
limit.throttle = true;
if (_rc_throttle->radio_out <= out_min_pwm) { // perhaps being at min throttle itself is not a problem, only being under is
limit.throttle_lower = true;
}
// 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] = _rc_roll->pwm_out * _roll_factor[i] +
@ -167,16 +175,21 @@ void AP_MotorsMatrix::output_armed()
}
}
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000)
// this value is either:
// mid throttle - average of highest and lowest motor
// the higher of the pilot's throttle input or hover-throttle -- this ensure we never increase the throttle above hover throttle unless the pilot has commanded that
// 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_max_range = min(out_mid - motor_mid, max(_rc_throttle->radio_out, (_rc_throttle->radio_out+_hover_out)/2));
out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle->radio_out, (_rc_throttle->radio_out+_hover_out)/2));
// 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 - out_max_range, out_max_range - out_min) - (rpy_high-rpy_low)/2;
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, AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM);
if (_rc_yaw->pwm_out >= 0) {
@ -197,6 +210,8 @@ void AP_MotorsMatrix::output_armed()
}
// 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] +
@ -214,32 +229,44 @@ void AP_MotorsMatrix::output_armed()
}
// check everything fits
thr_adj = _rc_throttle->radio_out - out_max_range;
thr_adj = _rc_throttle->radio_out - out_best_thr_pwm;
// calc upper and lower limits of thr_adj
int16_t thr_adj_max = out_max_pwm-(out_best_thr_pwm+rpy_high);
// 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
if (thr_adj > out_max-(rpy_high+out_max_range)){
thr_adj = out_max-(rpy_high+out_max_range);
// 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 = true;
limit.throttle_upper = true;
}
}else if(thr_adj < 0){
// decrease throttle as close as possible to requested throttle
// without going under out_min or over out_max
// earlier code ensures we can't break both boundaryies
thr_adj = max(min(thr_adj,out_max-(rpy_high+out_max_range)), min(out_min-(rpy_low+out_max_range),0));
// 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;
limit.throttle_lower = true;
}
}
// do we need to reduce roll, pitch, yaw command
// earlier code does not allow both limit's to be passed simultainiously with abs(_yaw_factor)<1
if ((rpy_low+out_max_range)+thr_adj < out_min){
rpy_scale = (float)(out_min-thr_adj-out_max_range)/rpy_low;
if ((rpy_low+out_best_thr_pwm)+thr_adj < out_min_pwm){
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_max_range)+thr_adj > out_max){
rpy_scale = (float)(out_max-thr_adj-out_max_range)/rpy_high;
}else if((rpy_high+out_best_thr_pwm)+thr_adj > out_max_pwm){
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;
@ -248,7 +275,7 @@ void AP_MotorsMatrix::output_armed()
// 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_max_range+thr_adj +
motor_out[i] = out_best_thr_pwm+thr_adj +
rpy_scale*rpy_out[i];
}
}
@ -264,7 +291,7 @@ void AP_MotorsMatrix::output_armed()
// 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, out_max);
motor_out[i] = constrain_int16(motor_out[i], out_min_pwm, out_max_pwm);
}
}
}

3
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -124,7 +124,8 @@ public:
struct AP_Motors_limit {
uint8_t roll_pitch : 1; // we have reached roll or pitch limit
uint8_t yaw : 1; // we have reached yaw limit
uint8_t throttle : 1; // we have reached throttle limit
uint8_t throttle_lower : 1; // we have reached throttle's lower limit
uint8_t throttle_upper : 1; // we have reached throttle's upper limit
} limit;
// var_info for holding Parameter information