dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

srf02 driver: add -b and -a flags to choose i2c busses and remove nuttx stuff such as exit functions

This commit is contained in:
DanielePettenuzzo 2018-06-21 11:02:10 +02:00 committed by Beat Küng
parent 5b259a008a
commit fed6c9ae34
4 changed files with 146 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -696,6 +696,11 @@ 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);

173
src/drivers/distance_sensor/srf02/srf02.cpp
View File

@ -52,14 +52,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>
@ -71,7 +69,7 @@
#include <board_config.h>
/* Configuration Constants */
#define SRF02_BUS PX4_I2C_BUS_EXPANSION
#define SRF02_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
#define SRF02_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */
#define SRF02_DEVICE_PATH "/dev/srf02"
@ -96,7 +94,7 @@
class SRF02 : public device::I2C
{
public:
SRF02(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SRF02_BUS,
SRF02(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SRF02_BUS_DEFAULT,
int address = SRF02_BASEADDR);
virtual ~SRF02();
@ -688,26 +686,74 @@ namespace srf02
SRF02 *g_dev;
void start(uint8_t rotation);
void stop();
void test();
void reset();
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 reset();
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");
PX4_ERR("already started");
return PX4_ERROR;
}
/* create the driver */
g_dev = new SRF02(rotation, SRF02_BUS);
g_dev = new SRF02(rotation, i2c_bus);
if (g_dev == nullptr) {
goto fail;
@ -729,7 +775,7 @@ start(uint8_t rotation)
}
close(fd);
exit(0);
return PX4_OK;
fail:
@ -742,23 +788,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;
}
/**
@ -766,7 +815,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;
@ -776,21 +825,24 @@ test()
int fd = open(SRF02_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'srf02 start' if the driver is not running", SRF02_DEVICE_PATH);
PX4_ERR("%s open failed (try 'srf02 start' if the driver is not running)", SRF02_DEVICE_PATH);
return PX4_ERROR;
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "immediate read failed");
PX4_ERR("immediate read failed");
return PX4_ERROR;
}
print_message(report);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
PX4_ERR("failed to set 2Hz poll rate");
return PX4_ERROR;
}
/* read the sensor 5x and report each value */
@ -803,14 +855,16 @@ test()
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
PX4_ERR("timed out waiting for sensor data");
return PX4_ERROR;
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
PX4_ERR("periodic read failed");
return PX4_ERROR;
}
print_message(report);
@ -818,53 +872,72 @@ test()
/* reset the sensor polling to default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
PX4_ERR("failed to set default poll rate");
return PX4_ERROR;
}
errx(0, "PASS");
PX4_INFO("PASS");
return PX4_OK;
}
/**
* Reset the driver.
*/
void
int
reset()
{
int fd = open(SRF02_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
PX4_ERR("failed");
return PX4_ERROR;
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
PX4_ERR("driver reset failed");
return PX4_ERROR;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
PX4_ERR("driver poll restart failed");
return PX4_ERROR;
}
exit(0);
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 */
static void
srf02_usage()
{
PX4_INFO("usage: srf02 command [options]");
PX4_INFO("options:");
PX4_INFO("\t-b --bus i2cbus (%d)", SRF02_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|reset|info");
}
int
srf02_main(int argc, char *argv[])
{
@ -872,16 +945,27 @@ srf02_main(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = nullptr;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
bool start_all = false;
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
int i2c_bus = SRF02_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!");
return -1;
goto out_error;
}
}
@ -893,38 +977,43 @@ srf02_main(int argc, char *argv[])
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
srf02::start(rotation);
if (start_all) {
return srf02::start(rotation);
} else {
return srf02::start_bus(rotation, i2c_bus);
}
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
srf02::stop();
return srf02::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[myoptind], "test")) {
srf02::test();
return srf02::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[myoptind], "reset")) {
srf02::reset();
return srf02::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
srf02::info();
return srf02::info();
}
out_error:
PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
srf02_usage();
return PX4_ERROR;
}

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

@ -807,6 +807,11 @@ 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 TERARANGER(rotation, i2c_bus);

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

@ -1027,6 +1027,11 @@ 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 VL53LXX(rotation, i2c_bus);