50 #include <px4_platform_common/getopt.h> 51 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> 55 #define VL53LXX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION 57 #define VL53LXX_BASEADDR 0x29 58 #define VL53LXX_DEVICE_PATH "/dev/vl53lxx" 61 #define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89 62 #define MSRC_CONFIG_CONTROL_REG 0x60 63 #define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG 0x44 64 #define SYSTEM_SEQUENCE_CONFIG_REG 0x01 65 #define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG 0x4F 66 #define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG 0x4E 67 #define GLOBAL_CONFIG_REF_EN_START_SELECT_REG 0xB6 68 #define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 69 #define SYSTEM_INTERRUPT_CONFIG_GPIO_REG 0x0A 70 #define SYSTEM_SEQUENCE_CONFIG_REG 0x01 71 #define SYSRANGE_START_REG 0x00 72 #define RESULT_INTERRUPT_STATUS_REG 0x13 73 #define SYSTEM_INTERRUPT_CLEAR_REG 0x0B 74 #define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 75 #define GPIO_HV_MUX_ACTIVE_HIGH_REG 0x84 76 #define SYSTEM_INTERRUPT_CLEAR_REG 0x0B 77 #define RESULT_RANGE_STATUS_REG 0x14 78 #define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0 79 #define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA 81 #define VL53LXX_US 1000 // 1ms 82 #define VL53LXX_SAMPLE_RATE 50000 // 50ms 84 #define VL53LXX_MAX_RANGING_DISTANCE 2.0f 85 #define VL53LXX_MIN_RANGING_DISTANCE 0.0f 87 #define VL53LXX_BUS_CLOCK 400000 // 400kHz bus clock speed 89 class VL53LXX :
public device::I2C,
public px4::ScheduledWorkItem
92 VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
97 virtual int init()
override;
117 virtual int probe()
override;
137 int readRegister(
const uint8_t reg_address, uint8_t &value);
138 int readRegisterMulti(
const uint8_t reg_address, uint8_t *value,
const uint8_t length);
140 int writeRegister(
const uint8_t reg_address,
const uint8_t value);
141 int writeRegisterMulti(
const uint8_t reg_address,
const uint8_t *value,
const uint8_t length);
170 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(get_device_id())),
210 int ret = transfer(
nullptr, 0, &val[0], 2);
213 DEVICE_LOG(
"error reading from sensor: %d", ret);
219 uint16_t distance_mm = (val[0] << 8) | val[1];
220 float distance_m = distance_mm / 1000.f;
230 report.
type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
273 PX4_ERR(
"failed to create distance_sensor object");
283 uint8_t wait_for_measurement = 0;
284 uint8_t system_start = 0;
305 if ((system_start & 0x01) == 1) {
319 if ((system_start & 0x01) == 1) {
330 if ((wait_for_measurement & 0x07) == 0) {
337 int ret = transfer(&cmd,
sizeof(cmd),
nullptr, 0);
354 _reports->print_info(
"report queue");
371 size_t buffer_size = 0;
390 buffer_size +=
sizeof(*read_buffer);
396 if (buffer_size == 0) {
419 buffer_size =
sizeof(*read_buffer);
429 int ret = transfer(®_address,
sizeof(reg_address),
nullptr, 0);
437 ret = transfer(
nullptr, 0, &value, 1);
452 int ret = transfer(®_address, 1,
nullptr, 0);
460 ret = transfer(
nullptr, 0, &value[0], length);
490 uint8_t spad_count = 0;
491 uint8_t ref_spad_map[6] = {};
493 bool spad_type_is_aperture =
false;
511 while (val == 0x00) {
518 spad_count = val & 0x7f;
519 spad_type_is_aperture = (val >> 7) & 0x01;
540 uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0;
541 uint8_t spads_enabled = 0;
543 for (uint8_t i = 0; i < 48; i++) {
544 if (i < first_spad_to_enable || spads_enabled == spad_count) {
545 ref_spad_map[i / 8] &= ~(1 << (i % 8));
547 }
else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1) {
611 float rate_limit = 0.1 * 65536;
612 uint8_t rate_limit_split[2] = {};
614 rate_limit_split[0] = (((uint16_t)rate_limit) >> 8);
615 rate_limit_split[1] = (uint16_t)rate_limit;
721 while ((val & 0x07) == 0) {
751 cmd[0] = reg_address;
754 int ret = transfer(&cmd[0], 2,
nullptr, 0);
767 const uint8_t length)
769 if (length > 6 || length < 1) {
770 DEVICE_LOG(
"VL53LXX::writeRegisterMulti length out of range");
776 cmd[0] = reg_address;
778 memcpy(&cmd[1], &value[0], length);
780 int ret = transfer(&cmd[0], length + 1,
nullptr, 0);
800 int start(
const uint8_t rotation);
813 if (g_dev ==
nullptr) {
814 PX4_ERR(
"driver not running");
820 PX4_INFO(
"driver reset");
852 if (g_dev !=
nullptr) {
853 PX4_ERR(
"already started");
858 g_dev =
new VL53LXX(rotation, i2c_bus);
860 if (g_dev ==
nullptr) {
866 if (g_dev->
init() != PX4_OK) {
874 PX4_INFO(
"driver started");
884 if (g_dev ==
nullptr) {
885 PX4_ERR(
"driver not running");
900 if (g_dev !=
nullptr) {
906 PX4_INFO(
"driver stopped");
921 PX4_ERR(
"%s open failed (try 'vl53lxx start' if the driver is not running)",
VL53LXX_DEVICE_PATH);
927 ssize_t num_bytes =
read(fd, &report,
sizeof(report));
929 if (num_bytes !=
sizeof(report)) {
930 PX4_ERR(
"immediate read failed");
934 print_message(report);
945 PX4_INFO(
"usage: vl53lxx command [options]");
946 PX4_INFO(
"options:");
947 PX4_INFO(
"\t-a --all available busses");
949 PX4_INFO(
"\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING);
950 PX4_INFO(
"command:");
951 PX4_INFO(
"\treset|start|status|stop|test|usage");
963 const char *myoptarg =
nullptr;
969 uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
971 bool start_all =
false;
973 while ((ch = px4_getopt(argc, argv,
"ab:R:", &myoptind, &myoptarg)) != EOF) {
980 i2c_bus = atoi(myoptarg);
984 rotation = (uint8_t)atoi(myoptarg);
988 PX4_WARN(
"Unknown option!");
993 if (myoptind >= argc) {
998 if (!strcmp(argv[myoptind],
"reset")) {
1003 if (!strcmp(argv[myoptind],
"start")) {
1013 if (!strcmp(argv[myoptind],
"status")) {
1018 if (!strcmp(argv[myoptind],
"stop")) {
1023 if (!strcmp(argv[myoptind],
"test")) {
#define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG
#define VL53LXX_MIN_RANGING_DISTANCE
#define VL53LXX_MAX_RANGING_DISTANCE
#define SYSTEM_INTERRUPT_CONFIG_GPIO_REG
int status()
Print the driver status.
#define RANGE_FINDER_BASE_DEVICE_PATH
measure the time elapsed performing an event
int start_bus(const uint8_t rotation=0, const int i2c_bus=VL53LXX_BUS_DEFAULT)
Start the driver on a specific bus.
#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG
#define RESULT_INTERRUPT_STATUS_REG
perf_counter_t _sample_perf
#define RESULT_RANGE_STATUS_REG
int stop()
Stop the driver.
void usage(const char *reason)
Print the correct usage.
int readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length)
virtual int init() override
perf_counter_t _comms_errors
int start(const uint8_t rotation)
Attempt to start driver on all available I2C busses.
virtual ssize_t read(device::file_t *file_pointer, char *buffer, size_t buflen)
#define GLOBAL_CONFIG_REF_EN_START_SELECT_REG
count the number of times an event occurs
High-resolution timer with callouts and timekeeping.
#define VL53LXX_BUS_DEFAULT
int reset()
Reset the driver.
void stop()
Stop the automatic measurement state machine.
#define GPIO_HV_MUX_ACTIVE_HIGH_REG
#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG
int usage()
Prints info about the driver argument usage.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
void perf_count(perf_counter_t handle)
Count a performance event.
bool _measurement_started
int collect()
Collects the most recent sensor measurement data from the i2c bus.
void perf_free(perf_counter_t handle)
Free a counter.
#define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG
#define MSRC_CONFIG_CONTROL_REG
void init()
Activates/configures the hardware registers.
#define DEVICE_LOG(FMT,...)
Local functions in support of the shell command.
ringbuffer::RingBuffer * _reports
#define VL53LXX_DEVICE_PATH
void perf_end(perf_counter_t handle)
End a performance event.
int readRegister(const uint8_t reg_address, uint8_t &value)
int measure()
Sends an i2c measure command to the sensor.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
static const int i2c_bus_options[]
int writeRegisterMulti(const uint8_t reg_address, const uint8_t *value, const uint8_t length)
void start()
Initialise the automatic measurement state machine and start it.
orb_advert_t _distance_sensor_topic
#define SYSTEM_SEQUENCE_CONFIG_REG
#define SYSTEM_INTERRUPT_CLEAR_REG
#define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
void print_info()
Diagnostics - print some basic information about the driver.
int orb_unadvertise(orb_advert_t handle)
int writeRegister(const uint8_t reg_address, const uint8_t value)
VL53LXX(uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus=VL53LXX_BUS_DEFAULT, int address=VL53LXX_BASEADDR)
__EXPORT int vl53lxx_main(int argc, char *argv[])
Driver 'main' command.
#define VL53LXX_SAMPLE_RATE
#define NUM_I2C_BUS_OPTIONS
#define SYSRANGE_START_REG
int test()
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
void perf_begin(perf_counter_t handle)
Begin a performance event.
int singleRefCalibration(const uint8_t byte)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
virtual int probe() override
Performance measuring tools.
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
Base class for devices connected via I2C.