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

Copter: leonard's smoother alt-hold transition 

Target altitude when entering alt-hold is based on a projection from
current alt and climb rate
This commit is contained in:
Randy Mackay 2013-03-17 13:24:49 +09:00
parent 37040adfaa
commit fdcb78ccf7
3 changed files with 40 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/ArduCopter.pde
View File

@ -1800,13 +1800,13 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
case THROTTLE_STABILIZED_RATE:
case THROTTLE_DIRECT_ALT:
controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude
controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude
throttle_initialised = true;
break;
case THROTTLE_HOLD:
case THROTTLE_AUTO:
controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude
controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude
set_new_altitude(current_loc.alt); // by default hold the current altitude
if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the alt hold I terms if previous throttle mode was manual
reset_throttle_I();
@ -1818,7 +1818,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
case THROTTLE_LAND:
set_land_complete(false); // mark landing as incomplete
land_detector = 0; // A counter that goes up if our climb rate stalls out.
controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude
controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude
// Set target altitude to LAND_START_ALT if we are high, below this altitude the get_throttle_rate_stabilized will take care of setting the next_WP.alt
if (current_loc.alt >= LAND_START_ALT) {
set_new_altitude(LAND_START_ALT);

33
ArduCopter/Attitude.pde
View File

@ -957,6 +957,29 @@ static int32_t get_pilot_desired_direct_alt(int16_t throttle_control)
return desired_alt;
}
// get_initial_alt_hold - get new target altitude based on current altitude and climb rate
static int32_t
get_initial_alt_hold( int32_t alt_cm, int16_t climb_rate_cms)
{
int32_t target_alt;
int32_t linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt.
int32_t linear_velocity; // the velocity we swap between linear and sqrt.
linear_velocity = ALT_HOLD_ACCEL_MAX/g.pi_alt_hold.kP();
if (abs(climb_rate_cms) < linear_velocity) {
target_alt = alt_cm + climb_rate_cms/g.pi_alt_hold.kP();
} else {
linear_distance = ALT_HOLD_ACCEL_MAX/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if (climb_rate_cms > 0){
target_alt = alt_cm + linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX);
} else {
target_alt = alt_cm - ( linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX) );
}
}
return constrain(target_alt, alt_cm - ALT_HOLD_INIT_MAX_OVERSHOOT, alt_cm + ALT_HOLD_INIT_MAX_OVERSHOOT);
}
// get_throttle_rate - calculates desired accel required to achieve desired z_target_speed
// sets accel based throttle controller target
static void
@ -1025,19 +1048,19 @@ static void
get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate)
{
int32_t alt_error;
int16_t desired_rate;
int32_t linear_distance; // the distace we swap between linear and sqrt.
float desired_rate;
int32_t linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt.
// calculate altitude error
alt_error = target_alt - current_loc.alt;
// check kP to avoid division by zero
if( g.pi_alt_hold.kP() != 0 ) {
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
linear_distance = ALT_HOLD_ACCEL_MAX/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if( alt_error > 2*linear_distance ) {
desired_rate = safe_sqrt(2*250*(alt_error-linear_distance));
desired_rate = safe_sqrt(2*ALT_HOLD_ACCEL_MAX*(alt_error-linear_distance));
}else if( alt_error < -2*linear_distance ) {
desired_rate = -safe_sqrt(2*250*(-alt_error-linear_distance));
desired_rate = -safe_sqrt(2*ALT_HOLD_ACCEL_MAX*(-alt_error-linear_distance));
}else{
desired_rate = g.pi_alt_hold.get_p(alt_error);
}

9
ArduCopter/config.h
View File

@ -1038,6 +1038,15 @@
#endif
#define ACCELERATION_MAX_Z 750 // maximum veritcal acceleration in cm/s/s
// max distance in cm above or below current location that will be used for the alt target when transitioning to alt-hold mode
#ifndef ALT_HOLD_INIT_MAX_OVERSHOOT
# define ALT_HOLD_INIT_MAX_OVERSHOOT 200
#endif
// the acceleration used to define the distance-velocity curve
#ifndef ALT_HOLD_ACCEL_MAX
# define ALT_HOLD_ACCEL_MAX 250
#endif
// Throttle Accel control
#ifndef THROTTLE_ACCEL_P
# define THROTTLE_ACCEL_P 0.75f