Merge branch 'master' into vfr_fix

ROMFS/px4fmu_common/init.d/10015_tbs_discovery

@@ -27,3 +27,4 @@ fi
 set MIXER FMU_quad_w
 
 set PWM_OUTPUTS 1234
+set PWM_MIN 1200

Tools/px_uploader.py

@@ -178,9 +178,9 @@ class uploader(object):
         MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0')
         MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac')
 
-        def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5):
+        def __init__(self, portname, baudrate):
                 # open the port, keep the default timeout short so we can poll quickly
-                self.port = serial.Serial(portname, baudrate)
+                self.port = serial.Serial(portname, baudrate, timeout=0.5)
                 self.otp = b''
                 self.sn = b''
 
@@ -195,7 +195,7 @@ class uploader(object):
         def __recv(self, count=1):
                 c = self.port.read(count)
                 if len(c) < 1:
-                        raise RuntimeError("timeout waiting for data (%u bytes)", count)
+                        raise RuntimeError("timeout waiting for data (%u bytes)" % count)
 #               print("recv " + binascii.hexlify(c))
                 return c
 
@@ -459,6 +459,7 @@ if os.path.exists("/usr/sbin/ModemManager"):
 # Load the firmware file
 fw = firmware(args.firmware)
 print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
+print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.")
 
 # Spin waiting for a device to show up
 while True:
@@ -508,9 +509,12 @@ while True:
                 except Exception:
                         # most probably a timeout talking to the port, no bootloader, try to reboot the board
                         print("attempting reboot on %s..." % port)
+                        print("if the board does not respond, unplug and re-plug the USB connector.")
                         up.send_reboot()
                         # wait for the reboot, without we might run into Serial I/O Error 5 
                         time.sleep(0.5)
+                        # always close the port
+                        up.close()
                         continue
 
                 try:

Tools/sdlog2/sdlog2_dump.py

@@ -154,8 +154,8 @@ class SDLog2Parser:
                         first_data_msg = False
                     self.__parseMsg(msg_descr)
             bytes_read += self.__ptr
-        if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
-            self.__printCSVRow()
+            if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
+                self.__printCSVRow()
         f.close()
     
     def __bytesLeft(self):

Tools/usb_serialload.py

@@ -0,0 +1,55 @@
+import serial, time
+
+
+port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2)
+
+data = '01234567890123456789012345678901234567890123456789'
+#data = 'hellohello'
+outLine = 'echo %s\n' % data
+
+port.write('\n\n\n')
+port.write('free\n')
+line = port.readline(80)
+while line != '':
+    print(line)
+    line = port.readline(80)
+    
+
+i = 0
+bytesOut = 0
+bytesIn = 0
+
+startTime = time.time()
+lastPrint = startTime
+while True:
+    bytesOut += port.write(outLine)
+    line = port.readline(80)
+    bytesIn += len(line)
+    # check command line echo
+    if (data not in line):
+        print('command error %d: %s' % (i,line))
+        #break
+    # read echo output
+    line = port.readline(80)
+    if (data not in line):
+        print('echo output error %d: %s' % (i,line))
+        #break
+    bytesIn += len(line)
+    #print('%d: %s' % (i,line))
+    #print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn))
+    
+    elapsedT = time.time() - lastPrint
+    if (time.time() - lastPrint >= 5):
+        outRate = bytesOut / elapsedT
+        inRate = bytesIn / elapsedT
+        usbRate = (bytesOut + bytesIn) / elapsedT
+        lastPrint = time.time()
+        print('elapsed time: %f' % (time.time() - startTime))
+        print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate))
+        
+        bytesOut = 0
+        bytesIn = 0
+        
+    i += 1    
+    #if (i > 2): break
+    

src/drivers/boards/px4io-v2/board_config.h

@@ -77,6 +77,7 @@
 #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
 #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
 #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
 
 /* Safety switch button *******************************************************/
 

src/drivers/boards/px4io-v2/px4iov2_init.c

@@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void)
 	stm32_configgpio(GPIO_LED1);
 	stm32_configgpio(GPIO_LED2);
 	stm32_configgpio(GPIO_LED3);
+	stm32_configgpio(GPIO_LED4);
 
 	stm32_configgpio(GPIO_BTN_SAFETY);
 

src/drivers/drv_pwm_output.h

@@ -213,6 +213,9 @@ ORB_DECLARE(output_pwm);
 /** make failsafe non-recoverable (termination) if it occurs */
 #define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
+/** force safety switch on (to enable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 26)
+
 /*
  *
  *

src/drivers/l3gd20/l3gd20.cpp

@@ -176,6 +176,7 @@ static const int ERROR = -1;
 #define L3G4200D_DEFAULT_RATE			800
 #define L3GD20_DEFAULT_RANGE_DPS		2000
 #define L3GD20_DEFAULT_FILTER_FREQ		30
+#define L3GD20_TEMP_OFFSET_CELSIUS		40
 
 #ifndef SENSOR_BOARD_ROTATION_DEFAULT
 #define SENSOR_BOARD_ROTATION_DEFAULT		SENSOR_BOARD_ROTATION_270_DEG
@@ -856,7 +857,7 @@ L3GD20::measure()
 #pragma pack(push, 1)
 	struct {
 		uint8_t		cmd;
-		uint8_t		temp;
+		int8_t		temp;
 		uint8_t		status;
 		int16_t		x;
 		int16_t		y;
@@ -930,6 +931,8 @@ L3GD20::measure()
 
 	report.z_raw = raw_report.z;
 
+	report.temperature_raw = raw_report.temp;
+
 	report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
 	report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
 	report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
@@ -938,6 +941,8 @@ L3GD20::measure()
 	report.y = _gyro_filter_y.apply(report.y);
 	report.z = _gyro_filter_z.apply(report.z);
 
+	report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
+
 	// apply user specified rotation
 	rotate_3f(_rotation, report.x, report.y, report.z);
 
@@ -1091,9 +1096,11 @@ test()
 	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
 	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
 	warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
+	warnx("temp: \t%d\tC", (int)g_report.temperature);
 	warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
 	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
 	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
+	warnx("temp: \t%d\traw", (int)g_report.temperature_raw);
 	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
 	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
 

src/drivers/ll40ls/ll40ls.cpp

@@ -73,14 +73,19 @@
 
 /* Configuration Constants */
 #define LL40LS_BUS 			PX4_I2C_BUS_EXPANSION
-#define LL40LS_BASEADDR 	0x42 /* 7-bit address */
-#define LL40LS_DEVICE_PATH 	"/dev/ll40ls"
+#define LL40LS_BASEADDR 	0x62 /* 7-bit address */
+#define LL40LS_BASEADDR_OLD 	0x42 /* previous 7-bit address */
+#define LL40LS_DEVICE_PATH_INT 	"/dev/ll40ls_int"
+#define LL40LS_DEVICE_PATH_EXT 	"/dev/ll40ls_ext"
 
 /* LL40LS Registers addresses */
 
 #define LL40LS_MEASURE_REG		0x00		/* Measure range register */
-#define LL40LS_MSRREG_ACQUIRE	0x04		/* Value to initiate a measurement, varies based on sensor revision */
+#define LL40LS_MSRREG_ACQUIRE	    0x04		/* Value to initiate a measurement, varies based on sensor revision */
 #define LL40LS_DISTHIGH_REG		0x8F		/* High byte of distance register, auto increment */
+#define LL40LS_WHO_AM_I_REG         0x11
+#define LL40LS_WHO_AM_I_REG_VAL         0xCA
+#define LL40LS_SIGNAL_STRENGTH_REG  0x5b
 
 /* Device limits */
 #define LL40LS_MIN_DISTANCE (0.00f)
@@ -101,7 +106,7 @@ static const int ERROR = -1;
 class LL40LS : public device::I2C
 {
 public:
-	LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR);
+	LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR);
 	virtual ~LL40LS();
 
 	virtual int 		init();
@@ -116,6 +121,7 @@ public:
 
 protected:
 	virtual int			probe();
+	virtual int			read_reg(uint8_t reg, uint8_t &val);
 
 private:
 	float				_min_distance;
@@ -132,6 +138,10 @@ private:
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
+	uint16_t		_last_distance;
+
+	/**< the bus the device is connected to */
+	int			_bus;
 
 	/**
 	* Test whether the device supported by the driver is present at a
@@ -188,8 +198,8 @@ private:
  */
 extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
 
-LL40LS::LL40LS(int bus, int address) :
-	I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000),
+LL40LS::LL40LS(int bus, const char *path, int address) :
+	I2C("LL40LS", path, bus, address, 100000),
 	_min_distance(LL40LS_MIN_DISTANCE),
 	_max_distance(LL40LS_MAX_DISTANCE),
 	_reports(nullptr),
@@ -200,10 +210,12 @@ LL40LS::LL40LS(int bus, int address) :
 	_range_finder_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows"))
+	_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")),
+	_last_distance(0),
+	_bus(bus)
 {
 	// up the retries since the device misses the first measure attempts
-	I2C::_retries = 3;
+	_retries = 3;
 
 	// enable debug() calls
 	_debug_enabled = false;
@@ -270,9 +282,51 @@ out:
 	return ret;
 }
 
+int
+LL40LS::read_reg(uint8_t reg, uint8_t &val)
+{
+	return transfer(&reg, 1, &val, 1);
+}
+
 int
 LL40LS::probe()
 {
+	// cope with both old and new I2C bus address
+	const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
+
+	// more retries for detection
+	_retries = 10;
+
+	for (uint8_t i=0; i<sizeof(addresses); i++) {
+		uint8_t val=0, who_am_i=0;
+
+		// set the I2C bus address
+		set_address(addresses[i]);
+
+		if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) {
+			// it is responding correctly to a WHO_AM_I
+			goto ok;
+		}
+
+		if (read_reg(LL40LS_SIGNAL_STRENGTH_REG, val) == OK && val != 0) {
+			// very likely to be a ll40ls. px4flow does not
+			// respond to this
+			goto ok;
+		}
+
+		debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n", 
+		      (unsigned)who_am_i, 
+		      LL40LS_WHO_AM_I_REG_VAL, 
+		      (unsigned)val);
+	}
+
+	// not found on any address
+	return -EIO;
+
+ok:
+	_retries = 3;
+
+	// start a measurement
 	return measure();
 }
 
@@ -521,6 +575,8 @@ LL40LS::collect()
 	float si_units = distance * 0.01f; /* cm to m */
 	struct range_finder_report report;
 
+	_last_distance = distance;
+
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
@@ -648,6 +704,8 @@ LL40LS::print_info()
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
 	_reports->print_info("report queue");
+	printf("distance: %ucm (0x%04x)\n", 
+	       (unsigned)_last_distance, (unsigned)_last_distance);
 }
 
 /**
@@ -662,55 +720,89 @@ namespace ll40ls
 #endif
 const int ERROR = -1;
 
-LL40LS	*g_dev;
+LL40LS	*g_dev_int;
+LL40LS	*g_dev_ext;
 
-void	start();
-void	stop();
-void	test();
-void	reset();
-void	info();
+void	start(int bus);
+void	stop(int bus);
+void	test(int bus);
+void	reset(int bus);
+void	info(int bus);
+void	usage();
 
 /**
  * Start the driver.
  */
 void
-start()
+start(int bus)
 {
-	int fd;
-
-	if (g_dev != nullptr) {
-		errx(1, "already started");
+	/* create the driver, attempt expansion bus first */
+	if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
+		if (g_dev_ext != nullptr)
+			errx(0, "already started external");
+		g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
+		if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
+			delete g_dev_ext;
+			g_dev_ext = nullptr;
+		}
 	}
 
-	/* create the driver */
-	g_dev = new LL40LS(LL40LS_BUS);
+#ifdef PX4_I2C_BUS_ONBOARD
+	/* if this failed, attempt onboard sensor */
+	if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
+		if (g_dev_int != nullptr)
+			errx(0, "already started internal");
+		g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
+		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 (g_dev == nullptr) {
-		goto fail;
-	}
-
-	if (OK != g_dev->init()) {
-		goto fail;
+			if (bus == PX4_I2C_BUS_ONBOARD) {
+				goto fail;
+			}
+		}
+		if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
+			goto fail;
+		}
 	}
+#endif
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+	if (g_dev_int != nullptr) {
+		int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY);
+                if (fd == -1) {
+                    goto fail;
+                }
+		int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+		close(fd);
+		if (ret < 0) {
+			goto fail;
+		}
+        }
 
-	if (fd < 0) {
-		goto fail;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		goto fail;
-	}
+	if (g_dev_ext != nullptr) {
+		int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
+                if (fd == -1) {
+                    goto fail;
+                }
+		int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+		close(fd);
+		if (ret < 0) {
+			goto fail;
+		}
+        }
 
 	exit(0);
 
 fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
+		delete g_dev_int;
+		g_dev_int = nullptr;
+	}
+	if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
+		delete g_dev_ext;
+		g_dev_ext = nullptr;
 	}
 
 	errx(1, "driver start failed");
@@ -719,11 +811,12 @@ fail:
 /**
  * Stop the driver
  */
-void stop()
+void stop(int bus)
 {
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext);
+	if (*g_dev != nullptr) {
+		delete *g_dev;
+		*g_dev = nullptr;
 
 	} else {
 		errx(1, "driver not running");
@@ -738,16 +831,17 @@ void stop()
  * and automatic modes.
  */
 void
-test()
+test(int bus)
 {
 	struct range_finder_report report;
 	ssize_t sz;
 	int ret;
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
 
-	int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH);
+		err(1, "%s open failed (try 'll40ls start' if the driver is not running", path);
 	}
 
 	/* do a simple demand read */
@@ -803,9 +897,10 @@ test()
  * Reset the driver.
  */
 void
-reset()
+reset(int bus)
 {
-	int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0) {
 		err(1, "failed ");
@@ -826,8 +921,9 @@ reset()
  * Print a little info about the driver.
  */
 void
-info()
+info(int bus)
 {
+	LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
 	if (g_dev == nullptr) {
 		errx(1, "driver not running");
 	}
@@ -838,44 +934,76 @@ info()
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info'");
+	warnx("options:");
+	warnx("    -X only external bus");
+#ifdef PX4_I2C_BUS_ONBOARD
+	warnx("    -I only internal bus");
+#endif
+}
+
 } // namespace
 
 int
 ll40ls_main(int argc, char *argv[])
 {
+	int ch;
+	int bus = -1;
+
+	while ((ch = getopt(argc, argv, "XI")) != EOF) {
+		switch (ch) {
+#ifdef PX4_I2C_BUS_ONBOARD
+		case 'I':
+			bus = PX4_I2C_BUS_ONBOARD;
+			break;
+#endif
+		case 'X':
+			bus = PX4_I2C_BUS_EXPANSION;
+			break;
+		default:
+			ll40ls::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];	
+	
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start")) {
-		ll40ls::start();
+	if (!strcmp(verb, "start")) {
+		ll40ls::start(bus);
 	}
 
 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop")) {
-		ll40ls::stop();
+	if (!strcmp(verb, "stop")) {
+		ll40ls::stop(bus);
 	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test")) {
-		ll40ls::test();
+	if (!strcmp(verb, "test")) {
+		ll40ls::test(bus);
 	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset")) {
-		ll40ls::reset();
+	if (!strcmp(verb, "reset")) {
+		ll40ls::reset(bus);
 	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
-		ll40ls::info();
+	if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
+		ll40ls::info(bus);
 	}
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");

src/drivers/mpu6000/mpu6000.cpp

@@ -229,6 +229,7 @@ private:
 	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_bad_transfers;
+	perf_counter_t		_good_transfers;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
@@ -404,6 +405,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
 	_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
+	_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
 	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -456,6 +458,7 @@ MPU6000::~MPU6000()
 	perf_free(_accel_reads);
 	perf_free(_gyro_reads);
 	perf_free(_bad_transfers);
+	perf_free(_good_transfers);
 }
 
 int
@@ -1279,8 +1282,14 @@ MPU6000::measure()
 		// all zero data - probably a SPI bus error
 		perf_count(_bad_transfers);
 		perf_end(_sample_perf);
+                // note that we don't call reset() here as a reset()
+                // costs 20ms with interrupts disabled. That means if
+                // the mpu6k does go bad it would cause a FMU failure,
+                // regardless of whether another sensor is available,
 		return;
 	}
+
+	perf_count(_good_transfers);
 	    
 
 	/*
@@ -1399,6 +1408,8 @@ MPU6000::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_accel_reads);
 	perf_print_counter(_gyro_reads);
+	perf_print_counter(_bad_transfers);
+	perf_print_counter(_good_transfers);
 	_accel_reports->print_info("accel queue");
 	_gyro_reports->print_info("gyro queue");
 }

src/drivers/px4flow/px4flow.cpp

@@ -76,7 +76,7 @@
 #define PX4FLOW_BUS 			PX4_I2C_BUS_EXPANSION
 #define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
 //range 0x42 - 0x49
-
+ 
 /* PX4FLOW Registers addresses */
 #define PX4FLOW_REG	0x00		/* Measure Register */
 
@@ -212,8 +212,9 @@ PX4FLOW::~PX4FLOW()
 	stop();
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
 }
 
 int
@@ -222,22 +223,25 @@ PX4FLOW::init()
 	int ret = ERROR;
 
 	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
+	if (I2C::init() != OK) {
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
 
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
 		goto out;
+	}
 
 	/* get a publish handle on the px4flow topic */
 	struct optical_flow_s zero_report;
 	memset(&zero_report, 0, sizeof(zero_report));
 	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
 
-	if (_px4flow_topic < 0)
+	if (_px4flow_topic < 0) {
 		debug("failed to create px4flow object. Did you start uOrb?");
+	}
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
@@ -249,6 +253,17 @@ out:
 int
 PX4FLOW::probe()
 {
+	uint8_t val[22];
+
+	// to be sure this is not a ll40ls Lidar (which can also be on
+	// 0x42) we check if a 22 byte transfer works from address
+	// 0. The ll40ls gives an error for that, whereas the flow
+	// happily returns some data
+	if (transfer(nullptr, 0, &val[0], 22) != OK) {
+		return -EIO;
+	}
+
+	// that worked, so start a measurement cycle
 	return measure();
 }
 
@@ -260,20 +275,20 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
 
-				/* switching to manual polling */
+			/* switching to manual polling */
 			case SENSOR_POLLRATE_MANUAL:
 				stop();
 				_measure_ticks = 0;
 				return OK;
 
-				/* external signalling (DRDY) not supported */
+			/* external signalling (DRDY) not supported */
 			case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
+			/* zero would be bad */
 			case 0:
 				return -EINVAL;
 
-				/* set default/max polling rate */
+			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT: {
 					/* do we need to start internal polling? */
@@ -283,13 +298,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 					_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start();
+					}
 
 					return OK;
 				}
 
-				/* adjust to a legal polling interval in Hz */
+			/* adjust to a legal polling interval in Hz */
 			default: {
 					/* do we need to start internal polling? */
 					bool want_start = (_measure_ticks == 0);
@@ -298,15 +314,17 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 					unsigned ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
+					if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
 						return -EINVAL;
+					}
 
 					/* update interval for next measurement */
 					_measure_ticks = ticks;
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start();
+					}
 
 					return OK;
 				}
@@ -314,25 +332,29 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
+		if (_measure_ticks == 0) {
 			return SENSOR_POLLRATE_MANUAL;
+		}
 
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-		/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 1) || (arg > 100))
-			return -EINVAL;
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
 
-		irqstate_t flags = irqsave();
-		if (!_reports->resize(arg)) {
 			irqrestore(flags);
-			return -ENOMEM;
-		}
-		irqrestore(flags);
 
-		return OK;
-	}
+			return OK;
+		}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
@@ -355,8 +377,9 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
 	int ret = 0;
 
 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
@@ -417,13 +440,13 @@ PX4FLOW::measure()
 	uint8_t cmd = PX4FLOW_REG;
 	ret = transfer(&cmd, 1, nullptr, 0);
 
-	if (OK != ret)
-	{
+	if (OK != ret) {
 		perf_count(_comms_errors);
 		log("i2c::transfer returned %d", ret);
-		 printf("i2c::transfer flow returned %d");
+		printf("i2c::transfer flow returned %d");
 		return ret;
 	}
+
 	ret = OK;
 
 	return ret;
@@ -435,14 +458,13 @@ PX4FLOW::collect()
 	int	ret = -EIO;
 
 	/* read from the sensor */
-	uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
+	uint8_t val[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 
 	perf_begin(_sample_perf);
 
 	ret = transfer(nullptr, 0, &val[0], 22);
 
-	if (ret < 0)
-	{
+	if (ret < 0) {
 		log("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
@@ -467,12 +489,12 @@ PX4FLOW::collect()
 	int16_t gdist = val[21] << 8 | val[20];
 
 	struct optical_flow_s report;
-	report.flow_comp_x_m = float(flowcx)/1000.0f;
-	report.flow_comp_y_m = float(flowcy)/1000.0f;
-	report.flow_raw_x= val[3] << 8 | val[2];
-	report.flow_raw_y= val[5] << 8 | val[4];
-	report.ground_distance_m =float(gdist)/1000.0f;
-	report.quality=  val[10];
+	report.flow_comp_x_m = float(flowcx) / 1000.0f;
+	report.flow_comp_y_m = float(flowcy) / 1000.0f;
+	report.flow_raw_x = val[3] << 8 | val[2];
+	report.flow_raw_y = val[5] << 8 | val[4];
+	report.ground_distance_m = float(gdist) / 1000.0f;
+	report.quality =  val[10];
 	report.sensor_id = 0;
 	report.timestamp = hrt_absolute_time();
 
@@ -509,11 +531,13 @@ PX4FLOW::start()
 		true,
 		true,
 		true,
-		SUBSYSTEM_TYPE_OPTICALFLOW};
+		SUBSYSTEM_TYPE_OPTICALFLOW
+	};
 	static orb_advert_t pub = -1;
 
 	if (pub > 0) {
 		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
 	} else {
 		pub = orb_advertise(ORB_ID(subsystem_info), &info);
 	}
@@ -567,8 +591,9 @@ PX4FLOW::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
+	if (OK != measure()) {
 		log("measure error");
+	}
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -619,33 +644,37 @@ start()
 {
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver */
 	g_dev = new PX4FLOW(PX4FLOW_BUS);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
-	if (OK != g_dev->init())
+	if (OK != g_dev->init()) {
 		goto fail;
+	}
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
 		goto fail;
+	}
 
 	exit(0);
 
 fail:
 
-	if (g_dev != nullptr)
-	{
+	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
@@ -658,15 +687,14 @@ fail:
  */
 void stop()
 {
-	if (g_dev != nullptr)
-	{
+	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
-	}
-	else
-	{
+
+	} else {
 		errx(1, "driver not running");
 	}
+
 	exit(0);
 }
 
@@ -684,14 +712,17 @@ test()
 
 	int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
 	if (sz != sizeof(report))
-	//	err(1, "immediate read failed");
+	{
+		warnx("immediate read failed");
+	}
 
 	warnx("single read");
 	warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
@@ -700,8 +731,9 @@ test()
 
 
 	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
+	}
 
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 5; i++) {
@@ -712,14 +744,16 @@ test()
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 2000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "periodic read failed");
+		}
 
 		warnx("periodic read %u", i);
 		warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
@@ -740,14 +774,17 @@ reset()
 {
 	int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -758,8 +795,9 @@ reset()
 void
 info()
 {
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		errx(1, "driver not running");
+	}
 
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
@@ -775,32 +813,37 @@ px4flow_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		px4flow::start();
+	}
 
-	 /*
-	  * Stop the driver
-	  */
-	 if (!strcmp(argv[1], "stop"))
-		 px4flow::stop();
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		px4flow::stop();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		px4flow::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		px4flow::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		px4flow::info();
+	}
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
 }

src/drivers/px4fmu/fmu.cpp

@@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
 	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 		// these are no-ops, as no safety switch
 		break;
 

src/drivers/px4io/px4io.cpp

@@ -1247,28 +1247,40 @@ PX4IO::io_set_rc_config()
 	 */
 	param_get(param_find("RC_MAP_ROLL"), &ichan);
 
+	/* subtract one from 1-based index - this might be
+	 * a negative number now
+	 */
+	ichan -= 1;
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 0;
+		input_map[ichan] = 0;
 
 	param_get(param_find("RC_MAP_PITCH"), &ichan);
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 1;
+		input_map[ichan] = 1;
 
 	param_get(param_find("RC_MAP_YAW"), &ichan);
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 2;
+		input_map[ichan] = 2;
 
 	param_get(param_find("RC_MAP_THROTTLE"), &ichan);
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 3;
+		input_map[ichan] = 3;
+
+	param_get(param_find("RC_MAP_FLAPS"), &ichan);
+
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+		input_map[ichan] = 4;
 
 	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 4;
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
+		/* use out of normal bounds index to indicate special channel */
+		input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
+	}
 
 	/*
 	 * Iterate all possible RC inputs.
@@ -2271,6 +2283,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
 		break;
 
+	case PWM_SERVO_SET_FORCE_SAFETY_ON:
+		/* force safety switch on */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
+		break;
+
 	case PWM_SERVO_SET_FORCE_FAILSAFE:
 		/* force failsafe mode instantly */
 		if (arg == 0) {

src/lib/conversion/rotation.h

@@ -74,6 +74,7 @@ enum Rotation {
 	ROTATION_ROLL_270_YAW_135    = 23,
 	ROTATION_PITCH_90            = 24,
 	ROTATION_PITCH_270           = 25,
+	ROTATION_ROLL_270_YAW_270    = 26,
 	ROTATION_MAX
 };
 
@@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = {
 	{270,   0,  90 },
 	{270,   0, 135 },
 	{  0,  90,   0 },
-	{  0, 270,   0 }
+	{  0, 270,   0 },
+	{270,   0, 270 }
 };
 
 /**

src/lib/external_lgpl/tecs/tecs.cpp

@@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 	// Calculate throttle demand
 	// If underspeed condition is set, then demand full throttle
 	if (_underspeed) {
-		_throttle_dem_unc = 1.0f;
+		_throttle_dem = 1.0f;
 
 	} else {
 		// Calculate gain scaler from specific energy error to throttle
@@ -363,10 +363,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 		} else {
 			_throttle_dem = ff_throttle;
 		}
-	}
 
-	// Constrain throttle demand
-	_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+		// Constrain throttle demand
+		_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+	}
 }
 
 void TECS::_detect_bad_descent(void)

src/lib/external_lgpl/tecs/tecs.h

@@ -345,9 +345,6 @@ private:
 	// climbout mode
 	bool _climbOutDem;
 
-	// throttle demand before limiting
-	float _throttle_dem_unc;
-
 	// pitch demand before limiting
 	float _pitch_dem_unc;
 

src/modules/commander/accelerometer_calibration.cpp

@@ -83,7 +83,7 @@
  *     | accel_T[1][i] |
  *     [ accel_T[2][i] ]
  *
- * b = [ accel_corr_ref[0][i] ]	// One measurement per axis is enough
+ * b = [ accel_corr_ref[0][i] ]	// One measurement per side is enough
  *     | accel_corr_ref[2][i] |
  *     [ accel_corr_ref[4][i] ]
  *
@@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd)
 
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
+	mavlink_log_info(mavlink_fd, "You need to put the system on all six sides");
+	sleep(3);
+	mavlink_log_info(mavlink_fd, "Follow the instructions on the screen");
+	sleep(5);
+
 	struct accel_scale accel_scale = {
 		0.0f,
 		1.0f,
@@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 	const int samples_num = 2500;
 	float accel_ref[6][3];
 	bool data_collected[6] = { false, false, false, false, false, false };
-	const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
+	const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
 
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 
@@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			break;
 		}
 
-		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
-				 (!data_collected[0]) ? "x+ " : "",
-				 (!data_collected[1]) ? "x- " : "",
-				 (!data_collected[2]) ? "y+ " : "",
-				 (!data_collected[3]) ? "y- " : "",
-				 (!data_collected[4]) ? "z+ " : "",
-				 (!data_collected[5]) ? "z- " : "");
+		/* inform user which axes are still needed */
+		mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
+				 (!data_collected[0]) ? "front " : "",
+				 (!data_collected[1]) ? "back " : "",
+				 (!data_collected[2]) ? "left " : "",
+				 (!data_collected[3]) ? "right " : "",
+				 (!data_collected[4]) ? "up " : "",
+				 (!data_collected[5]) ? "down " : "");
+
+		/* allow user enough time to read the message */
+		sleep(3);
 
 		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
 
 		if (orient < 0) {
-			res = ERROR;
-			break;
-		}
-
-		if (data_collected[orient]) {
-			mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
+			mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
+			sleep(3);
 			continue;
 		}
 
-		mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
+		/* inform user about already handled side */
+		if (data_collected[orient]) {
+			mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
+			sleep(4);
+			continue;
+		}
+
+		mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
+		sleep(1);
 		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
-		mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
+		mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
 				 (double)accel_ref[orient][0],
 				 (double)accel_ref[orient][1],
 				 (double)accel_ref[orient][2]);
@@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
-					mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
+					mavlink_log_info(mavlink_fd, "detected rest position, hold still...");
 					t_still = t;
 					t_timeout = t + timeout;
 
@@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 				/* not still, reset still start time */
 				if (t_still != 0) {
 					mavlink_log_info(mavlink_fd, "detected motion, hold still...");
+					sleep(3);
 					t_still = 0;
 				}
 			}

src/modules/commander/airspeed_calibration.cpp

@@ -61,6 +61,15 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "dpress";
 
+#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
+
+static void feedback_calibration_failed(int mavlink_fd)
+{
+	sleep(5);
+	mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+	mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
+}
+
 int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
@@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd)
 		float analog_scaling = 0.0f;
 		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
 		if (fabsf(analog_scaling) < 0.1f) {
-			mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
+			mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+			feedback_calibration_failed(mavlink_fd);
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd)
 		}
 
 	} else {
-		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		feedback_calibration_failed(mavlink_fd);
 		close(diff_pres_sub);
 		return ERROR;
 	}
@@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
 				if (calibration_counter % 500 == 0) {
-					mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
+					mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
 				}
 				continue;
@@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			/* do not allow negative values */
 			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
-				mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
 				mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
+				mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
 				close(diff_pres_sub);
 
 				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 				close(diff_pres_sub);
 
-				mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+				feedback_calibration_failed(mavlink_fd);
 				return ERROR;
 			} else {
 				mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
@@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+			feedback_calibration_failed(mavlink_fd);
 			close(diff_pres_sub);
 			return ERROR;
 		}
 	}
 
 	if (calibration_counter == maxcount) {
-		mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		feedback_calibration_failed(mavlink_fd);
 		close(diff_pres_sub);
 		return ERROR;
 	}

src/modules/commander/commander.cpp

@@ -207,7 +207,9 @@ void usage(const char *reason);
 /**
  * React to commands that are sent e.g. from the mavlink module.
  */
-bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub);
+bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
+		    struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
+		    orb_advert_t *home_pub);
 
 /**
  * Mainloop of commander.
@@ -230,7 +232,8 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *status,
+		struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
 transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
 
@@ -260,7 +263,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
-					     2950,
+					     3200,
 					     commander_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 
@@ -393,7 +396,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 
 	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
 	// output appropriate error messages if the state cannot transition.
-	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed,
+					     true /* fRunPreArmChecks */, mavlink_fd_local);
 
 	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
 		mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -406,11 +410,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 }
 
 bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
-	struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
-	struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
+		    struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
+		    struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
 {
 	/* only handle commands that are meant to be handled by this system and component */
-	if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
+	if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
+			&& (cmd->target_component != 0))) { // component_id 0: valid for all components
 		return false;
 	}
 
@@ -537,13 +542,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 
 			} else {
 				mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
-					(double)cmd->param1,
-					(double)cmd->param2,
-					(double)cmd->param3,
-					(double)cmd->param4,
-					(double)cmd->param5,
-					(double)cmd->param6,
-					(double)cmd->param7);
+						     (double)cmd->param1,
+						     (double)cmd->param2,
+						     (double)cmd->param3,
+						     (double)cmd->param4,
+						     (double)cmd->param5,
+						     (double)cmd->param6,
+						     (double)cmd->param7);
 			}
 		}
 		break;
@@ -554,35 +559,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				//XXX update state machine?
 				armed_local->force_failsafe = true;
 				warnx("forcing failsafe (termination)");
+
 			} else {
 				armed_local->force_failsafe = false;
 				warnx("disabling failsafe (termination)");
 			}
+
 			/* param2 is currently used for other failsafe modes */
 			status_local->engine_failure_cmd = false;
 			status_local->data_link_lost_cmd = false;
 			status_local->gps_failure_cmd = false;
 			status_local->rc_signal_lost_cmd = false;
+
 			if ((int)cmd->param2 <= 0) {
 				/* reset all commanded failure modes */
 				warnx("reset all non-flighttermination failsafe commands");
+
 			} else if ((int)cmd->param2 == 1) {
 				/* trigger engine failure mode */
 				status_local->engine_failure_cmd = true;
 				warnx("engine failure mode commanded");
+
 			} else if ((int)cmd->param2 == 2) {
 				/* trigger data link loss mode */
 				status_local->data_link_lost_cmd = true;
 				warnx("data link loss mode commanded");
+
 			} else if ((int)cmd->param2 == 3) {
 				/* trigger gps loss mode */
 				status_local->gps_failure_cmd = true;
 				warnx("gps loss mode commanded");
+
 			} else if ((int)cmd->param2 == 4) {
 				/* trigger rc loss mode */
 				status_local->rc_signal_lost_cmd = true;
 				warnx("rc loss mode commanded");
 			}
+
 			cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 		}
 		break;
@@ -633,21 +646,27 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			}
 		}
 		break;
+
 	case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
 			transition_result_t res = TRANSITION_DENIED;
 			static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
+
 			if (status_local->main_state != MAIN_STATE_OFFBOARD) {
 				main_state_pre_offboard = status_local->main_state;
 			}
+
 			if (cmd->param1 > 0.5f) {
 				res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+
 				if (res == TRANSITION_DENIED) {
 					print_reject_mode(status_local, "OFFBOARD");
 					status_local->offboard_control_set_by_command = false;
+
 				} else {
 					/* Set flag that offboard was set via command, main state is not overridden by rc */
 					status_local->offboard_control_set_by_command = true;
 				}
+
 			} else {
 				/* If the mavlink command is used to enable or disable offboard control:
 				 * switch back to previous mode when disabling */
@@ -656,6 +675,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			}
 		}
 		break;
+
 	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -802,6 +822,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+
 	if (status_pub < 0) {
 		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
 		warnx("exiting.");
@@ -827,11 +848,14 @@ int commander_thread_main(int argc, char *argv[])
 	/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
 	orb_advert_t mission_pub = -1;
 	mission_s mission;
+
 	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
 		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
-			warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq);
+			warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count,
+			      mission.current_seq);
 			mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
-												mission.dataman_id, mission.count, mission.current_seq);
+					 mission.dataman_id, mission.count, mission.current_seq);
+
 		} else {
 			const char *missionfail = "reading mission state failed";
 			warnx("%s", missionfail);
@@ -1097,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
 				status.offboard_control_signal_lost = false;
 				status_changed = true;
 			}
+
 		} else {
 			if (!status.offboard_control_signal_lost) {
 				status.offboard_control_signal_lost = true;
@@ -1115,9 +1140,9 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* perform system checks when new telemetry link connected */
 				if (mavlink_fd &&
-					telemetry_last_heartbeat[i] == 0 &&
-					telemetry.heartbeat_time > 0 &&
-					hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
+				    telemetry_last_heartbeat[i] == 0 &&
+				    telemetry.heartbeat_time > 0 &&
+				    hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
 
 					(void)rc_calibration_check(mavlink_fd);
 				}
@@ -1130,6 +1155,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+
 			/* Check if the barometer is healthy and issue a warning in the GCS if not so.
 			 * Because the barometer is used for calculating AMSL altitude which is used to ensure
 			 * vertical separation from other airtraffic the operator has to know when the
@@ -1142,6 +1168,7 @@ int commander_thread_main(int argc, char *argv[])
 					status_changed = true;
 					mavlink_log_critical(mavlink_fd, "baro healthy");
 				}
+
 			} else {
 				if (!status.barometer_failure) {
 					status.barometer_failure = true;
@@ -1164,10 +1191,11 @@ int commander_thread_main(int argc, char *argv[])
 
 			if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
 				if (system_power.servo_valid &&
-					!system_power.brick_valid &&
-					!system_power.usb_connected) {
+				    !system_power.brick_valid &&
+				    !system_power.usb_connected) {
 					/* flying only on servo rail, this is unsafe */
 					status.condition_power_input_valid = false;
+
 				} else {
 					status.condition_power_input_valid = true;
 				}
@@ -1187,9 +1215,11 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* disarm if safety is now on and still armed */
 			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
-				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
+								   ARMING_STATE_STANDBY_ERROR);
 
-				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
+						true /* fRunPreArmChecks */, mavlink_fd)) {
 					mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
 					arming_state_changed = true;
 				}
@@ -1233,7 +1263,8 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
+			    &status_changed);
 
 		/* update home position */
 		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
@@ -1283,8 +1314,11 @@ int commander_thread_main(int argc, char *argv[])
 				local_eph_good = false;
 			}
 		}
-		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
-		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
+
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid
+			    && local_eph_good, &(status.condition_local_position_valid), &status_changed);
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,
+			    &(status.condition_local_altitude_valid), &status_changed);
 
 		if (status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
@@ -1315,7 +1349,8 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* get throttle (if armed), as we only care about energy negative throttle also counts */
 				float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
-				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
+				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah,
+							   throttle);
 			}
 		}
 
@@ -1372,8 +1407,8 @@ int commander_thread_main(int argc, char *argv[])
 			last_idle_time = system_load.tasks[0].total_runtime;
 
 			/* check if board is connected via USB */
-			//struct stat statbuf;
-			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
+			struct stat statbuf;
+			on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
 		}
 
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
@@ -1383,26 +1418,30 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
 
-		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+		} else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
+			   && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
+								     mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
 				}
 
 			} else {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
+								     mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
 				}
 			}
+
 			status_changed = true;
 		}
 
@@ -1411,7 +1450,8 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			/* TODO: check for sensors */
-			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
+							     mavlink_fd);
 
 			if (arming_ret == TRANSITION_CHANGED) {
 				arming_state_changed = true;
@@ -1439,23 +1479,25 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* Initialize map projection if gps is valid */
 		if (!map_projection_global_initialized()
-				&& (gps_position.eph < eph_threshold)
-				&& (gps_position.epv < epv_threshold)
-				&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
+		    && (gps_position.eph < eph_threshold)
+		    && (gps_position.epv < epv_threshold)
+		    && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) {
 			/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
-			globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
+			globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
+						  (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
 		}
 
 		/* check if GPS fix is ok */
 		if (status.circuit_breaker_engaged_gpsfailure_check ||
-				(gps_position.fix_type >= 3 &&
-				hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
+		    (gps_position.fix_type >= 3 &&
+		     hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
 			/* handle the case where gps was regained */
 			if (status.gps_failure) {
 				status.gps_failure = false;
 				status_changed = true;
 				mavlink_log_critical(mavlink_fd, "gps regained");
 			}
+
 		} else {
 			if (!status.gps_failure) {
 				status.gps_failure = true;
@@ -1477,12 +1519,14 @@ int commander_thread_main(int argc, char *argv[])
 				armed.force_failsafe = true;
 				status_changed = true;
 				static bool flight_termination_printed = false;
+
 				if (!flight_termination_printed) {
 					warnx("Flight termination because of navigator request or geofence");
 					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
 					flight_termination_printed = true;
 				}
-				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
+
+				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
 				}
 			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
@@ -1490,7 +1534,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
-				hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
+		    hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
@@ -1515,11 +1559,15 @@ int commander_thread_main(int argc, char *argv[])
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
-					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
+									   ARMING_STATE_STANDBY_ERROR);
+					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
+									     mavlink_fd);
+
 					if (arming_ret == TRANSITION_CHANGED) {
 						arming_state_changed = true;
 					}
+
 					stick_off_counter = 0;
 
 				} else {
@@ -1541,8 +1589,11 @@ int commander_thread_main(int argc, char *argv[])
 					 */
 					if (status.main_state != MAIN_STATE_MANUAL) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+
 					} else {
-						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
+										     mavlink_fd);
+
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
 						}
@@ -1565,6 +1616,7 @@ int commander_thread_main(int argc, char *argv[])
 				} else {
 					mavlink_log_info(mavlink_fd, "DISARMED by RC");
 				}
+
 				arming_state_changed = true;
 
 			} else if (arming_ret == TRANSITION_DENIED) {
@@ -1600,18 +1652,20 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* data links check */
 		bool have_link = false;
+
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
 			if (telemetry_last_heartbeat[i] != 0 &&
-					hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
+			    hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
 				/* handle the case where data link was regained,
 				 * accept datalink as healthy only after datalink_regain_timeout seconds
 				 * */
 				if (telemetry_lost[i] &&
-					hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
+				    hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
 
 					mavlink_log_critical(mavlink_fd, "data link %i regained", i);
 					telemetry_lost[i] = false;
 					have_link = true;
+
 				} else if (!telemetry_lost[i]) {
 					/* telemetry was healthy also in last iteration
 					 * we don't have to check a timeout */
@@ -1620,6 +1674,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			} else {
 				telemetry_last_dl_loss[i]  = hrt_absolute_time();
+
 				if (!telemetry_lost[i]) {
 					mavlink_log_critical(mavlink_fd, "data link %i lost", i);
 					telemetry_lost[i] = true;
@@ -1647,24 +1702,26 @@ int commander_thread_main(int argc, char *argv[])
 		 * only for fixed wing for now
 		 */
 		if (!status.circuit_breaker_engaged_enginefailure_check &&
-				status.is_rotary_wing == false &&
-				armed.armed &&
-				((actuator_controls.control[3] > ef_throttle_thres &&
-				battery.current_a/actuator_controls.control[3] <
-				ef_current2throttle_thres) ||
-				 (status.engine_failure))) {
+		    status.is_rotary_wing == false &&
+		    armed.armed &&
+		    ((actuator_controls.control[3] > ef_throttle_thres &&
+		      battery.current_a / actuator_controls.control[3] <
+		      ef_current2throttle_thres) ||
+		     (status.engine_failure))) {
 			/* potential failure, measure time */
 			if (timestamp_engine_healthy > 0 &&
-					hrt_elapsed_time(&timestamp_engine_healthy) >
-					ef_time_thres * 1e6 &&
-					!status.engine_failure) {
-					status.engine_failure = true;
-					status_changed = true;
-					mavlink_log_critical(mavlink_fd, "Engine Failure");
+			    hrt_elapsed_time(&timestamp_engine_healthy) >
+			    ef_time_thres * 1e6 &&
+			    !status.engine_failure) {
+				status.engine_failure = true;
+				status_changed = true;
+				mavlink_log_critical(mavlink_fd, "Engine Failure");
 			}
+
 		} else {
 			/* no failure reset flag */
 			timestamp_engine_healthy = hrt_absolute_time();
+
 			if (status.engine_failure) {
 				status.engine_failure = false;
 				status_changed = true;
@@ -1691,20 +1748,22 @@ int commander_thread_main(int argc, char *argv[])
 			 * If  we are not in a manual (RC stick controlled mode)
 			 * and both failed we want to terminate the flight */
 			if (status.main_state != MAIN_STATE_MANUAL &&
-					status.main_state != MAIN_STATE_ACRO &&
-					status.main_state != MAIN_STATE_ALTCTL &&
-					status.main_state != MAIN_STATE_POSCTL &&
-					((status.data_link_lost && status.gps_failure) ||
-					(status.data_link_lost_cmd && status.gps_failure_cmd))) {
+			    status.main_state != MAIN_STATE_ACRO &&
+			    status.main_state != MAIN_STATE_ALTCTL &&
+			    status.main_state != MAIN_STATE_POSCTL &&
+			    ((status.data_link_lost && status.gps_failure) ||
+			     (status.data_link_lost_cmd && status.gps_failure_cmd))) {
 				armed.force_failsafe = true;
 				status_changed = true;
 				static bool flight_termination_printed = false;
+
 				if (!flight_termination_printed) {
 					warnx("Flight termination because of data link loss && gps failure");
 					mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
 					flight_termination_printed = true;
 				}
-				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
+
+				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 					mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
 				}
 			}
@@ -1713,19 +1772,21 @@ int commander_thread_main(int argc, char *argv[])
 			 * If we are in manual (controlled with RC):
 			 * if both failed we want to terminate the flight */
 			if ((status.main_state == MAIN_STATE_ACRO ||
-					status.main_state == MAIN_STATE_MANUAL ||
-					status.main_state == MAIN_STATE_ALTCTL ||
-					status.main_state == MAIN_STATE_POSCTL) &&
-					((status.rc_signal_lost && status.gps_failure) ||
-					(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
+			     status.main_state == MAIN_STATE_MANUAL ||
+			     status.main_state == MAIN_STATE_ALTCTL ||
+			     status.main_state == MAIN_STATE_POSCTL) &&
+			    ((status.rc_signal_lost && status.gps_failure) ||
+			     (status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
 				armed.force_failsafe = true;
 				status_changed = true;
 				static bool flight_termination_printed = false;
+
 				if (!flight_termination_printed) {
 					warnx("Flight termination because of RC signal loss && gps failure");
 					flight_termination_printed = true;
 				}
-				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
+
+				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 					mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination");
 				}
 			}
@@ -1766,6 +1827,7 @@ int commander_thread_main(int argc, char *argv[])
 				/* mark home position as set */
 				status.condition_home_position_valid = true;
 			}
+
 			arming_state_changed = false;
 		}
 
@@ -1810,7 +1872,8 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* play arming and battery warning tunes */
-		if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
+		if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available
+							&& safety.safety_off))) {
 			/* play tune when armed */
 			set_tune(TONE_ARMING_WARNING_TUNE);
 			arm_tune_played = true;
@@ -1988,6 +2051,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	/* offboard switch overrides main switch */
 	if (sp_man->offboard_switch == SWITCH_POS_ON) {
 		res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+
 		if (res == TRANSITION_DENIED) {
 			print_reject_mode(status_local, "OFFBOARD");
 
@@ -2009,6 +2073,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		} else {
 			res = main_state_transition(status_local, MAIN_STATE_MANUAL);
 		}
+
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -2023,7 +2088,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			print_reject_mode(status_local, "POSCTL");
 		}
 
-        // fallback to ALTCTL
+		// fallback to ALTCTL
 		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
@@ -2047,14 +2112,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 				break;	// changed successfully or already in this state
 			}
 
-            print_reject_mode(status_local, "AUTO_RTL");
+			print_reject_mode(status_local, "AUTO_RTL");
 
-            // fallback to LOITER if home position not set
-            res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+			// fallback to LOITER if home position not set
+			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
 
-            if (res != TRANSITION_DENIED) {
-                break;  // changed successfully or already in this state
-            }
+			if (res != TRANSITION_DENIED) {
+				break;  // changed successfully or already in this state
+			}
 
 		} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
 			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
@@ -2063,7 +2128,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 				break;	// changed successfully or already in this state
 			}
 
-            print_reject_mode(status_local, "AUTO_LOITER");
+			print_reject_mode(status_local, "AUTO_LOITER");
 
 		} else {
 			res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
@@ -2072,22 +2137,22 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 				break;	// changed successfully or already in this state
 			}
 
-            print_reject_mode(status_local, "AUTO_MISSION");
+			print_reject_mode(status_local, "AUTO_MISSION");
 
-            // fallback to LOITER if home position not set
-            res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+			// fallback to LOITER if home position not set
+			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
 
-            if (res != TRANSITION_DENIED) {
-                break;  // changed successfully or already in this state
-            }
+			if (res != TRANSITION_DENIED) {
+				break;  // changed successfully or already in this state
+			}
 		}
 
-        // fallback to POSCTL
-        res = main_state_transition(status_local, MAIN_STATE_POSCTL);
+		// fallback to POSCTL
+		res = main_state_transition(status_local, MAIN_STATE_POSCTL);
 
-        if (res != TRANSITION_DENIED) {
-            break;  // changed successfully or already in this state
-        }
+		if (res != TRANSITION_DENIED) {
+			break;  // changed successfully or already in this state
+		}
 
 		// fallback to ALTCTL
 		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
@@ -2169,6 +2234,7 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
+
 		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
 			control_mode.flag_control_rates_enabled = true;
 			control_mode.flag_control_attitude_enabled = true;
@@ -2177,6 +2243,7 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
+
 		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
 			control_mode.flag_control_rates_enabled = true;
 			control_mode.flag_control_attitude_enabled = false;
@@ -2186,6 +2253,7 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
+
 		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
 		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
 		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
@@ -2198,6 +2266,7 @@ set_control_mode()
 			control_mode.flag_control_velocity_enabled = true;
 			//XXX: the flags could depend on sp_offboard.ignore
 			break;
+
 		default:
 			control_mode.flag_control_rates_enabled = false;
 			control_mode.flag_control_attitude_enabled = false;
@@ -2206,6 +2275,7 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 		}
+
 		break;
 
 	case NAVIGATION_STATE_POSCTL:
@@ -2415,7 +2485,8 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed,
+						true /* fRunPreArmChecks */, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}

src/modules/commander/gyro_calibration.cpp

@@ -63,7 +63,10 @@ static const char *sensor_name = "gyro";
 int do_gyro_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "don't move system");
+	mavlink_log_info(mavlink_fd, "HOLD STILL");
+
+	/* wait for the user to respond */
+	sleep(2);
 
 	struct gyro_scale gyro_scale = {
 		0.0f,

src/modules/commander/mag_calibration.cpp

@@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd)
 		uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
 		unsigned poll_errcount = 0;
 
-		mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
+		mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
 
 		calibration_counter = 0;
 

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -597,12 +597,12 @@ FixedwingEstimator::check_filter_state()
 
 	const char* const feedback[] = { 0,
 					"NaN in states, resetting",
-					"stale IMU data, resetting",
+					"stale sensor data, resetting",
 					"got initial position lock",
 					"excessive gyro offsets",
-					"GPS velocity divergence",
+					"velocity diverted, check accel config",
 					"excessive covariances",
-					"unknown condition"};
+					"unknown condition, resetting"};
 
 	// Print out error condition
 	if (check) {
@@ -614,7 +614,7 @@ FixedwingEstimator::check_filter_state()
 		}
 
 		warnx("reset: %s", feedback[warn_index]);
-		mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]);
+		mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
 	}
 
 	struct estimator_status_report rep;

src/modules/mavlink/mavlink_main.cpp

@@ -763,7 +763,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
 	_last_write_try_time = hrt_absolute_time();
 
 	/* check if there is space in the buffer, let it overflow else */
-	if (buf_free < TX_BUFFER_GAP) {
+	if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
 		/* no enough space in buffer to send */
 		count_txerr();
 		count_txerrbytes(packet_len);
@@ -1634,7 +1634,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2700,
+		       2900,
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);
 

src/modules/mavlink/mavlink_messages.cpp

@@ -341,11 +341,18 @@ private:
 	/* do not allow top copying this class */
 	MavlinkStreamStatustext(MavlinkStreamStatustext &);
 	MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
+	FILE *fp = nullptr;
 
 protected:
 	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
+	~MavlinkStreamStatustext() {
+		if (fp) {
+			fclose(fp);
+		}
+	}
+
 	void send(const hrt_abstime t)
 	{
 		if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) {
@@ -359,6 +366,31 @@ protected:
 				strncpy(msg.text, logmsg.text, sizeof(msg.text));
 
 				_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
+
+				/* write log messages in first instance to disk */
+				if (_mavlink->get_instance_id() == 0) {
+					if (fp) {
+						fputs(msg.text, fp);
+						fputs("\n", fp);
+						fsync(fileno(fp));
+					} else {
+						/* string to hold the path to the log */
+						char log_file_name[32] = "";
+						char log_file_path[64] = "";
+
+						timespec ts;
+						clock_gettime(CLOCK_REALTIME, &ts);
+						/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
+						time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+						struct tm t;
+						gmtime_r(&gps_time_sec, &t);
+
+						// XXX we do not want to interfere here with the SD log app
+						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
+						snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
+						fp = fopen(log_file_path, "ab");
+					}
+				}
 			}
 		}
 	}

src/modules/mavlink/mavlink_receiver.cpp

@@ -391,11 +391,9 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 
 	f.timestamp = hrt_absolute_time();
 	f.flow_timestamp = flow.time_usec;
-	f.flow_raw_x = flow.flow_x;
-	f.flow_raw_y = flow.flow_y;
-	f.flow_comp_x_m = flow.flow_comp_m_x;
-	f.flow_comp_y_m = flow.flow_comp_m_y;
-	f.ground_distance_m = flow.ground_distance;
+	f.flow_raw_x = flow.integrated_x;
+	f.flow_raw_y = flow.integrated_y;
+	f.ground_distance_m = flow.distance;
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
 
@@ -413,7 +411,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 	r.timestamp = hrt_absolute_time();
 	r.error_count = 0;
 	r.type = RANGE_FINDER_TYPE_LASER;
-	r.distance = flow.ground_distance;
+	r.distance = flow.distance;
 	r.minimum_distance = 0.0f;
 	r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
 	r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
@@ -1398,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
-	param.sched_priority = SCHED_PRIORITY_MAX - 40;
+	param.sched_priority = SCHED_PRIORITY_MAX - 80;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
 	pthread_attr_setstacksize(&receiveloop_attr, 2900);

src/modules/px4iofirmware/controls.c

@@ -60,6 +60,8 @@ static perf_counter_t c_gather_ppm;
 
 static int _dsm_fd;
 
+static uint16_t rc_value_override = 0;
+
 bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 {
 	perf_begin(c_gather_dsm);
@@ -316,6 +318,9 @@ controls_tick() {
 					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
 					assigned_channels |= (1 << mapped);
 
+				} else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
+					/* pick out override channel, indicated by special mapping */
+					rc_value_override = SIGNED_TO_REG(scaled);
 				}
 			}
 		}
@@ -409,7 +414,7 @@ controls_tick() {
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
 		if (override) {

src/modules/px4iofirmware/mixer.cpp

@@ -60,13 +60,6 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		500000
 
-/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
-#define ROLL     0
-#define PITCH    1
-#define YAW      2
-#define THROTTLE 3
-#define OVERRIDE 4
-
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
 static bool should_arm = false;

src/modules/px4iofirmware/protocol.h

@@ -221,6 +221,7 @@ enum {							/* DSM bind states */
                                                            hence index 12 can safely be used. */
 #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
 
+#define PX4IO_P_SETUP_FORCE_SAFETY_ON		14	/* force safety switch into 'disarmed' (PWM disabled state) */
 #define PX4IO_FORCE_SAFETY_MAGIC		22027	/* required argument for force safety (random) */
 
 /* autopilot control values, -10000..10000 */
@@ -246,6 +247,7 @@ enum {							/* DSM bind states */
 #define PX4IO_P_RC_CONFIG_MAX			2		/**< highest input value */
 #define PX4IO_P_RC_CONFIG_DEADZONE		3		/**< band around center that is ignored */
 #define PX4IO_P_RC_CONFIG_ASSIGNMENT		4		/**< mapped input value */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH	100		/**< magic value for mode switch */
 #define PX4IO_P_RC_CONFIG_OPTIONS		5		/**< channel options bitmask */
 #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
 #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)

src/modules/px4iofirmware/px4io.c

@@ -124,6 +124,9 @@ heartbeat_blink(void)
 {
 	static bool heartbeat = false;
 	LED_BLUE(heartbeat = !heartbeat);
+#ifdef GPIO_LED4
+	LED_RING(heartbeat);
+#endif
 }
 
 static uint64_t reboot_time;
@@ -191,6 +194,9 @@ user_start(int argc, char *argv[])
 	LED_AMBER(false);
 	LED_BLUE(false);
 	LED_SAFETY(false);
+#ifdef GPIO_LED4
+	LED_RING(false);
+#endif
 
 	/* turn on servo power (if supported) */
 #ifdef POWER_SERVO

src/modules/px4iofirmware/px4io.h

@@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit;
 #define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
 #define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
 #define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
+#define LED_RING(_s)			stm32_gpiowrite(GPIO_LED4, !(_s))
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 

src/modules/px4iofirmware/registers.c

@@ -603,6 +603,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
 			break;
 
+		case PX4IO_P_SETUP_FORCE_SAFETY_ON:
+			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+			}
+			break;
+
 		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
 			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
@@ -686,7 +692,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
 					disabled = true;
-				} else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
+				} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
+					   (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
 					count++;
 				}
 

src/modules/px4iofirmware/sbus.c

@@ -61,7 +61,7 @@
 /*
   Measured values with Futaba FX-30/R6108SB:
     -+100% on TX:  PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700  (100% ATV)
-    -+140% on TX:  PCM 0.930/1.520/2.112ms -> SBus raw values:  78/1024/1964  (140% ATV)  
+    -+140% on TX:  PCM 0.930/1.520/2.112ms -> SBus raw values:  78/1024/1964  (140% ATV)
     -+152% on TX:  PCM 0.884/1.520/2.160ms -> SBus raw values:   1/1024/2047  (140% ATV plus dirty tricks)
 */
 
@@ -87,13 +87,15 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
+			bool *sbus_frame_drop, uint16_t max_channels);
 
 int
 sbus_init(const char *device)
 {
-	if (sbus_fd < 0)
+	if (sbus_fd < 0) {
 		sbus_fd = open(device, O_RDWR | O_NONBLOCK);
+	}
 
 	if (sbus_fd >= 0) {
 		struct termios t;
@@ -113,6 +115,7 @@ sbus_init(const char *device)
 	} else {
 		debug("S.Bus: open failed");
 	}
+
 	return sbus_fd;
 }
 
@@ -167,8 +170,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
 	ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
 
 	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
+	if (ret < 1) {
 		return false;
+	}
 
 	last_rx_time = now;
 
@@ -180,8 +184,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
 	/*
 	 * If we don't have a full frame, return
 	 */
-	if (partial_frame_count < SBUS_FRAME_SIZE)
+	if (partial_frame_count < SBUS_FRAME_SIZE) {
 		return false;
+	}
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -228,7 +233,8 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };
 
 static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
+	    uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f)) {
@@ -237,23 +243,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 	}
 
 	switch (frame[24]) {
-		case 0x00:
+	case 0x00:
 		/* this is S.BUS 1 */
 		break;
-		case 0x03:
+
+	case 0x03:
 		/* S.BUS 2 SLOT0: RX battery and external voltage */
 		break;
-		case 0x83:
+
+	case 0x83:
 		/* S.BUS 2 SLOT1 */
 		break;
-		case 0x43:
-		case 0xC3:
-		case 0x23:
-		case 0xA3:
-		case 0x63:
-		case 0xE3:
+
+	case 0x43:
+	case 0xC3:
+	case 0x23:
+	case 0xA3:
+	case 0x63:
+	case 0xE3:
 		break;
-		default:
+
+	default:
 		/* we expect one of the bits above, but there are some we don't know yet */
 		break;
 	}
@@ -283,7 +293,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 
 
 		/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
-		values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
+		values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
 	}
 
 	/* decode switch channels if data fields are wide enough */
@@ -304,16 +314,17 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 		/* report that we failed to read anything valid off the receiver */
 		*sbus_failsafe = true;
 		*sbus_frame_drop = true;
-	}
-	else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
+
+	} else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
 		/* set a special warning flag
-		 * 
-		 * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this 
-		 * condition as fail-safe greatly reduces the reliability and range of the radio link, 
+		 *
+		 * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
+		 * condition as fail-safe greatly reduces the reliability and range of the radio link,
 		 * e.g. by prematurely issueing return-to-launch!!! */
 
 		*sbus_failsafe = false;
 		*sbus_frame_drop = true;
+
 	} else {
 		*sbus_failsafe = false;
 		*sbus_frame_drop = false;

src/modules/sdlog2/sdlog2.c

@@ -90,6 +90,7 @@
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/servorail_status.h>
 #include <uORB/topics/wind_estimate.h>
+#include <uORB/topics/encoders.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -628,6 +629,9 @@ void sdlog2_start_log()
 	perf_print_all(perf_fd);
 	close(perf_fd);
 
+	/* reset performance counters to get in-flight min and max values in post flight log */
+	perf_reset_all();
+
 	logging_enabled = true;
 }
 
@@ -951,6 +955,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct servorail_status_s servorail_status;
 		struct satellite_info_s sat_info;
 		struct wind_estimate_s wind_estimate;
+		struct encoders_s encoders;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -993,6 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GS1B_s log_GS1B;
 			struct log_TECS_s log_TECS;
 			struct log_WIND_s log_WIND;
+			struct log_ENCD_s log_ENCD;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1030,12 +1036,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int system_power_sub;
 		int servorail_status_sub;
 		int wind_sub;
+		int encoders_sub;
 	} subs;
 
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -1054,9 +1060,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
 	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
 	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
-	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-		subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
-	}
 	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
 	subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
@@ -1065,6 +1068,25 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
 	/* we need to rate-limit wind, as we do not need the full update rate */
 	orb_set_interval(subs.wind_sub, 90);
+	subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
+
+	/* add new topics HERE */
+
+
+	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+		subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+	}
+
+	if (_extended_logging) {
+		subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
+	}
+
+	/* close non-needed fd's */
+
+	/* close stdin */
+	close(0);
+	/* close stdout */
+	close(1);
 
 	thread_running = true;
 
@@ -1649,6 +1671,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(WIND);
 		}
 
+		/* --- ENCODERS --- */
+		if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) {
+			log_msg.msg_type = LOG_ENCD_MSG;
+			log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0];
+			log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0];
+			log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1];
+			log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1];
+			LOGBUFFER_WRITE_AND_COUNT(ENCD);
+		}
+
 		/* signal the other thread new data, but not yet unlock */
 		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 			/* only request write if several packets can be written at once */

src/modules/sdlog2/sdlog2_messages.h

@@ -407,6 +407,16 @@ struct log_VISN_s {
 	float qw;
 };
 
+/* --- ENCODERS - ENCODER DATA --- */
+#define LOG_ENCD_MSG 39
+struct log_ENCD_s {
+	int64_t cnt0;
+	float vel0;
+	int64_t cnt1;
+	float vel1;
+};
+
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -471,6 +481,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(TECS, "fffffffffffffB",	"ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
+	LOG_FORMAT(ENCD, "qfqf",	"cnt0,vel0,cnt1,vel1"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */

src/modules/uORB/objects_common.cpp

@@ -201,6 +201,9 @@ ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
 ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
 ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
 
+#include "topics/test_motor.h"
+ORB_DEFINE(test_motor, struct test_motor_s);
+
 #include "topics/debug_key_value.h"
 ORB_DEFINE(debug_key_value, struct debug_key_value_s);
 

src/modules/uORB/topics/rc_channels.h

@@ -34,6 +34,8 @@
 /**
  * @file rc_channels.h
  * Definition of the rc_channels uORB topic.
+ *
+ * @deprecated DO NOT USE FOR NEW CODE
  */
 
 #ifndef RC_CHANNELS_H_
@@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION {
 	AUX_2,
 	AUX_3,
 	AUX_4,
-	AUX_5,
-	RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
+	AUX_5
 };
 
+// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
+
+#define RC_CHANNELS_FUNCTION_MAX 18
+
 /**
  * @addtogroup topics
  * @{
@@ -76,7 +81,6 @@ struct rc_channels_s {
 	uint64_t timestamp_last_valid;						/**< Timestamp of last valid RC signal */
 	float channels[RC_CHANNELS_FUNCTION_MAX];			/**< Scaled to -1..1 (throttle: 0..1) */
 	uint8_t channel_count;								/**< Number of valid channels */
-	char function_name[RC_CHANNELS_FUNCTION_MAX][20];	/**< String array to store the names of the functions */
 	int8_t function[RC_CHANNELS_FUNCTION_MAX];			/**< Functions mapping */
 	uint8_t rssi;										/**< Receive signal strength index */
 	bool signal_lost;									/**< Control signal lost, should be checked together with topic timeout */

src/modules/uORB/topics/test_motor.h

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_motor.h
+ *
+ * Test a single drive motor while the vehicle is disarmed
+ *
+ * Publishing values to this topic causes a single motor to spin, e.g. for
+ * ground test purposes. This topic will be ignored while the vehicle is armed.
+ *
+ */
+
+#ifndef TOPIC_TEST_MOTOR_H
+#define TOPIC_TEST_MOTOR_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_MOTOR_OUTPUTS	8
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct test_motor_s {
+	uint64_t 	timestamp;				/**< output timestamp in us since system boot */
+	unsigned 	motor_number;				/**< number of motor to spin */
+	float		value;					/**< output data, in natural output units */
+};
+
+/**
+ * @}
+ */
+
+/* actuator output sets; this list can be expanded as more drivers emerge */
+ORB_DECLARE(test_motor);
+
+#endif

src/systemcmds/motor_test/module.mk

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build the motor_test tool.
+#
+
+MODULE_COMMAND	 = motor_test
+SRCS		 = motor_test.c
+
+MODULE_STACKSIZE = 4096

src/systemcmds/motor_test/motor_test.c

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Author: Holger Steinhaus <hsteinhaus@gmx.de>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file motor_test.c
+ *
+ * Tool for drive testing
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/test_motor.h>
+
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+
+
+__EXPORT int	motor_test_main(int argc, char *argv[]);
+static void motor_test(unsigned channel, float value);
+static void usage(const char *reason);
+
+void motor_test(unsigned channel, float value)
+{
+	orb_advert_t        _test_motor_pub;
+	struct test_motor_s _test_motor;
+
+	_test_motor.motor_number = channel;
+	_test_motor.timestamp = hrt_absolute_time();
+	_test_motor.value = value;
+
+        if (_test_motor_pub > 0) {
+	    /* publish armed state */
+            orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
+        } else {
+            /* advertise and publish */
+            _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
+        }
+}
+
+static void usage(const char *reason)
+{
+	if (reason != NULL)
+		warnx("%s", reason);
+
+	errx(1,
+		"usage:\n"
+		"motor_test\n"
+		"    -m <channel>            Motor to test (0..7)\n"
+		"    -p <power>              Power (0..100)\n");
+}
+
+int motor_test_main(int argc, char *argv[])
+{
+	unsigned long channel, lval;
+	float value;
+	int ch;
+
+	if (argc != 5) {
+		usage("please specify motor and power");
+	}
+
+	while ((ch = getopt(argc, argv, "m:p:")) != EOF) {
+		switch (ch) {
+
+		case 'm':
+			/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+			channel = strtoul(optarg, NULL, 0);
+			break;
+
+		case 'p':
+			/* Read in custom low value */
+			lval = strtoul(optarg, NULL, 0);
+
+			if (lval > 100)
+				usage("value invalid");
+
+			value = (float)lval/100.f;
+			break;
+		default:
+			usage(NULL);
+		}
+	}
+
+	motor_test(channel, value);
+
+	printf("motor %d set to %.2f\n", channel, value);
+
+	exit(0);
+}

src/systemcmds/nshterm/module.mk

@@ -38,6 +38,8 @@
 MODULE_COMMAND	 = nshterm
 SRCS		 = nshterm.c
 
-MODULE_STACKSIZE = 1400
+MODULE_STACKSIZE = 1600
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_PRIORITY	= "SCHED_PRIORITY_DEFAULT-30"

src/systemcmds/nshterm/nshterm.c

@@ -87,7 +87,7 @@ nshterm_main(int argc, char *argv[])
 
     /* Back up the original uart configuration to restore it after exit */
     if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
-        warnx("ERROR get termios config %s: %d\n", argv[1], termios_state);
+        warnx("ERR get config %s: %d\n", argv[1], termios_state);
         close(fd);
         return -1;
     }
@@ -96,7 +96,7 @@ nshterm_main(int argc, char *argv[])
     uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/);
 
     if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
-        warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]);
+        warnx("ERR set config %s\n", argv[1]);
         close(fd);
         return -1;
     }

src/systemcmds/tests/tests_main.c

@@ -110,7 +110,9 @@ const struct {
 	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mtd",			test_mtd,	0},
+#ifndef TESTS_MATHLIB_DISABLE
 	{"mathlib",		test_mathlib,	0},
+#endif
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };