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;
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;
}
}

1
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -77,6 +77,7 @@ public:
BenewakeTFminiPlus = 25,
Lanbao = 26,
BenewakeTF03 = 27,
VL53L1X_Short = 28,
};
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
// @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),

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
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

7
libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h
View File

@ -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;