From 0303c5c4a88e2c8e86828ae999baeee00bcc8a3b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 4 Aug 2020 13:47:24 +1000 Subject: [PATCH] SITL: move calculation of simulated rangefinder range to inside SIM_Aircraft this will allow us to use the for non-serial rangefinder backends --- libraries/SITL/SIM_Aircraft.cpp | 38 +++++++++++++++++++++ libraries/SITL/SIM_Aircraft.h | 6 ++-- libraries/SITL/SIM_SerialRangeFinder.cpp | 42 ++---------------------- libraries/SITL/SIM_SerialRangeFinder.h | 3 -- 4 files changed, 44 insertions(+), 45 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 84b6f7910e..b51ce6a8e8 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -431,6 +431,44 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) } } +float Aircraft::rangefinder_range() const +{ + // swiped from sitl_rangefinder.cpp - we should unify them at some stage + + float altitude = range; // only sub appears to set this + if (is_equal(altitude, -1.0f)) { // Use SITL altitude as reading by default + altitude = sitl->height_agl; + } + + // sensor position offset in body frame + const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset; + + // adjust altitude for position of the sensor on the vehicle if position offset is non-zero + if (!relPosSensorBF.is_zero()) { + // get a rotation matrix following DCM conventions (body to earth) + Matrix3f rotmat; + sitl->state.quaternion.rotation_matrix(rotmat); + // rotate the offset into earth frame + const Vector3f relPosSensorEF = rotmat * relPosSensorBF; + // correct the altitude at the sensor + altitude -= relPosSensorEF.z; + } + + // If the attidude is non reversed for SITL OR we are using rangefinder from external simulator, + // We adjust the reading with noise, glitch and scaler as the reading is on analog port. + if ((fabs(sitl->state.rollDeg) < 90.0 && fabs(sitl->state.pitchDeg) < 90.0) || !is_equal(range, -1.0f)) { + if (is_equal(range, -1.0f)) { // disable for external reading that already handle this + // adjust for apparent altitude with roll + altitude /= cosf(radians(sitl->state.rollDeg)) * cosf(radians(sitl->state.pitchDeg)); + } + // Add some noise on reading + altitude += sitl->sonar_noise * rand_float(); + } + + return altitude; +} + + uint64_t Aircraft::get_wall_time_us() const { #if defined(__CYGWIN__) || defined(__CYGWIN64__) diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index eb0fbecbfa..f7e2ea823f 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -126,7 +126,9 @@ public: const Location &get_location() const { return location; } const Vector3f &get_position() const { return position; } - const float &get_range() const { return range; } + + // distance the rangefinder is perceiving + float rangefinder_range() const; void get_attitude(Quaternion &attitude) const { attitude.from_rotation_matrix(dcm); @@ -170,7 +172,7 @@ protected: float rpm[12]; uint8_t rcin_chan_count = 0; float rcin[12]; - float range = -1.0f; // rangefinder detection in m + float range = -1.0f; // externally supplied rangefinder value, assumed to have been corrected for vehicle attitude struct { // data from simulated laser scanner, if available diff --git a/libraries/SITL/SIM_SerialRangeFinder.cpp b/libraries/SITL/SIM_SerialRangeFinder.cpp index 6e1eb4c4f4..dafa511fea 100644 --- a/libraries/SITL/SIM_SerialRangeFinder.cpp +++ b/libraries/SITL/SIM_SerialRangeFinder.cpp @@ -20,45 +20,6 @@ using namespace SITL; -uint16_t SerialRangeFinder::calculate_range_cm(float range_value) const -{ - // swiped from sitl_rangefinder.cpp - we should unify them at some stage - - const SITL *sitl = AP::sitl(); - - float altitude = range_value; - if (is_equal(range_value, -1.0f)) { // Use SITL altitude as reading by default - altitude = AP::sitl()->height_agl; - } - - // sensor position offset in body frame - const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset; - - // adjust altitude for position of the sensor on the vehicle if position offset is non-zero - if (!relPosSensorBF.is_zero()) { - // get a rotation matrix following DCM conventions (body to earth) - Matrix3f rotmat; - sitl->state.quaternion.rotation_matrix(rotmat); - // rotate the offset into earth frame - const Vector3f relPosSensorEF = rotmat * relPosSensorBF; - // correct the altitude at the sensor - altitude -= relPosSensorEF.z; - } - - // If the attidude is non reversed for SITL OR we are using rangefinder from external simulator, - // We adjust the reading with noise, glitch and scaler as the reading is on analog port. - if ((fabs(sitl->state.rollDeg) < 90.0 && fabs(sitl->state.pitchDeg) < 90.0) || !is_equal(range_value, -1.0f)) { - if (is_equal(range_value, -1.0f)) { // disable for external reading that already handle this - // adjust for apparent altitude with roll - altitude /= cosf(radians(sitl->state.rollDeg)) * cosf(radians(sitl->state.pitchDeg)); - } - // Add some noise on reading - altitude += sitl->sonar_noise * rand_float(); - } - - return altitude * 100.0f; -} - void SerialRangeFinder::update(float range) { // just send a chunk of data at 5Hz: @@ -68,8 +29,9 @@ void SerialRangeFinder::update(float range) } last_sent_ms = now; + const uint16_t range_cm = uint16_t(range*100); uint8_t data[255]; - const uint32_t packetlen = packet_for_alt(calculate_range_cm(range), + const uint32_t packetlen = packet_for_alt(range_cm, data, ARRAY_SIZE(data)); diff --git a/libraries/SITL/SIM_SerialRangeFinder.h b/libraries/SITL/SIM_SerialRangeFinder.h index dfa9c8fb0d..e6df1dd8e1 100644 --- a/libraries/SITL/SIM_SerialRangeFinder.h +++ b/libraries/SITL/SIM_SerialRangeFinder.h @@ -41,9 +41,6 @@ public: private: uint32_t last_sent_ms; - - uint16_t calculate_range_cm(float range_value) const; - }; }