aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_Avoidance: adjust_velocity_z uses inav get_hgt_ctrl_limit

Browse Source 
inertial navigation's get_hgt_ctrl_limit provides an altitude-above-ekf-origin limit while flying with optical flow
This commit is contained in:
Randy Mackay 2017-01-16 13:49:51 +09:00
parent f73efd5249
commit 2133fd94ca
1 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -83,13 +83,29 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
// limit acceleration
float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
// stop before breaching fence altitude
if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) {
bool limit_alt = false;
float alt_diff_cm = 0.0f; // distance from altitude limit to vehicle in cm (positive means vehicle is below limit)
// calculate distance below fence
if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) {
// calculate distance from vehicle to safe altitude
float veh_alt = get_alt_above_home();
float alt_diff_cm = _fence.get_safe_alt() * 100.0f - veh_alt;
alt_diff_cm = _fence.get_safe_alt() * 100.0f - veh_alt;
limit_alt = true;
}
// calculate distance to optical flow altitude limit
float ekf_alt_limit_cm;
if (_inav.get_hgt_ctrl_limit(ekf_alt_limit_cm)) {
float ekf_alt_diff_cm = ekf_alt_limit_cm - _inav.get_altitude();
if (!limit_alt || ekf_alt_diff_cm < alt_diff_cm) {
alt_diff_cm = ekf_alt_diff_cm;
}
limit_alt = true;
}
// limit climb rate
if (limit_alt) {
// do not allow climbing if we've breached the safe altitude
if (alt_diff_cm <= 0.0f) {
climb_rate_cms = MIN(climb_rate_cms, 0.0f);