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

AC_PosControl: bug fix to vertical speed limit 

Vehicle was not reaching target climb or descent rate because of
incorrectly defaulted acceleration
This commit is contained in:
Randy Mackay 2014-04-21 21:34:33 +09:00
parent 12012c9530
commit 6f6c9e2585
2 changed files with 15 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -42,8 +42,8 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_speed_down_cms(POSCONTROL_SPEED_DOWN),
_speed_up_cms(POSCONTROL_SPEED_UP),
_speed_cms(POSCONTROL_SPEED),
_accel_z_cms(POSCONTROL_ACCEL_XY_MAX), // To-Do: check this default
_accel_cms(POSCONTROL_ACCEL_XY_MAX), // To-Do: check this default
_accel_z_cms(POSCONTROL_ACCEL_Z),
_accel_cms(POSCONTROL_ACCEL_XY),
_leash(POSCONTROL_LEASH_LENGTH_MIN),
_roll_target(0.0),
_pitch_target(0.0),
@ -145,17 +145,17 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
float linear_velocity; // the velocity we swap between linear and sqrt
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
linear_velocity = POSCONTROL_ALT_HOLD_ACCEL_MAX/_p_alt_pos.kP();
linear_velocity = _accel_z_cms/_p_alt_pos.kP();
if (fabs(curr_vel_z) < linear_velocity) {
// if our current velocity is below the cross-over point we use a linear function
stopping_point.z = curr_pos_z + curr_vel_z/_p_alt_pos.kP();
} else {
linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
if (curr_vel_z > 0){
stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX));
stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
} else {
stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z*curr_vel_z/(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX));
stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
}
}
stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_Z_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_Z_MAX);
@ -230,11 +230,11 @@ void AC_PosControl::pos_to_rate_z()
// check kP to avoid division by zero
if (_p_alt_pos.kP() != 0) {
linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
if (_pos_error.z > 2*linear_distance ) {
_vel_target.z = safe_sqrt(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX*(_pos_error.z-linear_distance));
_vel_target.z = safe_sqrt(2.0f*_accel_z_cms*(_pos_error.z-linear_distance));
}else if (_pos_error.z < -2.0f*linear_distance) {
_vel_target.z = -safe_sqrt(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX*(-_pos_error.z-linear_distance));
_vel_target.z = -safe_sqrt(2.0f*_accel_z_cms*(-_pos_error.z-linear_distance));
}else{
_vel_target.z = _p_alt_pos.get_p(_pos_error.z);
}

13
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -15,21 +15,20 @@
// position controller default definitions
#define POSCONTROL_THROTTLE_HOVER 450.0f // default throttle required to maintain hover
#define POSCONTROL_LEASH_Z 750.0f // leash length for z-axis altitude controller. To-Do: replace this with calculation based on alt_pos.kP()?
#define POSCONTROL_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller
#define POSCONTROL_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _accel parameter
#define POSCONTROL_ACCELERATION_MIN 50.0f // minimum horizontal acceleration in cm/s/s - used for sanity checking acceleration in leash length calculation
#define POSCONTROL_ACCEL_XY 100.0f // default horizontal acceleration in cm/s/s. This is overwritten by waypoint and loiter controllers
#define POSCONTROL_ACCEL_XY_MAX 980.0f // max horizontal acceleration in cm/s/s that the position velocity controller will ask from the lower accel controller
#define POSCONTROL_STOPPING_DIST_Z_MAX 200.0f // max stopping distance vertically
// should be 1.5 times larger than POSCONTROL_ACCELERATION.
// max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
#define POSCONTROL_TAKEOFF_JUMP_CM 20.0f // during take-off altitude target is set to current altitude + this value
#define POSCONTROL_SPEED 500.0f // maximum default horizontal speed in cm/s
#define POSCONTROL_SPEED_DOWN -150.0f // maximum default descent rate
#define POSCONTROL_SPEED_UP 250.0f // maximum default climb rate
#define POSCONTROL_SPEED 500.0f // default horizontal speed in cm/s
#define POSCONTROL_SPEED_DOWN -150.0f // default descent rate in cm/s
#define POSCONTROL_SPEED_UP 250.0f // default climb rate in cm/s
#define POSCONTROL_VEL_XY_MAX_FROM_POS_ERR 200.0f // max speed output from pos_to_vel controller when feed forward is used
#define POSCONTROL_ALT_HOLD_ACCEL_MAX 250.0f // hard coded copy of throttle controller's maximum acceleration in cm/s. To-Do: remove duplication with throttle controller definition
#define POSCONTROL_ACCEL_Z 250.0f // default vertical acceleration in cm/s/s.
#define POSCONTROL_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm