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

AR_AttitudeControl: add get_stopping_distance

Browse Source
This commit is contained in:
Randy Mackay 2018-03-13 21:05:47 +09:00
parent 9405c19e85
commit d139f0e28e
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -461,3 +461,18 @@ float AR_AttitudeControl::get_desired_speed() const
} }
return _desired_speed; 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;
}

3
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -92,6 +92,9 @@ public:
// get latest desired speed recorded during call to get_throttle_out_speed. For reporting purposes only // get latest desired speed recorded during call to get_throttle_out_speed. For reporting purposes only
float get_desired_speed() const; 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 // parameter var table
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];