/* 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; } } }