/*
find difference between home terrain height and the terrain height
at a given location, in meters. A positive result means the terrain
is higher than home.
return false is terrain at the given location or at home
location is not available
*/
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)) {
// we don't know the height of home
return false;
}
Location loc;
if (!ahrs.get_position(loc)) {
// we don't know where we are
return false;
}
if (!height_amsl(loc, height_loc)) {
if (!extrapolate || !have_current_loc_height) {
// we don't know the height of the given location
return false;
}
// we don't have data at the current location, but the caller
// has asked for extrapolation, so use the last available
// terrain height. This can be used to fill in while new data
// is fetched. It should be very rarely used
height_loc = last_current_loc_height;
}
terrain_difference = height_loc - height_home;
return true;
}
/*
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.
*/
void AP_Terrain::update(void)
{
// just schedule any needed disk IO
schedule_disk_io();
// try to ensure the home location is populated
float height;
height_amsl(ahrs.get_home(), height);
// update the cached current location height
Location loc;
if (ahrs.get_position(loc) && height_amsl(loc, height)) {
last_current_loc_height = height;
have_current_loc_height = true;
}
// check for pending mission data
update_mission_data();
// check for pending rally data
update_rally_data();
// update capabilities
if (enable) {
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
} else {
hal.util->clear_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
}
}
/*
log terrain data to dataflash log
*/
void AP_Terrain::log_terrain_data(DataFlash_Class &dataflash)
{
if (!enable) {
return;
}
Location loc;
if (!ahrs.get_position(loc)) {
// we don't know where we are
return;
}
float terrain_height = 0;
float current_height = 0;
uint16_t pending, loaded;
height_amsl(loc, terrain_height);
height_above_terrain(current_height, true);
get_statistics(pending, loaded);
struct log_TERRAIN pkt = {
LOG_PACKET_HEADER_INIT(LOG_TERRAIN_MSG),
time_us : hal.scheduler->micros64(),
status : (uint8_t)status(),
lat : loc.lat,
lng : loc.lng,
spacing : (uint16_t)grid_spacing,
terrain_height : terrain_height,
current_height : current_height,
pending : pending,
loaded : loaded
};
/*
find difference between home terrain height and the terrain height
at a given location, in meters. A positive result means the terrain
is higher than home.
return false is terrain at the given location or at home
location is not available
*/
bool AP_Terrain::height_terrain_difference_home(const Location &loc, float &terrain_difference)
{
float height_home, height_loc;
if (!height_amsl(ahrs.get_home(), height_home)) {
// we don't know the height of home
return false;
}
if (!height_amsl(loc, height_loc)) {
// we don't know the height of the given location
return false;
}
terrain_difference = height_loc - height_home;
return true;
}
/*
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.
*/
void AP_Terrain::update(void)
{
// just schedule any needed disk IO
schedule_disk_io();
// try to ensure the home location is populated
float height;
height_amsl(ahrs.get_home(), height);
}
/*
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.
*/
void AP_Terrain::update(void)
{
// just schedule any needed disk IO
schedule_disk_io();
const AP_AHRS &ahrs = AP::ahrs();
// try to ensure the home location is populated
float 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 = pos_valid && height_amsl(loc, height, false);
if (pos_valid && terrain_valid) {
last_current_loc_height = height;
have_current_loc_height = true;
}
// check for pending mission data
update_mission_data();
// check for pending rally data
update_rally_data();
// update capabilities and status
if (allocate()) {
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
if (!pos_valid) {
// we don't know where we are
system_status = TerrainStatusUnhealthy;
} else if (!terrain_valid) {
// we don't have terrain data at current location
system_status = TerrainStatusUnhealthy;
} else {
system_status = TerrainStatusOK;
}
} else {
hal.util->clear_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
system_status = TerrainStatusDisabled;
}
}
/*
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.
*/
void AP_Terrain::update(void)
{
// just schedule any needed disk IO
schedule_disk_io();
// try to ensure the home location is populated
float height;
height_amsl(ahrs.get_home(), height);
// update the cached current location height
Location loc;
if (ahrs.get_position(loc) && height_amsl(loc, height)) {
last_current_loc_height = height;
have_current_loc_height = true;
}
// check for pending mission data
update_mission_data();
// check for pending rally data
update_rally_data();
}
/*
calculate lookahead rise in terrain. This returns extra altitude
needed to clear upcoming terrain in meters
*/
float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio)
{
if (!enable || grid_spacing <= 0) {
return 0;
}
Location loc;
if (!ahrs.get_position(loc)) {
// we don't know where we are
return 0;
}
float base_height;
if (!height_amsl(loc, base_height)) {
// we don't know our current terrain height
return 0;
}
float climb = 0;
float lookahead_estimate = 0;
// check for terrain at grid spacing intervals
while (distance > 0) {
location_update(loc, bearing, grid_spacing);
climb += climb_ratio * grid_spacing;
distance -= grid_spacing;
float height;
if (height_amsl(loc, height)) {
float rise = (height - base_height) - climb;
if (rise > lookahead_estimate) {
lookahead_estimate = rise;
}
}
}
return lookahead_estimate;
}
/*
check that we have fetched all rally terrain data
*/
void AP_Terrain::update_rally_data(void)
{
if (last_rally_change_ms != rally.last_change_time_ms() ||
last_rally_spacing != grid_spacing) {
// a rally point has changed - start again
next_rally_index = 1;
last_rally_change_ms = rally.last_change_time_ms();
last_rally_spacing = grid_spacing;
}
if (next_rally_index == 0) {
// nothing to do
return;
}
uint16_t pending, loaded;
get_statistics(pending, loaded);
if (pending && ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// wait till we have fully filled the current set of grids
return;
}
while (true) {
// get next rally point
struct RallyLocation rp;
if (!rally.get_rally_point_with_index(next_rally_index, rp)) {
// nothing more to do
next_rally_index = 0;
return;
}
Location loc;
loc.lat = rp.lat;
loc.lng = rp.lng;
float height;
if (!height_amsl(loc, height)) {
// if we can't get data for a rally item then return and
// check again next time
return;
}
#if TERRAIN_DEBUG
hal.console->printf("checked rally point %u\n", (unsigned)next_rally_index);
#endif
// move to next rally point
next_rally_index++;
}
}
/*
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;
}
/*
check that we have fetched all mission terrain data
*/
void AP_Terrain::update_mission_data(void)
{
if (last_mission_change_ms != mission.last_change_time_ms() ||
last_mission_spacing != grid_spacing) {
// the mission has changed - start again
next_mission_index = 1;
next_mission_pos = 0;
last_mission_change_ms = mission.last_change_time_ms();
last_mission_spacing = grid_spacing;
}
if (next_mission_index == 0) {
// nothing to do
return;
}
uint16_t pending, loaded;
get_statistics(pending, loaded);
if (pending && ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// wait till we have fully filled the current set of grids
return;
}
// don't do more than 20 waypoints at a time, to prevent too much
// CPU usage
for (uint8_t i=0; i<20; i++) {
// get next mission command
AP_Mission::Mission_Command cmd;
if (!mission.read_cmd_from_storage(next_mission_index, cmd)) {
// nothing more to do
next_mission_index = 0;
return;
}
// we only want nav waypoint commands. That should be enough to
// prefill the terrain data and makes many things much simpler
while ((cmd.id != MAV_CMD_NAV_WAYPOINT &&
cmd.id != MAV_CMD_NAV_SPLINE_WAYPOINT) ||
(cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
next_mission_index++;
if (!mission.read_cmd_from_storage(next_mission_index, cmd)) {
// nothing more to do
next_mission_index = 0;
next_mission_pos = 0;
return;
}
}
// we will fetch 5 points around the waypoint. Four at 10 grid
// spacings away at 45, 135, 225 and 315 degrees, and the
// point itself
if (next_mission_pos != 4) {
location_update(cmd.content.location, 45+90*next_mission_pos, grid_spacing.get() * 10);
}
// we have a mission command to check
float height;
if (!height_amsl(cmd.content.location, height)) {
// if we can't get data for a mission item then return and
// check again next time
return;
}
next_mission_pos++;
if (next_mission_pos == 5) {
#if TERRAIN_DEBUG
hal.console->printf("checked waypoint %u\n", (unsigned)next_mission_index);
#endif
// move to next waypoint
next_mission_index++;
next_mission_pos = 0;
}
}
}
