fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 13:34:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/distance_sensor/vl53l1x: added multiple region of interest settings (#18073)

Browse Source 
Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
This commit is contained in:
Andrew Brahim 2021-08-23 13:15:41 -05:00 committed by GitHub
parent a4527485a3
commit 04adf6c9be
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 90 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,7 +36,28 @@
#include "vl53l1x.hpp"
#define VL53L1X_SAMPLE_RATE 20 // ms
#define VL53L1X_DELAY 20 //ms
#define VL53L1X_SAMPLE_RATE 200 // ms, default
#define VL53L1X_INTER_MEAS_MS 200 // ms
#define VL53L1X_SHORT_RANGE 1 // sub-2 meter distance mode
#define VL53L1X_LONG_RANGE 2 // sub-4 meter distance mode
#define VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS 13 // region of interest out of bounds error
#define VL53L1X_RANGE_STATUS_OK 0 // range status ok
#define VL53L1X_ROI_FAR_RIGHT 247 // ROI far right of optical center
#define VL53L1X_ROI_MID_RIGHT 215 // ROI middle right of optical center
#define VL53L1X_ROI_CENTER_LEFT 183 // ROI optical center left
#define VL53L1X_ROI_MID_LEFT 167 // ROI middle left of optical center
#define VL53L1X_ROI_FAR_LEFT 151 // ROI far left of optical center
#define VL53L1X_ROI_FAR_RIGHT_LO 10 // ROI far right lo
#define VL53L1X_ROI_MID_RIGHT_LO 42 // ROI middle right of optical center
#define VL53L1X_ROI_CENTER_LEFT_LO 74 // ROI optical center left lo
#define VL53L1X_ROI_MID_LEFT_LO 90 // ROI middle left of optical center
#define VL53L1X_ROI_FAR_LEFT_LO 106 // ROI far left lo
#define VL53L1X_ROI_FAR_RIGHT_HI 243 // ROI far right hi
#define VL53L1X_ROI_MID_RIGHT_HI 211 // ROI middle right of optical center
#define VL53L1X_ROI_CENTER_LEFT_HI 179 // ROI optical center left hi
#define VL53L1X_ROI_MID_LEFT_HI 163 // ROI middle left of optical center
#define VL53L1X_ROI_FAR_LEFT_HI 147 // ROI far left hi
/* ST */
const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = {
@ -140,15 +161,36 @@ static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
/* end ST */
// ROI array assignment
uint8_t roi_center[] = {VL53L1X_ROI_FAR_RIGHT, VL53L1X_ROI_MID_RIGHT, VL53L1X_ROI_CENTER_LEFT, VL53L1X_ROI_MID_LEFT, VL53L1X_ROI_FAR_LEFT, VL53L1X_ROI_FAR_RIGHT_LO, VL53L1X_ROI_MID_RIGHT_LO, VL53L1X_ROI_CENTER_LEFT_LO, VL53L1X_ROI_MID_LEFT_LO, VL53L1X_ROI_FAR_LEFT_LO, VL53L1X_ROI_FAR_RIGHT_HI, VL53L1X_ROI_MID_RIGHT_HI, VL53L1X_ROI_CENTER_LEFT_HI, VL53L1X_ROI_MID_LEFT_HI, VL53L1X_ROI_FAR_LEFT_HI};
VL53L1X::VL53L1X(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config),
_px4_rangefinder(get_device_id(), config.rotation)
{
// VL53L1X typical range 0-2 meters with 25 degree field of view
// Set distance mode (1 for ~2m ranging, 2 for ~4m ranging
_distance_mode = VL53L1X_LONG_RANGE;
// VL53L1X typical range 0-4 meters with 27 degree field of view
_px4_rangefinder.set_min_distance(0.f);
_px4_rangefinder.set_max_distance(2.f);
_px4_rangefinder.set_fov(math::radians(25.f));
if (_distance_mode == VL53L1X_SHORT_RANGE) {
_px4_rangefinder.set_max_distance(2.f);
} else {
_px4_rangefinder.set_max_distance(4.f);
}
_px4_rangefinder.set_fov(math::radians(27.f));
// Zone limits
_zone_limit = sizeof(roi_center) / sizeof(uint8_t);
// Zone index
_zone_index = 0;
// Allow 3 retries as the device typically misses the first measure attempts.
I2C::_retries = 3;
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_VL53L1X);
}
@ -163,7 +205,7 @@ VL53L1X::~VL53L1X()
int VL53L1X::collect()
{
uint8_t ret = 0;
uint8_t rangeStatus;
uint8_t rangeStatus = VL53L1X_RANGE_STATUS_OK;
uint16_t distance_mm = 0;
perf_begin(_sample_perf);
@ -172,7 +214,7 @@ int VL53L1X::collect()
ret = VL53L1X_GetRangeStatus(&rangeStatus);
if ((ret != PX4_OK) | (rangeStatus != PX4_OK)) {
if ((ret != PX4_OK) | (rangeStatus == VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS)) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return PX4_ERROR;
@ -220,20 +262,24 @@ int VL53L1X::probe()
void VL53L1X::RunImpl()
{
uint8_t dataReady = 0;
VL53L1X_CheckForDataReady(&dataReady);
if (dataReady == 1) {
collect();
}
ScheduleDelayed(VL53L1X_SAMPLE_RATE);
ScheduleDelayed(VL53L1X_DELAY);
// zone modulus increment
_zone_index = (_zone_index + 1) % _zone_limit;
// Set the ROI center based on zone incrementation
VL53L1X_SetROICenter(roi_center[_zone_index]);
}
int VL53L1X::init()
{
int ret = PX4_OK;
ret = device::I2C::init();
if (ret != PX4_OK) {
@ -241,9 +287,16 @@ int VL53L1X::init()
return PX4_ERROR;
}
// Spad width (x) & height (y)
uint8_t x = 4;
uint8_t y = 4;
ret |= VL53L1X_SensorInit();
ret |= VL53L1X_ConfigBig(2, VL53L1X_SAMPLE_RATE);
ret |= VL53L1X_SetInterMeasurementInMs(VL53L1X_SAMPLE_RATE);
ret |= VL53L1X_ConfigBig(_distance_mode, VL53L1X_SAMPLE_RATE);
ret |= VL53L1X_SetROI(x, y);
ret |= VL53L1X_SetROICenter(roi_center[_zone_index]);
ret |= VL53L1X_SetInterMeasurementInMs(VL53L1X_INTER_MEAS_MS);
ret |= VL53L1X_StartRanging();
if (ret != PX4_OK) {
@ -478,6 +531,18 @@ int8_t VL53L1X::VL53L1X_StopRanging()
return status;
}
int8_t VL53L1X::VL53L1X_SetROI(uint16_t x, uint16_t y)
{
// set ROI size x and y
return VL53L1_WrByte(ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, (y - 1) << 4 | (x - 1));
}
int8_t VL53L1X::VL53L1X_SetROICenter(uint8_t zone)
{
// Set ROI spad center
return VL53L1_WrByte(VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, zone);
}
/**
* @brief This function programs the Intermeasurement period in ms\n
* Intermeasurement period must be >/= timing budget.

14
src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp
View File

@ -94,6 +94,8 @@
/* Configuration Constants */
#define VL53L1X_BASEADDR 0x29
#define VL53L1X_SHORT_RANGE 1 // sub-2 meter distance mode
#define VL53L1X_LONG_RANGE 2 // sub-4 meter distance mode
class VL53L1X : public device::I2C, public I2CSPIDriver<VL53L1X>
{
@ -117,6 +119,15 @@ public:
*/
void RunImpl();
// Distance mode member variable
uint16_t _distance_mode{VL53L1X_LONG_RANGE};
// Zone index member variable
uint8_t _zone_index{0};
// Zone limit member variable
uint8_t _zone_limit{0};
private:
int probe() override;
@ -143,7 +154,8 @@ private:
// Remove if config store
int8_t VL53L1X_ConfigBig(uint16_t DM, uint16_t TimingBudgetInMs);
int8_t VL53L1X_SetInterMeasurementInMs(uint32_t InterMeasurementInMs);
int8_t VL53L1X_SetROICenter(uint8_t data);
int8_t VL53L1X_SetROI(uint16_t x, uint16_t y);
PX4Rangefinder _px4_rangefinder;
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};