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

AC_Avoidance: stop based on upward facing proximity sensor

This commit is contained in:
Randy Mackay 2017-01-16 15:58:43 +09:00
parent 2133fd94ca
commit e799e3a342
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -104,6 +104,16 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
limit_alt = true;
}
// get distance from proximity sensor (in meters, convert to cm)
float proximity_alt_diff_m;
if (_proximity.get_upward_distance(proximity_alt_diff_m)) {
float proximity_alt_diff_cm = proximity_alt_diff_m * 100.0f;
if (!limit_alt || proximity_alt_diff_cm < alt_diff_cm) {
alt_diff_cm = proximity_alt_diff_cm;
}
limit_alt = true;
}
// limit climb rate
if (limit_alt) {
// do not allow climbing if we've breached the safe altitude