From 619628f80eef5e9c804f0633d4c0586b0e92ebc4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 10 Aug 2023 09:57:58 +1000 Subject: [PATCH 1/4] SITL: adjust starting location altitudes SITL was starting with a negative height-above-ground, which is very confusing --- libraries/SITL/SITL.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 9d70ad013aab3..0524f39dc2dc2 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -285,7 +285,7 @@ const AP_Param::GroupInfo SIM::var_info2[] = { // @DisplayName: Original Position (Altitude) // @Description: Specifies vehicle's startup altitude (AMSL) // @User: Advanced - AP_GROUPINFO("OPOS_ALT", 53, SIM, opos.alt, 584.0f), + AP_GROUPINFO("OPOS_ALT", 53, SIM, opos.alt, 586.3f), // @Param: OPOS_HDG // @DisplayName: Original Position (Heading) // @Description: Specifies vehicle's startup heading (0-360) From d6305eab9db65f6835d60b883d5a05f57782f807 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 10 Aug 2023 09:57:58 +1000 Subject: [PATCH 2/4] Tools: adjust starting location altitudes SITL was starting with a negative height-above-ground, which is very confusing --- Tools/autotest/arducopter.py | 2 +- Tools/autotest/locations.txt | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 1cb332597dfa9..1292f72a7927a 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -30,7 +30,7 @@ # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) -SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 584, 270) +SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 586.3, 270) # Flight mode switch positions are set-up in arducopter.param to be # switch 1 = Circle diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index 3717b3613f989..6f1bfa5ca5c03 100644 --- a/Tools/autotest/locations.txt +++ b/Tools/autotest/locations.txt @@ -1,8 +1,8 @@ #NAME=latitude,longitude,absolute-altitude,heading OSRF0=37.4003371,-122.0800351,0,353 OSRF0_PILOTSBOX=37.4003371,-122.0800351,2,270 -CMAC=-35.363261,149.165230,584,353 -CMAC2=-35.362889,149.165221,584,270 +CMAC=-35.363261,149.165230,586.3,353 +CMAC2=-35.362889,149.165221,586.3,270 CMAC_South=-35.363261,149.165230,584,173 CMAC_PILOTSBOX=-35.362734,149.165300,586,270 CMAC_PILOTSBOX2=-35.362749,149.165359,584,270 From cb01e3515d64a4ddfe2e1f9ba451c537bcb64165 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 3 May 2023 18:10:45 +1000 Subject: [PATCH 3/4] AP_HAL_SITL: send a warning if we needed to clamp our height-above-ground to zero --- libraries/AP_HAL_SITL/SITL_State.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 361b837cf1dbe..cb072cb9e8bcc 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -19,6 +19,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -596,6 +597,14 @@ void SITL_State::set_height_agl(void) if (_terrain != nullptr && _terrain->height_amsl(location, terrain_height_amsl, false)) { _sitl->height_agl = _sitl->state.altitude - terrain_height_amsl; + if (_sitl->height_agl < 0) { + static bool sent_warning_clamp; + if (!sent_warning_clamp) { + sent_warning_clamp = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Clamping simulated height agl to zero from (%f)", _sitl->height_agl); + } + _sitl->height_agl = 0; + } return; } } From e5d309549c07f0c70ce8433f268d11ca2b073520 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 3 May 2023 18:42:16 +1000 Subject: [PATCH 4/4] SITL: mount rangefinder 5cm above the ground --- libraries/SITL/SIM_Aircraft.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 07d0f6064e66e..9ad1d7390bebc 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -555,6 +555,9 @@ float Aircraft::rangefinder_range() const // Add some noise on reading altitude += sitl->sonar_noise * rand_float(); + // this rangefinder is mounted on the simulated frame, 5cm off the ground: + altitude += 0.05; + return altitude; }