Merge pull request #1625 from tridge/pullrequest-hmc5883-bus-fix

hmc5883: fixed handling of 3 bus options

src/drivers/hmc5883/hmc5883.cpp

@@ -80,9 +80,6 @@
  * HMC5883 internal constants and data structures.
  */
 
-#define HMC5883L_DEVICE_PATH_INT	"/dev/hmc5883_int"
-#define HMC5883L_DEVICE_PATH_EXT	"/dev/hmc5883_ext"
-
 /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
 #define HMC5883_CONVERSION_INTERVAL	(1000000 / 150)	/* microseconds */
 
@@ -114,9 +111,10 @@
 #define STATUS_REG_DATA_READY		(1 << 0) /* page 16: set if all axes have valid measurements */
 
 enum HMC5883_BUS {
-	HMC5883_BUS_ALL,
+	HMC5883_BUS_ALL = 0,
-	HMC5883_BUS_INTERNAL,
+	HMC5883_BUS_I2C_INTERNAL,
-	HMC5883_BUS_EXTERNAL
+	HMC5883_BUS_I2C_EXTERNAL,
+	HMC5883_BUS_SPI
 };
 
 /* oddly, ERROR is not defined for c++ */
@@ -1297,169 +1295,129 @@ namespace hmc5883
 #endif
 const int ERROR = -1;
 
-HMC5883	*g_dev_int = nullptr;
+/*
-HMC5883	*g_dev_ext = nullptr;
+  list of supported bus configurations
-
+ */
-void	start(int bus, enum Rotation rotation);
+struct hmc5883_bus_option {
-void	test(int bus);
+	enum HMC5883_BUS busid;
-void	reset(int bus);
+	const char *devpath;
-int	info(int bus);
+	HMC5883_constructor interface_constructor;
-int	calibrate(int bus);
+	uint8_t busnum;
-const char* get_path(int bus);
+	HMC5883	*dev;
+} bus_options[] = {
+	{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
+#ifdef PX4_I2C_BUS_ONBOARD
+	{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
+#endif
+#ifdef PX4_SPIDEV_HMC
+	{ HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
+#endif
+};
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
+
+void	start(enum HMC5883_BUS busid, enum Rotation rotation);
+bool	start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation);
+struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid);
+void	test(enum HMC5883_BUS busid);
+void	reset(enum HMC5883_BUS busid);
+int	info(enum HMC5883_BUS busid);
+int	calibrate(enum HMC5883_BUS busid);
 void	usage();
 
 /**
- * Start the driver.
+ * start driver for a specific bus option
- *
- * This function call only returns once the driver
- * is either successfully up and running or failed to start.
  */
-void
+bool
-start(int external_bus, enum Rotation rotation)
+start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
 {
-	int fd;
+	if (bus.dev != nullptr)
-
+		errx(1,"bus option already started");
-	/* create the driver, attempt expansion bus first */
+
-	if (g_dev_ext != nullptr) {
+	device::Device *interface = bus.interface_constructor(bus.busnum);
-			warnx("already started external");
+	if (interface->init() != OK) {
-	} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) {
+		delete interface;
-
+		warnx("no device on bus %u", (unsigned)bus.busid);
-		device::Device *interface = nullptr;
+		return false;
-
+	}
-		/* create the driver, only attempt I2C for the external bus */
+	bus.dev = new HMC5883(interface, bus.devpath, rotation);
-		if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
+	if (bus.dev != nullptr && OK != bus.dev->init()) {
-			interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION);
+		delete bus.dev;
-
+		bus.dev = NULL;
-			if (interface->init() != OK) {
+		return false;
-				delete interface;
+	}
-				interface = nullptr;
-				warnx("no device on I2C bus #%u", PX4_I2C_BUS_EXPANSION);
-			}
-		}
-
-#ifdef PX4_I2C_BUS_ONBOARD
-		if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
-			interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD);
-
-			if (interface->init() != OK) {
-				delete interface;
-				interface = nullptr;
-				warnx("no device on I2C bus #%u", PX4_I2C_BUS_ONBOARD);
-			}
-		}
-#endif
-
-		/* interface will be null if init failed */
-		if (interface != nullptr) {
 			
-			g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation);
+	int fd = open(bus.devpath, O_RDONLY);
-			if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
+	if (fd < 0)
-				delete g_dev_ext;
+		return false;
-				g_dev_ext = nullptr;
-			}
 
-		}
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		close(fd);
+		errx(1,"Failed to setup poll rate");
 	}
 
+	return true;
+}
 
-	/* if this failed, attempt onboard sensor */
-	if (g_dev_int != nullptr) {
-		warnx("already started internal");
-	} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) {
-
-		device::Device *interface = nullptr;
 
-		/* create the driver, try SPI first, fall back to I2C if unsuccessful */
+/**
-#ifdef PX4_SPIDEV_HMC
+ * Start the driver.
-		if (HMC5883_SPI_interface != nullptr) {
+ *
-			interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS);
+ * This function call only returns once the driver
-		}
+ * is either successfully up and running or failed to start.
-#endif
+ */
+void
+start(enum HMC5883_BUS busid, enum Rotation rotation)
+{
+	uint8_t i;
+	bool started = false;
 
-#ifdef PX4_I2C_BUS_ONBOARD
+	for (i=0; i<NUM_BUS_OPTIONS; i++) {
-		/* this device is already connected as external if present above */
+		if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {
-		if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
+			// this device is already started
-			interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD);
+			continue;
-		}
-#endif
-		if (interface == nullptr) {
-			warnx("no internal bus scanned");
-			goto fail;
 		}
-
+		if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
-		if (interface->init() != OK) {
+			// not the one that is asked for
-			delete interface;
+			continue;
-			warnx("no device on internal bus");
-		} else {
-
-			g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation);
-			if (g_dev_int != nullptr && OK != g_dev_int->init()) {
-
-				/* tear down the failing onboard instance */
-				delete g_dev_int;
-				g_dev_int = nullptr;
-
-				if (external_bus == HMC5883_BUS_INTERNAL) {
-					goto fail;
-				}
-			}
-			if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) {
-				goto fail;
-			}
 		}
+		started |= start_bus(bus_options[i], rotation);
 	}
 
-	if (g_dev_int == nullptr && g_dev_ext == nullptr)
+	if (!started)
-		goto fail;
+		errx(1, "driver start failed");
-
-	/* set the poll rate to default, starts automatic data collection */
-	if (g_dev_int != nullptr) {
-		fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY);
-		if (fd < 0)
-			goto fail;
-
-		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-			goto fail;
-		close(fd);
-	}
-
-	if (g_dev_ext != nullptr) {
-		fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY);
-		if (fd < 0)
-			goto fail;
-
-		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-			goto fail;
-		close(fd);
-	}
 
+	// one or more drivers started OK
 	exit(0);
+}
 
-fail:
+/**
-	if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) {
+ * find a bus structure for a busid
-		delete g_dev_int;
+ */
-		g_dev_int = nullptr;
+struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid)
-	}
+{
-	if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) {
+	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
-		delete g_dev_ext;
+		if ((busid == HMC5883_BUS_ALL ||
-		g_dev_ext = nullptr;
+		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
+			return bus_options[i];
+		}
 	}	
-
+	errx(1,"bus %u not started", (unsigned)busid);
-	errx(1, "driver start failed");
 }
 
+
 /**
  * Perform some basic functional tests on the driver;
  * make sure we can collect data from the sensor in polled
  * and automatic modes.
  */
 void
-test(int bus)
+test(enum HMC5883_BUS busid)
 {
+	struct hmc5883_bus_option &bus = find_bus(busid);
 	struct mag_report report;
 	ssize_t sz;
 	int ret;
-	const char *path = get_path(bus);
+	const char *path = bus.devpath;
 
 	int fd = open(path, O_RDONLY);
 
@@ -1557,10 +1515,11 @@ test(int bus)
  * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
  * Using the self test method described above, the user can scale sensor
  */
-int calibrate(int bus)
+int calibrate(enum HMC5883_BUS busid)
 {
 	int ret;
-	const char *path = get_path(bus);
+	struct hmc5883_bus_option &bus = find_bus(busid);
+	const char *path = bus.devpath;
 
 	int fd = open(path, O_RDONLY);
 
@@ -1585,9 +1544,10 @@ int calibrate(int bus)
  * Reset the driver.
  */
 void
-reset(int bus)
+reset(enum HMC5883_BUS busid)
 {
-	const char *path = get_path(bus);
+	struct hmc5883_bus_option &bus = find_bus(busid);
+	const char *path = bus.devpath;
 
 	int fd = open(path, O_RDONLY);
 
@@ -1607,28 +1567,13 @@ reset(int bus)
  * Print a little info about the driver.
  */
 int
-info(int bus)
+info(enum HMC5883_BUS busid)
 {
-	int ret = 1;
+	struct hmc5883_bus_option &bus = find_bus(busid);
-
-	HMC5883 *g_dev = (bus == HMC5883_BUS_INTERNAL ? g_dev_int : g_dev_ext);
-	if (g_dev == nullptr) {
-		warnx("not running on bus %d", bus);
-	} else {
-
-		warnx("running on bus: %d (%s)\n", bus, ((HMC5883_BUS_INTERNAL) ? "onboard" : "offboard"));
-
-		g_dev->print_info();
-		ret = 0;
-	}
-
-	return ret;
-}
 
-const char*
+	warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath);
-get_path(int bus)
+	bus.dev->print_info();
-{
+	exit(0);
-	return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT);
 }
 
 void
@@ -1650,22 +1595,25 @@ int
 hmc5883_main(int argc, char *argv[])
 {
 	int ch;
-	int bus = HMC5883_BUS_ALL;
+	enum HMC5883_BUS busid = HMC5883_BUS_ALL;
 	enum Rotation rotation = ROTATION_NONE;
         bool calibrate = false;
 
-	while ((ch = getopt(argc, argv, "XIR:C")) != EOF) {
+	while ((ch = getopt(argc, argv, "XISR:C")) != EOF) {
 		switch (ch) {
 		case 'R':
 			rotation = (enum Rotation)atoi(optarg);
 			break;
 #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
 		case 'I':
-			bus = HMC5883_BUS_INTERNAL;
+			busid = HMC5883_BUS_I2C_INTERNAL;
 			break;
 #endif
 		case 'X':
-			bus = HMC5883_BUS_EXTERNAL;
+			busid = HMC5883_BUS_I2C_EXTERNAL;
+			break;
+		case 'S':
+			busid = HMC5883_BUS_SPI;
 			break;
 		case 'C':
 			calibrate = true;
@@ -1682,9 +1630,9 @@ hmc5883_main(int argc, char *argv[])
 	 * Start/load the driver.
 	 */
 	if (!strcmp(verb, "start")) {
-		hmc5883::start(bus, rotation);
+		hmc5883::start(busid, rotation);
 		if (calibrate) {
-			if (hmc5883::calibrate(bus) == 0) {
+			if (hmc5883::calibrate(busid) == 0) {
 				errx(0, "calibration successful");
 				
 			} else {
@@ -1697,38 +1645,25 @@ hmc5883_main(int argc, char *argv[])
 	 * Test the driver/device.
 	 */
 	if (!strcmp(verb, "test"))
-		hmc5883::test(bus);
+		hmc5883::test(busid);
 
 	/*
 	 * Reset the driver.
 	 */
 	if (!strcmp(verb, "reset"))
-		hmc5883::reset(bus);
+		hmc5883::reset(busid);
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
+	if (!strcmp(verb, "info") || !strcmp(verb, "status"))
-		if (bus == HMC5883_BUS_ALL) {
+		hmc5883::info(busid);
-			int ret = 0;
-			if (hmc5883::info(HMC5883_BUS_INTERNAL)) {
-				ret = 1;
-			}
-
-			if (hmc5883::info(HMC5883_BUS_EXTERNAL)) {
-				ret = 1;
-			}
-			exit(ret);
-		} else {
-			exit(hmc5883::info(bus));
-		}
-	}
 
 	/*
 	 * Autocalibrate the scaling
 	 */
 	if (!strcmp(verb, "calibrate")) {
-		if (hmc5883::calibrate(bus) == 0) {
+		if (hmc5883::calibrate(busid) == 0) {
 			errx(0, "calibration successful");
 
 		} else {

src/drivers/hmc5883/hmc5883.h

@@ -50,3 +50,4 @@
 /* interface factories */
 extern device::Device *HMC5883_SPI_interface(int bus) weak_function;
 extern device::Device *HMC5883_I2C_interface(int bus) weak_function;
+typedef device::Device* (*HMC5883_constructor)(int);