mirror of https://github.com/ArduPilot/ardupilot
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:
parent
2e2c7b50ad
commit
3534417a12
|
@ -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)
|
bool AP_Terrain::height_above_terrain(float &terrain_altitude, bool extrapolate)
|
||||||
{
|
{
|
||||||
float terrain_difference;
|
const AP_AHRS &ahrs = AP::ahrs();
|
||||||
if (!height_terrain_difference_home(terrain_difference, extrapolate)) {
|
|
||||||
|
Location current_loc;
|
||||||
|
if (!ahrs.get_location(current_loc)) {
|
||||||
|
// we don't know where we are
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
float relative_home_altitude;
|
float theight_loc;
|
||||||
AP::ahrs().get_relative_position_D_home(relative_home_altitude);
|
if (!height_amsl(current_loc, theight_loc)) {
|
||||||
relative_home_altitude = -relative_home_altitude;
|
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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -271,6 +285,22 @@ bool AP_Terrain::height_relative_home_equivalent(float terrain_altitude,
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
relative_home_altitude = terrain_altitude + terrain_difference;
|
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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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)
|
void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc, bool extrapolate)
|
||||||
{
|
{
|
||||||
float terrain_height = 0;
|
float terrain_height = 0;
|
||||||
float home_terrain_height = 0;
|
|
||||||
uint16_t spacing = 0;
|
uint16_t spacing = 0;
|
||||||
Location current_loc;
|
if (height_amsl(loc, terrain_height)) {
|
||||||
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)) {
|
|
||||||
// non-zero spacing indicates we have data
|
// non-zero spacing indicates we have data
|
||||||
spacing = grid_spacing;
|
spacing = grid_spacing;
|
||||||
} else if (extrapolate && have_current_loc_height) {
|
} else if (extrapolate && have_current_loc_height) {
|
||||||
// show the extrapolated height, so logs show what height is
|
// show the extrapolated height, so logs show what height is
|
||||||
// being used for navigation
|
// being used for navigation
|
||||||
terrain_height = last_current_loc_height;
|
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;
|
uint16_t pending, loaded;
|
||||||
get_statistics(pending, loaded);
|
get_statistics(pending, loaded);
|
||||||
|
|
||||||
float current_height = 0.0f;
|
float current_height = 0.0f;
|
||||||
if (spacing == 0 && !(extrapolate && have_current_loc_height)) {
|
height_above_terrain(current_height, extrapolate);
|
||||||
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;
|
|
||||||
|
|
||||||
if (HAVE_PAYLOAD_SPACE(chan, TERRAIN_REPORT)) {
|
if (HAVE_PAYLOAD_SPACE(chan, TERRAIN_REPORT)) {
|
||||||
mavlink_msg_terrain_report_send(chan, loc.lat, loc.lng, spacing,
|
mavlink_msg_terrain_report_send(chan, loc.lat, loc.lng, spacing,
|
||||||
|
|
Loading…
Reference in New Issue