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

Plane: fixed terrain RTL with rally points 

this fixes a bug where if the terrain database cache does not have the
tile for the location of a rally point then RTL to the rally point
with TERRAIN_FOLLOW=1 will not track terrain

The underlying issue is that Location::loc.change_alt_frame() will
return false if the location is not in the terrain memory cache. We
can't just extrapolate as the rally point could be in a totally
different terrain area to the current location. So instead we set it
as terrain_following_pending and fix it as soon as the terrain cache
is filled.

fixes https://github.com/ArduPilot/ardupilot/issues/25157
This commit is contained in:
Andrew Tridgell 2023-10-03 17:33:54 +11:00
parent 823a917ae1
commit c980e0488e
4 changed files with 25 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/Plane.h
View File

@ -735,6 +735,9 @@ private:
// are we trying to follow terrain?
bool terrain_following;
// are we waiting to load terrain data to init terrain following
bool terrain_following_pending;
// target altitude above terrain in cm, valid if terrain_following
// is set
int32_t terrain_alt_cm;
@ -855,7 +858,7 @@ private:
void reset_offset_altitude(void);
void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
bool above_location_current(const Location &loc);
void setup_terrain_target_alt(Location &loc) const;
void setup_terrain_target_alt(Location &loc);
int32_t adjusted_altitude_cm(void);
int32_t adjusted_relative_altitude_cm(void);
float mission_alt_offset(void);

14
ArduPlane/altitude.cpp
View File

@ -212,6 +212,12 @@ void Plane::set_target_altitude_location(const Location &loc)
target_altitude.amsl_cm += home.alt;
}
#if AP_TERRAIN_AVAILABLE
if (target_altitude.terrain_following_pending) {
/* we didn't get terrain data to init when we started on this
target, retry
*/
setup_terrain_target_alt(next_WP_loc);
}
/*
if this location has the terrain_alt flag set and we know the
terrain altitude of our current location then treat it as a
@ -469,12 +475,16 @@ bool Plane::above_location_current(const Location &loc)
modify a destination to be setup for terrain following if
TERRAIN_FOLLOW is enabled
*/
void Plane::setup_terrain_target_alt(Location &loc) const
void Plane::setup_terrain_target_alt(Location &loc)
{
#if AP_TERRAIN_AVAILABLE
if (terrain_enabled_in_current_mode()) {
loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN);
if (!loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) {
target_altitude.terrain_following_pending = true;
return;
}
}
target_altitude.terrain_following_pending = false;
#endif
}

4
ArduPlane/commands_logic.cpp
View File

@ -8,6 +8,10 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
// default to non-VTOL loiter
auto_state.vtol_loiter = false;
#if AP_TERRAIN_AVAILABLE
plane.target_altitude.terrain_following_pending = false;
#endif
// log when new commands start
if (should_log(MASK_LOG_CMD)) {
logger.Write_Mission_Cmd(mission, cmd);

4
ArduPlane/mode.cpp
View File

@ -97,6 +97,10 @@ bool Mode::enter()
quadplane.mode_enter();
#endif
#if AP_TERRAIN_AVAILABLE
plane.target_altitude.terrain_following_pending = false;
#endif
bool enter_result = _enter();
if (enter_result) {