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

AP_Terrain: added location_to_relative_home() function

This commit is contained in:
Andrew Tridgell 2014-07-24 18:34:21 +10:00
parent 8a48f06d18
commit e2e41d5da7
2 changed files with 34 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_Terrain/AP_Terrain.cpp
View File

@ -219,9 +219,36 @@ bool AP_Terrain::height_relative_home_equivalent(const Location &loc, float terr
return true;
}
/*
convert a Location altitude to a relative-to-home altitude in meters
This obeys the relative_alt and terrain_alt flags in Location.flags
*/
bool AP_Terrain::location_to_relative_home(const Location &loc, float &relative_altitude)
{
if (!loc.flags.terrain_alt) {
// its not a terrain alt
relative_altitude = loc.alt*0.01f;
if (!loc.flags.relative_alt) {
relative_altitude -= ahrs.get_home().alt*0.01f;
}
return true;
}
if (!height_relative_home_equivalent(loc, loc.alt*0.01f, relative_altitude)) {
return false;
}
// if terrain_alt is set and relative_alt is not set then Location
// is still offset by home alt
if (!loc.flags.relative_alt) {
relative_altitude -= ahrs.get_home().alt*0.01f;
}
return true;
}
/*
1Hz update function. This is here to ensure progress is made on disk
1hz update function. This is here to ensure progress is made on disk
IO even if no MAVLink send_request() operations are called for a
while.
*/

6
libraries/AP_Terrain/AP_Terrain.h
View File

@ -141,6 +141,12 @@ public:
*/
bool height_above_terrain(const Location &loc, float &terrain_altitude);
/*
convert a Location altitude to a relative-to-home altitude in meters
This obeys the relative_alt and terrain_alt flags in Location.flags
*/
bool location_to_relative_home(const Location &loc, float &relative_altitude);
private:
// allocate the terrain subsystem data
void allocate(void);