AP_Terrain: added status() interface for reporting

This commit is contained in:
Andrew Tridgell 2014-07-24 20:16:03 +10:00
parent 49c28b747f
commit c32595e6ed
2 changed files with 30 additions and 0 deletions

View File

@ -262,4 +262,25 @@ void AP_Terrain::update(void)
height_amsl(ahrs.get_home(), height);
}
/*
return status enum for health reporting
*/
enum AP_Terrain::TerrainStatus AP_Terrain::status(void)
{
if (!enable) {
return TerrainStatusDisabled;
}
Location loc;
if (!ahrs.get_position(loc)) {
// we don't know where we are
return TerrainStatusUnhealthy;
}
float height;
if (!height_amsl(loc, height)) {
// we don't have terrain data at current location
return TerrainStatusUnhealthy;
}
return TerrainStatusOK;
}
#endif // HAVE_AP_TERRAIN

View File

@ -83,11 +83,20 @@ public:
AP_Int8 enable;
AP_Int16 grid_spacing; // meters between grid points
enum TerrainStatus {
TerrainStatusDisabled = 0, // not enabled
TerrainStatusUnhealthy = 1, // no terrain data for current location
TerrainStatusOK = 2 // terrain data available
};
static const struct AP_Param::GroupInfo var_info[];
// update terrain state. Should be called at 1Hz or more
void update(void);
// return status enum for health reporting
enum TerrainStatus status(void);
// send any pending terrain request message
void send_request(mavlink_channel_t chan);