From 6f6c9e258564868b5dc6600c39aebbfe36c31aa0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 21 Apr 2014 21:34:33 +0900 Subject: [PATCH] AC_PosControl: bug fix to vertical speed limit Vehicle was not reaching target climb or descent rate because of incorrectly defaulted acceleration --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 18 +++++++++--------- libraries/AC_AttitudeControl/AC_PosControl.h | 13 ++++++------- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 53ffb3dd5e..4315780bea 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -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); } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index e2bc30b1b6..db16f38eb3 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -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