aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Terrain: height_amsl can correct for non-zero terrain alt at home position

Browse Source
This commit is contained in:
Randy Mackay 2016-03-29 09:30:57 +09:00
parent 7474e827ce
commit d84321be2e
4 changed files with 31 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Terrain/AP_Terrain.cpp
View File

@ -83,7 +83,7 @@ AP_Terrain::AP_Terrain(AP_AHRS &_ahrs, const AP_Mission &_mission, const AP_Rall
This function costs about 20 microseconds on Pixhawk
*/
bool AP_Terrain::height_amsl(const Location &loc, float &height)
bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected)
{
if (!enable || !allocate()) {
return false;
@ -93,6 +93,10 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
if (loc.lat == home_loc.lat &&
loc.lng == home_loc.lng) {
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;
}
@ -143,6 +147,11 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
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;
}
@ -158,7 +167,7 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool extrapolate)
{
float height_home, height_loc;
if (!height_amsl(ahrs.get_home(), height_home)) {
if (!height_amsl(ahrs.get_home(), height_home, false)) {
// we don't know the height of home
return false;
}
@ -169,7 +178,7 @@ bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool
return false;
}
if (!height_amsl(loc, height_loc)) {
if (!height_amsl(loc, height_loc, false)) {
if (!extrapolate || !have_current_loc_height) {
// we don't know the height of the given location
return false;
@ -260,7 +269,7 @@ float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio)
return 0;
}
float base_height;
if (!height_amsl(loc, base_height)) {
if (!height_amsl(loc, base_height, false)) {
// we don't know our current terrain height
return 0;
}
@ -274,7 +283,7 @@ float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio)
climb += climb_ratio * grid_spacing;
distance -= grid_spacing;
float height;
if (height_amsl(loc, height)) {
if (height_amsl(loc, height, false)) {
float rise = (height - base_height) - climb;
if (rise > lookahead_estimate) {
lookahead_estimate = rise;
@ -298,12 +307,12 @@ void AP_Terrain::update(void)
// try to ensure the home location is populated
float height;
height_amsl(ahrs.get_home(), height);
height_amsl(ahrs.get_home(), height, false);
// update the cached current location height
Location loc;
bool pos_valid = ahrs.get_position(loc);
bool terrain_valid = height_amsl(loc, height);
bool terrain_valid = height_amsl(loc, height, false);
if (pos_valid && terrain_valid) {
last_current_loc_height = height;
have_current_loc_height = true;
@ -351,7 +360,7 @@ void AP_Terrain::log_terrain_data(DataFlash_Class &dataflash)
float current_height = 0;
uint16_t pending, loaded;
height_amsl(loc, terrain_height);
height_amsl(loc, terrain_height, false);
height_above_terrain(current_height, true);
get_statistics(pending, loaded);

13
libraries/AP_Terrain/AP_Terrain.h
View File

@ -103,9 +103,16 @@ public:
void handle_terrain_check(mavlink_channel_t chan, mavlink_message_t *msg);
void handle_terrain_data(mavlink_message_t *msg);
// return terrain height in meters above sea level for a location
// return false if not available
bool height_amsl(const Location &loc, float &height);
/*
find the terrain height in meters above sea level for a location
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);
/*
find difference between home terrain height and the terrain

4
libraries/AP_Terrain/TerrainGCS.cpp
View File

@ -208,8 +208,8 @@ void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc
uint16_t spacing = 0;
Location current_loc;
if (ahrs.get_position(current_loc) &&
height_amsl(ahrs.get_home(), home_terrain_height) &&
height_amsl(loc, terrain_height)) {
height_amsl(ahrs.get_home(), home_terrain_height, false) &&
height_amsl(loc, terrain_height, false)) {
// non-zero spacing indicates we have data
spacing = grid_spacing;
} else if (extrapolate && have_current_loc_height) {

4
libraries/AP_Terrain/TerrainMission.cpp
View File

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