diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 206c7d35107448..b2926a8a35acf8 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -293,7 +293,7 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const 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); + stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_DOWN_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_UP_MAX); } /// init_takeoff - initialises target altitude if we are taking off diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 5c68f89f4133a6..608d6db4814535 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -16,9 +16,10 @@ #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_STOPPING_DIST_UP_MAX 300.0f // max stopping distance (in cm) vertically while climbing +#define POSCONTROL_STOPPING_DIST_DOWN_MAX 200.0f // max stopping distance (in cm) vertically while descending #define POSCONTROL_JERK_LIMIT_CMSSS 1700.0f // default jerk limit on horizontal acceleration (unit: m/s/s/s) #define POSCONTROL_SPEED 500.0f // default horizontal speed in cm/s