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

AC_Avoid: constify get_max_speed 

Also get_stopping_distance and get_margin
This commit is contained in:
Randy Mackay 2016-06-23 13:14:01 +09:00
parent 5153c3d195
commit c6f9889a25
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -87,7 +87,7 @@ Vector2f AC_Avoid::get_position()
* Computes the speed such that the stopping distance * Computes the speed such that the stopping distance
* of the vehicle will be exactly the input distance. * of the vehicle will be exactly the input distance.
*/ */
float AC_Avoid::get_max_speed(const float kP, const float accel_cmss, const float distance) float AC_Avoid::get_max_speed(const float kP, const float accel_cmss, const float distance) const
{ {
return AC_AttitudeControl::sqrt_controller(distance, kP, accel_cmss); return AC_AttitudeControl::sqrt_controller(distance, kP, accel_cmss);
} }
@ -97,7 +97,7 @@ float AC_Avoid::get_max_speed(const float kP, const float accel_cmss, const floa
* *
* Implementation copied from AC_PosControl. * Implementation copied from AC_PosControl.
*/ */
float AC_Avoid::get_stopping_distance(const float kP, const float accel_cmss, const float speed) float AC_Avoid::get_stopping_distance(const float kP, const float accel_cmss, const float speed) const
{ {
// avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
if (kP <= 0.0f || accel_cmss <= 0.0f || is_zero(speed)) { if (kP <= 0.0f || accel_cmss <= 0.0f || is_zero(speed)) {

6
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -48,17 +48,17 @@ private:
* Computes the speed such that the stopping distance * Computes the speed such that the stopping distance
* of the vehicle will be exactly the input distance. * of the vehicle will be exactly the input distance.
*/ */
float get_max_speed(const float kP, const float accel_cmss, const float distance); float get_max_speed(const float kP, const float accel_cmss, const float distance) const;
/* /*
* Computes distance required to stop, given current speed. * Computes distance required to stop, given current speed.
*/ */
float get_stopping_distance(const float kP, const float accel_cmss, const float speed); float get_stopping_distance(const float kP, const float accel_cmss, const float speed) const;
/* /*
* Gets the fence margin in cm * Gets the fence margin in cm
*/ */
float get_margin() { return _fence.get_margin() * 100.0f; } float get_margin() const { return _fence.get_margin() * 100.0f; }
// external references // external references
const AP_AHRS& _ahrs; const AP_AHRS& _ahrs;