From 7d281b26194488f24a31627c0b924029f96d30bb Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Mon, 18 Jun 2018 18:44:07 +0200 Subject: [PATCH] teraranger driver: add -b and -a flags to choose i2c busses and remove nuttx stuff such as exit functions --- .../distance_sensor/teraranger/teraranger.cpp | 168 +++++++++++++----- 1 file changed, 128 insertions(+), 40 deletions(-) diff --git a/src/drivers/distance_sensor/teraranger/teraranger.cpp b/src/drivers/distance_sensor/teraranger/teraranger.cpp index 5d988f8a59..697a5a0be3 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger.cpp @@ -71,7 +71,7 @@ #include /* Configuration Constants */ -#define TERARANGER_BUS PX4_I2C_BUS_EXPANSION +#define TERARANGER_BUS_DEFAULT PX4_I2C_BUS_EXPANSION #define TRONE_BASEADDR 0x30 /* 7-bit address */ #define TREVO_BASEADDR 0x31 /* 7-bit address */ #define TERARANGER_DEVICE_PATH "/dev/teraranger" @@ -101,7 +101,7 @@ class TERARANGER : public device::I2C { public: TERARANGER(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, - int bus = TERARANGER_BUS, int address = TRONE_BASEADDR); + int bus = TERARANGER_BUS_DEFAULT, int address = TRONE_BASEADDR); virtual ~TERARANGER(); virtual int init(); @@ -748,26 +748,69 @@ namespace teraranger TERARANGER *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) +{ + 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; if (g_dev != nullptr) { - errx(1, "already started"); + PX4_ERR("already started"); + return PX4_ERROR; } /* create the driver */ - g_dev = new TERARANGER(rotation, TERARANGER_BUS); + g_dev = new TERARANGER(rotation, i2c_bus); if (g_dev == nullptr) { @@ -789,7 +832,7 @@ start(uint8_t rotation) goto fail; } - exit(0); + return PX4_OK; fail: @@ -798,23 +841,26 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + PX4_WARN("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; } /** @@ -822,7 +868,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; @@ -832,21 +878,24 @@ test() int fd = open(TERARANGER_DEVICE_PATH, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'teraranger start' if the driver is not running", TERARANGER_DEVICE_PATH); + PX4_ERR("%s open failed (try 'teraranger start' if the driver is not running)", TERARANGER_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 50x and report each value */ @@ -859,14 +908,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); @@ -874,53 +925,73 @@ 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(TERARANGER_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 +teraranger_usage() +{ + PX4_INFO("usage: teraranger command [options]"); + PX4_INFO("options:"); + PX4_INFO("\t-b --bus i2cbus (%d)", TERARANGER_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 teraranger_main(int argc, char *argv[]) { @@ -928,16 +999,27 @@ teraranger_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; + + int i2c_bus = TERARANGER_BUS_DEFAULT; - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + 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; } } @@ -949,38 +1031,44 @@ teraranger_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(argv[myoptind], "start")) { - teraranger::start(rotation); + + if (start_all) { + return teraranger::start(rotation); + + } else { + return teraranger::start_bus(rotation, i2c_bus); + } } /* * Stop the driver */ if (!strcmp(argv[myoptind], "stop")) { - teraranger::stop(); + return teraranger::stop(); } /* * Test the driver/device. */ if (!strcmp(argv[myoptind], "test")) { - teraranger::test(); + return teraranger::test(); } /* * Reset the driver. */ if (!strcmp(argv[myoptind], "reset")) { - teraranger::reset(); + return teraranger::reset(); } /* * Print driver information. */ if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - teraranger::info(); + return teraranger::info(); } out_error: - PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); - return -1; + teraranger_usage(); + return PX4_ERROR; }