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

AP_Terrain: fixed assumption that HOME is on the ground 

this fixes height_above_terrain() to give a correct value when HOME is
not on the ground after the user has done a DO_SET_HOME with a
home position that is not at ground level
This commit is contained in:
Andrew Tridgell 2023-08-04 08:08:24 +10:00
parent 2e2c7b50ad
commit 3534417a12
2 changed files with 38 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Terrain/AP_Terrain.cpp
View File

@ -234,16 +234,30 @@ bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool
*/
bool AP_Terrain::height_above_terrain(float &terrain_altitude, bool extrapolate)
{
float terrain_difference;
if (!height_terrain_difference_home(terrain_difference, extrapolate)) {
const AP_AHRS &ahrs = AP::ahrs();
Location current_loc;
if (!ahrs.get_location(current_loc)) {
// we don't know where we are
return false;
}
float relative_home_altitude;
AP::ahrs().get_relative_position_D_home(relative_home_altitude);
relative_home_altitude = -relative_home_altitude;
float theight_loc;
if (!height_amsl(current_loc, theight_loc)) {
if (!extrapolate) {
return false;
}
// we don't have data at the current location, but the caller
// has asked for extrapolation, so use the last available
// terrain height. This can be used to fill in while new data
// is fetched. It should be very rarely used
theight_loc = last_current_loc_height;
}
terrain_altitude = relative_home_altitude - terrain_difference;
int32_t height_amsl_cm = 0;
UNUSED_RESULT(current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height_amsl_cm));
terrain_altitude = height_amsl_cm*0.01 - theight_loc;
return true;
}
@ -271,6 +285,22 @@ bool AP_Terrain::height_relative_home_equivalent(float terrain_altitude,
return false;
}
relative_home_altitude = terrain_altitude + terrain_difference;
/*
adjust for height of home above terrain height at home
*/
const AP_AHRS &ahrs = AP::ahrs();
const auto &home = ahrs.get_home();
int32_t home_height_amsl_cm = 0;
UNUSED_RESULT(home.get_alt_cm(Location::AltFrame::ABSOLUTE, home_height_amsl_cm));
float theight_home;
if (!height_amsl(home, theight_home)) {
return false;
}
relative_home_altitude += theight_home - home_height_amsl_cm*0.01;
return true;
}

19
libraries/AP_Terrain/TerrainGCS.cpp
View File

@ -212,35 +212,20 @@ void AP_Terrain::handle_data(mavlink_channel_t chan, const mavlink_message_t &ms
void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc, bool extrapolate)
{
float terrain_height = 0;
float home_terrain_height = 0;
uint16_t spacing = 0;
Location current_loc;
const AP_AHRS &ahrs = AP::ahrs();
if (ahrs.get_location(current_loc) &&
height_amsl(ahrs.get_home(), home_terrain_height) &&
height_amsl(loc, terrain_height)) {
if (height_amsl(loc, terrain_height)) {
// non-zero spacing indicates we have data
spacing = grid_spacing;
} else if (extrapolate && have_current_loc_height) {
// show the extrapolated height, so logs show what height is
// being used for navigation
terrain_height = last_current_loc_height;
} else {
// report terrain height if we can, but can't give current_height
height_amsl(loc, terrain_height);
}
uint16_t pending, loaded;
get_statistics(pending, loaded);
float current_height = 0.0f;
if (spacing == 0 && !(extrapolate && have_current_loc_height)) {
current_height = 0;
} else if (!current_loc.is_zero()) {
int32_t height_above_home_cm = 0;
UNUSED_RESULT(current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, height_above_home_cm));
current_height = height_above_home_cm * 0.01f; // cm -> m
}
current_height += home_terrain_height - terrain_height;
height_above_terrain(current_height, extrapolate);
if (HAVE_PAYLOAD_SPACE(chan, TERRAIN_REPORT)) {
mavlink_msg_terrain_report_send(chan, loc.lat, loc.lng, spacing,