diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 0a69eef2e6..5d83c0eabe 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp @@ -56,14 +56,12 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include @@ -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); diff --git a/src/drivers/distance_sensor/teraranger/teraranger.cpp b/src/drivers/distance_sensor/teraranger/teraranger.cpp index 8053ba5d31..f35ecaeb52 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger.cpp @@ -53,13 +53,11 @@ #include #include #include -#include #include #include #include #include -#include #include #include @@ -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; } diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index 8399379514..22680e9d05 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -53,13 +53,11 @@ #include #include #include -#include #include #include #include #include -#include #include #include @@ -67,16 +65,11 @@ #include #include -#include #include /* 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(®_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(®_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; }