mirror of https://github.com/ArduPilot/ardupilot
AP_Terrain: fetch more terrain data around waypoints
this ensures we have data for a wide region (1km) around each waypoint
This commit is contained in:
parent
daa32f9b62
commit
eeb04ba1b8
|
@ -385,6 +385,9 @@ private:
|
|||
// next mission command to check
|
||||
uint16_t next_mission_index;
|
||||
|
||||
// next mission position to check
|
||||
uint8_t next_mission_pos;
|
||||
|
||||
// last time the mission changed
|
||||
uint32_t last_mission_change_ms;
|
||||
|
||||
|
|
|
@ -37,6 +37,7 @@ void AP_Terrain::update_mission_data(void)
|
|||
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;
|
||||
}
|
||||
|
@ -47,7 +48,7 @@ void AP_Terrain::update_mission_data(void)
|
|||
|
||||
uint16_t pending, loaded;
|
||||
get_statistics(pending, loaded);
|
||||
if (pending) {
|
||||
if (pending && ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
// wait till we have fully filled the current set of grids
|
||||
return;
|
||||
}
|
||||
|
@ -72,10 +73,18 @@ void AP_Terrain::update_mission_data(void)
|
|||
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)) {
|
||||
|
@ -84,12 +93,16 @@ void AP_Terrain::update_mission_data(void)
|
|||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -112,7 +125,7 @@ void AP_Terrain::update_rally_data(void)
|
|||
|
||||
uint16_t pending, loaded;
|
||||
get_statistics(pending, loaded);
|
||||
if (pending) {
|
||||
if (pending && ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
// wait till we have fully filled the current set of grids
|
||||
return;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue