From fed6c9ae34055b48dded9a47ee8d5f4d6426b57a Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Thu, 21 Jun 2018 11:02:10 +0200 Subject: [PATCH] srf02 driver: add -b and -a flags to choose i2c busses and remove nuttx stuff such as exit functions --- src/drivers/distance_sensor/sf1xx/sf1xx.cpp | 5 + src/drivers/distance_sensor/srf02/srf02.cpp | 173 +++++++++++++----- .../distance_sensor/teraranger/teraranger.cpp | 5 + .../distance_sensor/vl53lxx/vl53lxx.cpp | 5 + 4 files changed, 146 insertions(+), 42 deletions(-) diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 5d83c0eabe..90f477575f 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp @@ -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); diff --git a/src/drivers/distance_sensor/srf02/srf02.cpp b/src/drivers/distance_sensor/srf02/srf02.cpp index d9d3c39fd5..3615a4bf41 100644 --- a/src/drivers/distance_sensor/srf02/srf02.cpp +++ b/src/drivers/distance_sensor/srf02/srf02.cpp @@ -52,14 +52,12 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include @@ -71,7 +69,7 @@ #include /* 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; } diff --git a/src/drivers/distance_sensor/teraranger/teraranger.cpp b/src/drivers/distance_sensor/teraranger/teraranger.cpp index f35ecaeb52..e4a33ff1a7 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger.cpp @@ -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); diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index 22680e9d05..639f9eee48 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -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);