diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 93b3d2dfc7..99e362294a 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -461,3 +461,18 @@ float AR_AttitudeControl::get_desired_speed() const } return _desired_speed; } + +// get minimum stopping distance (in meters) given a speed (in m/s) +float AR_AttitudeControl::get_stopping_distance(float speed) +{ + // get maximum vehicle deceleration + const float accel_max = get_accel_max(); + + // avoid divide by zero + if ((accel_max <= 0.0f) || is_zero(speed)) { + return 0.0f; + } + + // assume linear deceleration + return 0.5f * sq(speed) / accel_max; +} diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index f3f9ac411c..c30f0cf9c9 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -92,6 +92,9 @@ public: // get latest desired speed recorded during call to get_throttle_out_speed. For reporting purposes only float get_desired_speed() const; + // get minimum stopping distance (in meters) given a speed (in m/s) + float get_stopping_distance(float speed); + // parameter var table static const struct AP_Param::GroupInfo var_info[];