From 54494a71e6fba5f788bfa803c46daffe5683a3f4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 14 Nov 2018 11:51:54 +0900 Subject: [PATCH] AP_RangeFinder: benewake returns at least 12m or 22m when out-of-range value logged and shown to the user will be at least 12m for TFmini or 22m for TF02 which ensure the user won't see the range drop even if they have configured a very short useable distance --- libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index 51ddb9f326..b1ebab01aa 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -24,6 +24,8 @@ extern const AP_HAL::HAL& hal; #define BENEWAKE_FRAME_HEADER 0x59 #define BENEWAKE_FRAME_LENGTH 9 #define BENEWAKE_DIST_MAX_CM 32768 +#define BENEWAKE_TFMINI_OUT_OF_RANGE_CM 1200 +#define BENEWAKE_TF02_OUT_OF_RANGE_CM 2200 #define BENEWAKE_OUT_OF_RANGE_ADD_CM 100 // format of serial packets received from benewake lidar @@ -147,8 +149,10 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm) } if (count_out_of_range > 0) { - // if only out of range readings return max range + 1m - reading_cm = max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM; + // if only out of range readings return larger of + // driver defined maximum range for the model and user defined max range + 1m + float model_dist_max_cm = (model_type == BENEWAKE_TFmini) ? BENEWAKE_TFMINI_OUT_OF_RANGE_CM : BENEWAKE_TF02_OUT_OF_RANGE_CM; + reading_cm = MAX(model_dist_max_cm, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM); return true; }