Browse Source

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.
master
Peter Barker 3 years ago committed by Peter Barker
parent
commit
541cfa022b
  1. 24
      libraries/AP_Terrain/AP_Terrain.cpp
  2. 7
      libraries/AP_Terrain/AP_Terrain.h
  3. 19
      libraries/AP_Terrain/TerrainGCS.cpp

24
libraries/AP_Terrain/AP_Terrain.cpp

@ -348,6 +348,13 @@ void AP_Terrain::update(void) @@ -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) @@ -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

7
libraries/AP_Terrain/AP_Terrain.h

@ -342,6 +342,9 @@ private: @@ -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: @@ -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;

19
libraries/AP_Terrain/TerrainGCS.cpp

@ -139,21 +139,10 @@ void AP_Terrain::send_request(mavlink_channel_t chan) @@ -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;
}

Loading…
Cancel
Save