Merge remote-tracking branch 'upstream/master' into ros

Conflicts:
	src/platforms/px4_middleware.h

4 files changed, 136 insertions, 197 deletions

diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index fe70bd37f..2a10b0063 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/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_INTERNAL,
-	HMC5883_BUS_EXTERNAL
+	HMC5883_BUS_ALL = 0,
+	HMC5883_BUS_I2C_INTERNAL,
+	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;
-
-void	start(int bus, enum Rotation rotation);
-void	test(int bus);
-void	reset(int bus);
-int	info(int bus);
-int	calibrate(int bus);
-const char* get_path(int bus);
+/*
+  list of supported bus configurations
+ */
+struct hmc5883_bus_option {
+	enum HMC5883_BUS busid;
+	const char *devpath;
+	HMC5883_constructor interface_constructor;
+	uint8_t busnum;
+	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.
- *
- * This function call only returns once the driver
- * is either successfully up and running or failed to start.
+ * start driver for a specific bus option
  */
-void
-start(int external_bus, enum Rotation rotation)
+bool
+start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
 {
-	int fd;
-
-	/* create the driver, attempt expansion bus first */
-	if (g_dev_ext != nullptr) {
-			warnx("already started external");
-	} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) {
-
-		device::Device *interface = nullptr;
-
-		/* create the driver, only attempt I2C for the external bus */
-		if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
-			interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION);
-
-			if (interface->init() != OK) {
-				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);
-			if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
-				delete g_dev_ext;
-				g_dev_ext = nullptr;
-			}
-
-		}
+	if (bus.dev != nullptr)
+		errx(1,"bus option already started");
+
+	device::Device *interface = bus.interface_constructor(bus.busnum);
+	if (interface->init() != OK) {
+		delete interface;
+		warnx("no device on bus %u", (unsigned)bus.busid);
+		return false;
+	}
+	bus.dev = new HMC5883(interface, bus.devpath, rotation);
+	if (bus.dev != nullptr && OK != bus.dev->init()) {
+		delete bus.dev;
+		bus.dev = NULL;
+		return false;
 	}
 			
+	int fd = open(bus.devpath, O_RDONLY);
+	if (fd < 0)
+		return false;
 
-	/* 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
-		if (HMC5883_SPI_interface != nullptr) {
-			interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS);
-		}
-#endif
-
-#ifdef PX4_I2C_BUS_ONBOARD
-		/* this device is already connected as external if present above */
-		if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
-			interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD);
-		}
-#endif
-		if (interface == nullptr) {
-			warnx("no internal bus scanned");
-			goto fail;
-		}
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		close(fd);
+		errx(1,"Failed to setup poll rate");
+	}
 
-		if (interface->init() != OK) {
-			delete interface;
-			warnx("no device on internal bus");
-		} else {
+	return true;
+}
 
-			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;
+/**
+ * Start the driver.
+ *
+ * This function call only returns once the driver
+ * is either successfully up and running or failed to start.
+ */
+void
+start(enum HMC5883_BUS busid, enum Rotation rotation)
+{
+	uint8_t i;
+	bool started = false;
 
-				if (external_bus == HMC5883_BUS_INTERNAL) {
-					goto fail;
-				}
-			}
-			if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) {
-				goto fail;
-			}
+	for (i=0; i<NUM_BUS_OPTIONS; i++) {
+		if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {
+			// this device is already started
+			continue;
 		}
+		if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
+			// not the one that is asked for
+			continue;
+		}
+		started |= start_bus(bus_options[i], rotation);
 	}
 
-	if (g_dev_int == nullptr && g_dev_ext == nullptr)
-		goto fail;
-
-	/* 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);
-	}
+	if (!started)
+		errx(1, "driver start failed");
 
+	// one or more drivers started OK
 	exit(0);
+}
 
-fail:
-	if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) {
-		delete g_dev_int;
-		g_dev_int = nullptr;
-	}
-	if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) {
-		delete g_dev_ext;
-		g_dev_ext = nullptr;
-	}
-
-	errx(1, "driver start failed");
+/**
+ * find a bus structure for a busid
+ */
+struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid)
+{
+	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+		if ((busid == HMC5883_BUS_ALL ||
+		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
+			return bus_options[i];
+		}
+	}	
+	errx(1,"bus %u not started", (unsigned)busid);
 }
 
+
 /**
  * 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*
-get_path(int bus)
-{
-	return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT);
+	warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath);
+	bus.dev->print_info();
+	exit(0);
 }
 
 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 (bus == HMC5883_BUS_ALL) {
-			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));
-		}
-	}
+	if (!strcmp(verb, "info") || !strcmp(verb, "status"))
+		hmc5883::info(busid);
 
 	/*
 	 * Autocalibrate the scaling
 	 */
 	if (!strcmp(verb, "calibrate")) {
-		if (hmc5883::calibrate(bus) == 0) {
+		if (hmc5883::calibrate(busid) == 0) {
 			errx(0, "calibration successful");
 
 		} else {
diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h
index 0eb773629..e91e91fc0 100644
--- a/src/drivers/hmc5883/hmc5883.h
+++ b/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);
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 6cac28a7d..168b34ea9 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -251,6 +251,8 @@ private:
 	perf_counter_t		_bad_registers;
 	perf_counter_t		_good_transfers;
 	perf_counter_t		_reset_retries;
+	perf_counter_t		_system_latency_perf;
+	perf_counter_t		_controller_latency_perf;
 
 	uint8_t			_register_wait;
 	uint64_t		_reset_wait;
@@ -491,6 +493,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")),
 	_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
 	_reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")),
+	_system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")),
+	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
 	_register_wait(0),
 	_reset_wait(0),
 	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -1731,6 +1735,9 @@ MPU6000::measure()
 	_gyro->parent_poll_notify();
 
 	if (!(_pub_blocked)) {
+		/* log the time of this report */
+		perf_begin(_controller_latency_perf);
+		perf_begin(_system_latency_perf);
 		/* publish it */
 		orb_publish(_accel_orb_id, _accel_topic, &arb);
 	}
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 58390ba4c..96ebedd83 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -260,9 +260,9 @@ private:
 
 	int			_mavlink_fd;		///< mavlink file descriptor.
 
-	perf_counter_t		_perf_update;		///<local performance counter for status updates
-	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
-	perf_counter_t		_perf_chan_count;	///<local performance counter for channel number changes
+	perf_counter_t		_perf_update;		///< local performance counter for status updates
+	perf_counter_t		_perf_write;		///< local performance counter for PWM control writes
+	perf_counter_t		_perf_sample_latency;	///< total system latency (based on passed-through timestamp)
 
 	/* cached IO state */
 	uint16_t		_status;		///< Various IO status flags
@@ -493,7 +493,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_mavlink_fd(-1),
 	_perf_update(perf_alloc(PC_ELAPSED, "io update")),
 	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
-	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
+	_perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")),
 	_status(0),
 	_alarms(0),
 	_t_actuator_controls_0(-1),
@@ -551,7 +551,7 @@ PX4IO::~PX4IO()
 	/* deallocate perfs */
 	perf_free(_perf_update);
 	perf_free(_perf_write);
-	perf_free(_perf_chan_count);
+	perf_free(_perf_sample_latency);
 
 	g_dev = nullptr;
 }
@@ -1111,6 +1111,7 @@ PX4IO::io_set_control_state(unsigned group)
 
 			if (changed) {
 				orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
+				perf_set(_perf_sample_latency, hrt_elapsed_time(&controls.timestamp_sample));
 			}
 		}
 		break;
@@ -1570,11 +1571,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 		channel_count = RC_INPUT_MAX_CHANNELS;
 	}
 
-	/* count channel count changes to identify signal integrity issues */
-	if (channel_count != _rc_chan_count) {
-		perf_count(_perf_chan_count);
-	}
-
 	_rc_chan_count = channel_count;
 
 	input_rc.timestamp_publication = hrt_absolute_time();