diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index f4cb15e884..5474e0c8a4 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -35,6 +35,8 @@ extern const AP_HAL::HAL& hal; +AP_Terrain *AP_Terrain::singleton; + // table of user settable parameters const AP_Param::GroupInfo AP_Terrain::var_info[] = { // @Param: ENABLE @@ -62,6 +64,13 @@ AP_Terrain::AP_Terrain(const AP_Mission &_mission) : fd(-1) { AP_Param::setup_object_defaults(this, var_info); + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (singleton != nullptr) { + AP_HAL::panic("Terrain must be singleton"); + } +#endif + singleton = this; } /* diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index 2e5173a3d6..10135e4994 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -82,6 +82,8 @@ public: AP_Terrain(const AP_Terrain &other) = delete; AP_Terrain &operator=(const AP_Terrain&) = delete; + static AP_Terrain *get_singleton(void) { return singleton; } + enum TerrainStatus { TerrainStatusDisabled = 0, // not enabled TerrainStatusUnhealthy = 1, // no terrain data for current location @@ -413,5 +415,7 @@ private: // status enum TerrainStatus system_status = TerrainStatusDisabled; + + static AP_Terrain *singleton; }; #endif // AP_TERRAIN_AVAILABLE