/* return estimated equivalent relative-to-home altitude in meters of a given height above the terrain for a given location. This function allows existing height controllers which work on barometric altitude (relative to home) to be used with terrain based target altitude, by translating the "above terrain" altitude into an equivalent barometric relative height. return false if terrain data is not available either at the given location or at the home location. */ bool AP_Terrain::height_relative_home_equivalent(const Location &loc, float terrain_altitude, float &relative_home_altitude) { float terrain_difference; if (!height_terrain_difference_home(loc, terrain_difference)) { return false; } relative_home_altitude = terrain_altitude + terrain_difference; return true; }
/* return estimated height above the terrain given a relative-to-home altitude (such as a barometric altitude) for a given location return false if terrain data is not available either at the given location or at the home location. */ bool AP_Terrain::height_above_terrain(const Location &loc, float relative_home_altitude, float &terrain_altitude) { float terrain_difference; if (!height_terrain_difference_home(loc, terrain_difference)) { return false; } terrain_altitude = relative_home_altitude - terrain_difference; return true; }
/* return estimated equivalent relative-to-home altitude in meters of a given height above the terrain at the current location This function allows existing height controllers which work on barometric altitude (relative to home) to be used with terrain based target altitude, by translating the "above terrain" altitude into an equivalent barometric relative height. return false if terrain data is not available either at the given location or at the home location. If extrapolate is true then allow return of an extrapolated terrain altitude based on the last available data */ bool AP_Terrain::height_relative_home_equivalent(float terrain_altitude, float &relative_home_altitude, bool extrapolate) { float terrain_difference; if (!height_terrain_difference_home(terrain_difference, extrapolate)) { return false; } relative_home_altitude = terrain_altitude + terrain_difference; return true; }
/* return current height above terrain at current AHRS position. If extrapolate is true then extrapolate from most recently available terrain data is terrain data is not available for the current location. Return true if height is available, otherwise false. */ bool AP_Terrain::height_above_terrain(float &terrain_altitude, bool extrapolate) { float terrain_difference; if (!height_terrain_difference_home(terrain_difference, extrapolate)) { return false; } float relative_home_altitude; AP::ahrs().get_relative_position_D_home(relative_home_altitude); relative_home_altitude = -relative_home_altitude; terrain_altitude = relative_home_altitude - terrain_difference; return true; }
/* return current height above terrain at current AHRS position. If extrapolate is true then extrapolate from most recently available terrain data is terrain data is not available for the current location. Return true if height is available, otherwise false. */ bool AP_Terrain::height_above_terrain(float &terrain_altitude, bool extrapolate) { float terrain_difference; if (!height_terrain_difference_home(terrain_difference, extrapolate)) { return false; } Location loc; if (!ahrs.get_position(loc)) { // we don't know where we are return false; } float relative_home_altitude = loc.alt*0.01f; if (!loc.flags.relative_alt) { // loc.alt has home altitude added, remove it relative_home_altitude -= ahrs.get_home().alt*0.01f; } terrain_altitude = relative_home_altitude - terrain_difference; return true; }