From 05a0205c21f69251d311df24b6378c3fb1cf1d84 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 15 Feb 2021 13:51:06 +1100 Subject: [PATCH] SITL: change to use terrain singleton --- libraries/SITL/SIM_Aircraft.cpp | 9 ++++++--- libraries/SITL/SIM_Aircraft.h | 2 -- libraries/SITL/SIM_Ship.cpp | 8 +------- 3 files changed, 7 insertions(+), 12 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 22d1374665..d1428dc222 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -33,6 +33,7 @@ #include #include #include +#include using namespace SITL; @@ -63,8 +64,6 @@ Aircraft::Aircraft(const char *frame_str) : sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed(); } - terrain = AP::terrain(); - // init rangefinder array to -1 to signify no data for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++){ rangefinder_m[i] = -1.0f; @@ -94,14 +93,18 @@ void Aircraft::set_start_location(const Location &start_loc, const float start_y */ float Aircraft::ground_height_difference() const { +#if AP_TERRAIN_AVAILABLE + AP_Terrain *terrain = AP::terrain(); float h1, h2; if (sitl && - sitl->terrain_enable && terrain && + terrain != nullptr && + sitl->terrain_enable && terrain->height_amsl(home, h1, false) && terrain->height_amsl(location, h2, false)) { h2 += local_ground_level; return h2 - h1; } +#endif return local_ground_level; } diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index aedac06334..62503f5c3d 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -22,7 +22,6 @@ #include "SITL.h" #include "SITL_Input.h" -#include #include "SIM_Sprayer.h" #include "SIM_Gripper_Servo.h" #include "SIM_Gripper_EPM.h" @@ -243,7 +242,6 @@ protected: bool use_smoothing; - AP_Terrain *terrain; float ground_height_difference() const; virtual bool on_ground() const; diff --git a/libraries/SITL/SIM_Ship.cpp b/libraries/SITL/SIM_Ship.cpp index 5032603442..3584f7f06d 100644 --- a/libraries/SITL/SIM_Ship.cpp +++ b/libraries/SITL/SIM_Ship.cpp @@ -174,21 +174,15 @@ void ShipSim::send_report(void) Location loc = home; loc.offset_double(ship.position.x, ship.position.y); - int32_t alt; - bool have_alt = false; + int32_t alt = home.alt; // assume home altitude #if AP_TERRAIN_AVAILABLE auto terrain = AP::terrain(); float height; if (terrain != nullptr && terrain->enabled() && terrain->height_amsl(loc, height, true)) { alt = height * 1000; - have_alt = true; } #endif - if (!have_alt) { - // assume home altitude - alt = home.alt; - } Vector2f vel(ship.speed, 0); vel.rotate(radians(ship.heading_deg));