Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Michael du Breuil 2019-12-10 20:10:02 -07:00 committed by Andrew Tridgell
parent 224ea744e3
commit 8bd4f2b097
5 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -371,13 +371,16 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
} }
break; break;
case Type::VL53L0X: case Type::VL53L0X:
case Type::VL53L1X_Short:
FOREACH_I2C(i) { FOREACH_I2C(i) {
if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance], if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance],
hal.i2c_mgr->get_device(i, params[instance].address)))) { hal.i2c_mgr->get_device(i, params[instance].address)))) {
break; break;
} }
if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance], 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; break;
} }
} }

1
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

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

2
libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
View File

@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @Param: TYPE // @Param: TYPE
// @DisplayName: Rangefinder type // @DisplayName: Rangefinder type
// @Description: What type of rangefinder device that is connected // @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 // @User: Standard
AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0), AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0),

14
libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp
View File

@ -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 trying to take a reading on I2C. If we get a result the sensor is
there. 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) { if (!dev) {
return nullptr; return nullptr;
@ -58,7 +58,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_
sensor->dev->get_semaphore()->take_blocking(); sensor->dev->get_semaphore()->take_blocking();
if (!sensor->check_id() || !sensor->init()) { if (!sensor->check_id() || !sensor->init(mode)) {
sensor->dev->get_semaphore()->give(); sensor->dev->get_semaphore()->give();
delete sensor; delete sensor;
return nullptr; return nullptr;
@ -101,7 +101,7 @@ bool AP_RangeFinder_VL53L1X::reset(void) {
/* /*
initialise sensor 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 // 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()) { 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_register16(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 200 << 8) &&
write_register(DSS_CONFIG__ROI_MODE_CONTROL, 2) && // REQUESTED_EFFFECTIVE_SPADS write_register(DSS_CONFIG__ROI_MODE_CONTROL, 2) && // REQUESTED_EFFFECTIVE_SPADS
read_register16(MM_CONFIG__OUTER_OFFSET_MM, mm_config_outer_offset_mm) && read_register16(MM_CONFIG__OUTER_OFFSET_MM, mm_config_outer_offset_mm) &&
setDistanceMode(Long) && setDistanceMode(mode) &&
setMeasurementTimingBudget(40000) && setMeasurementTimingBudget(40000) &&
// the API triggers this change in VL53L1_init_and_start_range() once a // the API triggers this change in VL53L1_init_and_start_range() once a
// measurement is started; assumes MM1 and MM2 are disabled // measurement is started; assumes MM1 and MM2 are disabled
@ -185,7 +185,7 @@ bool AP_RangeFinder_VL53L1X::setDistanceMode(DistanceMode distance_mode)
} }
switch (distance_mode) { switch (distance_mode) {
case Short: case DistanceMode::Short:
// from VL53L1_preset_mode_standard_ranging_short_range() // from VL53L1_preset_mode_standard_ranging_short_range()
if (!(// timing config if (!(// timing config
@ -203,7 +203,7 @@ bool AP_RangeFinder_VL53L1X::setDistanceMode(DistanceMode distance_mode)
break; break;
case Medium: case DistanceMode::Medium:
// from VL53L1_preset_mode_standard_ranging() // from VL53L1_preset_mode_standard_ranging()
if (!(// timing config if (!(// timing config
@ -221,7 +221,7 @@ bool AP_RangeFinder_VL53L1X::setDistanceMode(DistanceMode distance_mode)
break; break;
case Long: case DistanceMode::Long:
// from VL53L1_preset_mode_standard_ranging_long_range() // from VL53L1_preset_mode_standard_ranging_long_range()
if (!(// timing config if (!(// timing config

7
libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h
View File

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