mirror of https://github.com/ArduPilot/ardupilot
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:
parent
5ee8f99e7b
commit
541cfa022b
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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…
Reference in New Issue