AP_RangeFinder: Add TF03-180

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

@@ -26,6 +26,7 @@ extern const AP_HAL::HAL& hal;
 #define BENEWAKE_DIST_MAX_CM 32768
 #define BENEWAKE_TFMINI_OUT_OF_RANGE_CM 1200
 #define BENEWAKE_TF02_OUT_OF_RANGE_CM 2200
+#define BENEWAKE_TF03_OUT_OF_RANGE_CM 18000
 #define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
 
 // format of serial packets received from benewake lidar
@@ -37,9 +38,12 @@ extern const AP_HAL::HAL& hal;
 // byte 2               DIST_L          Distance (in cm) low 8 bits
 // byte 3               DIST_H          Distance (in cm) high 8 bits
 // byte 4               STRENGTH_L      Strength low 8 bits
+// bute 4 (TF03)        (Reserved)
 // byte 5               STRENGTH_H      Strength high 8 bits
+// bute 5 (TF03)        (Reserved)
 // byte 6 (TF02)        SIG             Reliability in 8 levels, 7 & 8 means reliable
 // byte 6 (TFmini)      Distance Mode   0x02 for short distance (mm), 0x07 for long distance (cm)
+// byte 6 (TF03)        (Reserved)
 // byte 7 (TF02 only)   TIME            Exposure time in two levels 0x03 and 0x06
 // byte 8               Checksum        Checksum byte, sum of bytes 0 to bytes 7
 
@@ -121,7 +125,7 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
                     if (dist >= BENEWAKE_DIST_MAX_CM) {
                         // this reading is out of range
                         count_out_of_range++;
-                    } else if (model_type == BENEWAKE_TFmini) {
+                    } else if (model_type == BENEWAKE_TFmini || model_type == BENEWAKE_TF03) {
                         // no signal byte from TFmini so add distance to sum
                         sum_cm += dist;
                         count++;
@@ -152,7 +156,18 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
     if (count_out_of_range > 0) {
         // 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;
+        float model_dist_max_cm = 0.0f;
+        switch (model_type) {
+        case BENEWAKE_TFmini:
+            model_dist_max_cm = BENEWAKE_TFMINI_OUT_OF_RANGE_CM;
+            break;
+        case BENEWAKE_TF02:
+            model_dist_max_cm = BENEWAKE_TF02_OUT_OF_RANGE_CM;
+            break;
+        case BENEWAKE_TF03:
+            model_dist_max_cm = BENEWAKE_TF03_OUT_OF_RANGE_CM;
+            break;
+        }
         reading_cm = MAX(model_dist_max_cm, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM);
         return true;
     }

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h

@@ -10,7 +10,8 @@ public:
 
     enum benewake_model_type {
         BENEWAKE_TF02 = 0,
-        BENEWAKE_TFmini = 1
+        BENEWAKE_TFmini = 1,
+        BENEWAKE_TF03 = 2,
     };
 
     // constructor

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFMiniPlus
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFMiniPlus,27:BenewakeTF03
     // @User: Standard
     AP_GROUPINFO("TYPE",    1, AP_RangeFinder_Params, type, 0),
 

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -503,6 +503,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
             drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini);
         }
         break;
+    case RangeFinder_TYPE_BenewakeTF03:
+        if (AP_RangeFinder_Benewake::detect(serial_instance)) {
+            drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF03);
+        }
+        break;
     case RangeFinder_TYPE_PWM:
         if (AP_RangeFinder_PWM::detect()) {
             drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height);

libraries/AP_RangeFinder/RangeFinder.h

@@ -73,6 +73,7 @@ public:
         RangeFinder_TYPE_UAVCAN = 24,
         RangeFinder_TYPE_BenewakeTFminiPlus = 25,
         RangeFinder_TYPE_Lanbao = 26,
+        RangeFinder_TYPE_BenewakeTF03 = 27,
     };
 
     enum RangeFinder_Function {