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

Plane: fixed LOITER_TO_ALT with terrain target 

many thanks to Pompecukor for finding this!

Fixes #14951
This commit is contained in:
Andrew Tridgell 2020-07-31 19:12:33 +10:00
parent d4d1287261
commit b2fe3e71e6
1 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
ArduPlane/navigation.cpp
View File

@ -36,8 +36,9 @@ void Plane::loiter_angle_update(void)
const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
int32_t loiter_delta_cd;
const bool reached_target = reached_loiter_target();
if (loiter.sum_cd == 0 && !reached_loiter_target()) {
if (loiter.sum_cd == 0 && !reached_target) {
// we don't start summing until we are doing the real loiter
loiter_delta_cd = 0;
} else if (loiter.sum_cd == 0) {
@ -53,7 +54,35 @@ void Plane::loiter_angle_update(void)
loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
loiter.sum_cd += loiter_delta_cd * loiter.direction;
if (labs(current_loc.alt - next_WP_loc.alt) < 500) {
bool reached_target_alt = false;
if (reached_target) {
// once we reach the position target we start checking the
// altitude target
bool terrain_status_ok = false;
#if AP_TERRAIN_AVAILABLE
/*
if doing terrain following then we check against terrain
target, fetch the terrain information
*/
float altitude_agl = 0;
if (target_altitude.terrain_following) {
if (terrain.status() == AP_Terrain::TerrainStatusOK &&
terrain.height_above_terrain(altitude_agl, true)) {
terrain_status_ok = true;
}
}
if (terrain_status_ok &&
fabsf(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) {
reached_target_alt = true;
} else
#endif
if (!terrain_status_ok && labs(current_loc.alt - target_altitude.amsl_cm) < 500) {
reached_target_alt = true;
}
}
if (reached_target_alt) {
loiter.reached_target_alt = true;
loiter.unable_to_acheive_target_alt = false;
loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;