From 0e63a833e9b2e0be92ece618e73e98460b55d283 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 28 Sep 2020 17:45:45 -0700 Subject: [PATCH] AP_RangeFinder: TFMiniPlus: fix out-of-range returned a no-data Other drivers consider that they received data even if the value is reported as "out of range" by sensor. On the I2C driver for TFMiniPlus we considered this case, too. However when the signal strength is very low (and thus the distance would likely be out of range), we would end up ignoring the new sample. With enough samples without any value this would lead the status to turn to "NoData". --- .../AP_RangeFinder_Benewake_TFMiniPlus.cpp | 22 +++++++++++++------ .../AP_RangeFinder_Benewake_TFMiniPlus.h | 2 +- 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp index 706f073f1d..8ec86ba392 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp @@ -149,20 +149,26 @@ void AP_RangeFinder_Benewake_TFMiniPlus::update() } } -bool AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(le16_t distance_raw, le16_t strength_raw, +void AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(le16_t distance_raw, le16_t strength_raw, uint16_t &output_distance_cm) { uint16_t strength = le16toh(strength_raw); + const uint16_t MAX_DIST_CM = 1200; + const uint16_t MIN_DIST_CM = 10; output_distance_cm = le16toh(distance_raw); - if (strength < 100 || strength == 0xFFFF) { - return false; + if (strength < 100 || strength == 0xFFFF || output_distance_cm > MAX_DIST_CM) { + /* + * From manual: "when the signal strength is lower than 100 or equal to + * 65535, the detection is unreliable, TFmini Plus will set distance + * 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 = constrain_int16(output_distance_cm, 10, 1200); - - return true; + 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) @@ -203,7 +209,9 @@ void AP_RangeFinder_Benewake_TFMiniPlus::timer() if (u.val.header1 != 0x59 || u.val.header2 != 0x59 || !check_checksum(u.arr, sizeof(u))) return; - if (process_raw_measure(u.val.distance, u.val.strength, distance)) { + process_raw_measure(u.val.distance, u.val.strength, distance); + + { WITH_SEMAPHORE(_sem); accum.sum += distance; accum.count++; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h index 84ce2e420b..b19a44c087 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h @@ -48,7 +48,7 @@ private: bool init(); void timer(); - bool process_raw_measure(le16_t distance_raw, le16_t strength_raw, + void process_raw_measure(le16_t distance_raw, le16_t strength_raw, uint16_t &output_distance_cm); bool check_checksum(uint8_t *arr, int pkt_len);