From 33422700de69f559c82f3c2cc32a4263b315d9d3 Mon Sep 17 00:00:00 2001 From: hdiethelm Date: Thu, 1 Nov 2018 15:44:41 +0100 Subject: [PATCH] lis3mdl: fix handling when multiple sensors are available (#10753) * lis3mdl: Report calibration successful when starting with -C option * lis3mdl: Use PX4_OK, PX4_ERROR for return value / orb_unadvertise * Fixes #10740 --- src/drivers/magnetometer/lis3mdl/lis3mdl.cpp | 4 + src/drivers/magnetometer/lis3mdl/lis3mdl.h | 2 +- .../magnetometer/lis3mdl/lis3mdl_main.cpp | 266 ++++++++++-------- .../magnetometer/lis3mdl/lis3mdl_main.h | 21 +- 4 files changed, 161 insertions(+), 132 deletions(-) diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp index 0200ae24ad..2c8e3d5e87 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp @@ -99,6 +99,10 @@ LIS3MDL::~LIS3MDL() /* make sure we are truly inactive */ stop(); + if (_mag_topic != nullptr) { + orb_unadvertise(_mag_topic); + } + if (_reports != nullptr) { delete _reports; } diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.h b/src/drivers/magnetometer/lis3mdl/lis3mdl.h index 58a9d25f19..d27520e034 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.h +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.h @@ -62,7 +62,7 @@ /* Max measurement rate is 80Hz */ #define LIS3MDL_CONVERSION_INTERVAL (1000000 / 80) /* 12,500 microseconds */ -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) +#define NUM_BUS_OPTIONS (sizeof(lis3mdl::bus_options)/sizeof(lis3mdl::bus_options[0])) #define ADDR_WHO_AM_I 0x0f #define ID_WHO_AM_I 0x3d diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp index 4ed189bce6..b961aa7fc9 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp @@ -46,17 +46,18 @@ extern "C" __EXPORT int lis3mdl_main(int argc, char *argv[]); int -lis3mdl::calibrate(LIS3MDL_BUS bus_id) +lis3mdl::calibrate(struct lis3mdl_bus_option &bus) { int ret; - struct lis3mdl_bus_option &bus = find_bus(bus_id); const char *path = bus.devpath; + PX4_INFO("running on bus: %u (%s)", (unsigned)bus.bus_id, bus.devpath); + int fd = open(path, O_RDONLY); if (fd < 0) { PX4_WARN("%s open failed (try 'lis3mdl start' if the driver is not running", path); - return 1; + return PX4_ERROR; } if ((ret = ioctl(fd, MAGIOCCALIBRATE, fd)) != OK) { @@ -69,31 +70,28 @@ lis3mdl::calibrate(LIS3MDL_BUS bus_id) } int -lis3mdl::info(LIS3MDL_BUS bus_id) +lis3mdl::info(struct lis3mdl_bus_option &bus) { - struct lis3mdl_bus_option &bus = find_bus(bus_id); - - PX4_WARN("running on bus: %u (%s)\n", (unsigned)bus.bus_id, bus.devpath); + PX4_INFO("running on bus: %u (%s)", (unsigned)bus.bus_id, bus.devpath); bus.dev->print_info(); - return 1; + return PX4_OK; } -bool -lis3mdl::init(LIS3MDL_BUS bus_id) +int +lis3mdl::init(struct lis3mdl_bus_option &bus) { - struct lis3mdl_bus_option &bus = find_bus(bus_id); const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) { - return false; + return PX4_ERROR; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); errx(1, "Failed to setup poll rate"); - return false; + return PX4_ERROR; } else { PX4_INFO("Poll rate set to max (80hz)"); @@ -101,15 +99,15 @@ lis3mdl::init(LIS3MDL_BUS bus_id) close(fd); - return true; + return PX4_OK; } -bool +int lis3mdl::start_bus(struct lis3mdl_bus_option &bus, Rotation rotation) { if (bus.dev != nullptr) { errx(1, "bus option already started"); - return false; + return PX4_ERROR; } device::Device *interface = bus.interface_constructor(bus.busnum); @@ -117,7 +115,7 @@ lis3mdl::start_bus(struct lis3mdl_bus_option &bus, Rotation rotation) if (interface->init() != OK) { delete interface; warnx("no device on bus %u", (unsigned)bus.bus_id); - return false; + return PX4_ERROR; } bus.dev = new LIS3MDL(interface, bus.devpath, rotation); @@ -126,66 +124,54 @@ lis3mdl::start_bus(struct lis3mdl_bus_option &bus, Rotation rotation) bus.dev->init() != OK) { delete bus.dev; bus.dev = NULL; - return false; + return PX4_ERROR; } - return true; + return PX4_OK; } int -lis3mdl::start(LIS3MDL_BUS bus_id, Rotation rotation) +lis3mdl::start(struct lis3mdl_bus_option &bus, Rotation rotation) { - bool started = false; - - for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { - if (bus_id == LIS3MDL_BUS_ALL && bus_options[i].dev != NULL) { - // this device is already started - continue; - } - - if (bus_id != LIS3MDL_BUS_ALL && bus_options[i].bus_id != bus_id) { - // not the one that is asked for - continue; - } + if (bus.dev == NULL) { + return start_bus(bus, rotation); - started |= start_bus(bus_options[i], rotation); - //init(bus_id); + } else { + // this device is already started + return PX4_ERROR; } - - return started; } int -lis3mdl::stop() +lis3mdl::stop(struct lis3mdl_bus_option &bus) { - bool stopped = false; - - for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { - if (bus_options[i].dev != nullptr) { - bus_options[i].dev->stop(); - delete bus_options[i].dev; - bus_options[i].dev = nullptr; - stopped = true; - } - } + if (bus.dev != NULL) { + bus.dev->stop(); + delete bus.dev; + bus.dev = nullptr; + return PX4_OK; - return !stopped; + } else { + // this device is already stopped + return PX4_ERROR; + } } -bool -lis3mdl::test(LIS3MDL_BUS bus_id) +int +lis3mdl::test(struct lis3mdl_bus_option &bus) { - struct lis3mdl_bus_option &bus = find_bus(bus_id); struct mag_report report; ssize_t sz; int ret; const char *path = bus.devpath; + PX4_INFO("running on bus: %u (%s)", (unsigned)bus.bus_id, bus.devpath); + int fd = open(path, O_RDONLY); if (fd < 0) { PX4_WARN("%s open failed (try 'lis3mdl start')", path); - return 1; + return PX4_ERROR; } /* do a simple demand read */ @@ -193,7 +179,7 @@ lis3mdl::test(LIS3MDL_BUS bus_id) if (sz != sizeof(report)) { PX4_WARN("immediate read failed"); - return 1; + return PX4_ERROR; } print_message(report); @@ -201,19 +187,19 @@ lis3mdl::test(LIS3MDL_BUS bus_id) /* check if mag is onboard or external */ if (ioctl(fd, MAGIOCGEXTERNAL, 0) < 0) { PX4_WARN("failed to get if mag is onboard or external"); - return 1; + return PX4_ERROR; } /* set the queue depth to 5 */ if (ioctl(fd, SENSORIOCSQUEUEDEPTH, 10) != OK) { PX4_WARN("failed to set queue depth"); - return 1; + return PX4_ERROR; } /* start the sensor polling at 2Hz */ if (ioctl(fd, SENSORIOCSPOLLRATE, 2) != OK) { PX4_WARN("failed to set 2Hz poll rate"); - return 1; + return PX4_ERROR; } struct pollfd fds; @@ -228,7 +214,7 @@ lis3mdl::test(LIS3MDL_BUS bus_id) if (ret != 1) { PX4_WARN("timed out waiting for sensor data"); - return 1; + return PX4_ERROR; } /* now go get it */ @@ -236,40 +222,41 @@ lis3mdl::test(LIS3MDL_BUS bus_id) if (sz != sizeof(report)) { PX4_WARN("periodic read failed"); - return 1; + return PX4_ERROR; } print_message(report); } PX4_INFO("PASS"); - return 1; + return PX4_OK; } -bool -lis3mdl::reset(LIS3MDL_BUS bus_id) +int +lis3mdl::reset(struct lis3mdl_bus_option &bus) { - struct lis3mdl_bus_option &bus = find_bus(bus_id); const char *path = bus.devpath; + PX4_INFO("running on bus: %u (%s)", (unsigned)bus.bus_id, bus.devpath); + int fd = open(path, O_RDONLY); if (fd < 0) { PX4_WARN("open failed "); - return 1; + return PX4_ERROR; } if (ioctl(fd, SENSORIOCRESET, 0) < 0) { PX4_WARN("driver reset failed"); - return 1; + return PX4_ERROR; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_WARN("driver poll restart failed"); - return 1; + return PX4_ERROR; } - return 0; + return PX4_OK; } void @@ -280,6 +267,7 @@ lis3mdl::usage() PX4_WARN(" -R rotation"); PX4_WARN(" -C calibrate on start"); PX4_WARN(" -X only external bus"); + PX4_WARN(" -S only spi bus"); #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_LIS) PX4_WARN(" -I only internal bus"); #endif @@ -322,84 +310,126 @@ lis3mdl_main(int argc, char *argv[]) default: lis3mdl::usage(); - return 0; + return PX4_ERROR; } } if (myoptind >= argc) { lis3mdl::usage(); - return -1; + return PX4_ERROR; } const char *verb = argv[myoptind]; + int ret; + bool dev_found = false; + bool cmd_found = false; - // Start/load the driver if (!strcmp(verb, "start")) { + // Start/load the driver - if (lis3mdl::start(bus_id, rotation)) { - if (calibrate) { - if (OK != lis3mdl::calibrate(bus_id)) { - PX4_WARN("calibration failed"); - return 1; - } + cmd_found = true; + ret = 1; // default: failed, will be set to success if one start succeeds + + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (bus_id != LIS3MDL_BUS_ALL && bus_id != lis3mdl::bus_options[i].bus_id) { + // not the one that is asked for + continue; } - lis3mdl::init(bus_id); + dev_found = true; + + // Start/load the driver + if (lis3mdl::start(lis3mdl::bus_options[i], rotation) == OK) { + if (calibrate) { + if (lis3mdl::calibrate(lis3mdl::bus_options[i]) != OK) { + PX4_WARN("calibration failed"); + lis3mdl::stop(lis3mdl::bus_options[i]); //Stop, failed - return 0; + } else { + PX4_INFO("calibration successful"); + lis3mdl::init(lis3mdl::bus_options[i]); + ret = 0; // one succeed + } - } else { - return 1; + } else { + lis3mdl::init(lis3mdl::bus_options[i]); + ret = 0; // one succeed + } + } } - } - // Stop the driver - if (!strcmp(verb, "stop")) { - return lis3mdl::stop(); - } + } else { + // Other commands - // Test the driver/device - if (!strcmp(verb, "test")) { - return lis3mdl::test(bus_id); - } + ret = 0; // default: success, will be set to failed if one action fails - // Reset the driver - if (!strcmp(verb, "reset")) { - return lis3mdl::reset(bus_id); - } + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (bus_id != LIS3MDL_BUS_ALL && bus_id != lis3mdl::bus_options[i].bus_id) { + // not the one that is asked for + continue; + } - // Print driver information - if (!strcmp(verb, "info") || - !strcmp(verb, "status")) { - return lis3mdl::info(bus_id); - } + if (lis3mdl::bus_options[i].dev == NULL) { + if (bus_id != LIS3MDL_BUS_ALL) { + PX4_ERR("bus %u not started", (unsigned)bus_id); + return PX4_ERROR; - // Autocalibrate the scaling - if (!strcmp(verb, "calibrate")) { - if (lis3mdl::calibrate(bus_id) == 0) { - PX4_INFO("calibration successful"); - return 0; + } else { + continue; + } + } - } else { - PX4_INFO("calibration failed"); - return 1; - } + dev_found = true; - } + // Stop the driver + if (!strcmp(verb, "stop")) { + cmd_found = true; + ret |= lis3mdl::stop(lis3mdl::bus_options[i]); + } - PX4_INFO("unrecognized command, try 'start', 'test', 'reset', 'calibrate' 'or 'info'"); - return 1; -} + // Test the driver/device + if (!strcmp(verb, "test")) { + cmd_found = true; + ret |= lis3mdl::test(lis3mdl::bus_options[i]); + } -struct -lis3mdl::lis3mdl_bus_option &lis3mdl::find_bus(LIS3MDL_BUS bus_id) -{ - for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { - if ((bus_id == LIS3MDL_BUS_ALL || - bus_id == bus_options[i].bus_id) && bus_options[i].dev != NULL) { - return bus_options[i]; + // Reset the driver + if (!strcmp(verb, "reset")) { + cmd_found = true; + ret |= lis3mdl::reset(lis3mdl::bus_options[i]); + } + + // Print driver information + if (!strcmp(verb, "info") || + !strcmp(verb, "status")) { + cmd_found = true; + ret |= lis3mdl::info(lis3mdl::bus_options[i]); + } + + // Autocalibrate the scaling + if (!strcmp(verb, "calibrate")) { + cmd_found = true; + + if (lis3mdl::calibrate(lis3mdl::bus_options[i]) == OK) { + PX4_INFO("calibration successful"); + + } else { + PX4_WARN("calibration failed"); + ret = 1; + } + } } } - errx(1, "bus %u not started", (unsigned)bus_id); + if (!dev_found) { + PX4_WARN("no device found, please start driver first"); + return PX4_ERROR; + + } else if (!cmd_found) { + PX4_WARN("unrecognized command, try 'start', 'test', 'reset', 'calibrate' 'or 'info'"); + return PX4_ERROR; + + } else { + return ret; + } } diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_main.h b/src/drivers/magnetometer/lis3mdl/lis3mdl_main.h index b64e00555a..655eeb7c31 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_main.h +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_main.h @@ -68,11 +68,6 @@ struct lis3mdl_bus_option { #endif /* PX4_SPIDEV_LIS */ }; -/** - * @brief Finds a bus structure for a bus_id - */ -lis3mdl_bus_option &find_bus(LIS3MDL_BUS bus_id); - /** * @brief Calibrate and self test. Self test feature cannot be used to calculate scale. * @@ -85,45 +80,45 @@ lis3mdl_bus_option &find_bus(LIS3MDL_BUS bus_id); * field. According to ST datasheet, those values must stay between thresholds in order * to pass the self test. */ -int calibrate(LIS3MDL_BUS bus_id); +int calibrate(struct lis3mdl_bus_option &bus); /** * @brief Prints info about the driver. */ -int info(LIS3MDL_BUS bus_id); +int info(struct lis3mdl_bus_option &bus); /** * @brief Initializes the driver -- sets defaults and starts a cycle */ -bool init(LIS3MDL_BUS bus_id); +int init(struct lis3mdl_bus_option &bus); /** * @brief Resets the driver. */ -bool reset(LIS3MDL_BUS bus_id); +int reset(struct lis3mdl_bus_option &bus); /** * @brief Starts the driver for a specific bus option */ -bool start_bus(struct lis3mdl_bus_option &bus, Rotation rotation); +int start_bus(struct lis3mdl_bus_option &bus, Rotation rotation); /** * @brief Starts the driver. This function call only returns once the driver * is either successfully up and running or failed to start. */ -int start(LIS3MDL_BUS bus_id, Rotation rotation); +int start(struct lis3mdl_bus_option &bus, Rotation rotation); /** * @brief Stop the driver. */ -int stop(); +int stop(struct lis3mdl_bus_option &bus); /** * @brief Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ -bool test(LIS3MDL_BUS bus_id); +int test(struct lis3mdl_bus_option &bus); /** * @brief Prints info about the driver argument usage.