Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-10 01:44:00 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: add AUTO_VELZ_MIN, AUTO_VELZ_MAX and PILOT_VELZ_MAX to allow better control of climb/descent rate in auto and manual throttle modes

Browse Source
This commit is contained in:
rmackay9 2012-12-22 12:52:49 +09:00 committed by Andrew Tridgell
parent 2de676306e
commit e850ab7ccd
5 changed files with 81 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/ArduCopter.pde
View File

@ -871,7 +871,7 @@ AP_Limit_Altitude altitude_limit(&current_loc);
////////////////////////////////////////////////////////////////////////////////
// function definitions to keep compiler from complaining about undeclared functions
////////////////////////////////////////////////////////////////////////////////
void get_throttle_althold(int32_t target_alt, int16_t max_climb_rate = ALTHOLD_MAX_CLIMB_RATE);
void get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate);
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
@ -1901,7 +1901,7 @@ void update_throttle_mode(void)
}else{
// To-Do: this should update the global desired altitude variable next_WP.alt
int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in);
get_throttle_althold(desired_alt);
get_throttle_althold(desired_alt, g.auto_velocity_z_min, g.auto_velocity_z_max);
}
break;
@ -1914,10 +1914,8 @@ void update_throttle_mode(void)
case THROTTLE_AUTO:
// auto pilot altitude controller with target altitude held in next_WP.alt
if(motors.auto_armed() == true) {
get_throttle_althold(next_WP.alt);
// TO-DO: need to somehow set nav_throttle
get_throttle_althold(next_WP.alt, g.auto_velocity_z_min, g.auto_velocity_z_max);
}
// TO-DO: what if auto_armed is not true?! throttle stuck at unknown position?
break;
case THROTTLE_LAND:

68
ArduCopter/Attitude.pde
View File

@ -813,7 +813,7 @@ get_throttle_accel(int16_t z_target_accel)
// Calculate Earth Frame Z acceleration
z_accel_meas = ahrs.get_accel_ef().z;
// calculate accel error and Filter with fc = 1 Hz
// calculate accel error and Filter with fc = 2 Hz
z_accel_error = z_accel_error + 0.11164 * (constrain(z_target_accel + z_accel_meas, -32000, 32000) - z_accel_error);
// separately calculate p, i, d values for logging
@ -867,10 +867,10 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
// check throttle is above, below or in the deadband
if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) {
// below the deadband
desired_rate = (int32_t)VELOCITY_MAX_Z * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
}else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) {
// above the deadband
desired_rate = (int32_t)VELOCITY_MAX_Z * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
}else{
// must be in the deadband
desired_rate = 0;
@ -984,6 +984,35 @@ get_throttle_rate(int16_t z_target_speed)
}
}
// get_throttle_althold - hold at the desired altitude in cm
// updates accel based throttle controller targets
// Note: max_climb_rate is an optional parameter to allow reuse of this function by landing controller
static void
get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate)
{
int32_t _altitude_error;
int16_t desired_rate;
int32_t linear_distance; // the distace we swap between linear and sqrt.
// calculate altitude error
altitude_error = target_alt - current_loc.alt;
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if( altitude_error < linear_distance ) {
desired_rate = g.pi_alt_hold.get_p(altitude_error);
}else{
desired_rate = sqrt(2*250*(altitude_error-linear_distance));
}
desired_rate = constrain(desired_rate, min_climb_rate, max_climb_rate);
// call rate based throttle controller which will update accel based throttle controller targets
get_throttle_rate(desired_rate);
// TO-DO: enabled PID logging for this controller
}
// get_throttle_rate_stabilized - rate controller with additional 'stabilizer'
// 'stabilizer' ensure desired rate is being met
// calls normal throttle rate controller which updates accel based throttle controller targets
@ -1008,36 +1037,7 @@ get_throttle_rate_stabilized(int16_t target_rate)
set_new_altitude(target_alt);
get_throttle_althold(target_alt, ALTHOLD_MAX_CLIMB_RATE);
}
// get_throttle_althold - hold at the desired altitude in cm
// updates accel based throttle controller targets
// Note: max_climb_rate is an optional parameter to allow reuse of this function by landing controller
static void
get_throttle_althold(int32_t target_alt, int16_t max_climb_rate)
{
int32_t _altitude_error;
int16_t desired_rate;
int32_t linear_distance; // the distace we swap between linear and sqrt.
// calculate altitude error
altitude_error = target_alt - current_loc.alt;
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if( altitude_error < linear_distance ) {
desired_rate = g.pi_alt_hold.get_p(altitude_error);
}else{
desired_rate = sqrt(2*250*(altitude_error-linear_distance));
}
desired_rate = constrain(desired_rate, ALTHOLD_MIN_CLIMB_RATE, max_climb_rate);
// call rate based throttle controller which will update accel based throttle controller targets
get_throttle_rate(desired_rate);
// TO-DO: enabled PID logging for this controller
get_throttle_althold(target_alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller
}
// get_throttle_land - high level landing logic
@ -1050,7 +1050,7 @@ get_throttle_land()
{
// if we are above 10m perform regular alt hold descent
if (current_loc.alt >= LAND_START_ALT) {
get_throttle_althold(LAND_START_ALT, -abs(g.land_speed));
get_throttle_althold(LAND_START_ALT, g.auto_velocity_z_min, -abs(g.land_speed));
}else{
get_throttle_rate_stabilized(-abs(g.land_speed));

10
ArduCopter/Parameters.h
View File

@ -76,7 +76,8 @@ public:
k_param_rssi_pin,
k_param_throttle_accel_enabled,
k_param_yaw_override_behaviour,
k_param_acro_trainer_enabled, // 27
k_param_acro_trainer_enabled,
k_param_pilot_velocity_z_max, // 28
// 65: AP_Limits Library
k_param_limits = 65,
@ -207,7 +208,9 @@ public:
k_param_waypoint_radius,
k_param_circle_radius,
k_param_waypoint_speed_max,
k_param_land_speed, // 217
k_param_land_speed,
k_param_auto_velocity_z_min,
k_param_auto_velocity_z_max, // 219
//
// 220: PI/D Controllers
@ -287,6 +290,9 @@ public:
AP_Int16 crosstrack_min_distance;
AP_Int32 rtl_loiter_time;
AP_Int16 land_speed;
AP_Int16 auto_velocity_z_min; // minimum vertical velocity (i.e. maximum descent) the autopilot may request
AP_Int16 auto_velocity_z_max; // maximum vertical velocity the autopilot may request
AP_Int16 pilot_velocity_z_max; // maximum vertical velocity the pilot may request
// Throttle

27
ArduCopter/Parameters.pde
View File

@ -263,6 +263,33 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED),
// @Param: AUTO_VELZ_MIN
// @DisplayName: Autopilot's min vertical speed (max descent) in cm/s
// @Description: The minimum vertical velocity (i.e. descent speed) the autopilot may request in cm/s
// @Units: Centimeters/Second
// @Range: -500 -50
// @Increment: 10
// @User: Standard
GSCALAR(auto_velocity_z_min, "AUTO_VELZ_MIN", AUTO_VELZ_MIN),
// @Param: AUTO_VELZ_MAX
// @DisplayName: Auto pilot's max vertical speed in cm/s
// @Description: The maximum vertical velocity the autopilot may request in cm/s
// @Units: Centimeters/Second
// @Range: 50 500
// @Increment: 10
// @User: Standard
GSCALAR(auto_velocity_z_max, "AUTO_VELZ_MAX", AUTO_VELZ_MAX),
// @Param: PILOT_VELZ_MAX
// @DisplayName: Pilot maximum vertical speed
// @Description: The maximum vertical velocity the pilot may request in cm/s
// @Units: Centimeters/Second
// @Range: 10 500
// @Increment: 10
// @User: Standard
GSCALAR(pilot_velocity_z_max, "PILOT_VELZ_MAX", PILOT_VELZ_MAX),
// @Param: THR_MIN
// @DisplayName: Minimum Throttle
// @Description: The minimum throttle that will be sent to the motors to keep them spinning

16
ArduCopter/config.h
View File

@ -926,16 +926,18 @@
#endif
// minimum and maximum climb rates while in alt hold mode
#ifndef ALTHOLD_MAX_CLIMB_RATE
# define ALTHOLD_MAX_CLIMB_RATE 250
// default minimum and maximum vertical velocity the autopilot may request
#ifndef AUTO_VELZ_MIN
# define AUTO_VELZ_MIN -125
#endif
#ifndef ALTHOLD_MIN_CLIMB_RATE
# define ALTHOLD_MIN_CLIMB_RATE -ALTHOLD_MAX_CLIMB_RATE
#ifndef AUTO_VELZ_MAX
# define AUTO_VELZ_MAX 125
#endif
// max allowed acceleration
#define VELOCITY_MAX_Z 250 // maximum vertical velocity in cm/s
// default maximum vertical velocity the pilot may request
#ifndef PILOT_VELZ_MAX
# define PILOT_VELZ_MAX 250 // maximum vertical velocity in cm/s
#endif
#define ACCELERATION_MAX_Z 750 // maximum veritcal acceleration in cm/s/s
// Throttle Accel control