diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 0b21a01027..a97eb18a36 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -58,7 +58,6 @@ AP_Terrain::AP_Terrain(AP_AHRS &_ahrs, const AP_Mission &_mission, const AP_Rall mission(_mission), rally(_rally), disk_io_state(DiskIoIdle), - last_request_time_ms(0), fd(-1), timer_setup(false), file_lat_degrees(0), @@ -72,6 +71,7 @@ AP_Terrain::AP_Terrain(AP_AHRS &_ahrs, const AP_Mission &_mission, const AP_Rall AP_Param::setup_object_defaults(this, var_info); memset(&home_loc, 0, sizeof(home_loc)); memset(&disk_block, 0, sizeof(disk_block)); + memset(last_request_time_ms, 0, sizeof(last_request_time_ms)); } /* diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index df81003c53..ba503a6458 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -352,7 +352,7 @@ private: union grid_io_block disk_block; // last time we asked for more grids - uint32_t last_request_time_ms; + uint32_t last_request_time_ms[MAVLINK_COMM_NUM_BUFFERS]; static const uint64_t bitmap_mask = (((uint64_t)1U)<<(TERRAIN_GRID_BLOCK_MUL_X*TERRAIN_GRID_BLOCK_MUL_Y)) - 1; diff --git a/libraries/AP_Terrain/TerrainGCS.cpp b/libraries/AP_Terrain/TerrainGCS.cpp index 56403894c7..73e32f7cab 100644 --- a/libraries/AP_Terrain/TerrainGCS.cpp +++ b/libraries/AP_Terrain/TerrainGCS.cpp @@ -64,7 +64,7 @@ bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcac ask the GCS to send a set of 4x4 grids */ mavlink_msg_terrain_request_send(chan, grid.lat, grid.lon, grid_spacing, bitmap_mask & ~grid.bitmap); - last_request_time_ms = hal.scheduler->millis(); + last_request_time_ms[chan] = hal.scheduler->millis(); return true; } @@ -102,7 +102,7 @@ void AP_Terrain::send_request(mavlink_channel_t chan) send_terrain_report(chan, loc, true); // did we request recently? - if (hal.scheduler->millis() - last_request_time_ms < 2000) { + if (hal.scheduler->millis() - last_request_time_ms[chan] < 2000) { // too soon to request again return; }