From 9854b1de3b647ca8e13e8c16117ae54c5e58a179 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:47 +1100 Subject: [PATCH 01/19] AP_DAL: allow for more than 327m range rangefinders --- libraries/AP_DAL/AP_DAL_RangeFinder.cpp | 18 +++++++++--------- libraries/AP_DAL/AP_DAL_RangeFinder.h | 6 +++--- libraries/AP_DAL/LogStructure.h | 10 +++++----- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/libraries/AP_DAL/AP_DAL_RangeFinder.cpp b/libraries/AP_DAL/AP_DAL_RangeFinder.cpp index 9603a2d39499c..467e5109937da 100644 --- a/libraries/AP_DAL/AP_DAL_RangeFinder.cpp +++ b/libraries/AP_DAL/AP_DAL_RangeFinder.cpp @@ -36,7 +36,7 @@ AP_DAL_RangeFinder::AP_DAL_RangeFinder() #endif } -int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const +float AP_DAL_RangeFinder::ground_clearance_orient(enum Rotation orientation) const { #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) const auto *rangefinder = AP::rangefinder(); @@ -44,25 +44,25 @@ int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation if (orientation != ROTATION_PITCH_270) { // the EKF only asks for this from a specific orientation. Thankfully. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - return rangefinder->ground_clearance_cm_orient(orientation); + return rangefinder->ground_clearance_orient(orientation); } #endif - return _RRNH.ground_clearance_cm; + return _RRNH.ground_clearance; } -int16_t AP_DAL_RangeFinder::max_distance_cm_orient(enum Rotation orientation) const +float AP_DAL_RangeFinder::max_distance_orient(enum Rotation orientation) const { #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) if (orientation != ROTATION_PITCH_270) { const auto *rangefinder = AP::rangefinder(); // the EKF only asks for this from a specific orientation. Thankfully. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - return rangefinder->max_distance_cm_orient(orientation); + return rangefinder->max_distance_orient(orientation); } #endif - return _RRNH.max_distance_cm; + return _RRNH.max_distance; } void AP_DAL_RangeFinder::start_frame() @@ -75,8 +75,8 @@ void AP_DAL_RangeFinder::start_frame() const log_RRNH old = _RRNH; // EKF only asks for this *down*. - _RRNH.ground_clearance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270); - _RRNH.max_distance_cm = rangefinder->max_distance_cm_orient(ROTATION_PITCH_270); + _RRNH.ground_clearance = rangefinder->ground_clearance_orient(ROTATION_PITCH_270); + _RRNH.max_distance = rangefinder->max_distance_orient(ROTATION_PITCH_270); WRITE_REPLAY_BLOCK_IFCHANGED(RRNH, _RRNH, old); @@ -102,7 +102,7 @@ void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) { _RRNI.orientation = backend->orientation(); _RRNI.status = (uint8_t)backend->status(); _RRNI.pos_offset = backend->get_pos_offset(); - _RRNI.distance_cm = backend->distance_cm(); + _RRNI.distance = backend->distance(); WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old); } diff --git a/libraries/AP_DAL/AP_DAL_RangeFinder.h b/libraries/AP_DAL/AP_DAL_RangeFinder.h index 5bb60392cb02d..cb2372d2241bc 100644 --- a/libraries/AP_DAL/AP_DAL_RangeFinder.h +++ b/libraries/AP_DAL/AP_DAL_RangeFinder.h @@ -20,8 +20,8 @@ class AP_DAL_RangeFinder { Good }; - int16_t ground_clearance_cm_orient(enum Rotation orientation) const; - int16_t max_distance_cm_orient(enum Rotation orientation) const; + float ground_clearance_orient(enum Rotation orientation) const; + float max_distance_orient(enum Rotation orientation) const; // return true if we have a range finder with the specified orientation bool has_orientation(enum Rotation orientation) const; @@ -59,7 +59,7 @@ class AP_DAL_RangeFinder_Backend { return (AP_DAL_RangeFinder::Status)_RRNI.status; } - uint16_t distance_cm() const { return _RRNI.distance_cm; } + float distance() const { return _RRNI.distance; } const Vector3f &get_pos_offset() const { return _RRNI.pos_offset; } diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index c8ef343584438..4a42e38b1b39b 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/libraries/AP_DAL/LogStructure.h @@ -172,8 +172,8 @@ struct log_RBRI { // @Description: Replay Data Rangefinder Header struct log_RRNH { // this is rotation-pitch-270! - int16_t ground_clearance_cm; - int16_t max_distance_cm; + float ground_clearance; + float max_distance; uint8_t num_sensors; uint8_t _end; }; @@ -182,7 +182,7 @@ struct log_RRNH { // @Description: Replay Data Rangefinder Instance struct log_RRNI { Vector3f pos_offset; - uint16_t distance_cm; + float distance; uint8_t orientation; uint8_t status; uint8_t instance; @@ -420,9 +420,9 @@ struct log_RBOH { { LOG_RBRI_MSG, RLOG_SIZE(RBRI), \ "RBRI", "IfBB", "LastUpdate,Alt,H,I", "---#", "----" }, \ { LOG_RRNH_MSG, RLOG_SIZE(RRNH), \ - "RRNH", "hhB", "GCl,MaxD,NumSensors", "???", "???" }, \ + "RRNH", "ffB", "GCl,MaxD,NumSensors", "mm-", "00-" }, \ { LOG_RRNI_MSG, RLOG_SIZE(RRNI), \ - "RRNI", "fffHBBB", "PX,PY,PZ,Dist,Orient,Status,I", "------#", "-------" }, \ + "RRNI", "ffffBBB", "PX,PY,PZ,Dist,Orient,Status,I", "---m--#", "---0---" }, \ { LOG_RGPH_MSG, RLOG_SIZE(RGPH), \ "RGPH", "BB", "NumInst,Primary", "--", "--" }, \ { LOG_RGPI_MSG, RLOG_SIZE(RGPI), \ From 120e96738f34af72ff5d643d8593c59d7a74e84d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:47 +1100 Subject: [PATCH 02/19] AP_Logger: allow for more than 327m range rangefinders --- libraries/AP_Logger/LogStructure.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 3176e5d8931c6..13584cddca771 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -456,7 +456,7 @@ struct PACKED log_RFND { LOG_PACKET_HEADER; uint64_t time_us; uint8_t instance; - uint16_t dist; + float dist; uint8_t status; uint8_t orient; int8_t quality; @@ -1181,7 +1181,7 @@ LOG_STRUCTURE_FROM_MOUNT \ { LOG_MODE_MSG, sizeof(log_Mode), \ "MODE", "QMBB", "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \ { LOG_RFND_MSG, sizeof(log_RFND), \ - "RFND", "QBCBBb", "TimeUS,Instance,Dist,Stat,Orient,Quality", "s#m--%", "F-B---", true }, \ + "RFND", "QBfBBb", "TimeUS,Instance,Dist,Stat,Orient,Quality", "s#m--%", "F-0---", true }, \ { LOG_MAV_STATS, sizeof(log_MAV_Stats), \ "DMS", "QIIIIBBBBBBBBB", "TimeUS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "F-------------" }, \ LOG_STRUCTURE_FROM_BEACON \ From ce55eca6bfa41896d7922c42dbca3dc08447d208 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:47 +1100 Subject: [PATCH 03/19] AP_NavEKF2: allow for more than 327m range rangefinders --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 4 ++-- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index b58f2254dbbbf..be95fcbc742a7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -483,7 +483,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Param: RNG_USE_HGT // @DisplayName: Range finder switch height percentage - // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND_MAX_CM). This will not work unless Baro or GPS height is selected as the primary height source vis EK2_ALT_SOURCE = 0 or 2 respectively. This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point. + // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND*_MAX). This will not work unless Baro or GPS height is selected as the primary height source vis EK2_ALT_SOURCE = 0 or 2 respectively. This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point. // @Range: -1 70 // @Increment: 1 // @User: Advanced diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index cdd25bc13383c..724b2a2e05eaa 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -28,7 +28,7 @@ void NavEKF2_core::readRangeFinder(void) return; } - rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f); + rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f); // read range finder at 20Hz // TODO better way of knowing if it has new data @@ -51,7 +51,7 @@ void NavEKF2_core::readRangeFinder(void) rngMeasIndex[sensorIndex] = 0; } storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25; - storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f; + storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance(); } else { continue; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 6d5937dd2247b..7f209b6b28d25 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -66,7 +66,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const // we really, really shouldn't be here. return false; } - height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f); + height = MAX(float(_rng->max_distance_orient(ROTATION_PITCH_270)) * 0.7f - 1.0f, 1.0f); #else return false; #endif diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index debcaaf2c29e8..2f3382a8e9c40 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -979,7 +979,7 @@ void NavEKF2_core::selectHeightForFusion() activeHgtSource = HGT_SOURCE_RNG; } else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) { // determine if we are above or below the height switch region - ftype rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; + const ftype rangeMaxUse = 1e-2f * _rng->max_distance_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; From 2570d37b0e772f2c024eb081e8235db74c0c64d8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:47 +1100 Subject: [PATCH 04/19] AP_NavEKF3: allow for more than 327m range rangefinders --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 4 ++-- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 57b837d9e47d0..44eb4f22dadc7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -480,7 +480,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: RNG_USE_HGT // @DisplayName: Range finder switch height percentage - // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFNDx_MAX_CM) and the primary height source is Baro or GPS (see EK3_SRCx_POSZ). This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point. + // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFNDx_MAX) and the primary height source is Baro or GPS (see EK3_SRCx_POSZ). This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point. // @Range: -1 70 // @Increment: 1 // @User: Advanced diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index a7146dd06e71b..b4c09f9835db5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -25,7 +25,7 @@ void NavEKF3_core::readRangeFinder(void) if (_rng == nullptr) { return; } - rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f); + rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f); // limit update rate to maximum allowed by data buffers if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) { @@ -47,7 +47,7 @@ void NavEKF3_core::readRangeFinder(void) rngMeasIndex[sensorIndex] = 0; } storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25; - storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f; + storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance(); } else { continue; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 8fc369c392f7d..c358e1d07cbf2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -74,7 +74,7 @@ bool NavEKF3_core::getHeightControlLimit(float &height) const // we really, really shouldn't be here. return false; } - height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f); + height = MAX(float(_rng->max_distance_orient(ROTATION_PITCH_270)) * 0.7f - 1.0f, 1.0f); #else return false; #endif diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 98aaf8cd27fbf..a73d177e620f7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1208,7 +1208,7 @@ void NavEKF3_core::selectHeightForFusion() activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER; } else if ((frontend->_useRngSwHgt > 0) && ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) || (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS)) && _rng && rangeFinderDataIsFresh) { // determine if we are above or below the height switch region - ftype rangeMaxUse = 1e-4 * (ftype)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; + const ftype rangeMaxUse = 1e-2 * (ftype)_rng->max_distance_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; bool belowLowerSwHgt = ((terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse) && (imuSampleTime_ms - gndHgtValidTime_ms < 1000); From 60dd5e8e73c962e70a6bba740274e6d779743f42 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 05/19] AP_Proximity: allow for more than 327m range rangefinders --- libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp index 4ec86b7f6e943..b69d3e5e91005 100644 --- a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp @@ -51,8 +51,8 @@ void AP_Proximity_RangeFinder::update(void) const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle); // distance in meters const float distance = sensor->distance(); - _distance_min = sensor->min_distance_cm() * 0.01f; - _distance_max = sensor->max_distance_cm() * 0.01f; + _distance_min = sensor->min_distance(); + _distance_max = sensor->max_distance(); if ((distance <= _distance_max) && (distance >= _distance_min) && !ignore_reading(angle, distance, false)) { frontend.boundary.set_face_attributes(face, angle, distance, state.instance); // update OA database @@ -64,11 +64,11 @@ void AP_Proximity_RangeFinder::update(void) } // check upward facing range finder if (sensor->orientation() == ROTATION_PITCH_90) { - int16_t distance_upward = sensor->distance_cm(); - int16_t up_distance_min = sensor->min_distance_cm(); - int16_t up_distance_max = sensor->max_distance_cm(); + const float distance_upward = sensor->distance(); + const float up_distance_min = sensor->min_distance(); + const float up_distance_max = sensor->max_distance(); if ((distance_upward >= up_distance_min) && (distance_upward <= up_distance_max)) { - _distance_upward = distance_upward * 0.01f; + _distance_upward = distance_upward; } else { _distance_upward = -1.0; // mark an valid reading } From 9fe7e52e94857a39ec10dc212c154842de8b7295 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 06/19] AP_RangeFinder: allow for more than 327m range rangefinders --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 35 ++++++++------- libraries/AP_RangeFinder/AP_RangeFinder.h | 31 ++++++++++--- .../AP_RangeFinder_Ainstein_LR_D1.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_Backend.cpp | 4 +- .../AP_RangeFinder/AP_RangeFinder_Backend.h | 7 ++- .../AP_RangeFinder/AP_RangeFinder_Bebop.h | 4 +- .../AP_RangeFinder_Benewake.cpp | 4 +- .../AP_RangeFinder_Benewake_TFMiniPlus.cpp | 2 +- .../AP_RangeFinder_JRE_Serial.cpp | 6 +-- .../AP_RangeFinder_LeddarVu8.cpp | 6 +-- .../AP_RangeFinder_LightWareI2C.cpp | 12 ++--- .../AP_RangeFinder_LightWareI2C.h | 5 +-- .../AP_RangeFinder_LightWareSerial.cpp | 4 +- .../AP_RangeFinder/AP_RangeFinder_MAVLink.cpp | 39 ++++++++-------- .../AP_RangeFinder/AP_RangeFinder_MAVLink.h | 10 ++--- .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 44 ++++++++++++------- .../AP_RangeFinder/AP_RangeFinder_Params.h | 7 +-- .../AP_RangeFinder/AP_RangeFinder_RDS02UF.h | 12 +++-- .../AP_RangeFinder_TeraRangerI2C.cpp | 4 +- .../AP_RangeFinder_TeraRanger_Serial.cpp | 11 ++--- .../AP_RangeFinder/AP_RangeFinder_analog.cpp | 6 +-- .../examples/RFIND_test/RFIND_test.cpp | 4 +- 22 files changed, 147 insertions(+), 112 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 82e23c11e30b0..f6b25ea33e335 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -190,6 +190,14 @@ RangeFinder::RangeFinder() _singleton = this; } +void RangeFinder::convert_params(void) +{ + // PARAMETER_CONVERSION - Added: Dec-2024 for 4.6->4.7 + for (auto &p : params) { + p.convert_min_max_params(); + } +} + /* initialise the RangeFinder class. We do detection of attached range finders here. For now we won't allow for hot-plugging of @@ -197,6 +205,8 @@ RangeFinder::RangeFinder() */ void RangeFinder::init(enum Rotation orientation_default) { + convert_params(); + if (num_instances != 0) { // don't re-init if we've found some sensors already return; @@ -702,11 +712,6 @@ float RangeFinder::distance_orient(enum Rotation orientation) const return backend->distance(); } -uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const -{ - return distance_orient(orientation) * 100.0; -} - int8_t RangeFinder::signal_quality_pct_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); @@ -716,31 +721,31 @@ int8_t RangeFinder::signal_quality_pct_orient(enum Rotation orientation) const return backend->signal_quality_pct(); } -int16_t RangeFinder::max_distance_cm_orient(enum Rotation orientation) const +float RangeFinder::min_distance_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); if (backend == nullptr) { - return 0; + return 0; // consider NaN } - return backend->max_distance_cm(); + return backend->min_distance(); } -int16_t RangeFinder::min_distance_cm_orient(enum Rotation orientation) const +float RangeFinder::max_distance_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); if (backend == nullptr) { - return 0; + return 0; // consider NaN } - return backend->min_distance_cm(); + return backend->max_distance(); } -int16_t RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const +float RangeFinder::ground_clearance_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); if (backend == nullptr) { - return 0; + return 0; // consider NaN } - return backend->ground_clearance_cm(); + return backend->ground_clearance(); } bool RangeFinder::has_data_orient(enum Rotation orientation) const @@ -821,7 +826,7 @@ void RangeFinder::Log_RFND() const LOG_PACKET_HEADER_INIT(LOG_RFND_MSG), time_us : AP_HAL::micros64(), instance : i, - dist : s->distance_cm(), + dist : s->distance(), status : (uint8_t)s->status(), orient : s->orientation(), quality : s->signal_quality_pct(), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 4d66101b8b6ab..53734fd1d5a81 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -35,7 +35,7 @@ #endif #endif -#define RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT 10 +#define RANGEFINDER_GROUND_CLEARANCE_DEFAULT 0.10 #define RANGEFINDER_PREARM_ALT_MAX_CM 200 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 0 @@ -268,15 +268,30 @@ class RangeFinder uint8_t get_address(uint8_t id) const { return id >= RANGEFINDER_MAX_INSTANCES? 0 : uint8_t(params[id].address.get()); } - + // methods to return a distance on a particular orientation from // any sensor which can current supply it - float distance_orient(enum Rotation orientation) const; - uint16_t distance_cm_orient(enum Rotation orientation) const; int8_t signal_quality_pct_orient(enum Rotation orientation) const; - int16_t max_distance_cm_orient(enum Rotation orientation) const; - int16_t min_distance_cm_orient(enum Rotation orientation) const; - int16_t ground_clearance_cm_orient(enum Rotation orientation) const; +#if AP_SCRIPTING_ENABLED + // centimetre accessors - do not use, reduce use where possible + uint16_t distance_cm_orient(enum Rotation orientation) const { + return distance_orient(orientation) * 100.0; + } + int32_t max_distance_cm_orient(enum Rotation orientation) const { + return max_distance_orient(orientation) * 100; + } + int32_t min_distance_cm_orient(enum Rotation orientation) const { + return min_distance_orient(orientation) * 100; + } + int32_t ground_clearance_cm_orient(enum Rotation orientation) const { + return ground_clearance_orient(orientation) * 100; + } +#endif + // metre accessors - use these in preference to the cm accessors + float distance_orient(enum Rotation orientation) const; + float max_distance_orient(enum Rotation orientation) const; + float min_distance_orient(enum Rotation orientation) const; + float ground_clearance_orient(enum Rotation orientation) const; MAV_DISTANCE_SENSOR get_mav_distance_sensor_type_orient(enum Rotation orientation) const; RangeFinder::Status status_orient(enum Rotation orientation) const; bool has_data_orient(enum Rotation orientation) const; @@ -310,6 +325,8 @@ class RangeFinder float estimated_terrain_height; Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests + void convert_params(void); + void detect_instance(uint8_t instance, uint8_t& serial_instance); bool _add_backend(AP_RangeFinder_Backend *driver, uint8_t instance, uint8_t serial_instance=0); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp index 775a004268af1..14d4d2134a482 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp @@ -95,7 +95,7 @@ bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m) has_data = false; if (snr == 0) { state.status = RangeFinder::Status::OutOfRangeHigh; - reading_m = MAX(656, max_distance_cm() * 0.01 + 1); + reading_m = MAX(656, max_distance() + 1); } else { state.status = RangeFinder::Status::NoData; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp index 3ae267a928cf3..bf5df397bc070 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp @@ -60,9 +60,9 @@ bool AP_RangeFinder_Backend::has_data() const { void AP_RangeFinder_Backend::update_status(RangeFinder::RangeFinder_State &state_arg) const { // check distance - if (state_arg.distance_m > max_distance_cm() * 0.01f) { + if (state_arg.distance_m > max_distance()) { set_status(state_arg, RangeFinder::Status::OutOfRangeHigh); - } else if (state_arg.distance_m < min_distance_cm() * 0.01f) { + } else if (state_arg.distance_m < min_distance()) { set_status(state_arg, RangeFinder::Status::OutOfRangeLow); } else { set_status(state_arg, RangeFinder::Status::Good); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index e1ae5b50122c1..087b504ed7952 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -53,12 +53,11 @@ class AP_RangeFinder_Backend enum Rotation orientation() const { return (Rotation)params.orientation.get(); } float distance() const { return state.distance_m; } - uint16_t distance_cm() const { return state.distance_m*100.0f; } int8_t signal_quality_pct() const WARN_IF_UNUSED { return state.signal_quality_pct; } uint16_t voltage_mv() const { return state.voltage_mv; } - virtual int16_t max_distance_cm() const { return params.max_distance_cm; } - virtual int16_t min_distance_cm() const { return params.min_distance_cm; } - int16_t ground_clearance_cm() const { return params.ground_clearance_cm; } + virtual float max_distance() const { return params.max_distance; } + virtual float min_distance() const { return params.min_distance; } + float ground_clearance() const { return params.ground_clearance; } MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const; RangeFinder::Status status() const; RangeFinder::Type type() const { return (RangeFinder::Type)params.type.get(); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h index e32be0ebe84f0..6d1821a4a616f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h @@ -141,8 +141,8 @@ class AP_RangeFinder_Bebop : public AP_RangeFinder_Backend { unsigned int _filtered_capture_size; struct echo _echoes[RNFD_BEBOP_MAX_ECHOES]; unsigned int _filter_average = 4; - int16_t _last_max_distance_cm = 850; - int16_t _last_min_distance_cm = 32; + float _last_max_distance = 8.50; + float _last_min_distance = 0.32; }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index f04056588ddb0..95d149182d60d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal; #define BENEWAKE_FRAME_HEADER 0x59 #define BENEWAKE_FRAME_LENGTH 9 #define BENEWAKE_DIST_MAX_CM 32768 -#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100 +#define BENEWAKE_OUT_OF_RANGE_ADD 1.00 // metres // format of serial packets received from benewake lidar // @@ -129,7 +129,7 @@ bool AP_RangeFinder_Benewake::get_reading(float &reading_m) if (count_out_of_range > 0) { // if only out of range readings return larger of // driver defined maximum range for the model and user defined max range + 1m - reading_m = MAX(model_dist_max_cm(), max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM) * 0.01f; + reading_m = MAX(model_dist_max_cm() * 0.01, max_distance() + BENEWAKE_OUT_OF_RANGE_ADD); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp index 0e3e954816611..76f8d0c2ccfea 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp @@ -169,7 +169,7 @@ void AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(le16_t distance_raw * value to 0." - force it to the max distance so status is set to OutOfRangeHigh * rather than NoData. */ - output_distance_cm = MAX(MAX_DIST_CM, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM); + output_distance_cm = MAX(MAX_DIST_CM, max_distance()*100 + BENEWAKE_OUT_OF_RANGE_ADD_CM); } else { output_distance_cm = constrain_int16(output_distance_cm, MIN_DIST_CM, MAX_DIST_CM); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp index d54ef55d5f081..7bc16f2c11c3b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp @@ -25,8 +25,8 @@ #define FRAME_HEADER_2_B 'B' // 0x42 #define FRAME_HEADER_2_C 'C' // 0x43 -#define DIST_MAX_CM 5000 -#define OUT_OF_RANGE_ADD_CM 100 +#define DIST_MAX 50.00 +#define OUT_OF_RANGE_ADD 1.0 // metres void AP_RangeFinder_JRE_Serial::move_preamble_in_buffer(uint8_t search_start_pos) { @@ -134,7 +134,7 @@ bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) // all readings were invalid so return out-of-range-high value if (invalid_count > 0) { no_signal = true; - reading_m = MIN(MAX(DIST_MAX_CM, max_distance_cm() + OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f; + reading_m = MAX(DIST_MAX, max_distance() + OUT_OF_RANGE_ADD); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp index 6a2f1bf169714..c8ac42120a91a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp @@ -26,8 +26,8 @@ extern const AP_HAL::HAL& hal; // https://autonomoustuff.com/product/leddartech-vu8/ #define LEDDARVU8_ADDR_DEFAULT 0x01 // modbus default device id -#define LEDDARVU8_DIST_MAX_CM 18500 // maximum possible distance reported by lidar -#define LEDDARVU8_OUT_OF_RANGE_ADD_CM 100 // add this many cm to out-of-range values +#define LEDDARVU8_DIST_MAX 185.00 // maximum possible distance reported by lidar +#define LEDDARVU8_OUT_OF_RANGE_ADD 1.00 // add this many metres to out-of-range values #define LEDDARVU8_TIMEOUT_MS 200 // timeout in milliseconds if no distance messages received // distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal @@ -78,7 +78,7 @@ bool AP_RangeFinder_LeddarVu8::get_reading(float &reading_m) } else { // if only out of range readings return larger of // driver defined maximum range for the model and user defined max range + 1m - reading_m = MAX(LEDDARVU8_DIST_MAX_CM, max_distance_cm() + LEDDARVU8_OUT_OF_RANGE_ADD_CM)*0.01f; + reading_m = MAX(LEDDARVU8_DIST_MAX, max_distance() + LEDDARVU8_OUT_OF_RANGE_ADD); } return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index 5e600ca463dd2..940d15943bf87 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal; #define LIGHTWARE_LOST_SIGNAL_TIMEOUT_WRITE_REG 23 #define LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE 20 // number of lost signal confirmations for legacy protocol only -#define LIGHTWARE_OUT_OF_RANGE_ADD_CM 100 +#define LIGHTWARE_OUT_OF_RANGE_ADD 1.00 // metres static const size_t lx20_max_reply_len_bytes = 32; static const size_t lx20_max_expected_stream_reply_len_bytes = 14; @@ -351,7 +351,7 @@ bool AP_RangeFinder_LightWareI2C::legacy_get_reading(float &reading_m) int16_t signed_val = int16_t(be16toh(val)); if (signed_val < 0) { // some lidar firmwares will return 65436 for out of range - reading_m = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM) * 0.01f; + reading_m = max_distance() + LIGHTWARE_OUT_OF_RANGE_ADD; } else { reading_m = uint16_t(signed_val) * 0.01f; } @@ -381,7 +381,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_get_reading(float &reading_m) } if (i==0) { - reading_m = sf20_stream_val[0] * 0.01f; + reading_m = sf20_stream_val[0]; } // Increment the stream sequence @@ -400,7 +400,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_get_reading(float &reading_m) bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf, size_t *p_num_processed_chars, const char *string_identifier, - uint16_t &val) + float &val) { size_t string_identifier_len = strlen(string_identifier); for (uint32_t i = 0 ; i < string_identifier_len ; i++) { @@ -416,7 +416,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf, we will return max distance */ if (strncmp((const char *)&stream_buf[*p_num_processed_chars], "-1.00", 5) == 0) { - val = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM); + val = max_distance() + LIGHTWARE_OUT_OF_RANGE_ADD; (*p_num_processed_chars) += 5; return true; } @@ -451,7 +451,7 @@ bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf, } accumulator *= final_multiplier; - val = accumulator; + val = accumulator * 0.01; return number_found; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h index ab15b6a9e55d6..fa10dc89efbaf 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h @@ -32,7 +32,7 @@ class AP_RangeFinder_LightWareI2C : public AP_RangeFinder_Backend private: - uint16_t sf20_stream_val[NUM_SF20_DATA_STREAMS]; + float sf20_stream_val[NUM_SF20_DATA_STREAMS]; int currentStreamSequenceIndex = 0; // constructor @@ -59,8 +59,7 @@ class AP_RangeFinder_LightWareI2C : public AP_RangeFinder_Backend bool sf20_parse_stream(uint8_t *stream_buf, size_t *p_num_processed_chars, const char *string_identifier, - uint16_t &val); - void data_log(uint16_t *val); + float &val); AP_HAL::OwnPtr _dev; }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index 61c2ed9c2d975..4eebdf66d2682 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp @@ -37,7 +37,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m) uint16_t invalid_count = 0; // number of invalid readings // max distance the sensor can reliably measure - read from parameters - const int16_t distance_cm_max = max_distance_cm(); + const auto distance_cm_max = max_distance()*100; // read any available lines from the lidar for (auto i=0; i<8192; i++) { @@ -128,7 +128,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m) // all readings were invalid so return out-of-range-high value if (invalid_count > 0) { - reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f; + reading_m = MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM); no_signal = true; return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index b4fc5fb1ff597..d4880738e98cd 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp @@ -30,9 +30,9 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg) // only accept distances for the configured orientation if (packet.orientation == orientation()) { state.last_reading_ms = AP_HAL::millis(); - distance_cm = packet.current_distance; - _max_distance_cm = packet.max_distance; - _min_distance_cm = packet.min_distance; + distance = packet.current_distance * 0.01; + _max_distance = packet.max_distance * 0.01; + _min_distance = packet.min_distance * 0.01; sensor_type = (MAV_DISTANCE_SENSOR)packet.type; signal_quality = packet.signal_quality; if (signal_quality == 0) { @@ -45,28 +45,31 @@ void AP_RangeFinder_MAVLink::handle_msg(const mavlink_message_t &msg) } } -int16_t AP_RangeFinder_MAVLink::max_distance_cm() const +float AP_RangeFinder_MAVLink::max_distance() const { - if (_max_distance_cm == 0 && _min_distance_cm == 0) { + const auto baseclass_max_distance = AP_RangeFinder_Backend::max_distance(); + + if (is_zero(_max_distance) && is_zero(_min_distance)) { // we assume if both of these are zero that we ignore both - return params.max_distance_cm; + return baseclass_max_distance; } - if (params.max_distance_cm < _max_distance_cm) { - return params.max_distance_cm; - } - return _max_distance_cm; + // return the larger of the base class's distance and what we + // receive from the network: + return MAX(baseclass_max_distance, _max_distance); } -int16_t AP_RangeFinder_MAVLink::min_distance_cm() const +float AP_RangeFinder_MAVLink::min_distance() const { - if (_max_distance_cm == 0 && _min_distance_cm == 0) { + const auto baseclass_min_distance = AP_RangeFinder_Backend::min_distance(); + + if (is_zero(_max_distance) && is_zero(_min_distance)) { // we assume if both of these are zero that we ignore both - return params.min_distance_cm; + return baseclass_min_distance; } - if (params.min_distance_cm > _min_distance_cm) { - return params.min_distance_cm; - } - return _min_distance_cm; + + // return the smaller of the base class's distance and what we + // receive from the network: + return MIN(baseclass_min_distance, _min_distance); } /* @@ -81,7 +84,7 @@ void AP_RangeFinder_MAVLink::update(void) state.distance_m = 0.0f; state.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN; } else { - state.distance_m = distance_cm * 0.01f; + state.distance_m = distance; state.signal_quality_pct = signal_quality; update_status(); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h index 47cd080c13f9a..89f4232367505 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h @@ -28,8 +28,8 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend // Get update from mavlink void handle_msg(const mavlink_message_t &msg) override; - int16_t max_distance_cm() const override; - int16_t min_distance_cm() const override; + float max_distance() const override; + float min_distance() const override; protected: @@ -40,9 +40,9 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend private: // stored data from packet: - uint16_t distance_cm; - uint16_t _max_distance_cm; - uint16_t _min_distance_cm; + float distance; + float _max_distance; + float _min_distance; int8_t signal_quality; // start a reading diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 72630f95f3949..e068295694ff5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -49,21 +49,21 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @User: Standard AP_GROUPINFO("FUNCTION", 5, AP_RangeFinder_Params, function, 0), - // @Param: MIN_CM + // @Param: MIN // @DisplayName: Rangefinder minimum distance - // @Description: Minimum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 + // @Description: Minimum distance in metres that rangefinder can reliably read + // @Units: m + // @Increment: 0.01 // @User: Standard - AP_GROUPINFO("MIN_CM", 6, AP_RangeFinder_Params, min_distance_cm, 20), + AP_GROUPINFO("MIN", 6, AP_RangeFinder_Params, min_distance, 0.20), - // @Param: MAX_CM + // @Param: MAX // @DisplayName: Rangefinder maximum distance - // @Description: Maximum distance in centimeters that rangefinder can reliably read - // @Units: cm - // @Increment: 1 + // @Description: Maximum distance in metres that rangefinder can reliably read + // @Units: m + // @Increment: 0.01 // @User: Standard - AP_GROUPINFO("MAX_CM", 7, AP_RangeFinder_Params, max_distance_cm, 700), + AP_GROUPINFO("MAX", 7, AP_RangeFinder_Params, max_distance, 7.00), // @Param: STOP_PIN // @DisplayName: Rangefinder stop pin @@ -89,14 +89,14 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @User: Standard AP_GROUPINFO("PWRRNG", 11, AP_RangeFinder_Params, powersave_range, 0), - // @Param: GNDCLEAR - // @DisplayName: Distance (in cm) from the range finder to the ground - // @Description: This parameter sets the expected range measurement(in cm) that the range finder should return when the vehicle is on the ground. - // @Units: cm - // @Range: 5 127 - // @Increment: 1 + // @Param: GNDCLR + // @DisplayName: Distance from the range finder to the ground + // @Description: This parameter sets the expected range measurement that the range finder should return when the vehicle is on the ground. + // @Units: m + // @Range: 0.05 1.5 + // @Increment: 0.01 // @User: Standard - AP_GROUPINFO("GNDCLEAR", 12, AP_RangeFinder_Params, ground_clearance_cm, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), + AP_GROUPINFO("GNDCLR", 12, AP_RangeFinder_Params, ground_clearance, RANGEFINDER_GROUND_CLEARANCE_DEFAULT), // @Param: ADDR // @DisplayName: Bus address of sensor @@ -141,6 +141,16 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { AP_GROUPEND }; + +// PARAMETER_CONVERSION - Added: Dec-2024 for 4.7 +void AP_RangeFinder_Params::convert_min_max_params(void) +{ + // ./Tools/autotest/test_param_upgrade.py --vehicle=arducopter --param "RNGFND1_MAX_CM=300->RNGFND1_MAX=3.00" --param "RNGFND2_MIN_CM=678->RNGFND2_MIN=6.78" --param "RNGFNDA_MIN_CM=1->RNGFNDA_MIN=0.01" --param "RNGFND5_GNDCLEAR=103->RNGFND5_GNDCLR=1.03" + max_distance.convert_parameter_width(AP_PARAM_INT16, 0.01); + min_distance.convert_parameter_width(AP_PARAM_INT16, 0.01); + ground_clearance.convert_parameter_width(AP_PARAM_INT8, 0.01); +} + AP_RangeFinder_Params::AP_RangeFinder_Params(void) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.h b/libraries/AP_RangeFinder/AP_RangeFinder_Params.h index 17893c3e6812f..f0b58b31642df 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.h @@ -12,6 +12,7 @@ class AP_RangeFinder_Params { static const struct AP_Param::GroupInfo var_info[]; AP_RangeFinder_Params(void); + void convert_min_max_params(); /* Do not allow copies */ CLASS_NO_COPY(AP_RangeFinder_Params); @@ -20,14 +21,14 @@ class AP_RangeFinder_Params { AP_Float scaling; AP_Float offset; AP_Int16 powersave_range; - AP_Int16 min_distance_cm; - AP_Int16 max_distance_cm; + AP_Float min_distance; + AP_Float max_distance; AP_Int8 type; AP_Int8 pin; AP_Int8 ratiometric; AP_Int8 stop_pin; AP_Int8 function; - AP_Int8 ground_clearance_cm; + AP_Float ground_clearance; AP_Int8 address; AP_Int8 orientation; }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h index 6b480a17765c8..cccb2689ff190 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h @@ -23,8 +23,8 @@ #include "AP_RangeFinder_Backend_Serial.h" #define RDS02_BUFFER_SIZE 50 -#define RDS02UF_DIST_MAX_CM 2000 -#define RDS02UF_DIST_MIN_CM 150 +#define RDS02UF_DIST_MAX 20.00 +#define RDS02UF_DIST_MIN 1.50 #define RDS02UF_DATA_LEN 10 class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial @@ -57,8 +57,12 @@ class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial uint16_t read_timeout_ms() const override { return 500; } // make sure readings go out-of-range when necessary - int16_t max_distance_cm()const override { return MIN(params.max_distance_cm, RDS02UF_DIST_MAX_CM); } - int16_t min_distance_cm() const override { return MAX(params.min_distance_cm, RDS02UF_DIST_MIN_CM); } + float max_distance() const override { + return MIN(AP_RangeFinder_Backend::max_distance(), RDS02UF_DIST_MAX); + } + float min_distance() const override { + return MAX(AP_RangeFinder_Backend::min_distance(), RDS02UF_DIST_MIN); + } // Data Format for Benewake Rds02UF // =============================== diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp index ab63b192dd115..fe034246d8cc8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp @@ -139,14 +139,14 @@ bool AP_RangeFinder_TeraRangerI2C::process_raw_measure(uint16_t raw_distance, ui // Check for error codes if (raw_distance == 0xFFFF) { // Too far away - output_distance_cm = max_distance_cm() + TR_OUT_OF_RANGE_ADD_CM; + output_distance_cm = max_distance()*100 + TR_OUT_OF_RANGE_ADD_CM; } else if (raw_distance == 0x0000) { // Too close output_distance_cm = 0; } else if (raw_distance == 0x0001) { // Unable to measure // This can also include the sensor pointing to the horizon when used as a proximity sensor - output_distance_cm = max_distance_cm() + TR_OUT_OF_RANGE_ADD_CM; + output_distance_cm = max_distance()*100 + TR_OUT_OF_RANGE_ADD_CM; } else { output_distance_cm = raw_distance/10; // Conversion to centimeters } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp index 088472b6b8387..bf1b3fd513877 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp @@ -18,17 +18,14 @@ #if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED -#include #include #include #include -extern const AP_HAL::HAL& hal; - #define FRAME_HEADER 0x54 #define FRAME_LENGTH 5 -#define DIST_MAX_CM 3000 -#define OUT_OF_RANGE_ADD_CM 1000 +#define DIST_MAX 30.00 +#define OUT_OF_RANGE_ADD 10.00 #define STATUS_MASK 0x1F #define DISTANCE_ERROR 0x0001 @@ -77,7 +74,7 @@ bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m) if (crc == linebuf[FRAME_LENGTH-1]) { // calculate distance uint16_t dist = ((uint16_t)linebuf[1] << 8) | linebuf[2]; - if (dist >= DIST_MAX_CM *10) { + if (dist >= DIST_MAX *1000) { // this reading is out of range and a bad read bad_read++; } else { @@ -107,7 +104,7 @@ bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m) if (bad_read > 0) { // if a bad read has occurred this update overwrite return with larger of // driver defined maximum range for the model and user defined max range + 1m - reading_m = MAX(DIST_MAX_CM, max_distance_cm() + OUT_OF_RANGE_ADD_CM) * 0.01f; + reading_m = MAX(DIST_MAX, max_distance() + OUT_OF_RANGE_ADD); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index b9ffb1f06c674..8cba8ac052574 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -89,7 +89,7 @@ void AP_RangeFinder_analog::update(void) float scaling = params.scaling; float offset = params.offset; RangeFinder::Function function = (RangeFinder::Function)params.function.get(); - int16_t _max_distance_cm = params.max_distance_cm; + const float _max_distance = max_distance(); switch (function) { case RangeFinder::Function::LINEAR: @@ -106,8 +106,8 @@ void AP_RangeFinder_analog::update(void) } else { dist_m = scaling / (v - offset); } - if (dist_m > _max_distance_cm * 0.01f) { - dist_m = _max_distance_cm * 0.01f; + if (dist_m > _max_distance) { + dist_m = _max_distance; } break; } diff --git a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp index a281962e1b473..2cf3fc04cb5e7 100644 --- a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp +++ b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp @@ -50,11 +50,11 @@ void loop() if (!sensor->has_data()) { continue; } - hal.console->printf("All: device_%u type %d status %d distance_cm %d\n", + hal.console->printf("All: device_%u type=%d status=%d distance=%f\n", i, (int)sensor->type(), (int)sensor->status(), - sensor->distance_cm()); + sensor->distance()); had_data = true; } if (!had_data) { From 390fa233cf2bc889f5532a868521c877b4336b27 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 07/19] AP_Scripting: allow for more than 327m range rangefinders --- libraries/AP_Scripting/docs/docs.lua | 26 ++++++++++++++++++- .../examples/rangefinder_quality_test.lua | 12 ++++----- .../examples/rangefinder_test.lua | 23 ++++++++++++---- .../generator/description/bindings.desc | 12 +++++++-- 4 files changed, 59 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 4e15c6d33d033..ddde5f4731ac5 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3160,29 +3160,53 @@ function rangefinder:status_orient(orientation) end -- desc ---@param orientation integer ---@return integer +---@deprecated -- Use ground_clearance_orient (in metres) function rangefinder:ground_clearance_cm_orient(orientation) end -- desc ---@param orientation integer ---@return integer +---@deprecated -- Use min_distance_orient (in metres) function rangefinder:min_distance_cm_orient(orientation) end -- desc ---@param orientation integer ---@return integer +---@deprecated -- Use max_distance_orient (in metres) function rangefinder:max_distance_cm_orient(orientation) end -- desc ---@param orientation integer ---@return integer +---@deprecated -- Use distance_orient (in metres) function rangefinder:distance_cm_orient(orientation) end +-- Configured distance between rangefinder of a particular orientation and the vehicle's side which would rest on the ground. Usually used for a downward-facing rangefinder to give ground clearance. +---@param orientation integer +---@return number +function rangefinder:ground_clearance_orient(orientation) end + +-- Configured minimum distance the rangefinder in supplied orientation can measure +---@param orientation integer +---@return number +function rangefinder:min_distance_orient(orientation) end + +-- Configured maximum distance the rangefinder in supplied orientation can measure +---@param orientation integer +---@return number +function rangefinder:max_distance_orient(orientation) end + +-- Distance rangefinder in supplied orientation is currently measuring +---@param orientation integer +---@return number +function rangefinder:distance_orient(orientation) end + -- Current distance measurement signal quality for range finder at this orientation ---@param orientation integer ---@return integer function rangefinder:signal_quality_pct_orient(orientation) end --- desc +-- Returns true if there is a rangefinder measuring in supplied orientation ---@param orientation integer ---@return boolean function rangefinder:has_orientation(orientation) end diff --git a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua index 45772d4d35d70..1767b6bb8540d 100644 --- a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua +++ b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua @@ -7,8 +7,8 @@ -- Parameters should be set as follows before this test is loaded. -- "RNGFND1_TYPE": 36, -- "RNGFND1_ORIENT": 25, --- "RNGFND1_MIN_CM": 10, --- "RNGFND1_MAX_CM": 5000, +-- "RNGFND1_MIN": 0.10, +-- "RNGFND1_MAX": 50.00, ---@diagnostic disable: cast-local-type @@ -43,8 +43,8 @@ local SIGNAL_QUALITY_MAX = 100 local SIGNAL_QUALITY_UNKNOWN = -1 -- Read parameters for min and max valid range finder ranges. -local RNGFND1_MIN_CM = Parameter("RNGFND1_MIN_CM"):get() -local RNGFND1_MAX_CM = Parameter("RNGFND1_MAX_CM"):get() +local RNGFND1_MIN = Parameter("RNGFND1_MIN"):get() +local RNGFND1_MAX = Parameter("RNGFND1_MAX"):get() local function send(str) gcs:send_text(3, string.format("%s %s", TEST_ID_STR, str)) @@ -253,9 +253,9 @@ local function _update_begin_test() -- The full state udata must be initialized. local rf_state = RangeFinder_State() -- Set the status - if dist_m_in < RNGFND1_MIN_CM * 0.01 then + if dist_m_in < RNGFND1_MIN then rf_state:status(RNDFND_STATUS_OUT_OF_RANGE_LOW) - elseif dist_m_in > RNGFND1_MAX_CM * 0.01 then + elseif dist_m_in > RNGFND1_MAX then rf_state:status(RNDFND_STATUS_OUT_OF_RANGE_HIGH) else rf_state:status(RNDFND_STATUS_GOOD) diff --git a/libraries/AP_Scripting/examples/rangefinder_test.lua b/libraries/AP_Scripting/examples/rangefinder_test.lua index b6832bf54be35..ca5078f7b0acf 100644 --- a/libraries/AP_Scripting/examples/rangefinder_test.lua +++ b/libraries/AP_Scripting/examples/rangefinder_test.lua @@ -19,13 +19,26 @@ function update() end function info(rotation) - local ground_clearance = rangefinder:ground_clearance_cm_orient(rotation) - local distance_min = rangefinder:min_distance_cm_orient(rotation) - local distance_max = rangefinder:max_distance_cm_orient(rotation) + local ground_clearance = rangefinder:ground_clearance_orient(rotation) + local distance_min = rangefinder:min_distance_orient(rotation) + local distance_max = rangefinder:max_distance_orient(rotation) local offset = rangefinder:get_pos_offset_orient(rotation) - local distance_cm = rangefinder:distance_cm_orient(rotation) + local distance = rangefinder:distance_orient(rotation) - gcs:send_text(0, string.format("Rotation %d %.0f cm range %d - %d offset %.0f %.0f %.0f ground clearance %.0f", rotation, distance_cm, distance_min, distance_max, offset:x(), offset:y(), offset:z(), ground_clearance)) + gcs:send_text( + 0, + string.format( + "rot=%d distance=%.2f min=%.2f max=%.2f offset=(%.0f,%.0f,%.0f) ground-clearance=%.2f", + rotation, + distance, + distance_min, + distance_max, + offset:x(), + offset:y(), + offset:z(), + ground_clearance + ) + ) end return update(), 1000 -- first message may be displayed 1 seconds after start-up diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 3772ca51c0754..baa842152ec10 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -252,10 +252,18 @@ singleton RangeFinder rename rangefinder singleton RangeFinder method num_sensors uint8_t singleton RangeFinder method has_orientation boolean Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method distance_cm_orient deprecate Use distance_orient (in metres) +singleton RangeFinder method distance_orient float Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method signal_quality_pct_orient int8_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 -singleton RangeFinder method max_distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 -singleton RangeFinder method min_distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method max_distance_cm_orient int32_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method max_distance_cm_orient deprecate Use max_distance_orient (in metres) +singleton RangeFinder method min_distance_cm_orient int32_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method min_distance_cm_orient deprecate Use min_distance_orient (in metres) singleton RangeFinder method ground_clearance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method ground_clearance_cm_orient deprecate Use ground_clearance_orient (in metres) +singleton RangeFinder method max_distance_orient float Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method min_distance_orient float Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method ground_clearance_orient float Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method status_orient uint8_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method has_data_orient boolean Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method get_pos_offset_orient Vector3f Rotation'enum ROTATION_NONE ROTATION_MAX-1 From 84a1d4a08963e86b2d93a2ecc1f7058fcd5b2c77 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 08/19] GCS_MAVLink: allow for more than 327m range rangefinders --- libraries/GCS_MAVLink/GCS_Common.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 8fb6bc3f8f40b..1325b67df3ef7 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -429,9 +429,9 @@ void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, con mavlink_msg_distance_sensor_send( chan, AP_HAL::millis(), // time since system boot TODO: take time of measurement - sensor->min_distance_cm(), // minimum distance the sensor can measure in centimeters - sensor->max_distance_cm(), // maximum distance the sensor can measure in centimeters - sensor->distance_cm(), // current distance reading + MIN(sensor->min_distance() * 100, 65535),// minimum distance the sensor can measure in centimeters + MIN(sensor->max_distance() * 100, 65535),// maximum distance the sensor can measure in centimeters + MIN(sensor->distance() * 100, 65535), // current distance reading sensor->get_mav_distance_sensor_type(), // type from MAV_DISTANCE_SENSOR enum instance, // onboard ID of the sensor == instance sensor->orientation(), // direction the sensor faces from MAV_SENSOR_ORIENTATION enum From 3f15b972f715c06b93c7e93041403d533e5243f3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 09/19] SITL: allow for more than 327m range rangefinders --- .../SilentWings/Params/Rolladen-Schneider-LS8b.param | 12 ++++++------ .../SilentWings/Params/Schleicher-ASW27B.param | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index ccba94be53b18..5bf3c82c55360 100644 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -451,10 +451,10 @@ RLL2SRV_RMAX,75 RLL2SRV_TCONST,0.45 RNGFND_ADDR,0 RNGFND_FUNCTION,0 -RNGFND_GNDCLEAR,10 +RNGFND_GNDCLR,0.1 RNGFND_LANDING,0 -RNGFND_MAX_CM,700 -RNGFND_MIN_CM,20 +RNGFND_MAX,7.00 +RNGFND_MIN,0.20 RNGFND_OFFSET,0 RNGFND_ORIENT,25 RNGFND_PIN,-1 @@ -469,9 +469,9 @@ RNGFND_STOP_PIN,-1 RNGFND_TYPE,0 RNGFND2_ADDR,0 RNGFND2_FUNCTION,0 -RNGFND2_GNDCLEAR,10 -RNGFND2_MAX_CM,700 -RNGFND2_MIN_CM,20 +RNGFND2_GNDCLR,0.1 +RNGFND2_MAX,7.00 +RNGFND2_MIN,0.20 RNGFND2_OFFSET,0 RNGFND2_ORIENT,25 RNGFND2_PIN,-1 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index 353379fa5a10d..5eb9fc9cde642 100644 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -471,10 +471,10 @@ RLL2SRV_RMAX,50 RLL2SRV_TCONST,0.45 RNGFND_ADDR,0 RNGFND_FUNCTION,0 -RNGFND_GNDCLEAR,10 +RNGFND_GNDCLR,0.1 RNGFND_LANDING,0 -RNGFND_MAX_CM,700 -RNGFND_MIN_CM,20 +RNGFND_MAX,7.00 +RNGFND_MIN,0.20 RNGFND_OFFSET,0 RNGFND_ORIENT,25 RNGFND_PIN,-1 @@ -489,9 +489,9 @@ RNGFND_STOP_PIN,-1 RNGFND_TYPE,0 RNGFND2_ADDR,0 RNGFND2_FUNCTION,0 -RNGFND2_GNDCLEAR,10 -RNGFND2_MAX_CM,700 -RNGFND2_MIN_CM,20 +RNGFND2_GNDCLR,0.1 +RNGFND2_MAX,7.00 +RNGFND2_MIN,0.20 RNGFND2_OFFSET,0 RNGFND2_ORIENT,25 RNGFND2_PIN,-1 From 9c683de7f4fb7c17736e37b3d6e35cff77156df5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:48 +1100 Subject: [PATCH 10/19] ArduCopter: allow for more than 327m range rangefinders --- ArduCopter/AP_Arming.cpp | 4 ++-- ArduCopter/heli.cpp | 2 +- ArduCopter/mode_guided.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 6ef06d421641a..3e2c45ae3fe83 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -267,8 +267,8 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) return false; } // check if RTL_ALT is higher than rangefinder's max range - if (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT>RNGFND_MAX_CM"); + if (copter.g.rtl_altitude > copter.rangefinder.max_distance_orient(ROTATION_PITCH_270) * 100) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT (in cm) above RNGFND_MAX (in metres)"); return false; } #else diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index bff2a3de17041..4ca8cffae8a73 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -46,7 +46,7 @@ void Copter::check_dynamic_flight(void) if (!moving && rangefinder_state.enabled && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) { // when we are more than 2m from the ground with good // rangefinder lock consider it to be dynamic flight - moving = (rangefinder.distance_cm_orient(ROTATION_PITCH_270) > 200); + moving = (rangefinder.distance_orient(ROTATION_PITCH_270) > 2); } #endif diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index ba0321a1444ec..cfc8e27eff747 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -139,7 +139,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) #if AP_RANGEFINDER_ENABLED if (wp_nav->rangefinder_used_and_healthy() && wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER && - takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { + takeoff_alt_cm < copter.rangefinder.max_distance_orient(ROTATION_PITCH_270)*100) { // can't takeoff downwards if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) { return false; From 5049fa882de6e7ef134ad7237691074f05a00ffa Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 14:46:49 +1100 Subject: [PATCH 11/19] ArduSub: allow for more than 327m range rangefinders --- ArduSub/Log.cpp | 6 +++--- ArduSub/Sub.h | 12 ++++++------ ArduSub/mode_surftrak.cpp | 4 ++-- ArduSub/sensors.cpp | 18 +++++++++--------- 4 files changed, 20 insertions(+), 20 deletions(-) diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 51d02ea560ac7..865fb215e9332 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -16,7 +16,7 @@ struct PACKED log_Control_Tuning { float inav_alt; float baro_alt; int16_t desired_rangefinder_alt; - int16_t rangefinder_alt; + float rangefinder_alt; float terr_alt; int16_t target_climb_rate; int16_t climb_rate; @@ -48,7 +48,7 @@ void Sub::Log_Write_Control_Tuning() inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : barometer.get_altitude(), desired_rangefinder_alt : (int16_t)mode_surftrak.get_rangefinder_target_cm(), - rangefinder_alt : rangefinder_state.alt_cm, + rangefinder_alt : rangefinder_state.alt, terr_alt : terr_alt, target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(), climb_rate : climb_rate @@ -264,7 +264,7 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target const struct LogStructure Sub::log_structure[] = { LOG_COMMON_STRUCTURES, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qfffffffccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BBBBBB" }, + "CTUN", "Qffffffffcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BB0BBB" }, { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t), "D16", "QBh", "TimeUS,Id,Value", "s--", "F--" }, { LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 91af53aa83292..bdf70e40d3fc1 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -145,15 +145,15 @@ class Sub : public AP_Vehicle { AP_LeakDetector leak_detector; struct { - bool enabled:1; - bool alt_healthy:1; // true if we can trust the altitude from the rangefinder - int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder - int16_t min_cm; // min rangefinder distance (in cm) - int16_t max_cm; // max rangefinder distance (in cm) + bool enabled; + bool alt_healthy; // true if we can trust the altitude from the rangefinder + float alt; // tilt compensated altitude from rangefinder + float min; // min rangefinder distance + float max; // max rangefinder distance uint32_t last_healthy_ms; float inertial_alt_cm; // inertial alt at time of last rangefinder sample float rangefinder_terrain_offset_cm; // terrain height above EKF origin - LowPassFilterFloat alt_cm_filt; // altitude filter + LowPassFilterFloat alt_filt; // altitude filter } rangefinder_state = { false, false, 0, 0, 0, 0, 0, 0 }; #if AP_RPM_ENABLED diff --git a/ArduSub/mode_surftrak.cpp b/ArduSub/mode_surftrak.cpp index f119e02a8981f..c5ac051040c80 100644 --- a/ArduSub/mode_surftrak.cpp +++ b/ArduSub/mode_surftrak.cpp @@ -67,9 +67,9 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm) sub.gcs().send_text(MAV_SEVERITY_WARNING, "wrong mode, rangefinder target not set"); } else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) { sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to set rangefinder target", sub.g.surftrak_depth * 0.01f); - } else if (target_cm < (float)sub.rangefinder_state.min_cm) { + } else if (target_cm < sub.rangefinder_state.min*100) { sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target below minimum, ignored"); - } else if (target_cm > (float)sub.rangefinder_state.max_cm) { + } else if (target_cm > sub.rangefinder_state.max*100) { sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target above maximum, ignored"); } else { success = true; diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index f918e81a71f51..834595659a25e 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -20,7 +20,7 @@ void Sub::init_rangefinder() #if AP_RANGEFINDER_ENABLED rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN); rangefinder.init(ROTATION_PITCH_270); - rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); + rangefinder_state.alt_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270); #endif } @@ -39,30 +39,30 @@ void Sub::read_rangefinder() (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX) && (signal_quality_pct == -1 || signal_quality_pct >= g.rangefinder_signal_min); - int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270); + float temp_alt_m = rangefinder.distance_orient(ROTATION_PITCH_270); #if RANGEFINDER_TILT_CORRECTION // correct alt for angle of the rangefinder - temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); + temp_alt_m = temp_alt_m * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); #endif - rangefinder_state.alt_cm = temp_alt; + rangefinder_state.alt = temp_alt_m; rangefinder_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm(); - rangefinder_state.min_cm = rangefinder.min_distance_cm_orient(ROTATION_PITCH_270); - rangefinder_state.max_cm = rangefinder.max_distance_cm_orient(ROTATION_PITCH_270); + rangefinder_state.min = rangefinder.min_distance_orient(ROTATION_PITCH_270); + rangefinder_state.max = rangefinder.max_distance_orient(ROTATION_PITCH_270); // calculate rangefinder_terrain_offset_cm if (rangefinder_state.alt_healthy) { uint32_t now = AP_HAL::millis(); if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) { // reset filter if we haven't used it within the last second - rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm); + rangefinder_state.alt_filt.reset(rangefinder_state.alt); } else { - rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.05f); + rangefinder_state.alt_filt.apply(rangefinder_state.alt, 0.05f); } rangefinder_state.last_healthy_ms = now; rangefinder_state.rangefinder_terrain_offset_cm = - sub.rangefinder_state.inertial_alt_cm - sub.rangefinder_state.alt_cm_filt.get(); + sub.rangefinder_state.inertial_alt_cm - (sub.rangefinder_state.alt_filt.get() * 100); } // send rangefinder altitude and health to waypoint navigation library From c7268b5db0e9fcde230189378389100a7d69ced5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 15:13:10 +1100 Subject: [PATCH 12/19] ArduPlane: allow for more than 327m range rangefinders --- ArduPlane/altitude.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 230a16d067d0e..e96e778b82fa5 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -719,10 +719,10 @@ void Plane::rangefinder_height_update(void) // to misconfiguration or a faulty sensor if (rangefinder_state.in_range_count < 10) { if (!is_equal(distance, rangefinder_state.last_distance) && - fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01f) { + fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_orient(rangefinder_orientation())) { rangefinder_state.in_range_count++; } - if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01*0.2) { + if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_orient(rangefinder_orientation())*0.2) { // changes by more than 20% of full range will reset counter rangefinder_state.in_range_count = 0; } From f7a197bc72abe54e4b61f9d6c7c1678e597c621e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 15:13:10 +1100 Subject: [PATCH 13/19] AP_Frsky_Telem: allow for more than 327m range rangefinders --- libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index c6e69d626c3e2..fbe31d6b1ad62 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -696,7 +696,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void) attiandrng |= ((uint16_t)roundf((pitch * RAD_TO_DEG * 100 + 9000) * 0.05f) & ATTIANDRNG_PITCH_LIMIT)<distance_cm_orient(ROTATION_PITCH_270) : 0, 3, 1)<distance_orient(ROTATION_PITCH_270)*100 : 0, 3, 1)< Date: Fri, 11 Oct 2024 15:13:10 +1100 Subject: [PATCH 14/19] AP_SurfaceDistance: allow for more than 327m range rangefinders --- libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp | 2 +- libraries/AP_SurfaceDistance/AP_SurfaceDistance.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp index af8ec0b42f091..9dfdb232d55d7 100644 --- a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp +++ b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp @@ -69,7 +69,7 @@ void AP_SurfaceDistance::update() } // tilt corrected but unfiltered, not glitch protected alt - alt_cm = tilt_correction * rangefinder->distance_cm_orient(rotation); + alt_cm = tilt_correction * rangefinder->distance_orient(rotation)*100; // remember inertial alt to allow us to interpolate rangefinder inertial_alt_cm = inertial_nav.get_position_z_up_cm(); diff --git a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h index 1b331a30810c1..c8aefad62e0cd 100644 --- a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h +++ b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h @@ -26,10 +26,10 @@ class AP_SurfaceDistance { bool enabled; // not to be confused with rangefinder enabled, this state is to be set by the vehicle. bool alt_healthy; // true if we can trust the altitude from the rangefinder - int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder + int32_t alt_cm; // tilt compensated altitude (in cm) from rangefinder float inertial_alt_cm; // inertial alt at time of last rangefinder sample LowPassFilterFloat alt_cm_filt {0.5}; // altitude filter - int16_t alt_cm_glitch_protected; // last glitch protected altitude + int32_t alt_cm_glitch_protected; // last glitch protected altitude int8_t glitch_count; // non-zero number indicates rangefinder is glitching uint32_t glitch_cleared_ms; // system time glitch cleared float terrain_offset_cm; // filtered terrain offset (e.g. terrain's height above EKF origin) From 49c08c347d5495a408f74b5290c1d349ffa7373e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Oct 2024 15:13:11 +1100 Subject: [PATCH 15/19] Tools: allow for more than 327m range rangefinders --- .../Parrot_Disco/Parrot_Disco.param | 4 +- Tools/Frame_params/deset-mapping-boat.param | 4 +- Tools/Frame_params/eLAB_EX700_AC34.param | 4 +- Tools/autotest/arducopter.py | 188 +++++++++++++++++- Tools/autotest/ardusub.py | 15 +- .../default_params/copter-optflow.parm | 6 +- .../default_params/copter-rangefinder.parm | 6 +- .../autotest/default_params/gazebo-iris.parm | 2 +- Tools/autotest/default_params/periph.parm | 2 +- Tools/autotest/default_params/quad-can.parm | 2 +- .../default_params/quadplane-can.parm | 2 +- Tools/autotest/default_params/sub.parm | 2 +- .../default_params/vee-gull_005.param | 12 +- Tools/autotest/models/Callisto.param | 2 +- Tools/autotest/quadplane.py | 2 +- Tools/autotest/vehicle_test_suite.py | 6 +- 16 files changed, 216 insertions(+), 43 deletions(-) diff --git a/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param b/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param index 0d26d2c7c84cf..9f88fe5e11f08 100644 --- a/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param +++ b/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param @@ -43,8 +43,8 @@ RLL_RATE_P 0.040758 RLL2SRV_RMAX 75.000000 RLL2SRV_TCONST 0.450000 RNGFND_LANDING 1 -RNGFND_MAX_CM 700 -RNGFND_MIN_CM 20 +RNGFND_MAX 7.00 +RNGFND_MIN 0.20 RNGFND_PIN -1 RNGFND_SCALING 1 RNGFND_TYPE 9 diff --git a/Tools/Frame_params/deset-mapping-boat.param b/Tools/Frame_params/deset-mapping-boat.param index 44b9baeea3f99..c9e84cf411aa7 100644 --- a/Tools/Frame_params/deset-mapping-boat.param +++ b/Tools/Frame_params/deset-mapping-boat.param @@ -25,8 +25,8 @@ MOT_SLEWRATE,25 MOT_VEC_ANGLEMAX,30 NAVL1_DAMPING,0.8 NAVL1_PERIOD,16 -RNGFND1_MAX_CM,20000 -RNGFND1_MIN_CM,0 +RNGFND1_MAX,200.00 +RNGFND1_MIN,0 RNGFND1_TYPE,17 SERIAL2_BAUD,19 SERIAL2_PROTOCOL,39 diff --git a/Tools/Frame_params/eLAB_EX700_AC34.param b/Tools/Frame_params/eLAB_EX700_AC34.param index 85fce24f5652e..e8453feb39e27 100644 --- a/Tools/Frame_params/eLAB_EX700_AC34.param +++ b/Tools/Frame_params/eLAB_EX700_AC34.param @@ -39,8 +39,8 @@ MOT_THST_HOVER,0.25 MOT_THST_EXPO,0.7 MOT_BAT_VOLT_MAX,25.2 MOT_BAT_VOLT_MIN,21.0 -RNGFND_MAX_CM,5000 -RNGFND_MIN_CM,5 +RNGFND_MAX,50.00 +RNGFND_MIN,0.05 RNGFND_SCALING,1 RNGFND_TYPE,8 PILOT_THR_BHV,1 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d2af1575e39b4..0b4d1d4b22c2d 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2870,7 +2870,7 @@ def EK3_RNG_USE_HGT(self): self.set_analog_rangefinder_parameters() # set use-height to 20m (the parameter is a percentage of max range) self.set_parameters({ - 'EK3_RNG_USE_HGT': 200000 / self.get_parameter('RNGFND1_MAX_CM'), + 'EK3_RNG_USE_HGT': (20 / self.get_parameter('RNGFND1_MAX')) * 100, }) self.reboot_sitl() @@ -3044,7 +3044,7 @@ def CANGPSCopterMission(self): "COMPASS_USE3" : 0, # use DroneCAN rangefinder "RNGFND1_TYPE" : 24, - "RNGFND1_MAX_CM" : 11000, + "RNGFND1_MAX" : 110.00, # use DroneCAN battery monitoring, and enforce with a arming voltage "BATT_MONITOR" : 8, "BATT_ARM_VOLT" : 12.0, @@ -4026,7 +4026,7 @@ def test_rangefinder_switchover(self): self.set_analog_rangefinder_parameters() self.set_parameters({ - "RNGFND1_MAX_CM": 1500 + "RNGFND1_MAX": 15.00 }) # configure EKF to use rangefinder for altitude at low altitudes @@ -4757,7 +4757,9 @@ def fly_guided_move_local(self, x, y, z_up, timeout=100): while True: if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Did not reach destination") - if self.distance_to_local_position((x, y, -z_up)) < 1: + dist = self.distance_to_local_position((x, y, -z_up)) + if dist < 1: + self.progress(f"Reach distance ({dist})") break def test_guided_local_position_target(self, x, y, z_up): @@ -8512,7 +8514,7 @@ def fly_rangefinder_mavlink_distance_sensor(self): self.set_parameter("SERIAL5_PROTOCOL", 1) self.set_parameter("RNGFND1_TYPE", 10) self.reboot_sitl() - self.set_parameter("RNGFND1_MAX_CM", 32767) + self.set_parameter("RNGFND1_MAX", 327.67) self.progress("Should be unhealthy while we don't send messages") self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, False) @@ -8777,10 +8779,10 @@ def MaxBotixI2CXL(self): self.set_parameters({ "RNGFND1_TYPE": 2, # maxbotix "RNGFND1_ADDR": 0x70, - "RNGFND1_MIN_CM": 150, + "RNGFND1_MIN": 1.50, "RNGFND2_TYPE": 2, # maxbotix "RNGFND2_ADDR": 0x71, - "RNGFND2_MIN_CM": 250, + "RNGFND2_MIN": 2.50, }) self.reboot_sitl() self.do_timesync_roundtrip() @@ -8922,6 +8924,176 @@ def RangeFinderDriversMaxAlt(self): self.do_RTL() + def RangeFinderDriversLongRange(self): + '''test rangefinder above 327m''' + # FIXME: when we get a driver+simulator for a rangefinder with + # >327m range change this to use that driver + self.set_parameters({ + "SERIAL4_PROTOCOL": 9, + "RNGFND1_TYPE": 19, # BenewakeTF02 + "WPNAV_SPEED_UP": 1000, # cm/s + }) + self.customise_SITL_commandline([ + "--serial4=sim:benewake_tf02", + ]) + + text_good = "GOOD" + text_out_of_range_high = "OUT_OF_RANGE_HIGH" + + rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION + + alt = 3 + self.takeoff(alt, mode='GUIDED') + self.assert_parameter_value("RNGFND1_MAX", 7.00) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001) + self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001) + + self.send_statustext(text_good) + + alt = 10 + self.fly_guided_move_local(0, 0, alt) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001) + self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001) + self.send_statustext(text_out_of_range_high) + + self.progress("Moving the goalposts") + self.set_parameter("RNGFND1_MAX", 20) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001) + self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001) + self.send_statustext(text_good) + self.delay_sim_time(2) + + dfreader = self.dfreader_for_current_onboard_log() + + self.do_RTL() + + self.progress("Checking in/out of range markers in log") + required_range = None + count = 0 + while True: + m = dfreader.recv_match(type=["MSG", "RFND"]) + if m is None: + break + m_type = m.get_type() + if m_type == "MSG": + for t in [text_good, text_out_of_range_high]: + if t in m.Message: + required_range = t + continue + if m_type == "RFND": + if required_range is None: + continue + if required_range == text_good: + required_state = 4 + elif required_range == text_out_of_range_high: + required_state = 3 + else: + raise ValueError(f"Unexpected text {required_range}") + if m.Stat != required_state: + raise NotAchievedException(f"Not in expected state want={required_state} got={m.Stat} dist={m.Dist}") + self.progress(f"In expected state {required_range}") + required_range = None + count += 1 + + if count < 3: + raise NotAchievedException("Did not see range markers") + + def RangeFinderSITLLongRange(self): + '''test rangefinder above 327m''' + # FIXME: when we get a driver+simulator for a rangefinder with + # >327m range change this to use that driver + self.set_parameters({ + "RNGFND1_TYPE": 100, # SITL + "WPNAV_SPEED_UP": 1000, # cm/s + "RNGFND1_MAX": 7000, # metres + }) + self.reboot_sitl() + + text_good = "GOOD" + text_high = "HIGH" + + rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION + + alt = 3 + self.takeoff(alt, mode='GUIDED') + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + got = m.current_distance * 0.01 + if abs(got - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range {got=}") + + self.send_statustext(text_good) + + alt = 635 + self.fly_guided_move_local(0, 0, alt) + self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True) + m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True) + if abs(m.current_distance * 0.01 - alt) > 1: + raise NotAchievedException(f"Expected {alt}m range") + + self.send_statustext(text_high) + + dfreader = self.dfreader_for_current_onboard_log() + + # fast descent! + def hook(msg, m): + if m.get_type() != 'HEARTBEAT': + return + # tell vehicle to only pay attention to the attitude: + bitmask = 0 + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE + bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE + + self.mav.mav.set_attitude_target_send( + 0, # time_boot_ms + 1, # target sysid + 1, # target compid + 0, # bitmask of things to ignore + mavextra.euler_to_quat([ + math.radians(-180), + math.radians(0), + math.radians(0)]), # att + 0, # roll rate (rad/s) + 0, # pitch rate (rad/s) + 0, # yaw rate (rad/s) + 1 # thrust, 0 to 1, translated to a climb/descent rate + ) + + self.install_message_hook_context(hook) + + self.wait_altitude(0, 30, timeout=200, relative=True) + self.do_RTL() + + self.progress("Checking in/out of range markers in log") + good = False + while True: + m = dfreader.recv_match(type=["MSG", "RFND"]) + if m is None: + break + m_type = m.get_type() + if m_type == "RFND": + if m.Dist < 600: + continue + good = True + break + + if not good: + raise NotAchievedException("Did not see good alt") + def ShipTakeoff(self): '''Fly Simulated Ship Takeoff''' # test ship takeoff @@ -10866,6 +11038,8 @@ def tests1e(self): self.ModeFollow, self.RangeFinderDrivers, self.RangeFinderDriversMaxAlt, + self.RangeFinderDriversLongRange, + self.RangeFinderSITLLongRange, self.MaxBotixI2CXL, self.MAVProximity, self.ParameterValidation, diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 5947640790b44..88b498721f6c8 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -16,7 +16,6 @@ import vehicle_test_suite from vehicle_test_suite import NotAchievedException from vehicle_test_suite import AutoTestTimeoutException -from vehicle_test_suite import PreconditionFailedException if sys.version_info[0] < 3: ConnectionResetError = AutoTestTimeoutException @@ -196,8 +195,8 @@ def RngfndQuality(self): "SCR_ENABLE": 1, "RNGFND1_TYPE": 36, "RNGFND1_ORIENT": 25, - "RNGFND1_MIN_CM": 10, - "RNGFND1_MAX_CM": 5000, + "RNGFND1_MIN": 0.10, + "RNGFND1_MAX": 50.00, }) self.install_example_script_context("rangefinder_quality_test.lua") @@ -244,8 +243,7 @@ def watch_distance_maintained(self, delta=0.3, timeout=5.0): def Surftrak(self): """Test SURFTRAK mode""" - if self.get_parameter('RNGFND1_MAX_CM') != 3000.0: - raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0) + self.assert_parameter_value('RNGFND1_MAX', 30) # Something closer to Bar30 noise self.context_push() @@ -292,8 +290,8 @@ def prepare_synthetic_seafloor_test(self, sea_floor_depth, rf_target): "SCR_ENABLE": 1, "RNGFND1_TYPE": 36, "RNGFND1_ORIENT": 25, - "RNGFND1_MIN_CM": 10, - "RNGFND1_MAX_CM": 3000, + "RNGFND1_MIN": 0.10, + "RNGFND1_MAX": 30.00, "SCR_USER1": 2, # Configuration bundle "SCR_USER2": sea_floor_depth, # Depth in meters "SCR_USER3": 101, # Output log records @@ -788,8 +786,7 @@ def MAV_CMD_DO_REPOSITION(self): def TerrainMission(self): """Mission using surface tracking""" - if self.get_parameter('RNGFND1_MAX_CM') != 3000.0: - raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0) + self.assert_parameter_value('RNGFND1_MAX', 30) filename = "terrain_mission.txt" self.progress("Executing mission %s" % filename) diff --git a/Tools/autotest/default_params/copter-optflow.parm b/Tools/autotest/default_params/copter-optflow.parm index 162c037074101..90f4ee41937c6 100644 --- a/Tools/autotest/default_params/copter-optflow.parm +++ b/Tools/autotest/default_params/copter-optflow.parm @@ -5,8 +5,8 @@ EK3_SRC1_VELXY 5 EK3_SRC1_VELZ 0 FLOW_TYPE 10 RNGFND1_TYPE 1 -RNGFND1_MIN_CM 0 -RNGFND1_MAX_CM 4000 +RNGFND1_MIN 0 +RNGFND1_MAX 40.00 RNGFND1_PIN 0 RNGFND1_SCALING 12.12 -SIM_FLOW_ENABLE 1 \ No newline at end of file +SIM_FLOW_ENABLE 1 diff --git a/Tools/autotest/default_params/copter-rangefinder.parm b/Tools/autotest/default_params/copter-rangefinder.parm index 0005a89571eea..1963fe33b4987 100644 --- a/Tools/autotest/default_params/copter-rangefinder.parm +++ b/Tools/autotest/default_params/copter-rangefinder.parm @@ -1,5 +1,5 @@ RNGFND1_TYPE 1 -RNGFND1_MIN_CM 0 -RNGFND1_MAX_CM 4000 +RNGFND1_MIN 0 +RNGFND1_MAX 40.00 RNGFND1_PIN 0 -RNGFND1_SCALING 12.12 \ No newline at end of file +RNGFND1_SCALING 12.12 diff --git a/Tools/autotest/default_params/gazebo-iris.parm b/Tools/autotest/default_params/gazebo-iris.parm index 9e586a53fdd18..7fba567f3fdb0 100644 --- a/Tools/autotest/default_params/gazebo-iris.parm +++ b/Tools/autotest/default_params/gazebo-iris.parm @@ -10,4 +10,4 @@ SIM_SONAR_SCALE 10 RNGFND1_TYPE 1 RNGFND1_SCALING 10 RNGFND1_PIN 0 -RNGFND1_MAX_CM 5000 +RNGFND1_MAX 50.00 diff --git a/Tools/autotest/default_params/periph.parm b/Tools/autotest/default_params/periph.parm index d1674bb220f22..ca13f126fdbac 100644 --- a/Tools/autotest/default_params/periph.parm +++ b/Tools/autotest/default_params/periph.parm @@ -5,7 +5,7 @@ COMPASS_ENABLE 1 BARO_ENABLE 1 ARSPD_TYPE 100 RNGFND1_TYPE 100 -RNGFND1_MAX_CM 12000 +RNGFND1_MAX 120.00 BATT_MONITOR 16 BATT_I2C_BUS 2 diff --git a/Tools/autotest/default_params/quad-can.parm b/Tools/autotest/default_params/quad-can.parm index 2f6df0c33ba05..90711c5bed888 100644 --- a/Tools/autotest/default_params/quad-can.parm +++ b/Tools/autotest/default_params/quad-can.parm @@ -4,5 +4,5 @@ SIM_CAN_SRV_MSK 0xFf SIM_VIB_MOT_MAX 270 GPS1_TYPE 9 RNGFND1_TYPE 24 -RNGFND1_MAX_CM 11000 +RNGFND1_MAX 110.00 BATT_MONITOR 8 diff --git a/Tools/autotest/default_params/quadplane-can.parm b/Tools/autotest/default_params/quadplane-can.parm index 8bb2195a7f783..2596901289a66 100644 --- a/Tools/autotest/default_params/quadplane-can.parm +++ b/Tools/autotest/default_params/quadplane-can.parm @@ -8,6 +8,6 @@ SIM_VIB_MOT_MAX 270 GPS1_TYPE 9 ARSPD_TYPE 8 RNGFND1_TYPE 24 -RNGFND1_MAX_CM 11000 +RNGFND1_MAX 110.00 RNGFND_LANDING 1 BATT_MONITOR 8 diff --git a/Tools/autotest/default_params/sub.parm b/Tools/autotest/default_params/sub.parm index 65615aa5f2abd..bf0b9ddfafd00 100644 --- a/Tools/autotest/default_params/sub.parm +++ b/Tools/autotest/default_params/sub.parm @@ -75,7 +75,7 @@ PSC_POSXY_P 2.5 PSC_VELXY_D 0.8 PSC_VELXY_I 0.5 PSC_VELXY_P 5.0 -RNGFND1_MAX_CM 3000 +RNGFND1_MAX 30.00 RNGFND1_PIN 0 RNGFND1_SCALING 12.12 RNGFND1_TYPE 1 diff --git a/Tools/autotest/default_params/vee-gull_005.param b/Tools/autotest/default_params/vee-gull_005.param index 9415f055bdedc..c5305222954ab 100644 --- a/Tools/autotest/default_params/vee-gull_005.param +++ b/Tools/autotest/default_params/vee-gull_005.param @@ -465,10 +465,10 @@ RLL2SRV_RMAX,0 RLL2SRV_TCONST,0.5 RNGFND1_ADDR,0 RNGFND1_FUNCTION,0 -RNGFND1_GNDCLEAR,10 +RNGFND1_GNDCLR,0.01 RNGFND1_LANDING,0 -RNGFND1_MAX_CM,700 -RNGFND1_MIN_CM,20 +RNGFND1_MAX,7.00 +RNGFND1_MIN,0.20 RNGFND1_OFFSET,0 RNGFND1_ORIENT,25 RNGFND1_PIN,-1 @@ -483,9 +483,9 @@ RNGFND1_STOP_PIN,-1 RNGFND1_TYPE,0 RNGFND2_ADDR,0 RNGFND2_FUNCTION,0 -RNGFND2_GNDCLEAR,10 -RNGFND2_MAX_CM,700 -RNGFND2_MIN_CM,20 +RNGFND2_GNDCLR,0.01 +RNGFND2_MAX,7.00 +RNGFND2_MIN,0.20 RNGFND2_OFFSET,0 RNGFND2_ORIENT,25 RNGFND2_PIN,-1 diff --git a/Tools/autotest/models/Callisto.param b/Tools/autotest/models/Callisto.param index df57aea2e9064..b3789db7da7b0 100644 --- a/Tools/autotest/models/Callisto.param +++ b/Tools/autotest/models/Callisto.param @@ -55,7 +55,7 @@ PSC_ANGLE_MAX,45 PSC_JERK_Z,10 PSC_POSZ_P,0.5 PSC_VELZ_P,2.5 -RNGFND1_MAX_CM,10000 +RNGFND1_MAX,100.00 RNGFND1_PIN,0 RNGFND1_SCALING,12.1212 RNGFND1_TYPE,1 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 69fdd6eb0cb5b..8e2500a9e04c1 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1524,7 +1524,7 @@ def PrecisionLanding(self): "RNGFND1_TYPE": 100, "RNGFND1_PIN" : 0, "RNGFND1_SCALING" : 12.2, - "RNGFND1_MAX_CM" : 5000, + "RNGFND1_MAX" : 50.00, "RNGFND_LANDING" : 1, }) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 978483c70e20c..f28e6074268c9 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -5825,8 +5825,8 @@ def send_mavlink_run_prearms_command(self): def analog_rangefinder_parameters(self): return { "RNGFND1_TYPE": 1, - "RNGFND1_MIN_CM": 0, - "RNGFND1_MAX_CM": 4000, + "RNGFND1_MIN": 0, + "RNGFND1_MAX": 40.00, "RNGFND1_SCALING": 12.12, "RNGFND1_PIN": 0, } @@ -7211,6 +7211,8 @@ def assert_rangefinder_distance_between(self, dist_min, dist_max): raise NotAchievedException("above max height (%f > %f)" % (m.distance, dist_max)) + self.progress(f"Rangefinder distance {m.distance} is between {dist_min} and {dist_max}") + def assert_distance_sensor_quality(self, quality): m = self.assert_receive_message('DISTANCE_SENSOR') From 94414fff05567539a69a2e8575ba8fb0edc81330 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 24 Dec 2024 08:24:26 +1100 Subject: [PATCH 16/19] Sub: log desired rangefinder alt (DSAlt) in metres not cm --- ArduSub/Log.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 865fb215e9332..0da4731a153e3 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -15,7 +15,7 @@ struct PACKED log_Control_Tuning { float desired_alt; float inav_alt; float baro_alt; - int16_t desired_rangefinder_alt; + float desired_rangefinder_alt; float rangefinder_alt; float terr_alt; int16_t target_climb_rate; @@ -47,7 +47,7 @@ void Sub::Log_Write_Control_Tuning() desired_alt : pos_control.get_pos_target_z_cm() * 0.01f, inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : barometer.get_altitude(), - desired_rangefinder_alt : (int16_t)mode_surftrak.get_rangefinder_target_cm(), + desired_rangefinder_alt : mode_surftrak.get_rangefinder_target_cm() * 0.01, rangefinder_alt : rangefinder_state.alt, terr_alt : terr_alt, target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(), @@ -264,7 +264,7 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target const struct LogStructure Sub::log_structure[] = { LOG_COMMON_STRUCTURES, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qffffffffcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BB0BBB" }, + "CTUN", "Qffffffffffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B00BBB" }, { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t), "D16", "QBh", "TimeUS,Id,Value", "s--", "F--" }, { LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t), From 906ac7462988159a11cdbd15c4655284e89d8edc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 24 Dec 2024 08:44:25 +1100 Subject: [PATCH 17/19] AP_Scripting: update examples to not use deprecated methods --- libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua | 2 +- libraries/AP_Scripting/applets/plane_package_place.lua | 2 +- libraries/AP_Scripting/applets/plane_precland.lua | 2 +- libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua | 2 +- libraries/AP_Scripting/examples/ahrs-source.lua | 2 +- libraries/AP_Scripting/examples/copter-wall-climber.lua | 2 +- libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua | 2 +- libraries/AP_Scripting/examples/land_hagl.lua | 2 +- libraries/AP_Scripting/examples/rangefinder_quality_test.lua | 4 ++-- 9 files changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua b/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua index 7b980ce88fa31..7f4fac241e567 100644 --- a/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua +++ b/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua @@ -184,7 +184,7 @@ function update() -- get rangefinder distance local rngfnd_distance_m = 0 if rangefinder:has_data_orient(rangefinder_rotation) then - rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_rotation) * 0.01 + rngfnd_distance_m = rangefinder:distance_orient(rangefinder_rotation) end local rngfnd_over_threshold = (rngfnd_distance_m == 0) or (rngfnd_distance_m > ESRC_RNGFND_MAX:get()) diff --git a/libraries/AP_Scripting/applets/plane_package_place.lua b/libraries/AP_Scripting/applets/plane_package_place.lua index 360d958a1b0ca..7985d107c16bf 100644 --- a/libraries/AP_Scripting/applets/plane_package_place.lua +++ b/libraries/AP_Scripting/applets/plane_package_place.lua @@ -108,7 +108,7 @@ function update() -- check the distance, if less than RNG_ORIENT_DOWN then release local dist_m if not landed then - dist_m = rangefinder:distance_cm_orient(RNG_ORIENT_DOWN) * 0.01 + dist_m = rangefinder:distance_orient(RNG_ORIENT_DOWN) else dist_m = 0.0 end diff --git a/libraries/AP_Scripting/applets/plane_precland.lua b/libraries/AP_Scripting/applets/plane_precland.lua index 1d5dc96cd8f00..74833af8b4610 100644 --- a/libraries/AP_Scripting/applets/plane_precland.lua +++ b/libraries/AP_Scripting/applets/plane_precland.lua @@ -166,7 +166,7 @@ local function update() --[[ get rangefinder distance, and if PLND_ALT_CUTOFF is set then stop precland operation if below the cutoff --]] - local rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_orient) * 0.01 + local rngfnd_distance_m = rangefinder:distance_orient(rangefinder_orient) if PLND_ALT_CUTOFF:get() > 0 and rngfnd_distance_m < PLND_ALT_CUTOFF:get() then return end diff --git a/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua b/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua index 87d9bf3a232fc..4d1634fd3f4ff 100644 --- a/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua +++ b/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua @@ -121,7 +121,7 @@ function update() -- get rangefinder distance local rngfnd_distance_m = 0 if rangefinder:has_data_orient(rangefinder_rotation) then - rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_rotation) * 0.01 + rngfnd_distance_m = rangefinder:distance_orient(rangefinder_rotation) end local rngfnd_over_threshold = (rngfnd_distance_m == 0) or (rngfnd_distance_m > rangefinder_thresh_dist) diff --git a/libraries/AP_Scripting/examples/ahrs-source.lua b/libraries/AP_Scripting/examples/ahrs-source.lua index d65104dfea40c..9eb050e48fc8e 100644 --- a/libraries/AP_Scripting/examples/ahrs-source.lua +++ b/libraries/AP_Scripting/examples/ahrs-source.lua @@ -85,7 +85,7 @@ function update() -- get rangefinder distance local rngfnd_distance_m = 0 if rangefinder:has_data_orient(rangefinder_rotation) then - rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_rotation) * 0.01 + rngfnd_distance_m = rangefinder:distance_orient(rangefinder_rotation) end local rngfnd_over_threshold = (rngfnd_distance_m == 0) or (rngfnd_distance_m > rangefinder_thresh_dist) diff --git a/libraries/AP_Scripting/examples/copter-wall-climber.lua b/libraries/AP_Scripting/examples/copter-wall-climber.lua index 0028776c30442..714c9344896ba 100644 --- a/libraries/AP_Scripting/examples/copter-wall-climber.lua +++ b/libraries/AP_Scripting/examples/copter-wall-climber.lua @@ -140,7 +140,7 @@ function update() if (climb_mode == 1) then -- convert rangefinder distance to pitch speed if rangefinder:has_data_orient(0) then - local distance_m = rangefinder:distance_cm_orient(0) * 0.01 + local distance_m = rangefinder:distance_orient(0) wall_pitch_speed = (distance_m - wall_dist_target) * wall_dist_to_speed_P wall_pitch_speed = math.min(wall_pitch_speed, wall_pitch_speed_max) wall_pitch_speed = math.max(wall_pitch_speed, -wall_pitch_speed_max) diff --git a/libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua b/libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua index 582e0ce08967b..9d6b53a271674 100644 --- a/libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua +++ b/libraries/AP_Scripting/examples/jump_tags_calibrate_agl.lua @@ -61,7 +61,7 @@ function sample_rangefinder_to_get_AGL() end -- we're actively sampling rangefinder distance to ground - local distance_raw_m = rangefinder:distance_cm_orient(ROTATION_PITCH_270) * 0.01 + local distance_raw_m = rangefinder:distance_orient(ROTATION_PITCH_270) -- correct the range for attitude (multiply by DCM.c.z, which is cos(roll)*cos(pitch)) local ahrs_get_rotation_body_to_ned_c_z = math.cos(ahrs:get_roll())*math.cos(ahrs:get_pitch()) diff --git a/libraries/AP_Scripting/examples/land_hagl.lua b/libraries/AP_Scripting/examples/land_hagl.lua index 3bdbc36329aea..d630633cff4f5 100644 --- a/libraries/AP_Scripting/examples/land_hagl.lua +++ b/libraries/AP_Scripting/examples/land_hagl.lua @@ -33,7 +33,7 @@ local function send_HAGL() last_active = false return end - local rangefinder_dist = rangefinder:distance_cm_orient(RANGEFINDER_ORIENT)*0.01 + local rangefinder_dist = rangefinder:distance_orient(RANGEFINDER_ORIENT) local correction = math.cos(ahrs:get_roll())*math.cos(ahrs:get_pitch()) local rangefinder_corrected = rangefinder_dist * correction if RANGEFINDER_ORIENT == ROTATION_PITCH_90 then diff --git a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua index 1767b6bb8540d..4c93bf4ac289d 100644 --- a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua +++ b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua @@ -88,12 +88,12 @@ local function get_and_eval(test_idx, dist_m_in, signal_quality_pct_in, status_e -- L U A I N T E R F A C E T E S T -- Check that the distance and signal_quality from the frontend are as expected - local distance1_cm_out = rangefinder:distance_cm_orient(RNGFND_ORIENTATION_DOWN) + local distance1_cm_out = rangefinder:distance_orient(RNGFND_ORIENTATION_DOWN) * 100 local signal_quality1_pct_out = rangefinder:signal_quality_pct_orient(RNGFND_ORIENTATION_DOWN) -- Make sure data was returned if not distance1_cm_out or not signal_quality1_pct_out then - return "No data returned from rangefinder:distance_cm_orient()" + return "No data returned from rangefinder:distance_orient()" end send(string.format("Frontend test %i dist in_m: %.2f out_cm: %.2f, signal_quality_pct in: %.1f out: %.1f", From 6ec46c82a6630c554f1aacac808d03472dd5e5d3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 27 Dec 2024 09:31:13 +1100 Subject: [PATCH 18/19] autotest: correct copter-tailsitter parameter file for RNGFND1_MIN_CM rename also fix a race condition in the quadplane-tailsitter test - which is unlikely to ever trigger --- .../autotest/default_params/quadplane-copter_tailsitter.parm | 4 ++-- Tools/autotest/quadplane.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/default_params/quadplane-copter_tailsitter.parm b/Tools/autotest/default_params/quadplane-copter_tailsitter.parm index a936774c6ce33..901bd7eda8d6a 100644 --- a/Tools/autotest/default_params/quadplane-copter_tailsitter.parm +++ b/Tools/autotest/default_params/quadplane-copter_tailsitter.parm @@ -38,8 +38,8 @@ SIM_SONAR_SCALE 10 RNGFND1_TYPE 100 RNGFND1_PIN 0 RNGFND1_SCALING 10 -RNGFND1_MIN_CM 10 -RNGFND1_MAX_CM 5000 +RNGFND1_MIN 0.10 +RNGFND1_MAX 50 RNGFND1_ORIENT 12 RNGFND_LANDING 1 RNGFND_LND_ORNT 12 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 8e2500a9e04c1..4583846883863 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1077,7 +1077,7 @@ def CopterTailsitter(self): self.context_collect("STATUSTEXT") self.progress("Starting QLAND") self.change_mode("QLAND") - self.wait_statustext("Rangefinder engaged") + self.wait_statustext("Rangefinder engaged", check_context=True) self.wait_disarmed(timeout=100) def setup_ICEngine_vehicle(self): From de165351d95fa246071b44f5c336066a6efcd842 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 1 Jan 2025 13:57:57 +1100 Subject: [PATCH 19/19] Replay: add example script for converting from cm to m in RRNI and RRNH messages --- .../examples/rewrite-RFND-values-to-floats.py | 132 ++++++++++++++++++ 1 file changed, 132 insertions(+) create mode 100755 Tools/Replay/examples/rewrite-RFND-values-to-floats.py diff --git a/Tools/Replay/examples/rewrite-RFND-values-to-floats.py b/Tools/Replay/examples/rewrite-RFND-values-to-floats.py new file mode 100755 index 0000000000000..7688d30359dc3 --- /dev/null +++ b/Tools/Replay/examples/rewrite-RFND-values-to-floats.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python +'''In ArduPilot 4.7 the distance values were moved to float to +facilitate supporting rangefinders with more than 327m of range. + +The dataflash Replay log messages changes to support this. + +This script will take an older log and convert those messages such +that older logs can be replayed with newer code. + +AP_FLAKE8_CLEAN +''' + +from argparse import ArgumentParser + +import struct + +from pymavlink import DFReader +from pymavlink import mavutil + +new_RRNH_format = "ffB" +new_RRNI_format = "ffffBBB" + + +class Rewrite(): + def __init__(self, rewrite_fmt, rewrite_fmtu, rewrite_instance): + self.rewrite_fmt = rewrite_fmt + self.rewrite_fmtu = rewrite_fmtu + self.rewrite_instance = rewrite_instance + + def format_to_struct(fmt): + ret = bytes("<", 'ascii') + for c in fmt: + (s, mul, type) = DFReader.FORMAT_TO_STRUCT[c] + ret += bytes(s, 'ascii') + return bytes(ret) + + +rewrites = { + # convert RRNI.distance_cm > RRNI.distance + "RRNH": Rewrite( + rewrite_fmt=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct("BBnNZ"), + m.Type, + struct.calcsize(Rewrite.format_to_struct(new_RRNH_format)) + 3, # m.Length + bytes(m.Name, 'ascii'), + bytes(new_RRNH_format, 'ascii'), + bytes(m.Columns, 'ascii') + ), + rewrite_fmtu=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct("QBNN"), + m.TimeUS, + m.FmtType, + bytes("mm-", 'ascii'), # new units + bytes("00-", 'ascii') # new mults + ), + rewrite_instance=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct(new_RRNH_format), + m.GCl * 0.01, + m.MaxD * 0.01, + m.NumSensors + ), + ), + "RRNI": Rewrite( + rewrite_fmt=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct("BBnNZ"), + m.Type, + struct.calcsize(Rewrite.format_to_struct(new_RRNI_format)) + 3, # m.Length + bytes(m.Name, 'ascii'), + bytes(new_RRNI_format, 'ascii'), + bytes(m.Columns, 'ascii') + ), + rewrite_fmtu=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct("QBNN"), + m.TimeUS, + m.FmtType, + bytes("???m--#", 'ascii'), # new units + bytes("???0---", 'ascii') # new mults + ), + rewrite_instance=lambda buf, m : buf[:3] + struct.pack( + Rewrite.format_to_struct(new_RRNI_format), + m.PX, + m.PY, + m.PZ, + m.Dist * 0.01, + m.Orient, + m.Status, + m.I + ), + ), +} + + +parser = ArgumentParser(description=__doc__) + +parser.add_argument("login") +parser.add_argument("logout") + +args = parser.parse_args() + +login = mavutil.mavlink_connection(args.login) +output = open(args.logout, mode='wb') + +type_name_map = {} + + +def rewrite_message(m): + buf = bytearray(m.get_msgbuf()) + + mtype = m.get_type() + if mtype == 'FMT': + type_name_map[m.Type] = m.Name + if m.Name in rewrites: + return rewrites[m.Name].rewrite_fmt(buf, m) + + if mtype == 'FMTU': + if m.FmtType not in type_name_map: + raise ValueError(f"Have not seen format for ID {m.FmtType}") + name = type_name_map[m.FmtType] + if name in rewrites: + return rewrites[name].rewrite_fmtu(buf, m) + + if mtype in rewrites: + return rewrites[mtype].rewrite_instance(buf, m) + + return buf + + +while True: + m = login.recv_msg() + if m is None: + break + output.write(rewrite_message(m))