yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

distance_sensor/vl53l0x: move to PX4Rangefinder and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2020-03-30 16:45:59 -04:00
parent 3f15ccd2dd
commit 1cc8e4dc1a
7 changed files with 179 additions and 313 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
boards/bitcraze/crazyflie/default.cmake
View File

@@ -8,7 +8,7 @@ px4_add_board(
ROMFSROOT px4fmu_common ROMFSROOT px4fmu_common
DRIVERS DRIVERS
barometer/lps25h barometer/lps25h
distance_sensor/vl53lxx distance_sensor/vl53l0x
gps gps
imu/mpu9250 imu/mpu9250
optical_flow/pmw3901 optical_flow/pmw3901

2
boards/bitcraze/crazyflie/init/rc.board_sensors
View File

@@ -12,4 +12,4 @@ mpu9250 -I -R 12 start
lps25h -I start lps25h -I start
# Optical flow deck # Optical flow deck
vl53lxx start -X vl53l0x start -X

2
src/drivers/distance_sensor/CMakeLists.txt
View File

@@ -44,4 +44,4 @@ add_subdirectory(srf02)
add_subdirectory(teraranger) add_subdirectory(teraranger)
add_subdirectory(tfmini) add_subdirectory(tfmini)
add_subdirectory(ulanding_radar) add_subdirectory(ulanding_radar)
add_subdirectory(vl53lxx) add_subdirectory(vl53l0x)

12
src/drivers/distance_sensor/vl53lxx/CMakeLists.txt → src/drivers/distance_sensor/vl53l0x/CMakeLists.txt
View File

@@ -32,10 +32,12 @@
############################################################################ ############################################################################
px4_add_module( px4_add_module(
MODULE drivers__vl53lxx MODULE drivers__distance_sensor__vl53l0x
MAIN vl53lxx MAIN vl53l0x
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS SRCS
vl53lxx.cpp VL53L0X.cpp
VL53L0X.hpp
DEPENDS
drivers_rangefinder
px4_work_queue
) )

356
src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp → src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp
View File

@@ -31,32 +31,9 @@
* *
****************************************************************************/ ****************************************************************************/
/** #include "VL53L0X.hpp"
* @file vl53lxx.cpp
* @author Daniele Pettenuzzo
*
* Driver for the vl53lxx ToF Sensor from ST Microelectronics connected via I2C.
*/
#include <stdlib.h> /* VL53L0X Registers addresses */
#include <string.h>
#include <containers/Array.hpp>
#include <drivers/device/i2c.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include <uORB/topics/distance_sensor.h>
/* Configuration Constants */
#define VL53LXX_BASEADDR 0x29
#define VL53LXX_DEVICE_PATH "/dev/vl53lxx"
/* VL53LXX Registers addresses */
#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89 #define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89
#define MSRC_CONFIG_CONTROL_REG 0x60 #define MSRC_CONFIG_CONTROL_REG 0x60
#define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG 0x44 #define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG 0x44
@@ -74,206 +51,62 @@
#define GPIO_HV_MUX_ACTIVE_HIGH_REG 0x84 #define GPIO_HV_MUX_ACTIVE_HIGH_REG 0x84
#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B #define SYSTEM_INTERRUPT_CLEAR_REG 0x0B
#define RESULT_RANGE_STATUS_REG 0x14 #define RESULT_RANGE_STATUS_REG 0x14
#define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0 #define VL53L0X_RA_IDENTIFICATION_MODEL_ID 0xC0
#define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA #define VL53L0X_IDENTIFICATION_MODEL_ID 0xEEAA
#define VL53LXX_US 1000 // 1ms #define VL53L0X_US 1000 // 1ms
#define VL53LXX_SAMPLE_RATE 50000 // 50ms #define VL53L0X_SAMPLE_RATE 50000 // 50ms
#define VL53LXX_MAX_RANGING_DISTANCE 2.0f #define VL53L0X_BUS_CLOCK 400000 // 400kHz bus clock speed
#define VL53LXX_MIN_RANGING_DISTANCE 0.0f
#define VL53LXX_BUS_CLOCK 400000 // 400kHz bus clock speed VL53L0X::VL53L0X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
I2C("VL53L0X", nullptr, bus, address, bus_frequency),
class VL53LXX : public device::I2C, public I2CSPIDriver<VL53LXX>
{
public:
VL53LXX(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
int address = VL53LXX_BASEADDR);
virtual ~VL53LXX();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual int init() override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_status() override;
virtual ssize_t read(device::file_t *file_pointer, char *buffer, size_t buflen);
/**
* Initialise the automatic measurement state machine and start it.
*/
void start();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void RunImpl();
protected:
virtual int probe() override;
private:
/**
* Collects the most recent sensor measurement data from the i2c bus.
*/
int collect();
/**
* Sends an i2c measure command to the sensor.
*/
int measure();
int readRegister(const uint8_t reg_address, uint8_t &value);
int readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length);
int writeRegister(const uint8_t reg_address, const uint8_t value);
int writeRegisterMulti(const uint8_t reg_address, const uint8_t *value, const uint8_t length);
int sensorInit();
int sensorTuning();
int singleRefCalibration(const uint8_t byte);
int spadCalculations();
bool _collect_phase{false};
bool _measurement_started{false};
bool _new_measurement{true};
int _class_instance{-1};
int _measure_interval{VL53LXX_SAMPLE_RATE};
int _orb_class_instance{-1};
uint8_t _rotation{0};
uint8_t _stop_variable{0};
orb_advert_t _distance_sensor_topic{nullptr};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "vl53lxx_com_err")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "vl53lxx_read")};
ringbuffer::RingBuffer *_reports{nullptr};
};
VL53LXX::VL53LXX(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_rotation(rotation) _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation)
{ {
// VL53L0X typical range 0-2 meters with 25 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));
// Allow 3 retries as the device typically misses the first measure attempts. // Allow 3 retries as the device typically misses the first measure attempts.
I2C::_retries = 3; I2C::_retries = 3;
} }
VL53LXX::~VL53LXX() VL53L0X::~VL53L0X()
{ {
// Free any existing reports.
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
}
// Unadvertise uORB topics.
if (_distance_sensor_topic != nullptr) {
orb_unadvertise(_distance_sensor_topic);
}
// free perf counters // free perf counters
perf_free(_sample_perf); perf_free(_sample_perf);
perf_free(_comms_errors); perf_free(_comms_errors);
} }
int int VL53L0X::collect()
VL53LXX::collect()
{ {
// Read from the sensor. // Read from the sensor.
uint8_t val[2] = {}; uint8_t val[2] {};
perf_begin(_sample_perf); perf_begin(_sample_perf);
_collect_phase = false; _collect_phase = false;
int ret = transfer(nullptr, 0, &val[0], 2); const hrt_abstime timestamp_sample = hrt_absolute_time();
if (ret != PX4_OK) { if (transfer(nullptr, 0, &val[0], 2) != PX4_OK) {
DEVICE_LOG("error reading from sensor: %d", ret);
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
return ret; return PX4_ERROR;
} }
perf_end(_sample_perf);
uint16_t distance_mm = (val[0] << 8) | val[1]; uint16_t distance_mm = (val[0] << 8) | val[1];
float distance_m = distance_mm / 1000.f; float distance_m = distance_mm / 1000.f;
struct distance_sensor_s report; _px4_rangefinder.update(timestamp_sample, distance_m);
report.timestamp = hrt_absolute_time();
report.current_distance = distance_m;
report.id = 0; // TODO: set propoer ID
report.max_distance = VL53LXX_MAX_RANGING_DISTANCE;
report.min_distance = VL53LXX_MIN_RANGING_DISTANCE;
report.orientation = _rotation;
report.signal_quality = -1;
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.variance = 0.0f;
// Publish the report data if we have a valid topic.
if (_class_instance == CLASS_DEVICE_PRIMARY) {
orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report,
&_orb_class_instance, ORB_PRIO_DEFAULT);
}
_reports->force(&report);
// Notify anyone waiting for data.
poll_notify(POLLIN);
perf_end(_sample_perf);
return PX4_OK; return PX4_OK;
} }
int int VL53L0X::measure()
VL53LXX::init()
{
set_device_address(VL53LXX_BASEADDR);
// Initialize I2C (and probe) first.
if (I2C::init() != OK) {
return PX4_ERROR;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
return PX4_ERROR;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
// Get a publish handle on the obstacle distance topic.
distance_sensor_s report {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &report,
&_orb_class_instance, ORB_PRIO_DEFAULT);
if (_distance_sensor_topic == nullptr) {
PX4_ERR("failed to create distance_sensor object");
return PX4_ERROR;
}
return PX4_OK;
}
int
VL53LXX::measure()
{ {
uint8_t wait_for_measurement = 0; uint8_t wait_for_measurement = 0;
uint8_t system_start = 0; uint8_t system_start = 0;
@@ -298,13 +131,12 @@ VL53LXX::measure()
readRegister(SYSRANGE_START_REG, system_start); readRegister(SYSRANGE_START_REG, system_start);
if ((system_start & 0x01) == 1) { if ((system_start & 0x01) == 1) {
ScheduleDelayed(VL53LXX_US); ScheduleDelayed(VL53L0X_US);
return PX4_OK; return PX4_OK;
} else { } else {
_measurement_started = true; _measurement_started = true;
} }
} }
if (!_collect_phase && !_measurement_started) { if (!_collect_phase && !_measurement_started) {
@@ -312,7 +144,7 @@ VL53LXX::measure()
readRegister(SYSRANGE_START_REG, system_start); readRegister(SYSRANGE_START_REG, system_start);
if ((system_start & 0x01) == 1) { if ((system_start & 0x01) == 1) {
ScheduleDelayed(VL53LXX_US); ScheduleDelayed(VL53L0X_US);
return PX4_OK; return PX4_OK;
} else { } else {
@@ -323,7 +155,7 @@ VL53LXX::measure()
readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement); readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement);
if ((wait_for_measurement & 0x07) == 0) { if ((wait_for_measurement & 0x07) == 0) {
ScheduleDelayed(VL53LXX_US); // reschedule every 1 ms until measurement is ready ScheduleDelayed(VL53L0X_US); // reschedule every 1 ms until measurement is ready
return PX4_OK; return PX4_OK;
} }
@@ -340,18 +172,15 @@ VL53LXX::measure()
return PX4_OK; return PX4_OK;
} }
void void VL53L0X::print_status()
VL53LXX::print_status()
{ {
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
PX4_INFO("measure interval: %u msec", _measure_interval / 1000); _px4_rangefinder.print_status();
_reports->print_info("report queue");
} }
int int VL53L0X::probe()
VL53LXX::probe()
{ {
if (sensorInit() == PX4_OK) { if (sensorInit() == PX4_OK) {
return PX4_OK; return PX4_OK;
@@ -361,65 +190,7 @@ VL53LXX::probe()
return -EIO; return -EIO;
} }
ssize_t int VL53L0X::readRegister(const uint8_t reg_address, uint8_t &value)
VL53LXX::read(device::file_t *file_pointer, char *buffer, size_t buffer_length)
{
size_t buffer_size = 0;
unsigned int count = buffer_length / sizeof(struct distance_sensor_s);
struct distance_sensor_s *read_buffer = reinterpret_cast<distance_sensor_s *>(buffer);
// Buffer must be large enough.
if (count < 1) {
return -ENOSPC;
}
// If automatic measurement is enabled.
if (_measure_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(read_buffer)) {
buffer_size += sizeof(*read_buffer);
read_buffer++;
}
}
// If there was no data, warn the caller.
if (buffer_size == 0) {
return -EAGAIN;
}
}
// Flush the report ring buffer.
_reports->flush();
// Trigger a measurement.
if (measure() != PX4_OK) {
return -EIO;
}
// Wait for the measurement to complete.
usleep(_measure_interval);
// Run the collection phase.
if (collect() != PX4_OK) {
return -EIO;
}
// State machine will have generated a report, copy it out.
if (_reports->get(read_buffer)) {
buffer_size = sizeof(*read_buffer);
}
return buffer_size;
}
int
VL53LXX::readRegister(const uint8_t reg_address, uint8_t &value)
{ {
// Write register address to the sensor. // Write register address to the sensor.
int ret = transfer(&reg_address, sizeof(reg_address), nullptr, 0); int ret = transfer(&reg_address, sizeof(reg_address), nullptr, 0);
@@ -440,9 +211,7 @@ VL53LXX::readRegister(const uint8_t reg_address, uint8_t &value)
return PX4_OK; return PX4_OK;
} }
int VL53L0X::readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length)
int
VL53LXX::readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length)
{ {
// Write register address to the sensor. // Write register address to the sensor.
int ret = transfer(&reg_address, 1, nullptr, 0); int ret = transfer(&reg_address, 1, nullptr, 0);
@@ -463,8 +232,7 @@ VL53LXX::readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint
return PX4_OK; return PX4_OK;
} }
void void VL53L0X::RunImpl()
VL53LXX::RunImpl()
{ {
measure(); measure();
@@ -475,12 +243,11 @@ VL53LXX::RunImpl()
collect(); collect();
ScheduleDelayed(_measure_interval); ScheduleDelayed(VL53L0X_SAMPLE_RATE);
} }
} }
int int VL53L0X::spadCalculations()
VL53LXX::spadCalculations()
{ {
uint8_t val = 0; uint8_t val = 0;
uint8_t spad_count = 0; uint8_t spad_count = 0;
@@ -568,8 +335,7 @@ VL53LXX::spadCalculations()
return OK; return OK;
} }
int int VL53L0X::sensorInit()
VL53LXX::sensorInit()
{ {
uint8_t val = 0; uint8_t val = 0;
@@ -618,8 +384,7 @@ VL53LXX::sensorInit()
return PX4_OK; return PX4_OK;
} }
int int VL53L0X::sensorTuning()
VL53LXX::sensorTuning()
{ {
// Magic register settings taken from the ST Micro API. // Magic register settings taken from the ST Micro API.
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
@@ -706,8 +471,7 @@ VL53LXX::sensorTuning()
return PX4_OK; return PX4_OK;
} }
int int VL53L0X::singleRefCalibration(const uint8_t byte)
VL53LXX::singleRefCalibration(const uint8_t byte)
{ {
uint8_t val = 0; uint8_t val = 0;
@@ -724,23 +488,15 @@ VL53LXX::singleRefCalibration(const uint8_t byte)
return PX4_OK; return PX4_OK;
} }
void void VL53L0X::start()
VL53LXX::start()
{ {
// Flush the report ring buffer.
_reports->flush();
// Schedule the first cycle. // Schedule the first cycle.
ScheduleNow(); ScheduleNow();
} }
int int VL53L0X::writeRegister(const uint8_t reg_address, const uint8_t value)
VL53LXX::writeRegister(const uint8_t reg_address, const uint8_t value)
{ {
uint8_t cmd[2] = {}; uint8_t cmd[2] {reg_address, value};
cmd[0] = reg_address;
cmd[1] = value;
int ret = transfer(&cmd[0], 2, nullptr, 0); int ret = transfer(&cmd[0], 2, nullptr, 0);
if (ret != PX4_OK) { if (ret != PX4_OK) {
@@ -751,18 +507,15 @@ VL53LXX::writeRegister(const uint8_t reg_address, const uint8_t value)
return PX4_OK; return PX4_OK;
} }
int VL53L0X::writeRegisterMulti(const uint8_t reg_address, const uint8_t *value, const uint8_t length)
int
VL53LXX::writeRegisterMulti(const uint8_t reg_address, const uint8_t *value,
const uint8_t length) /* bytes are send in order as they are in the array */
{ {
if (length > 6 || length < 1) { if (length > 6 || length < 1) {
DEVICE_LOG("VL53LXX::writeRegisterMulti length out of range"); DEVICE_LOG("VL53L0X::writeRegisterMulti length out of range");
return PX4_ERROR; return PX4_ERROR;
} }
/* be careful: for uint16_t to send first higher byte */ /* be careful: for uint16_t to send first higher byte */
uint8_t cmd[7] = {}; uint8_t cmd[7] {};
cmd[0] = reg_address; cmd[0] = reg_address;
memcpy(&cmd[1], &value[0], length); memcpy(&cmd[1], &value[0], length);
@@ -777,10 +530,9 @@ VL53LXX::writeRegisterMulti(const uint8_t reg_address, const uint8_t *value,
return PX4_OK; return PX4_OK;
} }
void void VL53L0X::print_usage()
VL53LXX::print_usage()
{ {
PRINT_MODULE_USAGE_NAME("vl53lxx", "driver"); PRINT_MODULE_USAGE_NAME("vl53l0x", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
@@ -788,10 +540,10 @@ VL53LXX::print_usage()
PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
} }
I2CSPIDriverBase *VL53LXX::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, I2CSPIDriverBase *VL53L0X::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance) int runtime_instance)
{ {
VL53LXX *instance = new VL53LXX(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); VL53L0X *instance = new VL53L0X(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency);
if (instance == nullptr) { if (instance == nullptr) {
PX4_ERR("alloc failed"); PX4_ERR("alloc failed");
@@ -807,14 +559,10 @@ I2CSPIDriverBase *VL53LXX::instantiate(const BusCLIArguments &cli, const BusInst
return instance; return instance;
} }
extern "C" __EXPORT int vl53l0x_main(int argc, char *argv[])
/**
* Driver 'main' command.
*/
extern "C" __EXPORT int vl53lxx_main(int argc, char *argv[])
{ {
int ch; int ch;
using ThisDriver = VL53LXX; using ThisDriver = VL53L0X;
BusCLIArguments cli{true, false}; BusCLIArguments cli{true, false};
cli.default_i2c_frequency = 400000; cli.default_i2c_frequency = 400000;
cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
@@ -834,7 +582,7 @@ extern "C" __EXPORT int vl53lxx_main(int argc, char *argv[])
return -1; return -1;
} }
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_VL53LXX); BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_VL53L0X);
if (!strcmp(verb, "start")) { if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator); return ThisDriver::module_start(cli, iterator);

116
src/drivers/distance_sensor/vl53l0x/VL53L0X.hpp Normal file
View File

@@ -0,0 +1,116 @@
/****************************************************************************
*
* Copyright (c) 2018-2020 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file VL53L0X.hpp
*
* Driver for the ST VL53L0X ToF Sensor connected via I2C.
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
/* Configuration Constants */
#define VL53L0X_BASEADDR 0x29
class VL53L0X : public device::I2C, public I2CSPIDriver<VL53L0X>
{
public:
VL53L0X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
int address = VL53L0X_BASEADDR);
~VL53L0X() override;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_status() override;
/**
* Initialise the automatic measurement state machine and start it.
*/
void start();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void RunImpl();
private:
int probe() override;
/**
* Collects the most recent sensor measurement data from the i2c bus.
*/
int collect();
/**
* Sends an i2c measure command to the sensor.
*/
int measure();
int readRegister(const uint8_t reg_address, uint8_t &value);
int readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length);
int writeRegister(const uint8_t reg_address, const uint8_t value);
int writeRegisterMulti(const uint8_t reg_address, const uint8_t *value, const uint8_t length);
int sensorInit();
int sensorTuning();
int singleRefCalibration(const uint8_t byte);
int spadCalculations();
PX4Rangefinder _px4_rangefinder;
bool _collect_phase{false};
bool _measurement_started{false};
bool _new_measurement{true};
uint8_t _stop_variable{0};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
};

2
src/drivers/drv_sensor.h
View File

@@ -137,7 +137,7 @@
#define DRV_DIST_DEVTYPE_SF1XX 0x73 #define DRV_DIST_DEVTYPE_SF1XX 0x73
#define DRV_DIST_DEVTYPE_SRF02 0x74 #define DRV_DIST_DEVTYPE_SRF02 0x74
#define DRV_DIST_DEVTYPE_TERARANGER 0x75 #define DRV_DIST_DEVTYPE_TERARANGER 0x75
#define DRV_DIST_DEVTYPE_VL53LXX 0x76 #define DRV_DIST_DEVTYPE_VL53L0X 0x76
#define DRV_POWER_DEVTYPE_INA226 0x77 #define DRV_POWER_DEVTYPE_INA226 0x77
#define DRV_POWER_DEVTYPE_VOXLPM 0x78 #define DRV_POWER_DEVTYPE_VOXLPM 0x78
#define DRV_LED_DEVTYPE_BLINKM 0x79 #define DRV_LED_DEVTYPE_BLINKM 0x79