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".

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++;

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);