From 4be9b4171bf762ebc96a22b7e4f7bf962fc3b778 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 28 Sep 2020 17:49:17 -0700 Subject: [PATCH] AP_RangeFinder: TFMiniPlus: respect max configured via param Do like other drivers do and cap the maximum reported value with what is configured in the parameter. --- .../AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp index 8ec86ba392..f31cc8f347 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp @@ -23,6 +23,7 @@ extern const AP_HAL::HAL& hal; #define DRIVER "TFMiniPlus" +#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100 /* * Command format: @@ -165,10 +166,10 @@ 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_DIST_CM; + output_distance_cm = MAX(MAX_DIST_CM, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM); + } else { + output_distance_cm = constrain_int16(output_distance_cm, MIN_DIST_CM, MAX_DIST_CM); } - - output_distance_cm = constrain_int16(output_distance_cm, MIN_DIST_CM, MAX_DIST_CM); } bool AP_RangeFinder_Benewake_TFMiniPlus::check_checksum(uint8_t *arr, int pkt_len)