From 33422700de69f559c82f3c2cc32a4263b315d9d3 Mon Sep 17 00:00:00 2001
From: hdiethelm <hdiethel@hsr.ch>
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.