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

Conflicts:
	src/platforms/px4_middleware.h

CONTRIBUTING.md

@@ -16,7 +16,7 @@ git checkout -b mydescriptivebranchname
 
 ### Edit and build the code
 
-The [developer guide](http://pixhawk.org/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://pixhawk.org/dev/code_style) when editing files.
+The [developer guide](http://px4.io/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://px4.io/dev/code_style) when editing files.
 
 ### Commit your changes
 
@@ -41,4 +41,4 @@ Since we care about safety, we will regularly ask you for test results. Best is
 
 Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/).
 
-Make sure to provide some testing feedback and if possible the link to a flight log file.
+Make sure to provide some testing feedback and if possible the link to a flight log file. Upload flight log files to [Log Muncher](http://dash.oznet.ch) and link the resulting report.

README.md

@@ -6,7 +6,7 @@
 
 *   Official Website: http://px4.io
 *   License: BSD 3-clause (see LICENSE.md)
-*   Supported airframes:
+*   Supported airframes (more experimental are supported):
   * [Multicopters](http://px4.io/platforms/multicopters/start)
   * [Fixed wing](http://px4.io/platforms/planes/start)
   * [VTOL](http://px4.io/platforms/vtol/start)
@@ -16,7 +16,7 @@
 
 ### Users ###
 
-Please refer to the [user documentation](https://pixhawk.org/users/start) for flying drones with PX4.
+Please refer to the [user documentation](https://pixhawk.org/users/start) for flying drones with the PX4 flight stack.
 
 ### Developers ###
 
@@ -26,9 +26,12 @@ http://px4.io/dev/contributing
 Developer guide:
 http://px4.io/dev/
 
+Testing guide:
+http://px4.io/dev/unit_tests
+
 This repository contains code supporting these boards:
-  * PX4FMUv1.x
+  * FMUv1.x
-  * PX4FMUv2.x
+  * FMUv2.x
   * AeroCore (v1 and v2)
 
 ## NuttShell (NSH) ##

ROMFS/px4fmu_common/init.d/10018_tbs_endurance

@@ -2,7 +2,7 @@
 #
 # Team Blacksheep Discovery Long Range Quadcopter
 # 
-# Setup: 15 x 6.5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors
+# Setup: 15 x 5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors
 #
 # Simon Wilks <simon@px4.io>
 #
@@ -11,21 +11,27 @@ sh /etc/init.d/rc.mc_defaults
 
 if [ $AUTOCNF == yes ]
 then
+	param set BAT_N_CELLS 6
+	param set BAT_V_EMPTY 3.5
+
 	param set MC_ROLL_P 7.0
-	param set MC_ROLLRATE_P 0.07
+	param set MC_ROLLRATE_P 0.08
 	param set MC_ROLLRATE_I 0.02
 	param set MC_ROLLRATE_D 0.003
 	param set MC_PITCH_P 7.0
-	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_P 0.13
-	param set MC_PITCHRATE_I 0.05
+	param set MC_PITCHRATE_I 0.02
-	param set MC_PITCHRATE_D 0.003
+	param set MC_PITCHRATE_D 0.005
 	param set MC_YAW_P 2.8
-	param set MC_YAWRATE_P 0.4
+	param set MC_YAWRATE_P 0.2
 	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
+
+	param set MPC_XY_FF 0.2
+	param set MPC_XY_VEL_MAX 2
 fi
 
 set MIXER quad_w
 
-set PWM_OUTPUTS 1234
+set PWM_OUT 1234
-set PWM_MIN 1200
+set PWM_MIN 1080

ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol

@@ -1,4 +1,3 @@
-#!nsh
 #
 # Generic configuration file for caipirinha VTOL version
 #

ROMFS/px4fmu_common/init.d/rc.autostart

@@ -252,9 +252,17 @@ fi
 # 13000 is historically reserved for the quadshot
 
 #
-# VTOL caipririnha
+# VTOL Caipririnha (Tailsitter)
 #
- if param compare SYS_AUTOSTART 13001
+if param compare SYS_AUTOSTART 13001
- then
+then
-	 sh /etc/init.d/13001_caipirinha_vtol
+	sh /etc/init.d/13001_caipirinha_vtol
- fi
+fi
+
+#
+# VTOL BirdsEyeView FireFly x6 (Tilt-Rotor)
+#
+if param compare SYS_AUTOSTART 13002
+then
+	sh /etc/init.d/13002_firefly6
+fi

ROMFS/px4fmu_common/init.d/rcS

@@ -230,6 +230,7 @@ then
 				tone_alarm $TUNE_ERR
 			fi
 		fi
+		unset IO_FILE
 
 		if [ $IO_PRESENT == no ]
 		then

Tools/px_uploader.py

@@ -287,11 +287,12 @@ class uploader(object):
 
                         #Draw progress bar (erase usually takes about 9 seconds to complete)
                         estimatedTimeRemaining = deadline-time.time()
-                        if estimatedTimeRemaining > 0:
+                        if estimatedTimeRemaining >= 9.0:
                             self.__drawProgressBar(20.0-estimatedTimeRemaining, 9.0)
                         else:
                             self.__drawProgressBar(10.0, 10.0)
-                            sys.stdout.write(" (timeout: %d seconds) " % int(time.time()-deadline) )
+                            sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) )
+                            sys.stdout.flush()
 
                         if self.__trySync():
                             self.__drawProgressBar(10.0, 10.0)

msg/actuator_controls.msg

@@ -1,4 +1,5 @@
 uint8 NUM_ACTUATOR_CONTROLS = 8
 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
 uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[8] control

msg/actuator_controls_0.msg

@@ -1,4 +1,5 @@
 uint8 NUM_ACTUATOR_CONTROLS = 8
 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
 uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[8] control

msg/actuator_controls_virtual_mc.msg

@@ -1,4 +1,5 @@
 uint8 NUM_ACTUATOR_CONTROLS = 8
 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
 uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[8] control

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,17 +1295,70 @@ namespace hmc5883
 #endif
 const int ERROR = -1;
 
-HMC5883	*g_dev_int = nullptr;
+/*
-HMC5883	*g_dev_ext = nullptr;
+  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(int bus, enum Rotation rotation);
+void	start(enum HMC5883_BUS busid, enum Rotation rotation);
-void	test(int bus);
+bool	start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation);
-void	reset(int bus);
+struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid);
-int	info(int bus);
+void	test(enum HMC5883_BUS busid);
-int	calibrate(int bus);
+void	reset(enum HMC5883_BUS busid);
-const char* get_path(int bus);
+int	info(enum HMC5883_BUS busid);
+int	calibrate(enum HMC5883_BUS busid);
 void	usage();
 
+/**
+ * start driver for a specific bus option
+ */
+bool
+start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
+{
+	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 (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		close(fd);
+		errx(1,"Failed to setup poll rate");
+	}
+
+	return true;
+}
+
+
 /**
  * Start the driver.
  *
@@ -1315,151 +1366,58 @@ void	usage();
  * is either successfully up and running or failed to start.
  */
 void
-start(int external_bus, enum Rotation rotation)
+start(enum HMC5883_BUS busid, enum Rotation rotation)
 {
-	int fd;
+	uint8_t i;
+	bool started = false;
 
-	/* create the driver, attempt expansion bus first */
+	for (i=0; i<NUM_BUS_OPTIONS; i++) {
-	if (g_dev_ext != nullptr) {
+		if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {
-			warnx("already started external");
+			// this device is already started
-	} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) {
+			continue;
-
-		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);
-			}
 		}
-
+		if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
-#ifdef PX4_I2C_BUS_ONBOARD
+			// not the one that is asked for
-		if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
+			continue;
-			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 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 (interface->init() != OK) {
-			delete interface;
-			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)) {
-		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);
+	warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath);
-	if (g_dev == nullptr) {
+	bus.dev->print_info();
-		warnx("not running on bus %d", bus);
+	exit(0);
-	} 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);
 }
 
 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);

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);
 	}

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_update;		///< local performance counter for status updates
-	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
+	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_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();

src/examples/fixedwing_control/module.mk

@@ -42,4 +42,4 @@ SRCS		= main.c \
 
 MODULE_STACKSIZE = 1200
 
-EXTRACFLAGS = -Wframe-larger-than=1200
+EXTRACFLAGS = -Wframe-larger-than=1300

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -133,7 +133,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      7200,
+					      7700,
 					      attitude_estimator_ekf_thread_main,
 					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);

src/modules/commander/commander.cpp

@@ -739,9 +739,6 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
 	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 
-	/* welcome user */
-	warnx("starting");
-
 	const char *main_states_str[MAIN_STATE_MAX];
 	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
 	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
@@ -1246,6 +1243,7 @@ int commander_thread_main(int argc, char *argv[])
 		orb_check(safety_sub, &updated);
 
 		if (updated) {
+			bool previous_safety_off = safety.safety_off;
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
 
 			/* disarm if safety is now on and still armed */
@@ -1259,6 +1257,19 @@ int commander_thread_main(int argc, char *argv[])
 					arming_state_changed = true;
 				}
 			}
+
+			//Notify the user if the status of the safety switch changes
+			if(safety.safety_switch_available && previous_safety_off != safety.safety_off) {
+
+				if(safety.safety_off) {
+					set_tune(TONE_NOTIFY_POSITIVE_TUNE);
+				}
+				else {
+					tune_neutral(true);
+				}
+
+				status_changed = true;
+			}
 		}
 
 		/* update vtol vehicle status*/
@@ -1952,6 +1963,12 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* reset arm_tune_played when disarmed */
 		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
+
+			//Notify the user that it is safe to approach the vehicle
+			if(arm_tune_played) {
+				tune_neutral(true);
+			}
+
 			arm_tune_played = false;
 		}
 

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -280,6 +280,8 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         status.arming_state = test->current_state.arming_state;
         status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
         status.hil_state = test->hil_state;
+        // The power status of the test unit is not relevant for the unit test
+        status.circuit_breaker_engaged_power_check = true;
         safety.safety_switch_available = test->safety_switch_available;
         safety.safety_off = test->safety_off;
         armed.armed = test->current_state.armed;

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -1061,7 +1061,9 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
+			_actuators.timestamp_sample = _att.timestamp;
 			_actuators_airframe.timestamp = hrt_absolute_time();
+			_actuators_airframe.timestamp_sample = _att.timestamp;
 
 			/* publish the actuator controls */
 			if (_actuators_0_pub > 0) {

src/modules/mavlink/mavlink_main.cpp

@@ -1286,30 +1286,11 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	if (Mavlink::instance_exists(_device_name, this)) {
-		warnx("mavlink instance for %s already running", _device_name);
+		warnx("%s already running", _device_name);
 		return ERROR;
 	}
 
-	/* inform about mode */
+	warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
-	switch (_mode) {
-	case MAVLINK_MODE_NORMAL:
-		warnx("mode: NORMAL");
-		break;
-
-	case MAVLINK_MODE_CUSTOM:
-		warnx("mode: CUSTOM");
-		break;
-
-	case MAVLINK_MODE_ONBOARD:
-		warnx("mode: ONBOARD");
-		break;
-
-	default:
-		warnx("ERROR: Unknown mode");
-		break;
-	}
-
-	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
@@ -1337,7 +1318,7 @@ Mavlink::task_main(int argc, char *argv[])
 		 * marker ring buffer approach.
 		 */
 		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
-			errx(1, "can't allocate message buffer, exiting");
+			errx(1, "msg buf:");
 		}
 
 		/* initialize message buffer mutex */
@@ -1571,8 +1552,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_subscriptions = nullptr;
 
-	warnx("waiting for UART receive thread");
-
 	/* wait for threads to complete */
 	pthread_join(_receive_thread, NULL);
 

src/modules/mavlink/mavlink_mission.cpp

@@ -120,13 +120,11 @@ MavlinkMissionManager::init_offboard_mission()
 		_count = mission_state.count;
 		_current_seq = mission_state.current_seq;
 
-		warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq);
-
 	} else {
 		_dataman_id = 0;
 		_count = 0;
 		_current_seq = 0;
-		warnx("offboard mission init: ERROR, reading mission state failed");
+		warnx("offboard mission init: ERROR");
 	}
 }
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -141,6 +141,7 @@ private:
 	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_controller_latency_perf;
 
 	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
 	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
@@ -289,7 +290,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_actuators_0_circuit_breaker_enabled(false),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
+	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
 
 {
 	memset(&_v_att, 0, sizeof(_v_att));
@@ -886,10 +888,12 @@ MulticopterAttitudeControl::task_main()
 				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
 				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
 				_actuators.timestamp = hrt_absolute_time();
+				_actuators.timestamp_sample = _v_att.timestamp;
 
 				if (!_actuators_0_circuit_breaker_enabled) {
 					if (_actuators_0_pub > 0) {
 						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
+						perf_end(_controller_latency_perf);
 
 					} else if (_actuators_id) {
 						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);

src/modules/sdlog2/sdlog2.c

@@ -199,6 +199,8 @@ static bool space_warning_sent = false;
 static pthread_t logwriter_pthread = 0;
 static pthread_attr_t logwriter_attr;
 
+static perf_counter_t perf_write;
+
 /**
  * Log buffer writing thread. Open and close file here.
  */
@@ -451,10 +453,10 @@ int open_log_file()
 	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
 
 	if (fd < 0) {
-		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
 
 	} else {
-		mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
+		mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name);
 	}
 
 	return fd;
@@ -515,8 +517,6 @@ static void *logwriter_thread(void *arg)
 	/* set name */
 	prctl(PR_SET_NAME, "sdlog2_writer", 0);
 
-	perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write");
-
 	int log_fd = open_log_file();
 
 	if (log_fd < 0) {
@@ -620,16 +620,11 @@ static void *logwriter_thread(void *arg)
 	fsync(log_fd);
 	close(log_fd);
 
-	/* free performance counter */
-	perf_free(perf_write);
-
 	return NULL;
 }
 
 void sdlog2_start_log()
 {
-	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] start logging");
-
 	/* create log dir if needed */
 	if (create_log_dir() != 0) {
 		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
@@ -655,6 +650,9 @@ void sdlog2_start_log()
 
 	logwriter_should_exit = false;
 
+	/* allocate write performance counter */
+	perf_write = perf_alloc(PC_ELAPSED, "sd write");
+
 	/* start log buffer emptying thread */
 	if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) {
 		errx(1, "error creating logwriter thread");
@@ -674,8 +672,6 @@ void sdlog2_start_log()
 
 void sdlog2_stop_log()
 {
-	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] stop logging");
-
 	logging_enabled = false;
 
 	/* wake up write thread one last time */
@@ -701,6 +697,11 @@ void sdlog2_stop_log()
 	perf_print_all(perf_fd);
 	close(perf_fd);
 
+	/* free log writer performance counter */
+	perf_free(perf_write);
+
+	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped");
+
 	sdlog2_status();
 }
 

src/modules/systemlib/perf_counter.c

@@ -39,6 +39,7 @@
 
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
 #include <sys/queue.h>
 #include <drivers/drv_hrt.h>
 #include <math.h>
@@ -67,10 +68,13 @@ struct perf_ctr_count {
 struct perf_ctr_elapsed {
 	struct perf_ctr_header	hdr;
 	uint64_t		event_count;
+	uint64_t		event_overruns;
 	uint64_t		time_start;
 	uint64_t		time_total;
 	uint64_t		time_least;
 	uint64_t		time_most;
+	float			mean;
+	float			M2;
 };
 
 /**
@@ -126,6 +130,28 @@ perf_alloc(enum perf_counter_type type, const char *name)
 	return ctr;
 }
 
+perf_counter_t
+perf_alloc_once(enum perf_counter_type type, const char *name)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		if (!strcmp(handle->name, name)) {
+			if (type == handle->type) {
+				/* they are the same counter */
+				return handle;
+			} else {
+				/* same name but different type, assuming this is an error and not intended */
+				return NULL;
+			}
+		}
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+
+	/* if the execution reaches here, no existing counter of that name was found */
+	return perf_alloc(type, name);
+}
+
 void
 perf_free(perf_counter_t handle)
 {
@@ -213,17 +239,72 @@ perf_end(perf_counter_t handle)
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
 			if (pce->time_start != 0) {
-				hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
+				int64_t elapsed = hrt_absolute_time() - pce->time_start;
+
+				if (elapsed < 0) {
+					pce->event_overruns++;
+				} else {
+
+					pce->event_count++;
+					pce->time_total += elapsed;
+
+					if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0))
+						pce->time_least = elapsed;
+
+					if (pce->time_most < (uint64_t)elapsed)
+						pce->time_most = elapsed;
+
+					// maintain mean and variance of the elapsed time in seconds
+					// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+					float dt = elapsed / 1e6f;
+					float delta_intvl = dt - pce->mean;
+					pce->mean += delta_intvl / pce->event_count;
+					pce->M2 += delta_intvl * (dt - pce->mean);
+
+					pce->time_start = 0;
+				}
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+#include <systemlib/err.h>
+
+void
+perf_set(perf_counter_t handle, int64_t elapsed)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			if (elapsed < 0) {
+				pce->event_overruns++;
+			} else {
 
 				pce->event_count++;
 				pce->time_total += elapsed;
 
-				if ((pce->time_least > elapsed) || (pce->time_least == 0))
+				if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0))
 					pce->time_least = elapsed;
 
-				if (pce->time_most < elapsed)
+				if (pce->time_most < (uint64_t)elapsed)
 					pce->time_most = elapsed;
 
+				// maintain mean and variance of the elapsed time in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = elapsed / 1e6f;
+				float delta_intvl = dt - pce->mean;
+				pce->mean += delta_intvl / pce->event_count;
+				pce->M2 += delta_intvl * (dt - pce->mean);
+
 				pce->time_start = 0;
 			}
 		}
@@ -310,14 +391,17 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+		float rms = sqrtf(pce->M2 / (pce->event_count-1));
 
-		dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
-		       handle->name,
+			handle->name,
-		       pce->event_count,
+			pce->event_count,
-		       pce->time_total,
+			pce->event_overruns,
-		       pce->time_total / pce->event_count,
+			pce->time_total,
-		       pce->time_least,
+			pce->time_total / pce->event_count,
-		       pce->time_most);
+			pce->time_least,
+			pce->time_most,
+			(double)(1e6f * rms));
 		break;
 	}
 
@@ -325,14 +409,13 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 		float rms = sqrtf(pci->M2 / (pci->event_count-1));
 
-		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n",
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
-		       handle->name,
+			handle->name,
-		       pci->event_count,
+			pci->event_count,
-		       (pci->time_last - pci->time_first) / pci->event_count,
+			(pci->time_last - pci->time_first) / pci->event_count,
-		       pci->time_least,
+			pci->time_least,
-		       pci->time_most,
+			pci->time_most,
-			   (double)(1000 * pci->mean),
+			(double)(1e6f * rms));
-			   (double)(1000 * rms));
 		break;
 	}
 

src/modules/systemlib/perf_counter.h

@@ -57,7 +57,7 @@ typedef struct perf_ctr_header	*perf_counter_t;
 __BEGIN_DECLS
 
 /**
- * Create a new counter.
+ * Create a new local counter.
  *
  * @param type			The type of the new counter.
  * @param name			The counter name.
@@ -66,6 +66,16 @@ __BEGIN_DECLS
  */
 __EXPORT extern perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name);
 
+/**
+ * Get the reference to an existing counter or create a new one if it does not exist.
+ *
+ * @param type			The type of the counter.
+ * @param name			The counter name.
+ * @return			Handle for the counter, or NULL if a counter
+ *				could not be allocated.
+ */
+__EXPORT extern perf_counter_t	perf_alloc_once(enum perf_counter_type type, const char *name);
+
 /**
  * Free a counter.
  *
@@ -102,6 +112,18 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  */
 __EXPORT extern void		perf_end(perf_counter_t handle);
 
+/**
+ * Register a measurement
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresponding perf_begin call. It sets the
+ * value provided as argument as a new measurement.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ * @param elapsed		The time elapsed. Negative values lead to incrementing the overrun counter.
+ */
+__EXPORT extern void		perf_set(perf_counter_t handle, int64_t elapsed);
+
 /**
  * Cancel a performance event.
  *