diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 3f5ba26c31..e925662e16 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -56,9 +56,8 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = { }; // constructor -AP_Terrain::AP_Terrain(const AP_Mission &_mission, const AP_Rally &_rally) : +AP_Terrain::AP_Terrain(const AP_Mission &_mission) : mission(_mission), - rally(_rally), disk_io_state(DiskIoIdle), fd(-1) { diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index bcaad7dcdb..c090690aab 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -29,7 +29,6 @@ #include #include #include -#include #define TERRAIN_DEBUG 0 @@ -76,7 +75,7 @@ class AP_Terrain { public: - AP_Terrain(const AP_Mission &_mission, const AP_Rally &_rally); + AP_Terrain(const AP_Mission &_mission); /* Do not allow copies */ AP_Terrain(const AP_Terrain &other) = delete; @@ -340,10 +339,6 @@ private: // all waypoints const AP_Mission &mission; - // reference to AP_Rally, so we can ask preload terrain data for - // all rally points - const AP_Rally &rally; - // cache of grids in memory, LRU uint8_t cache_size = 0; struct grid_cache *cache = nullptr; diff --git a/libraries/AP_Terrain/TerrainMission.cpp b/libraries/AP_Terrain/TerrainMission.cpp index 02846ac99a..ffb2b2aa98 100644 --- a/libraries/AP_Terrain/TerrainMission.cpp +++ b/libraries/AP_Terrain/TerrainMission.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include "AP_Terrain.h" @@ -110,11 +111,16 @@ void AP_Terrain::update_mission_data(void) */ void AP_Terrain::update_rally_data(void) { - if (last_rally_change_ms != rally.last_change_time_ms() || + const AP_Rally *rally = AP::rally(); + if (rally == nullptr) { + return; + } + + if (last_rally_change_ms != rally->last_change_time_ms() || last_rally_spacing != grid_spacing) { // a rally point has changed - start again next_rally_index = 1; - last_rally_change_ms = rally.last_change_time_ms(); + last_rally_change_ms = rally->last_change_time_ms(); last_rally_spacing = grid_spacing; } if (next_rally_index == 0) { @@ -132,7 +138,7 @@ void AP_Terrain::update_rally_data(void) while (true) { // get next rally point struct RallyLocation rp; - if (!rally.get_rally_point_with_index(next_rally_index, rp)) { + if (!rally->get_rally_point_with_index(next_rally_index, rp)) { // nothing more to do next_rally_index = 0; return;