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

vl53lxx driver: add -b, -a and -R flags to choose i2c busses and sensor rotation and remove nuttx stuff such as exit functions

Browse Source
This commit is contained in:
DanielePettenuzzo 2018-06-20 14:26:25 +02:00 committed by Beat Küng
parent 040662ff04
commit 5b259a008a
3 changed files with 142 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/drivers/distance_sensor/sf1xx/sf1xx.cpp
View File

@ -56,14 +56,12 @@
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <vector>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
@ -673,6 +671,11 @@ int info();
int
start(uint8_t rotation)
{
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (start_bus(rotation, bus_options[i]) == PX4_OK) {
return PX4_OK;
@ -693,11 +696,6 @@ start_bus(uint8_t rotation, int i2c_bus)
{
int fd = -1;
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
/* create the driver */
g_dev = new SF1XX(rotation, i2c_bus);

21
src/drivers/distance_sensor/teraranger/teraranger.cpp
View File

@ -53,13 +53,11 @@
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
@ -784,6 +782,11 @@ int info();
int
start(uint8_t rotation)
{
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (start_bus(rotation, bus_options[i]) == PX4_OK) {
return PX4_OK;
@ -802,12 +805,7 @@ start(uint8_t rotation)
int
start_bus(uint8_t rotation, int i2c_bus)
{
int fd;
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
int fd = -1;
/* create the driver */
g_dev = new TERARANGER(rotation, i2c_bus);
@ -832,10 +830,15 @@ start_bus(uint8_t rotation, int i2c_bus)
goto fail;
}
close(fd);
return PX4_OK;
fail:
if (fd >= 0) {
close(fd);
}
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
@ -929,6 +932,7 @@ test()
return PX4_ERROR;
}
close(fd);
PX4_INFO("PASS");
return PX4_OK;
@ -957,6 +961,7 @@ reset()
return PX4_ERROR;
}
close(fd);
return PX4_OK;
}

172
src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp
View File

@ -53,13 +53,11 @@
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
@ -67,16 +65,11 @@
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/obstacle_distance.h>
#include <board_config.h>
/* Configuration Constants */
#ifdef PX4_I2C_BUS_EXPANSION
#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION
#else
#define VL53LXX_BUS 0
#endif
#define VL53LXX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
#define VL53LXX_BASEADDR 0b0101001 // 7-bit address
#define VL53LXX_DEVICE_PATH "/dev/vl53lxx"
@ -116,7 +109,7 @@ class VL53LXX : public device::I2C
{
public:
VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
int bus = VL53LXX_BUS, int address = VL53LXX_BASEADDR);
int bus = VL53LXX_BUS_DEFAULT, int address = VL53LXX_BASEADDR);
virtual ~VL53LXX();
@ -477,7 +470,6 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value)
ret = transfer(&reg_address, sizeof(reg_address), nullptr, 0);
if (OK != ret) {
DEVICE_LOG("i2c::transfer returned %d", ret);
perf_count(_comms_errors);
return ret;
}
@ -486,7 +478,6 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value)
ret = transfer(nullptr, 0, &value, 1);
if (OK != ret) {
DEVICE_LOG("error reading from sensor: %d", ret);
perf_count(_comms_errors);
return ret;
}
@ -505,7 +496,7 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length)
ret = transfer(&reg_address, 1, nullptr, 0);
if (OK != ret) {
DEVICE_LOG("i2c::transfer returned %d", ret);
perf_count(_comms_errors);
return ret;
}
@ -513,7 +504,6 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length)
ret = transfer(nullptr, 0, &value[0], length);
if (OK != ret) {
DEVICE_LOG("error reading from sensor: %d", ret);
perf_count(_comms_errors);
return ret;
}
@ -536,7 +526,6 @@ VL53LXX::writeRegister(uint8_t reg_address, uint8_t value)
if (OK != ret) {
perf_count(_comms_errors);
DEVICE_LOG("i2c::transfer returned %d", ret);
return ret;
}
@ -566,7 +555,6 @@ VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t *value,
if (OK != ret) {
perf_count(_comms_errors);
DEVICE_LOG("i2c::transfer returned %d", ret);
return ret;
}
@ -979,26 +967,68 @@ namespace vl53lxx
VL53LXX *g_dev;
void start(uint8_t rotation);
void stop();
void test();
void info();
int bus_options[] = {
#ifdef PX4_I2C_BUS_EXPANSION
PX4_I2C_BUS_EXPANSION,
#endif
#ifdef PX4_I2C_BUS_EXPANSION1
PX4_I2C_BUS_EXPANSION1,
#endif
#ifdef PX4_I2C_BUS_EXPANSION2
PX4_I2C_BUS_EXPANSION2,
#endif
#ifdef PX4_I2C_BUS_ONBOARD
PX4_I2C_BUS_ONBOARD,
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
int start(uint8_t rotation);
int start_bus(uint8_t rotation, int i2c_bus);
int stop();
int test();
int info();
/**
* Start the driver.
*
* Attempt to start driver on all available I2C busses.
*
* This function will return as soon as the first sensor
* is detected on one of the available busses or if no
* sensors are detected.
*
*/
void
int
start(uint8_t rotation)
{
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (start_bus(rotation, bus_options[i]) == PX4_OK) {
return PX4_OK;
}
}
return PX4_ERROR;
}
/**
* Start the driver on a specific bus.
*
* This function only returns if the sensor is up and running
* or could not be detected successfully.
*/
int
start_bus(uint8_t rotation, int i2c_bus)
{
int fd = -1;
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new VL53LXX(rotation, VL53LXX_BUS);
g_dev = new VL53LXX(rotation, i2c_bus);
if (g_dev == nullptr) {
goto fail;
@ -1019,7 +1049,8 @@ start(uint8_t rotation)
goto fail;
}
exit(0);
close(fd);
return PX4_OK;
fail:
@ -1032,23 +1063,26 @@ fail:
g_dev = nullptr;
}
errx(1, "driver start failed");
PX4_ERR("not started on bus %d", i2c_bus);
return PX4_ERROR;
}
/**
* Stop the driver
*/
void stop()
int
stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
PX4_ERR("driver not running");
return PX4_ERROR;
}
exit(0);
return PX4_OK;
}
/**
@ -1056,7 +1090,7 @@ void stop()
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
int
test()
{
struct distance_sensor_s report;
@ -1065,84 +1099,126 @@ test()
int fd = open(VL53LXX_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'vl53lxx start' if the driver is not running)", VL53LXX_DEVICE_PATH);
PX4_ERR("%s open failed (try 'vl53lxx start' if the driver is not running)", VL53LXX_DEVICE_PATH);
return PX4_ERROR;
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("ret: %d, expected: %d", sz, sizeof(report));
err(1, "immediate acc read failed");
PX4_ERR("immediate read failed");
return PX4_ERROR;
}
print_message(report);
close(fd);
errx(0, "PASS");
PX4_INFO("PASS");
return PX4_OK;
}
/**
* Print a little info about the driver.
*/
void
int
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
PX4_ERR("driver not running");
return PX4_ERROR;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
return PX4_OK;
}
} // namespace vl53lxx
static void
vl53lxx_usage()
{
PX4_INFO("usage: vl53lxx command [options]");
PX4_INFO("options:");
PX4_INFO("\t-b --bus i2cbus (%d)", VL53LXX_BUS_DEFAULT);
PX4_INFO("\t-a --all");
PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING);
PX4_INFO("command:");
PX4_INFO("\tstart|stop|test|info");
}
int
vl53lxx_main(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
bool start_all = false;
if (argc < 2) {
goto out;
int i2c_bus = VL53LXX_BUS_DEFAULT;
while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
break;
case 'b':
i2c_bus = atoi(myoptarg);
break;
case 'a':
start_all = true;
break;
default:
PX4_WARN("Unknown option!");
goto out_error;
}
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
vl53lxx::start(rotation);
if (start_all) {
return vl53lxx::start(rotation);
} else {
return vl53lxx::start_bus(rotation, i2c_bus);
}
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
vl53lxx::stop();
return vl53lxx::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[myoptind], "test")) {
vl53lxx::test();
return vl53lxx::test();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
vl53lxx::info();
return vl53lxx::info();
}
out:
out_error:
PX4_ERR("unrecognized command, try 'start', 'test', or 'info'");
vl53lxx_usage();
return PX4_ERROR;
}