AP_Terrain: fix bad prearms-pass-when-they-shouldn't issue

The squares surrounding the current location were only checked when we went to send a terrain request.  This means it was possible that:
 - the number of pending requests could go to zero
 - prearm checks would pass
 - vehicle arm is attempted by the user (or a script, or ....)
 - the code would call AP_Terrain::send_requests which would create more pending requests
 - the arming sequence would fail

It's also possible for the arming to succeed, and then we're flying in violation of the intended prearm checks.

Doing things in the update function has the additional advantage of making things more efficient as we can push out terrain requests more often.
This commit is contained in:
Peter Barker 2022-08-06 13:26:21 +10:00 committed by Peter Barker
parent 5ee8f99e7b
commit 541cfa022b
3 changed files with 35 additions and 15 deletions

View File

@ -348,6 +348,13 @@ void AP_Terrain::update(void)
// check for pending rally data
update_rally_data();
// update tiles surrounding our current location:
if (pos_valid) {
have_surrounding_tiles = update_surrounding_tiles(loc);
} else {
have_surrounding_tiles = false;
}
// update capabilities and status
if (allocate()) {
if (!pos_valid) {
@ -362,7 +369,24 @@ void AP_Terrain::update(void)
} else {
system_status = TerrainStatusDisabled;
}
}
bool AP_Terrain::update_surrounding_tiles(const Location &loc)
{
// also request a larger set of up to 9 grids
bool ret = true;
for (int8_t x=-1; x<=1; x++) {
for (int8_t y=-1; y<=1; y++) {
Location loc2 = loc;
loc2.offset(x*TERRAIN_GRID_BLOCK_SIZE_X*0.7f*grid_spacing,
y*TERRAIN_GRID_BLOCK_SIZE_Y*0.7f*grid_spacing);
float height;
if (!height_amsl(loc2, height)) {
ret = false;
}
}
}
return ret;
}
bool AP_Terrain::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) const

View File

@ -342,6 +342,9 @@ private:
void write_block(void);
void read_block(void);
// check for missing data in squares surrounding loc:
bool update_surrounding_tiles(const Location &loc);
/*
check for missing mission terrain data
*/
@ -426,6 +429,10 @@ private:
bool have_current_loc_height;
float last_current_loc_height;
// true if we have all of the data for the squares around the
// current location:
bool have_surrounding_tiles;
// next mission command to check
uint16_t next_mission_index;

View File

@ -139,21 +139,10 @@ void AP_Terrain::send_request(mavlink_channel_t chan)
return;
}
// also request a larger set of up to 9 grids
for (int8_t x=-1; x<=1; x++) {
for (int8_t y=-1; y<=1; y++) {
Location loc2 = loc;
loc2.offset(x*TERRAIN_GRID_BLOCK_SIZE_X*0.7f*grid_spacing,
y*TERRAIN_GRID_BLOCK_SIZE_Y*0.7f*grid_spacing);
struct grid_info info2;
calculate_grid_info(loc2, info2);
if (request_missing(chan, info2)) {
return;
}
}
}
// check cache blocks that may have been setup by a TERRAIN_CHECK
// check cache blocks that may have been setup by a TERRAIN_CHECK,
// mission items, rally items, squares surrounding our current
// location, favourite holiday destination, scripting, height
// reference location, ....
if (send_cache_request(chan)) {
return;
}