Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
Andrew Tridgell 2014-08-25 14:55:15 +10:00
parent daa32f9b62
commit eeb04ba1b8
2 changed files with 21 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Terrain/AP_Terrain.h
View File

@ -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;

23
libraries/AP_Terrain/TerrainMission.cpp
View File

@ -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);
hal.console->printf("checked waypoint %u\n", (unsigned)next_mission_index);
#endif
// move to next waypoint
next_mission_index++;
// 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;
}