AP_RangeFinder: TeraRangerI2C redefining the output distance logic with OutOfRange cases

libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp

@@ -28,6 +28,8 @@ extern const AP_HAL::HAL& hal;
 #define TR_WHOAMI  0x01
 #define TR_WHOAMI_VALUE 0xA1
 
+#define TR_OUT_OF_RANGE_ADD_CM 100 //cm
+
 AP_RangeFinder_TeraRangerI2C::AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFinder_State &_state,
                                                            AP_RangeFinder_Params &_params,
                                                            AP_HAL::OwnPtr<AP_HAL::I2CDevice> i2c_dev)
@@ -132,20 +134,21 @@ bool AP_RangeFinder_TeraRangerI2C::collect_raw(uint16_t &raw_distance)
 // Checks for error code and if correct converts to cm
 bool AP_RangeFinder_TeraRangerI2C::process_raw_measure(uint16_t raw_distance, uint16_t &output_distance_cm)
 {
-  // Check for error codes
-  if (raw_distance == 0xFFFF) {
-      // Too far away is unreliable so we dont enforce max range here
-      return false;
-  } else if (raw_distance == 0x0000) {
-      // Too close
-      return false;
-  } else if (raw_distance == 0x0001) {
-      // Unable to measure
-      return false;
-  } else {
-    output_distance_cm = raw_distance/10; // Conversion to centimeters
+    // Check for error codes
+    if (raw_distance == 0xFFFF) {
+        // Too far away is unreliable so we dont enforce max range here
+        return false;
+    } 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;
+    } else {
+        output_distance_cm = raw_distance/10; // Conversion to centimeters
+    }
     return true;
-  }
 }
 
 /*