AP_RangeFinder: Allow the VL53L1X to be put into short range mode

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -371,13 +371,16 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         }
         break;
     case Type::VL53L0X:
+    case Type::VL53L1X_Short:
             FOREACH_I2C(i) {
                 if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance],
                                                                  hal.i2c_mgr->get_device(i, params[instance].address)))) {
                     break;
                 }
                 if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance],
-                                                                hal.i2c_mgr->get_device(i, params[instance].address)))) {
+                                                                hal.i2c_mgr->get_device(i, params[instance].address),
+                                                                _type == Type::VL53L1X_Short ?  AP_RangeFinder_VL53L1X::DistanceMode::Short :
+                                                                                                AP_RangeFinder_VL53L1X::DistanceMode::Long))) {
                     break;
                 }
             }

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -77,6 +77,7 @@ public:
         BenewakeTFminiPlus = 25,
         Lanbao = 26,
         BenewakeTF03 = 27,
+        VL53L1X_Short = 28,
     };
 
     enum class Function {

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:LidarLite-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/Plus-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFmini/Plus-I2C,27:BenewakeTF03
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-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/Plus-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFmini/Plus-I2C,27:BenewakeTF03,28:VL53L1X-ShortRange
     // @User: Standard
     AP_GROUPINFO("TYPE",    1, AP_RangeFinder_Params, type, 0),
 

libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp

@@ -42,7 +42,7 @@ AP_RangeFinder_VL53L1X::AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_
    trying to take a reading on I2C. If we get a result the sensor is
    there.
 */
-AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
+AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev, DistanceMode mode)
 {
     if (!dev) {
         return nullptr;
@@ -58,7 +58,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_
 
     sensor->dev->get_semaphore()->take_blocking();
 
-    if (!sensor->check_id() || !sensor->init()) {
+    if (!sensor->check_id() || !sensor->init(mode)) {
         sensor->dev->get_semaphore()->give();
         delete sensor;
         return nullptr;
@@ -101,7 +101,7 @@ bool AP_RangeFinder_VL53L1X::reset(void) {
 /*
   initialise sensor
  */
-bool AP_RangeFinder_VL53L1X::init()
+bool AP_RangeFinder_VL53L1X::init(DistanceMode mode)
 {
     // we need to do resets and delays in order to configure the sensor, don't do this if we are trying to fast boot
     if (hal.util->was_watchdog_armed()) {
@@ -156,7 +156,7 @@ bool AP_RangeFinder_VL53L1X::init()
           write_register16(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 200 << 8) &&
           write_register(DSS_CONFIG__ROI_MODE_CONTROL, 2) && // REQUESTED_EFFFECTIVE_SPADS
           read_register16(MM_CONFIG__OUTER_OFFSET_MM, mm_config_outer_offset_mm) &&
-          setDistanceMode(Long) &&
+          setDistanceMode(mode) &&
           setMeasurementTimingBudget(40000) &&
           // the API triggers this change in VL53L1_init_and_start_range() once a
           // measurement is started; assumes MM1 and MM2 are disabled
@@ -185,7 +185,7 @@ bool AP_RangeFinder_VL53L1X::setDistanceMode(DistanceMode distance_mode)
     }
 
     switch (distance_mode) {
-        case Short:
+      case DistanceMode::Short:
             // from VL53L1_preset_mode_standard_ranging_short_range()
 
             if (!(// timing config
@@ -203,7 +203,7 @@ bool AP_RangeFinder_VL53L1X::setDistanceMode(DistanceMode distance_mode)
 
             break;
 
-        case Medium:
+        case DistanceMode::Medium:
             // from VL53L1_preset_mode_standard_ranging()
 
             if (!(// timing config
@@ -221,7 +221,7 @@ bool AP_RangeFinder_VL53L1X::setDistanceMode(DistanceMode distance_mode)
 
             break;
 
-        case Long:
+        case DistanceMode::Long:
             // from VL53L1_preset_mode_standard_ranging_long_range()
 
             if (!(// timing config

libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h

@@ -8,8 +8,10 @@ class AP_RangeFinder_VL53L1X : public AP_RangeFinder_Backend
 {
 
 public:
+    enum class DistanceMode { Short, Medium, Long, Unknown };
+
     // static detection function
-    static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev);
+    static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev, DistanceMode mode);
 
     // update state
     void update(void) override;
@@ -1242,7 +1244,7 @@ private:
     // constructor
     AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
 
-    bool init();
+    bool init(DistanceMode mode);
     void timer();
 
     // check sensor ID
@@ -1267,7 +1269,6 @@ private:
     // used in DSS calculations
     static const uint16_t TargetRate = 0x0A00;
 
-    enum DistanceMode { Short, Medium, Long, Unknown };
     uint16_t fast_osc_frequency;
     uint16_t osc_calibrate_val;
     uint32_t sum_mm;