mirror of https://github.com/ArduPilot/ardupilot
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:
parent
f88ea97131
commit
278587b04a
|
@ -723,6 +723,9 @@ private:
|
||||||
// are we trying to follow terrain?
|
// are we trying to follow terrain?
|
||||||
bool terrain_following;
|
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
|
// target altitude above terrain in cm, valid if terrain_following
|
||||||
// is set
|
// is set
|
||||||
int32_t terrain_alt_cm;
|
int32_t terrain_alt_cm;
|
||||||
|
@ -836,7 +839,7 @@ private:
|
||||||
void reset_offset_altitude(void);
|
void reset_offset_altitude(void);
|
||||||
void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
|
void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
|
||||||
bool above_location_current(const Location &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_altitude_cm(void);
|
||||||
int32_t adjusted_relative_altitude_cm(void);
|
int32_t adjusted_relative_altitude_cm(void);
|
||||||
float mission_alt_offset(void);
|
float mission_alt_offset(void);
|
||||||
|
|
|
@ -191,6 +191,12 @@ void Plane::set_target_altitude_location(const Location &loc)
|
||||||
target_altitude.amsl_cm += home.alt;
|
target_altitude.amsl_cm += home.alt;
|
||||||
}
|
}
|
||||||
#if AP_TERRAIN_AVAILABLE
|
#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
|
if this location has the terrain_alt flag set and we know the
|
||||||
terrain altitude of our current location then treat it as a
|
terrain altitude of our current location then treat it as a
|
||||||
|
@ -448,12 +454,16 @@ bool Plane::above_location_current(const Location &loc)
|
||||||
modify a destination to be setup for terrain following if
|
modify a destination to be setup for terrain following if
|
||||||
TERRAIN_FOLLOW is enabled
|
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 AP_TERRAIN_AVAILABLE
|
||||||
if (terrain_enabled_in_current_mode()) {
|
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
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -8,6 +8,10 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
|
||||||
// default to non-VTOL loiter
|
// default to non-VTOL loiter
|
||||||
auto_state.vtol_loiter = false;
|
auto_state.vtol_loiter = false;
|
||||||
|
|
||||||
|
#if AP_TERRAIN_AVAILABLE
|
||||||
|
plane.target_altitude.terrain_following_pending = false;
|
||||||
|
#endif
|
||||||
|
|
||||||
// log when new commands start
|
// log when new commands start
|
||||||
if (should_log(MASK_LOG_CMD)) {
|
if (should_log(MASK_LOG_CMD)) {
|
||||||
logger.Write_Mission_Cmd(mission, cmd);
|
logger.Write_Mission_Cmd(mission, cmd);
|
||||||
|
|
|
@ -91,6 +91,10 @@ bool Mode::enter()
|
||||||
quadplane.mode_enter();
|
quadplane.mode_enter();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if AP_TERRAIN_AVAILABLE
|
||||||
|
plane.target_altitude.terrain_following_pending = false;
|
||||||
|
#endif
|
||||||
|
|
||||||
bool enter_result = _enter();
|
bool enter_result = _enter();
|
||||||
|
|
||||||
if (enter_result) {
|
if (enter_result) {
|
||||||
|
|
Loading…
Reference in New Issue