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

AP_Terrain: removed terrain home correction

This commit is contained in:
Andrew Tridgell 2022-01-31 15:35:06 +11:00
parent 0de1185625
commit 5d3a0a78cb
4 changed files with 14 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Terrain/AP_Terrain.cpp
View File

@ -95,7 +95,7 @@ AP_Terrain::AP_Terrain() :
This function costs about 20 microseconds on Pixhawk This function costs about 20 microseconds on Pixhawk
*/ */
bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected) bool AP_Terrain::height_amsl(const Location &loc, float &height)
{ {
if (!allocate()) { if (!allocate()) {
return false; return false;
@ -107,10 +107,6 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected)
if (loc.lat == home_loc.lat && if (loc.lat == home_loc.lat &&
loc.lng == home_loc.lng) { loc.lng == home_loc.lng) {
height = home_height; height = home_height;
// apply correction which assumes home altitude is at terrain altitude
if (corrected) {
height += (ahrs.get_home().alt * 0.01f) - home_height;
}
return true; return true;
} }
@ -161,11 +157,6 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected)
home_loc = loc; home_loc = loc;
} }
// apply correction which assumes home altitude is at terrain altitude
if (corrected) {
height += (ahrs.get_home().alt * 0.01f) - home_height;
}
return true; return true;
} }
@ -186,7 +177,7 @@ bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool
const AP_AHRS &ahrs = AP::ahrs(); const AP_AHRS &ahrs = AP::ahrs();
float height_home, height_loc; float height_home, height_loc;
if (!height_amsl(ahrs.get_home(), height_home, false)) { if (!height_amsl(ahrs.get_home(), height_home)) {
// we don't know the height of home // we don't know the height of home
return false; return false;
} }
@ -197,7 +188,7 @@ bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool
return false; return false;
} }
if (!height_amsl(loc, height_loc, false)) { if (!height_amsl(loc, height_loc)) {
if (!extrapolate || !have_current_loc_height) { if (!extrapolate || !have_current_loc_height) {
// we don't know the height of the given location // we don't know the height of the given location
return false; return false;
@ -283,7 +274,7 @@ float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio)
return 0; return 0;
} }
float base_height; float base_height;
if (!height_amsl(loc, base_height, false)) { if (!height_amsl(loc, base_height)) {
// we don't know our current terrain height // we don't know our current terrain height
return 0; return 0;
} }
@ -297,7 +288,7 @@ float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio)
climb += climb_ratio * grid_spacing; climb += climb_ratio * grid_spacing;
distance -= grid_spacing; distance -= grid_spacing;
float height; float height;
if (height_amsl(loc, height, false)) { if (height_amsl(loc, height)) {
float rise = (height - base_height) - climb; float rise = (height - base_height) - climb;
if (rise > lookahead_estimate) { if (rise > lookahead_estimate) {
lookahead_estimate = rise; lookahead_estimate = rise;
@ -324,12 +315,12 @@ void AP_Terrain::update(void)
// try to ensure the home location is populated // try to ensure the home location is populated
float height; float height;
height_amsl(ahrs.get_home(), height, false); height_amsl(ahrs.get_home(), height);
// update the cached current location height // update the cached current location height
Location loc; Location loc;
bool pos_valid = ahrs.get_location(loc); bool pos_valid = ahrs.get_location(loc);
bool terrain_valid = pos_valid && height_amsl(loc, height, false); bool terrain_valid = pos_valid && height_amsl(loc, height);
if (pos_valid && terrain_valid) { if (pos_valid && terrain_valid) {
last_current_loc_height = height; last_current_loc_height = height;
have_current_loc_height = true; have_current_loc_height = true;
@ -372,7 +363,7 @@ void AP_Terrain::log_terrain_data()
float current_height = 0; float current_height = 0;
uint16_t pending, loaded; uint16_t pending, loaded;
height_amsl(loc, terrain_height, false); height_amsl(loc, terrain_height);
height_above_terrain(current_height, true); height_above_terrain(current_height, true);
get_statistics(pending, loaded); get_statistics(pending, loaded);

6
libraries/AP_Terrain/AP_Terrain.h
View File

@ -121,12 +121,8 @@ public:
find the terrain height in meters above sea level for a location find the terrain height in meters above sea level for a location
return false if not available return false if not available
if corrected is true then terrain alt is adjusted so that
the terrain altitude matches the home altitude at the home location
(i.e. we assume home is at the terrain altitude)
*/ */
bool height_amsl(const Location &loc, float &height, bool corrected); bool height_amsl(const Location &loc, float &height);
/* /*
find difference between home terrain height and the terrain find difference between home terrain height and the terrain

6
libraries/AP_Terrain/TerrainGCS.cpp
View File

@ -228,8 +228,8 @@ void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc
Location current_loc; Location current_loc;
const AP_AHRS &ahrs = AP::ahrs(); const AP_AHRS &ahrs = AP::ahrs();
if (ahrs.get_location(current_loc) && if (ahrs.get_location(current_loc) &&
height_amsl(ahrs.get_home(), home_terrain_height, false) && height_amsl(ahrs.get_home(), home_terrain_height) &&
height_amsl(loc, terrain_height, false)) { 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) {
@ -238,7 +238,7 @@ void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc
terrain_height = last_current_loc_height; terrain_height = last_current_loc_height;
} else { } else {
// report terrain height if we can, but can't give current_height // report terrain height if we can, but can't give current_height
height_amsl(loc, terrain_height, false); height_amsl(loc, terrain_height);
} }
uint16_t pending, loaded; uint16_t pending, loaded;
get_statistics(pending, loaded); get_statistics(pending, loaded);

4
libraries/AP_Terrain/TerrainMission.cpp
View File

@ -95,7 +95,7 @@ void AP_Terrain::update_mission_data(void)
// we have a mission command to check // we have a mission command to check
float height; float height;
if (!height_amsl(cmd.content.location, height, false)) { if (!height_amsl(cmd.content.location, height)) {
// if we can't get data for a mission item then return and // if we can't get data for a mission item then return and
// check again next time // check again next time
return; return;
@ -157,7 +157,7 @@ void AP_Terrain::update_rally_data(void)
loc.lat = rp.lat; loc.lat = rp.lat;
loc.lng = rp.lng; loc.lng = rp.lng;
float height; float height;
if (!height_amsl(loc, height, false)) { if (!height_amsl(loc, height)) {
// if we can't get data for a rally item then return and // if we can't get data for a rally item then return and
// check again next time // check again next time
return; return;