From 8bd4f2b097202c273efc3652770718787da76be1 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 10 Dec 2019 20:10:02 -0700 Subject: [PATCH] AP_RangeFinder: Allow the VL53L1X to be put into short range mode --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 5 ++++- libraries/AP_RangeFinder/AP_RangeFinder.h | 1 + libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp | 14 +++++++------- libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h | 7 ++++--- 5 files changed, 17 insertions(+), 12 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 3eb1abe249..eee8747b5e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/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; } } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index b7d634f895..548ecf7c32 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -77,6 +77,7 @@ public: BenewakeTFminiPlus = 25, Lanbao = 26, BenewakeTF03 = 27, + VL53L1X_Short = 28, }; enum class Function { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index e00699b0a6..720a77cb18 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/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), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp index 8c5ced2d2d..df3f3a6c6a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp +++ b/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 dev) +AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr 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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h index d047ff1639..742d0b053f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h +++ b/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 _dev); + static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr _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 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;