Merge branch 'fw_autoland_att_tecs_navigator_termination_controlgroups' into bottle_drop_navigator

Conflicts:
	src/drivers/px4fmu/fmu.cpp
	src/modules/dataman/dataman.c
	src/modules/dataman/dataman.h
	src/modules/mavlink/orb_listener.c
	src/modules/mavlink/waypoints.c
	src/modules/navigator/navigator_main.cpp
	src/modules/navigator/navigator_mission.cpp
	src/modules/navigator/navigator_mission.h
	src/modules/uORB/topics/mission.h

src/drivers/airspeed/airspeed.h

@@ -119,7 +119,7 @@ protected:
 	virtual int	collect() = 0;
 
 	work_s			_work;
-	float		_max_differential_pressure_pa;
+	float			_max_differential_pressure_pa;
 	bool			_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;

src/drivers/ardrone_interface/ardrone_motor_control.c

@@ -46,7 +46,6 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <systemlib/err.h>
 
 #include "ardrone_motor_control.h"
@@ -384,9 +383,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
 
 	static bool initialized = false;
-	/* publish effective outputs */
-	static struct actuator_controls_effective_s actuator_controls_effective;
-	static orb_advert_t actuator_controls_effective_pub;
 
 	/* linearly scale the control inputs from 0 to startpoint_full_control */
 	if (motor_thrust < startpoint_full_control) {
@@ -430,25 +426,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
 	}
 
-
-
-	/* publish effective outputs */
-	actuator_controls_effective.control_effective[0] = roll_control;
-	actuator_controls_effective.control_effective[1] = pitch_control;
-	/* yaw output after limiting */
-	actuator_controls_effective.control_effective[2] = yaw_control;
-	/* possible motor thrust limiting */
-	actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
-
-	if (!initialized) {
-		/* advertise and publish */
-		actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
-		initialized = true;
-	} else {
-		/* already initialized, just publishing */
-		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
-	}
-
 	/* set the motor values */
 
 	/* scale up from 0..1 to 10..500) */

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

@@ -79,17 +79,37 @@ __BEGIN_DECLS
 #define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
 #define GPIO_EXTI_MAG_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
 #define GPIO_EXTI_ACCEL_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_EXTI_MPU_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+
+/* Data ready pins off */
+#define GPIO_GYRO_DRDY_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_MAG_DRDY_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_ACCEL_DRDY_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_EXTI_MPU_DRDY	(GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+
+/* SPI1 off */
+#define GPIO_SPI1_SCK_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_SPI1_MISO_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_SPI1_MOSI_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
+
+/* SPI1 chip selects off */
+#define GPIO_SPI_CS_GYRO_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO_OFF                (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_MPU		(GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
 
 /* SPI chip selects */
-#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
-#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
-#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
-#define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+#define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
 
 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
 #define PX4_SPIDEV_GYRO		1
 #define PX4_SPIDEV_ACCEL_MAG	2
 #define PX4_SPIDEV_BARO		3
+#define PX4_SPIDEV_MPU		4
 
 /* I2C busses */
 #define PX4_I2C_BUS_EXPANSION	1

src/drivers/boards/px4fmu-v2/px4fmu2_init.c

@@ -215,9 +215,9 @@ __EXPORT int nsh_archinitialize(void)
 	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
 	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
 	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
-	stm32_configgpio(GPIO_ADC1_IN10);	/* unrouted */
-	stm32_configgpio(GPIO_ADC1_IN11);	/* unrouted */
-	stm32_configgpio(GPIO_ADC1_IN12);	/* unrouted */
+	// stm32_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
+	// stm32_configgpio(GPIO_ADC1_IN11);	/* unused */
+	// stm32_configgpio(GPIO_ADC1_IN12);	/* used by MPU6000 CS */
 	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
 	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
 	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
@@ -279,6 +279,7 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
 	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
 	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
 	up_udelay(20);
 
 	message("[boot] Successfully initialized SPI port 1\n");

src/drivers/boards/px4fmu-v2/px4fmu_spi.c

@@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void)
 	stm32_configgpio(GPIO_SPI_CS_GYRO);
 	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
 	stm32_configgpio(GPIO_SPI_CS_BARO);
+	stm32_configgpio(GPIO_SPI_CS_MPU);
 
 	/* De-activate all peripherals,
 	 * required for some peripheral
@@ -81,6 +82,12 @@ __EXPORT void weak_function stm32_spiinitialize(void)
 	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+
+	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
+	stm32_configgpio(GPIO_EXTI_MAG_DRDY);
+	stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
+	stm32_configgpio(GPIO_EXTI_MPU_DRDY);
 #endif
 
 #ifdef CONFIG_STM32_SPI2
@@ -99,6 +106,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_ACCEL_MAG:
@@ -106,6 +114,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_BARO:
@@ -113,6 +122,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		break;
+
+	case PX4_SPIDEV_MPU:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
 		break;
 
 	default:

src/drivers/device/cdev.cpp

@@ -108,6 +108,42 @@ CDev::~CDev()
 		unregister_driver(_devname);
 }
 
+int
+CDev::register_class_devname(const char *class_devname)
+{
+	if (class_devname == nullptr) {
+		return -EINVAL;
+	}
+	int class_instance = 0;
+	int ret = -ENOSPC;
+	while (class_instance < 4) {
+		if (class_instance == 0) {
+			ret = register_driver(class_devname, &fops, 0666, (void *)this);
+			if (ret == OK) break;
+		} else {
+			char name[32];
+			snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+			ret = register_driver(name, &fops, 0666, (void *)this);
+			if (ret == OK) break;
+		}
+		class_instance++;
+	}
+	if (class_instance == 4)
+		return ret;
+	return class_instance;
+}
+
+int
+CDev::unregister_class_devname(const char *class_devname, unsigned class_instance)
+{
+	if (class_instance > 0) {
+		char name[32];
+		snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+		return unregister_driver(name);
+	}
+	return unregister_driver(class_devname);
+}
+
 int
 CDev::init()
 {

src/drivers/device/device.h

@@ -396,6 +396,25 @@ protected:
 	 */
 	virtual int	close_last(struct file *filp);
 
+        /**
+	 * Register a class device name, automatically adding device
+	 * class instance suffix if need be.
+	 *
+	 * @param class_devname   Device class name
+	 * @return class_instamce Class instance created, or -errno on failure
+	 */
+	virtual int register_class_devname(const char *class_devname);
+
+        /**
+	 * Register a class device name, automatically adding device
+	 * class instance suffix if need be.
+	 *
+	 * @param class_devname   Device class name
+	 * @param class_instance  Device class instance from register_class_devname()
+	 * @return		  OK on success, -errno otherwise
+	 */
+	virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
+
 private:
 	static const unsigned _max_pollwaiters = 8;
 
@@ -488,4 +507,7 @@ private:
 
 } // namespace device
 
+// class instance for primary driver of each class
+#define CLASS_DEVICE_PRIMARY 0
+
 #endif /* _DEVICE_DEVICE_H */

src/drivers/device/spi.cpp

@@ -181,4 +181,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	return OK;
 }
 
+void 
+SPI::set_frequency(uint32_t frequency)
+{
+	_frequency = frequency;
+}
+
 } // namespace device

src/drivers/device/spi.h

@@ -101,6 +101,17 @@ protected:
 	 */
 	int		transfer(uint8_t *send, uint8_t *recv, unsigned len);
 
+	/**
+	 * Set the SPI bus frequency
+	 * This is used to change frequency on the fly. Some sensors
+	 * (such as the MPU6000) need a lower frequency for setup
+	 * registers and can handle higher frequency for sensor 
+	 * value registers
+	 *
+	 * @param frequency	Frequency to set (Hz)
+	 */
+        void		set_frequency(uint32_t frequency);
+
 	/**
 	 * Locking modes supported by the driver.
 	 */

src/drivers/drv_gpio.h

@@ -46,7 +46,7 @@
 /*
  * PX4FMU GPIO numbers.
  *
- * For shared pins, alternate function 1 selects the non-GPIO mode 
+ * For shared pins, alternate function 1 selects the non-GPIO mode
  * (USART2, CAN2, etc.)
  */
 # define GPIO_EXT_1		(1<<0)		/**< high-power GPIO 1 */
@@ -144,4 +144,6 @@
 /** read all the GPIOs and return their values in *(uint32_t *)arg */
 #define GPIO_GET	GPIOC(12)
 
+#define GPIO_SENSOR_RAIL_RESET	GPIOC(13)
+
 #endif /* _DRV_GPIO_H */

src/drivers/drv_hrt.h

@@ -141,6 +141,20 @@ __EXPORT extern bool	hrt_called(struct hrt_call *entry);
  */
 __EXPORT extern void	hrt_cancel(struct hrt_call *entry);
 
+/*
+ * initialise a hrt_call structure
+ */
+__EXPORT extern void	hrt_call_init(struct hrt_call *entry);
+
+/*
+ * delay a hrt_call_every() periodic call by the given number of
+ * microseconds. This should be called from within the callout to
+ * cause the callout to be re-scheduled for a later time. The periodic
+ * callouts will then continue from that new base time at the
+ * previously specified period.
+ */
+__EXPORT extern void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
+
 /*
  * Initialise the HRT.
  */

src/drivers/drv_rc_input.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 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
@@ -60,7 +60,7 @@
 /**
  * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
  */
-#define RC_INPUT_MAX_CHANNELS	18
+#define RC_INPUT_MAX_CHANNELS	20
 
 /**
  * Input signal type, value is a control position from zero to 100
@@ -89,6 +89,9 @@ struct rc_input_values {
 	/** number of channels actually being seen */
 	uint32_t		channel_count;
 
+	/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */
+	int32_t			rssi;
+
 	/** Input source */
 	enum RC_INPUT_SOURCE 	input_source;
 

src/drivers/hmc5883/hmc5883.cpp

@@ -77,6 +77,7 @@
  */
 
 #define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
+#define HMC5883L_DEVICE_PATH		"/dev/hmc5883"
 
 /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
 #define HMC5883_CONVERSION_INTERVAL	(1000000 / 150)	/* microseconds */
@@ -154,6 +155,7 @@ private:
 	float 			_range_scale;
 	float 			_range_ga;
 	bool			_collect_phase;
+	int			_class_instance;
 
 	orb_advert_t		_mag_topic;
 
@@ -315,12 +317,13 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 
 
 HMC5883::HMC5883(int bus) :
-	I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
+	I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
 	_measure_ticks(0),
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
 	_mag_topic(-1),
+	_class_instance(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -351,6 +354,9 @@ HMC5883::~HMC5883()
 	if (_reports != nullptr)
 		delete _reports;
 
+	if (_class_instance != -1)
+		unregister_class_devname(MAG_DEVICE_PATH, _class_instance);
+
 	// free perf counters
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
@@ -374,13 +380,17 @@ HMC5883::init()
 	/* reset the device configuration */
 	reset();
 
-	/* get a publish handle on the mag topic */
-	struct mag_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
+	_class_instance = register_class_devname(MAG_DEVICE_PATH);
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		/* get a publish handle on the mag topic if we are
+		 * the primary mag */
+		struct mag_report zero_report;
+		memset(&zero_report, 0, sizeof(zero_report));
+		_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
 
-	if (_mag_topic < 0)
-		debug("failed to create sensor_mag object");
+		if (_mag_topic < 0)
+			debug("failed to create sensor_mag object");
+	}
 
 	ret = OK;
 	/* sensor is ok, but not calibrated */
@@ -875,8 +885,10 @@ HMC5883::collect()
 	}
 #endif
 
-	/* publish it */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+	if (_mag_topic != -1) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+	}
 
 	/* post a report to the ring */
 	if (_reports->force(&new_report)) {
@@ -1256,7 +1268,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1288,10 +1300,10 @@ test()
 	ssize_t sz;
 	int ret;
 
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
@@ -1388,10 +1400,10 @@ int calibrate()
 {
 	int ret;
 
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
 
 	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
 		warnx("failed to enable sensor calibration mode");
@@ -1413,7 +1425,7 @@ int calibrate()
 void
 reset()
 {
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");

src/drivers/l3gd20/l3gd20.cpp

@@ -66,6 +66,8 @@
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 
+#define L3GD20_DEVICE_PATH "/dev/l3gd20"
+
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -92,9 +94,17 @@ static const int ERROR = -1;
 #define REG1_RATE_LP_MASK			0xF0 /* Mask to guard partial register update */
 /* keep lowpass low to avoid noise issues */
 #define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
-#define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define RATE_190HZ_LP_50HZ		((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define RATE_190HZ_LP_70HZ		((0<<7) | (1<<6) | (1<<5) | (1<<4))
 #define RATE_380HZ_LP_20HZ		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_380HZ_LP_25HZ		((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define RATE_380HZ_LP_50HZ		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_380HZ_LP_100HZ		((1<<7) | (0<<6) | (1<<5) | (1<<4))
 #define RATE_760HZ_LP_30HZ		((1<<7) | (1<<6) | (0<<5) | (0<<4))
+#define RATE_760HZ_LP_35HZ		((1<<7) | (1<<6) | (0<<5) | (1<<4))
+#define RATE_760HZ_LP_50HZ		((1<<7) | (1<<6) | (1<<5) | (0<<4))
+#define RATE_760HZ_LP_100HZ		((1<<7) | (1<<6) | (1<<5) | (1<<4))
 
 #define ADDR_CTRL_REG2			0x21
 #define ADDR_CTRL_REG3			0x22
@@ -191,6 +201,7 @@ private:
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
+	int			_class_instance;
 
 	unsigned		_current_rate;
 	unsigned		_orientation;
@@ -198,6 +209,8 @@ private:
 	unsigned		_read;
 
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_reschedules;
+	perf_counter_t		_errors;
 
 	math::LowPassFilter2p	_gyro_filter_x;
 	math::LowPassFilter2p	_gyro_filter_y;
@@ -218,6 +231,11 @@ private:
 	 */
 	void			reset();
 
+	/**
+	 * disable I2C on the chip
+	 */
+	void			disable_i2c();
+
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
@@ -298,16 +316,19 @@ private:
 };
 
 L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
-	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
+	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
 	_call_interval(0),
 	_reports(nullptr),
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
+	_class_instance(-1),
 	_current_rate(0),
 	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
 	_read(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
+	_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
+	_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
@@ -333,8 +354,13 @@ L3GD20::~L3GD20()
 	if (_reports != nullptr)
 		delete _reports;
 
+	if (_class_instance != -1)
+		unregister_class_devname(GYRO_DEVICE_PATH, _class_instance);
+
 	/* delete the perf counter */
 	perf_free(_sample_perf);
+	perf_free(_reschedules);
+	perf_free(_errors);
 }
 
 int
@@ -352,10 +378,13 @@ L3GD20::init()
 	if (_reports == nullptr)
 		goto out;
 
-	/* advertise sensor topic */
-	struct gyro_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
+	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
+        if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		/* advertise sensor topic */
+		struct gyro_report zero_report;
+		memset(&zero_report, 0, sizeof(zero_report));
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
+        }
 
 	reset();
 
@@ -574,6 +603,7 @@ L3GD20::read_reg(unsigned reg)
 	uint8_t cmd[2];
 
 	cmd[0] = reg | DIR_READ;
+	cmd[1] = 0;
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -653,16 +683,15 @@ L3GD20::set_samplerate(unsigned frequency)
 
 	} else if (frequency <= 200) {
 		_current_rate = 190;
-		bits |= RATE_190HZ_LP_25HZ;
+		bits |= RATE_190HZ_LP_50HZ;
 
 	} else if (frequency <= 400) {
 		_current_rate = 380;
-		bits |= RATE_380HZ_LP_20HZ;
+		bits |= RATE_380HZ_LP_50HZ;
 
 	} else if (frequency <= 800) {
 		_current_rate = 760;
-		bits |= RATE_760HZ_LP_30HZ;
-
+		bits |= RATE_760HZ_LP_50HZ;
 	} else {
 		return -EINVAL;
 	}
@@ -699,13 +728,31 @@ L3GD20::stop()
 	hrt_cancel(&_call);
 }
 
+void
+L3GD20::disable_i2c(void)
+{
+	uint8_t retries = 10;
+	while (retries--) {
+		// add retries
+		uint8_t a = read_reg(0x05);
+		write_reg(0x05, (0x20 | a));
+		if (read_reg(0x05) == (a | 0x20)) {
+			return;
+		}
+	}
+	debug("FAILED TO DISABLE I2C");
+}
+
 void
 L3GD20::reset()
 {
+	// ensure the chip doesn't interpret any other bus traffic as I2C
+	disable_i2c();
+
 	/* set default configuration */
 	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
 	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
+	write_reg(ADDR_CTRL_REG3, 0x08);        /* DRDY enable */
 	write_reg(ADDR_CTRL_REG4, REG4_BDU);
 	write_reg(ADDR_CTRL_REG5, 0);
 
@@ -716,7 +763,7 @@ L3GD20::reset()
 	 * callback fast enough to not miss data. */
 	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
 
-	set_samplerate(L3GD20_DEFAULT_RATE);
+	set_samplerate(0); // 760Hz
 	set_range(L3GD20_DEFAULT_RANGE_DPS);
 	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
 
@@ -732,9 +779,26 @@ L3GD20::measure_trampoline(void *arg)
 	dev->measure();
 }
 
+#ifdef GPIO_EXTI_GYRO_DRDY
+# define L3GD20_USE_DRDY 1
+#else
+# define L3GD20_USE_DRDY 0
+#endif
+
 void
 L3GD20::measure()
 {
+#if L3GD20_USE_DRDY
+	// if the gyro doesn't have any data ready then re-schedule
+	// for 100 microseconds later. This ensures we don't double
+	// read a value and then miss the next value
+	if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
+		perf_count(_reschedules);
+		hrt_call_delay(&_call, 100);
+		return;
+	}
+#endif
+
 	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {
@@ -753,9 +817,20 @@ L3GD20::measure()
 	perf_begin(_sample_perf);
 
 	/* fetch data from the sensor */
+	memset(&raw_report, 0, sizeof(raw_report));
 	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
 	transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
 
+#if L3GD20_USE_DRDY
+        if ((raw_report.status & 0xF) != 0xF) {
+            /*
+              we waited for DRDY, but did not see DRDY on all axes
+              when we captured. That means a transfer error of some sort
+             */
+            perf_count(_errors);            
+            return;
+        }
+#endif
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
 	 * 2) Subtract static offset (in SI units)
@@ -833,6 +908,8 @@ L3GD20::print_info()
 {
 	printf("gyro reads:          %u\n", _read);
 	perf_print_counter(_sample_perf);
+	perf_print_counter(_reschedules);
+	perf_print_counter(_errors);
 	_reports->print_info("report queue");
 }
 
@@ -883,7 +960,7 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+	g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -892,7 +969,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+	fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -900,6 +977,8 @@ start()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
+        close(fd);
+
 	exit(0);
 fail:
 
@@ -924,10 +1003,10 @@ test()
 	ssize_t sz;
 
 	/* get the driver */
-	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+	fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY);
 
 	if (fd_gyro < 0)
-		err(1, "%s open failed", GYRO_DEVICE_PATH);
+		err(1, "%s open failed", L3GD20_DEVICE_PATH);
 
 	/* reset to manual polling */
 	if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -948,6 +1027,8 @@ test()
 	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));
 
+        close(fd_gyro);
+
 	/* XXX add poll-rate tests here too */
 
 	reset();
@@ -960,7 +1041,7 @@ test()
 void
 reset()
 {
-	int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+	int fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -971,6 +1052,8 @@ reset()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "accel pollrate reset failed");
 
+        close(fd);
+
 	exit(0);
 }
 

src/drivers/lsm303d/lsm303d.cpp

@@ -39,6 +39,7 @@
 #include <nuttx/config.h>
 
 #include <sys/types.h>
+#include <sys/stat.h>
 #include <stdint.h>
 #include <stdbool.h>
 #include <stddef.h>
@@ -63,6 +64,7 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/drv_tone_alarm.h>
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -78,23 +80,29 @@ static const int ERROR = -1;
 #define DIR_WRITE				(0<<7)
 #define ADDR_INCREMENT			(1<<6)
 
-
+#define LSM303D_DEVICE_PATH_ACCEL	"/dev/lsm303d_accel"
+#define LSM303D_DEVICE_PATH_MAG		"/dev/lsm303d_mag"
 
 /* register addresses: A: accel, M: mag, T: temp */
 #define ADDR_WHO_AM_I			0x0F
-#define WHO_I_AM				0x49
+#define WHO_I_AM			0x49
 
-#define ADDR_OUT_L_T         	0x05
-#define ADDR_OUT_H_T        	0x06
-#define ADDR_STATUS_M           0x07
-#define ADDR_OUT_X_L_M          0x08
-#define ADDR_OUT_X_H_M          0x09
-#define ADDR_OUT_Y_L_M          0x0A
-#define ADDR_OUT_Y_H_M          0x0B
-#define ADDR_OUT_Z_L_M          0x0C
-#define ADDR_OUT_Z_H_M          0x0D
+#define ADDR_OUT_TEMP_L			0x05
+#define ADDR_OUT_TEMP_H			0x06
+#define ADDR_STATUS_M			0x07
+#define ADDR_OUT_X_L_M          	0x08
+#define ADDR_OUT_X_H_M          	0x09
+#define ADDR_OUT_Y_L_M          	0x0A
+#define ADDR_OUT_Y_H_M			0x0B
+#define ADDR_OUT_Z_L_M			0x0C
+#define ADDR_OUT_Z_H_M			0x0D
+
+#define ADDR_INT_CTRL_M			0x12
+#define ADDR_INT_SRC_M			0x13
+#define ADDR_REFERENCE_X		0x1c
+#define ADDR_REFERENCE_Y		0x1d
+#define ADDR_REFERENCE_Z		0x1e
 
-#define ADDR_OUT_TEMP_A			0x26
 #define ADDR_STATUS_A			0x27
 #define ADDR_OUT_X_L_A			0x28
 #define ADDR_OUT_X_H_A			0x29
@@ -112,6 +120,26 @@ static const int ERROR = -1;
 #define ADDR_CTRL_REG6			0x25
 #define ADDR_CTRL_REG7			0x26
 
+#define ADDR_FIFO_CTRL			0x2e
+#define ADDR_FIFO_SRC			0x2f
+
+#define ADDR_IG_CFG1			0x30
+#define ADDR_IG_SRC1			0x31
+#define ADDR_IG_THS1			0x32
+#define ADDR_IG_DUR1			0x33
+#define ADDR_IG_CFG2			0x34
+#define ADDR_IG_SRC2			0x35
+#define ADDR_IG_THS2			0x36
+#define ADDR_IG_DUR2			0x37
+#define ADDR_CLICK_CFG			0x38
+#define ADDR_CLICK_SRC			0x39
+#define ADDR_CLICK_THS			0x3a
+#define ADDR_TIME_LIMIT			0x3b
+#define ADDR_TIME_LATENCY		0x3c
+#define ADDR_TIME_WINDOW		0x3d
+#define ADDR_ACT_THS			0x3e
+#define ADDR_ACT_DUR			0x3f
+
 #define REG1_RATE_BITS_A		((1<<7) | (1<<6) | (1<<5) | (1<<4))
 #define REG1_POWERDOWN_A		((0<<7) | (0<<6) | (0<<5) | (0<<4))
 #define REG1_RATE_3_125HZ_A		((0<<7) | (0<<6) | (0<<5) | (1<<4))
@@ -201,6 +229,21 @@ public:
 	 */
 	void			print_info();
 
+	/**
+	 * dump register values
+	 */
+	void			print_registers();
+
+	/**
+	 * toggle logging
+	 */
+	void			toggle_logging();
+
+	/**
+	 * check for extreme accel values
+	 */
+	void			check_extremes(const accel_report *arb);
+
 protected:
 	virtual int		probe();
 
@@ -234,7 +277,7 @@ private:
 	unsigned		_mag_samplerate;
 
 	orb_advert_t		_accel_topic;
-	orb_advert_t		_mag_topic;
+	int			_class_instance;
 
 	unsigned		_accel_read;
 	unsigned		_mag_read;
@@ -243,6 +286,8 @@ private:
 	perf_counter_t		_mag_sample_perf;
 	perf_counter_t		_reg7_resets;
 	perf_counter_t		_reg1_resets;
+	perf_counter_t		_extreme_values;
+	perf_counter_t		_accel_reschedules;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
@@ -253,6 +298,15 @@ private:
 	uint8_t			_reg7_expected;
 	uint8_t			_reg1_expected;
 
+	// accel logging
+	int			_accel_log_fd;
+	bool			_accel_logging_enabled;
+	uint64_t		_last_extreme_us;	
+	uint64_t		_last_log_us;	
+	uint64_t		_last_log_sync_us;	
+	uint64_t		_last_log_reg_us;	
+	uint64_t		_last_log_alarm_us;	
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -270,6 +324,11 @@ private:
 	 */
 	void			reset();
 
+	/**
+	 * disable I2C on the chip
+	 */
+	void			disable_i2c();
+
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
@@ -408,6 +467,8 @@ public:
 	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
 
+	virtual int		init();
+
 protected:
 	friend class LSM303D;
 
@@ -415,6 +476,9 @@ protected:
 private:
 	LSM303D				*_parent;
 
+	orb_advert_t			_mag_topic;
+	int				_mag_class_instance;
+
 	void				measure();
 
 	void				measure_trampoline(void *arg);
@@ -422,7 +486,7 @@ private:
 
 
 LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
-	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
+	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
 	_mag(new LSM303D_mag(this)),
 	_call_accel_interval(0),
 	_call_mag_interval(0),
@@ -436,18 +500,26 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
 	_accel_topic(-1),
-	_mag_topic(-1),
+	_class_instance(-1),
 	_accel_read(0),
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
 	_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
 	_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
+	_extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")),
+	_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
 	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_reg1_expected(0),
-	_reg7_expected(0)
+	_reg7_expected(0),
+	_accel_log_fd(-1),
+	_accel_logging_enabled(false),
+	_last_log_us(0),
+	_last_log_sync_us(0),
+	_last_log_reg_us(0),
+	_last_log_alarm_us(0)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -479,11 +551,17 @@ LSM303D::~LSM303D()
 	if (_mag_reports != nullptr)
 		delete _mag_reports;
 
+	if (_class_instance != -1)
+		unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
+
 	delete _mag;
 
 	/* delete the perf counter */
 	perf_free(_accel_sample_perf);
 	perf_free(_mag_sample_perf);
+	perf_free(_reg1_resets);
+	perf_free(_reg7_resets);
+	perf_free(_extreme_values);
 }
 
 int
@@ -505,10 +583,6 @@ LSM303D::init()
 		goto out;
 
 	/* advertise accel topic */
-	struct accel_report zero_report;
-	memset(&zero_report, 0, sizeof(zero_report));
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
-
 	_mag_reports = new RingBuffer(2, sizeof(mag_report));
 
 	if (_mag_reports == nullptr)
@@ -516,26 +590,45 @@ LSM303D::init()
 
 	reset();
 
-	/* advertise mag topic */
-	struct mag_report zero_mag_report;
-	memset(&zero_mag_report, 0, sizeof(zero_mag_report));
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
-
-	/* do CDev init for the mag device node, keep it optional */
-	mag_ret = _mag->init();
-
-	if (mag_ret != OK) {
-		_mag_topic = -1;
+	/* do CDev init for the mag device node */
+	ret = _mag->init();
+	if (ret != OK) {
+		warnx("MAG init failed");
+		goto out;
+	}
+
+	_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		// we are the primary accel device, so advertise to
+		// the ORB
+		struct accel_report zero_report;
+		memset(&zero_report, 0, sizeof(zero_report));
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
 	}
 
-	ret = OK;
 out:
 	return ret;
 }
 
+void
+LSM303D::disable_i2c(void)
+{
+	uint8_t a = read_reg(0x02);
+	write_reg(0x02, (0x10 | a));
+	a = read_reg(0x02);
+	write_reg(0x02, (0xF7 & a));
+	a = read_reg(0x15);
+	write_reg(0x15, (0x80 | a));
+	a = read_reg(0x02);
+	write_reg(0x02, (0xE7 & a));
+}
+
 void
 LSM303D::reset()
 {
+	// ensure the chip doesn't interpret any other bus traffic as I2C
+	disable_i2c();
+
 	/* enable accel*/
 	_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
 	write_reg(ADDR_CTRL_REG1, _reg1_expected);
@@ -544,10 +637,17 @@ LSM303D::reset()
 	_reg7_expected = REG7_CONT_MODE_M;
 	write_reg(ADDR_CTRL_REG7, _reg7_expected);
 	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+	write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
+	write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
 
 	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
 	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
 	accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
+
+	// we setup the anti-alias on-chip filter as 50Hz. We believe
+	// this operates in the analog domain, and is critical for
+	// anti-aliasing. The 2 pole software filter is designed to
+	// operate in conjunction with this on-chip filter
 	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
 
 	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
@@ -572,6 +672,122 @@ LSM303D::probe()
 	return -EIO;
 }
 
+#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log"
+
+/**
+   check for extreme accelerometer values and log to a file on the SD card
+ */
+void
+LSM303D::check_extremes(const accel_report *arb)
+{
+	const float extreme_threshold = 30;
+        static bool boot_ok = false;
+	bool is_extreme = (fabsf(arb->x) > extreme_threshold && 
+			   fabsf(arb->y) > extreme_threshold && 
+			   fabsf(arb->z) > extreme_threshold);
+	if (is_extreme) {
+		perf_count(_extreme_values);
+		// force accel logging on if we see extreme values
+		_accel_logging_enabled = true;
+	} else {
+            boot_ok = true;
+        }
+
+	if (! _accel_logging_enabled) {
+		// logging has been disabled by user, close
+		if (_accel_log_fd != -1) {
+			::close(_accel_log_fd);
+			_accel_log_fd = -1;
+		}
+		return;
+	}
+	if (_accel_log_fd == -1) {
+		// keep last 10 logs
+		::unlink(ACCEL_LOGFILE ".9");
+		for (uint8_t i=8; i>0; i--) {
+			uint8_t len = strlen(ACCEL_LOGFILE)+3;
+			char log1[len], log2[len];
+			snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
+			snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
+			::rename(log1, log2);
+		}
+		::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");
+
+		// open the new logfile
+		_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
+		if (_accel_log_fd == -1) {
+			return;
+		}
+	}
+
+	uint64_t now = hrt_absolute_time();
+	// log accels at 1Hz
+	if (_last_log_us == 0 ||
+	    now - _last_log_us > 1000*1000) {
+		_last_log_us = now;
+		::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
+			  (unsigned long long)arb->timestamp, 
+			  arb->x, arb->y, arb->z,
+			  (int)arb->x_raw,
+			  (int)arb->y_raw,
+			  (int)arb->z_raw,
+			  (unsigned)boot_ok);
+	}
+
+        const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, 
+                                    ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, 
+                                    ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, 
+                                    ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, 
+                                    ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, 
+                                    ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, 
+                                    ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, 
+                                    ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, 
+                                    ADDR_ACT_THS, ADDR_ACT_DUR,
+                                    ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, 
+                                    ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
+        uint8_t regval[sizeof(reglist)];
+        for (uint8_t i=0; i<sizeof(reglist); i++) {
+            regval[i] = read_reg(reglist[i]);
+        }
+
+	// log registers at 10Hz when we have extreme values, or 0.5 Hz without
+	if (_last_log_reg_us == 0 ||
+	    (is_extreme && (now - _last_log_reg_us > 250*1000)) ||
+	    (now - _last_log_reg_us > 10*1000*1000)) {
+		_last_log_reg_us = now;
+		::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
+		for (uint8_t i=0; i<sizeof(reglist); i++) {
+			::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
+		}
+		::dprintf(_accel_log_fd, "\n");
+	}
+
+	// fsync at 0.1Hz
+	if (now - _last_log_sync_us > 10*1000*1000) {
+		_last_log_sync_us = now;
+		::fsync(_accel_log_fd);
+	}
+
+	// play alarm every 10s if we have had an extreme value
+	if (perf_event_count(_extreme_values) != 0 && 
+	    (now - _last_log_alarm_us > 10*1000*1000)) {
+		_last_log_alarm_us = now;
+		int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
+		if (tfd != -1) {
+			uint8_t tone = 3;
+			if (!is_extreme) {
+				tone = 3;
+			} else if (boot_ok) {
+				tone = 4;
+			} else {
+				tone = 5;
+			}
+			::ioctl(tfd, TONE_SET_ALARM, tone);
+			::close(tfd);
+		}		
+	}
+}
+
 ssize_t
 LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -590,6 +806,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 		 */
 		while (count--) {
 			if (_accel_reports->get(arb)) {
+				check_extremes(arb);
 				ret += sizeof(*arb);
 				arb++;
 			}
@@ -952,6 +1169,7 @@ LSM303D::read_reg(unsigned reg)
 	uint8_t cmd[2];
 
 	cmd[0] = reg | DIR_READ;
+	cmd[1] = 0;
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -1224,6 +1442,14 @@ LSM303D::mag_measure_trampoline(void *arg)
 void
 LSM303D::measure()
 {
+	// if the accel doesn't have any data ready then re-schedule
+	// for 100 microseconds later. This ensures we don't double
+	// read a value and then miss the next value
+	if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
+		perf_count(_accel_reschedules);
+		hrt_call_delay(&_accel_call, 100);
+		return;
+	}
 	if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
 		perf_count(_reg1_resets);
 		reset();
@@ -1248,6 +1474,7 @@ LSM303D::measure()
 	perf_begin(_accel_sample_perf);
 
 	/* fetch data from the sensor */
+	memset(&raw_accel_report, 0, sizeof(raw_accel_report));
 	raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
 	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
 
@@ -1290,8 +1517,10 @@ LSM303D::measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+	if (_accel_topic != -1) {
+		/* publish for subscribers */
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+	}
 
 	_accel_read++;
 
@@ -1325,6 +1554,7 @@ LSM303D::mag_measure()
 	perf_begin(_mag_sample_perf);
 
 	/* fetch data from the sensor */
+	memset(&raw_mag_report, 0, sizeof(raw_mag_report));
 	raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
 	transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
 
@@ -1361,8 +1591,10 @@ LSM303D::mag_measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+	if (_mag->_mag_topic != -1) {
+		/* publish for subscribers */
+		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
+	}
 
 	_mag_read++;
 
@@ -1380,14 +1612,111 @@ LSM303D::print_info()
 	_mag_reports->print_info("mag reports");
 }
 
+void
+LSM303D::print_registers()
+{
+	const struct {
+		uint8_t reg;
+		const char *name;
+	} regmap[] = {
+		{ ADDR_WHO_AM_I,    "WHO_AM_I" },
+		{ 0x02,             "I2C_CONTROL1" },
+		{ 0x15,             "I2C_CONTROL2" },
+		{ ADDR_STATUS_A,    "STATUS_A" },
+		{ ADDR_STATUS_M,    "STATUS_M" },
+		{ ADDR_CTRL_REG0,   "CTRL_REG0" },
+		{ ADDR_CTRL_REG1,   "CTRL_REG1" },
+		{ ADDR_CTRL_REG2,   "CTRL_REG2" },
+		{ ADDR_CTRL_REG3,   "CTRL_REG3" },
+		{ ADDR_CTRL_REG4,   "CTRL_REG4" },
+		{ ADDR_CTRL_REG5,   "CTRL_REG5" },
+		{ ADDR_CTRL_REG6,   "CTRL_REG6" },
+		{ ADDR_CTRL_REG7,   "CTRL_REG7" },
+		{ ADDR_OUT_TEMP_L,  "TEMP_L" },
+		{ ADDR_OUT_TEMP_H,  "TEMP_H" },
+		{ ADDR_INT_CTRL_M,  "INT_CTRL_M" },
+		{ ADDR_INT_SRC_M,   "INT_SRC_M" },
+		{ ADDR_REFERENCE_X, "REFERENCE_X" },
+		{ ADDR_REFERENCE_Y, "REFERENCE_Y" },
+		{ ADDR_REFERENCE_Z, "REFERENCE_Z" },
+		{ ADDR_OUT_X_L_A,   "ACCEL_XL" },
+		{ ADDR_OUT_X_H_A,   "ACCEL_XH" },
+		{ ADDR_OUT_Y_L_A,   "ACCEL_YL" },
+		{ ADDR_OUT_Y_H_A,   "ACCEL_YH" },
+		{ ADDR_OUT_Z_L_A,   "ACCEL_ZL" },
+		{ ADDR_OUT_Z_H_A,   "ACCEL_ZH" },
+		{ ADDR_FIFO_CTRL,   "FIFO_CTRL" },
+		{ ADDR_FIFO_SRC,    "FIFO_SRC" },
+		{ ADDR_IG_CFG1,     "IG_CFG1" },
+		{ ADDR_IG_SRC1,     "IG_SRC1" },
+		{ ADDR_IG_THS1,     "IG_THS1" },
+		{ ADDR_IG_DUR1,     "IG_DUR1" },
+		{ ADDR_IG_CFG2,     "IG_CFG2" },
+		{ ADDR_IG_SRC2,     "IG_SRC2" },
+		{ ADDR_IG_THS2,     "IG_THS2" },
+		{ ADDR_IG_DUR2,     "IG_DUR2" },
+		{ ADDR_CLICK_CFG,   "CLICK_CFG" },
+		{ ADDR_CLICK_SRC,   "CLICK_SRC" },
+		{ ADDR_CLICK_THS,   "CLICK_THS" },
+		{ ADDR_TIME_LIMIT,  "TIME_LIMIT" },
+		{ ADDR_TIME_LATENCY,"TIME_LATENCY" },
+		{ ADDR_TIME_WINDOW, "TIME_WINDOW" },
+		{ ADDR_ACT_THS,     "ACT_THS" },
+		{ ADDR_ACT_DUR,     "ACT_DUR" }
+	};
+	for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) {
+		printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name);
+	}
+	printf("_reg1_expected=0x%02x\n", _reg1_expected);
+	printf("_reg7_expected=0x%02x\n", _reg7_expected);
+}
+
+void
+LSM303D::toggle_logging()
+{
+	if (! _accel_logging_enabled) {
+		_accel_logging_enabled = true;
+		printf("Started logging to %s\n", ACCEL_LOGFILE);
+	} else {
+		_accel_logging_enabled = false;
+		printf("Stopped logging\n");
+	}
+}
+
 LSM303D_mag::LSM303D_mag(LSM303D *parent) :
-	CDev("LSM303D_mag", MAG_DEVICE_PATH),
-	_parent(parent)
+	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
+	_parent(parent),
+	_mag_topic(-1),
+	_mag_class_instance(-1)
 {
 }
 
 LSM303D_mag::~LSM303D_mag()
 {
+	if (_mag_class_instance != -1)
+		unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance);
+}
+
+int
+LSM303D_mag::init()
+{
+	int ret;
+
+	ret = CDev::init();
+	if (ret != OK)
+		goto out;
+
+	_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
+	if (_mag_class_instance == CLASS_DEVICE_PRIMARY) {
+		// we are the primary mag device, so advertise to
+		// the ORB
+		struct mag_report zero_report;
+		memset(&zero_report, 0, sizeof(zero_report));
+		_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
+	}
+
+out:
+	return ret;
 }
 
 void
@@ -1432,6 +1761,8 @@ void	start();
 void	test();
 void	reset();
 void	info();
+void	regdump();
+void	logging();
 
 /**
  * Start the driver.
@@ -1445,7 +1776,7 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+	g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
 
 	if (g_dev == nullptr) {
 		warnx("failed instantiating LSM303D obj");
@@ -1456,7 +1787,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1464,7 +1795,7 @@ start()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
-	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+	fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
 
 	/* don't fail if open cannot be opened */
 	if (0 <= fd_mag) {
@@ -1473,6 +1804,8 @@ start()
 		}
 	}
 
+        close(fd);
+        close(fd_mag);
 
 	exit(0);
 fail:
@@ -1499,10 +1832,10 @@ test()
 	int ret;
 
 	/* get the driver */
-	fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd_accel < 0)
-		err(1, "%s open failed", ACCEL_DEVICE_PATH);
+		err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL);
 
 	/* do a simple demand read */
 	sz = read(fd_accel, &accel_report, sizeof(accel_report));
@@ -1528,10 +1861,10 @@ test()
 	struct mag_report m_report;
 
 	/* get the driver */
-	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+	fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
 
 	if (fd_mag < 0)
-		err(1, "%s open failed", MAG_DEVICE_PATH);
+		err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG);
 
 	/* check if mag is onboard or external */
 	if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
@@ -1554,6 +1887,9 @@ test()
 
 	/* XXX add poll-rate tests here too */
 
+        close(fd_accel);
+        close(fd_mag);
+
 	reset();
 	errx(0, "PASS");
 }
@@ -1564,7 +1900,7 @@ test()
 void
 reset()
 {
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1575,7 +1911,9 @@ reset()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "accel pollrate reset failed");
 
-	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+        close(fd);
+
+	fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
 
 	if (fd < 0) {
 		warnx("mag could not be opened, external mag might be used");
@@ -1585,6 +1923,8 @@ reset()
 			err(1, "mag pollrate reset failed");
 	}
 
+        close(fd);
+
 	exit(0);
 }
 
@@ -1603,6 +1943,35 @@ info()
 	exit(0);
 }
 
+/**
+ * dump registers from device
+ */
+void
+regdump()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	printf("regdump @ %p\n", g_dev);
+	g_dev->print_registers();
+
+	exit(0);
+}
+
+/**
+ * toggle logging
+ */
+void
+logging()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	g_dev->toggle_logging();
+
+	exit(0);
+}
+
 
 } // namespace
 
@@ -1634,5 +2003,17 @@ lsm303d_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "info"))
 		lsm303d::info();
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	/*
+	 * dump device registers
+	 */
+	if (!strcmp(argv[1], "regdump"))
+		lsm303d::regdump();
+
+	/*
+	 * dump device registers
+	 */
+	if (!strcmp(argv[1], "logging"))
+		lsm303d::logging();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
 }

src/drivers/mkblctrl/mkblctrl.cpp

@@ -1,6 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *   Author: Marco Bauer <marco@wtns.de>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +36,8 @@
  * @file mkblctrl.cpp
  *
  * Driver/configurator for the Mikrokopter BL-Ctrl.
- * Marco Bauer
+ *
+ * @author Marco Bauer <marco@wtns.de>
  *
  */
 
@@ -73,7 +75,6 @@
 #include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
@@ -89,8 +90,8 @@
 #define BLCTRL_MIN_VALUE				-0.920F
 #define MOTOR_STATE_PRESENT_MASK		0x80
 #define MOTOR_STATE_ERROR_MASK		0x7F
-#define MOTOR_SPINUP_COUNTER			2500
-#define ESC_UORB_PUBLISH_DELAY			200
+#define MOTOR_SPINUP_COUNTER			30
+#define ESC_UORB_PUBLISH_DELAY		500000
 
 class MK : public device::I2C
 {
@@ -112,7 +113,7 @@ public:
 		FRAME_X,
 	};
 
-	MK(int bus);
+	MK(int bus, const char *_device_path);
 	~MK();
 
 	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
@@ -126,7 +127,7 @@ public:
 	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
 	int		set_px4mode(int px4mode);
 	int		set_frametype(int frametype);
-	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput);
+	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
 
 private:
 	static const unsigned _max_actuators = MAX_MOTORS;
@@ -141,9 +142,9 @@ private:
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
+	char				_device[20];					///< device
 
 	orb_advert_t	_t_outputs;
-	orb_advert_t	_t_actuators_effective;
 	orb_advert_t	_t_esc_status;
 
 	unsigned int	_num_outputs;
@@ -244,7 +245,7 @@ MK	*g_mk;
 
 } // namespace
 
-MK::MK(int bus) :
+MK::MK(int bus, const char *_device_path) :
 	I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
 	_mode(MODE_NONE),
 	_update_rate(50),
@@ -252,7 +253,6 @@ MK::MK(int bus) :
 	_t_actuators(-1),
 	_t_actuator_armed(-1),
 	_t_outputs(0),
-	_t_actuators_effective(0),
 	_t_esc_status(0),
 	_num_outputs(0),
 	_motortest(false),
@@ -265,6 +265,10 @@ MK::MK(int bus) :
 	_armed(false),
 	_mixers(nullptr)
 {
+	strncpy(_device, _device_path, sizeof(_device));
+	/* enforce null termination */
+	_device[sizeof(_device) - 1] = '\0';
+
 	_debug_enabled = true;
 }
 
@@ -291,7 +295,7 @@ MK::~MK()
 
 	/* clean up the alternate device node */
 	if (_primary_pwm_device)
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+		unregister_driver(_device);
 
 	g_mk = nullptr;
 }
@@ -313,13 +317,15 @@ MK::init(unsigned motors)
 
 	usleep(500000);
 
-	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+   if (sizeof(_device) > 0) {
+	   ret = register_driver(_device, &fops, 0666, (void *)this);
 
-	if (ret == OK) {
-		log("default PWM output device");
-		_primary_pwm_device = true;
-	}
+	   if (ret == OK) {
+			log("creating alternate output device");
+			_primary_pwm_device = true;
+		}
+
+   }
 
 	/* reset GPIOs */
 	gpio_reset();
@@ -525,13 +531,6 @@ MK::task_main()
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &outputs);
 
-	/* advertise the effective control inputs */
-	actuator_controls_effective_s controls_effective;
-	memset(&controls_effective, 0, sizeof(controls_effective));
-	/* advertise the effective control inputs */
-	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
-				   &controls_effective);
-
 	/* advertise the blctrl status */
 	esc_status_s esc;
 	memset(&esc, 0, sizeof(esc));
@@ -595,9 +594,6 @@ MK::task_main()
 				outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
 				outputs.timestamp = hrt_absolute_time();
 
-				// XXX output actual limited values
-				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
-
 				/* iterate actuators */
 				for (unsigned int i = 0; i < _num_outputs; i++) {
 
@@ -633,10 +629,7 @@ MK::task_main()
 					}
 
 					/* output to BLCtrl's */
-					if (_motortest == true) {
-						mk_servo_test(i);
-
-					} else {
+					if (_motortest != true) {
 						//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
 						// 11 Bit
 						Motor[i].SetPoint_PX4 = outputs.output[i];
@@ -668,7 +661,7 @@ MK::task_main()
 		 * Only update esc topic every half second.
 		 */
 
-		if (hrt_absolute_time() - esc.timestamp > 500000) {
+		if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) {
 			esc.counter++;
 			esc.timestamp = hrt_absolute_time();
 			esc.esc_count = (uint8_t) _num_outputs;
@@ -692,16 +685,22 @@ MK::task_main()
 				esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
 				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
 				esc.esc[i].esc_errorcount = (uint16_t) 0;
+
+   			// if motortest is requested - do it...
+   			if (_motortest == true) {
+	   		   mk_servo_test(i);
+			   }
+
 			}
 
 			orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
+
 		}
 
 	}
 
-	//::close(_t_esc_status);
+	::close(_t_esc_status);
 	::close(_t_actuators);
-	::close(_t_actuators_effective);
 	::close(_t_actuator_armed);
 
 
@@ -727,8 +726,12 @@ MK::mk_servo_arm(bool status)
 
 
 unsigned int
-MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
+MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
 {
+	if(initI2C) {
+		I2C::init();
+	}
+
 	_retries = 50;
 	uint8_t foundMotorCount = 0;
 
@@ -952,6 +955,7 @@ MK::mk_servo_test(unsigned int chan)
 		if (_motor >= _num_outputs) {
 			_motor = -1;
 			_motortest = false;
+			fprintf(stderr, "[mkblctrl] Motortest finished...\n");
 		}
 	}
 
@@ -1367,7 +1371,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 
 	/* count used motors */
 	do {
-		if (g_mk->mk_check_for_blctrl(8, false) != 0) {
+		if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
 			shouldStop = 4;
 
 		} else {
@@ -1377,7 +1381,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 		sleep(1);
 	} while (shouldStop < 3);
 
-	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
+	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
 
 	/* (re)set the PWM output mode */
 	g_mk->set_mode(servo_mode);
@@ -1390,13 +1394,13 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 }
 
 int
-mk_start(unsigned bus, unsigned motors)
+mk_start(unsigned bus, unsigned motors, char *device_path)
 {
 	int ret = OK;
 
 	if (g_mk == nullptr) {
 
-		g_mk = new MK(bus);
+		g_mk = new MK(bus, device_path);
 
 		if (g_mk == nullptr) {
 			ret = -ENOMEM;
@@ -1415,6 +1419,52 @@ mk_start(unsigned bus, unsigned motors)
 }
 
 
+int
+mk_check_for_i2c_esc_bus(char *device_path, int motors)
+{
+	int ret;
+
+	if (g_mk == nullptr) {
+
+		g_mk = new MK(3, device_path);
+
+		if (g_mk == nullptr) {
+			return -1;
+
+		} else {
+			ret = g_mk->mk_check_for_blctrl(8, false, true);
+			delete g_mk;
+			g_mk = nullptr;
+
+			if (ret > 0) {
+				return 3;
+			}
+
+		}
+
+
+		g_mk = new MK(1, device_path);
+
+		if (g_mk == nullptr) {
+			return -1;
+
+		} else {
+			ret = g_mk->mk_check_for_blctrl(8, false, true);
+			delete g_mk;
+			g_mk = nullptr;
+
+			if (ret > 0) {
+				return 1;
+			}
+
+		}
+	}
+
+	return -1;
+}
+
+
+
 } // namespace
 
 extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
@@ -1425,13 +1475,14 @@ mkblctrl_main(int argc, char *argv[])
 	PortMode port_mode = PORT_FULL_PWM;
 	int pwm_update_rate_in_hz = UPDATE_RATE;
 	int motorcount = 8;
-	int bus = 1;
+	int bus = -1;
 	int px4mode = MAPPING_PX4;
 	int frametype = FRAME_PLUS;	// + plus is default
 	bool motortest = false;
 	bool overrideSecurityChecks = false;
 	bool showHelp = false;
 	bool newMode = false;
+	char *devicepath = "";
 
 	/*
 	 * optional parameters
@@ -1491,36 +1542,69 @@ mkblctrl_main(int argc, char *argv[])
 			newMode = true;
 		}
 
+		/* look for the optional device parameter */
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
+			if (argc > i + 1) {
+				devicepath = argv[i + 1];
+				newMode = true;
+
+			} else {
+				errx(1, "missing the devicename (-d)");
+				return 1;
+			}
+		}
+
 	}
 
 	if (showHelp) {
 		fprintf(stderr, "mkblctrl: help:\n");
-		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n");
-		fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
-		fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n");
-		fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n");
+		fprintf(stderr, "  [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n");
+		fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
+		fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n");
+		fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
 		fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
+		fprintf(stderr, "\n");
+		fprintf(stderr, "Motortest:\n");
+		fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
+		fprintf(stderr, "mkblctrl -t\n");
+      fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
 		exit(1);
 	}
 
 
-	if (g_mk == nullptr) {
-		if (mk_start(bus, motorcount) != OK) {
-			errx(1, "failed to start the MK-BLCtrl driver");
+	if (!motortest) {
+   	if (g_mk == nullptr) {
+      	if (bus == -1) {
+      		bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
+      	}
 
-		} else {
-			newMode = true;
-		}
-	}
+      	if (bus != -1) {
+     			if (mk_start(bus, motorcount, devicepath) != OK) {
+     				errx(1, "failed to start the MK-BLCtrl driver");
+     			}
+      	} else {
+      		errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)");
+      	}
 
+      	/* parameter set ? */
+      	if (newMode) {
+      		/* switch parameter */
+      		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
+      	}
 
-	/* parameter set ? */
-	if (newMode) {
-		/* switch parameter */
-		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
-	}
+      	exit(0);
+      } else {
+         errx(1, "MK-BLCtrl driver already running");
+      }
 
-	/* test, etc. here g*/
+   } else {
+      if (g_mk == nullptr) {
+         errx(1, "MK-BLCtrl driver not running. You have to start it first.");
 
-	exit(1);
+      } else {
+         g_mk->set_motor_test(motortest);
+         exit(0);
+
+      }
+   }
 }

src/drivers/mpu6000/mpu6000.cpp

@@ -75,6 +75,9 @@
 #define DIR_READ			0x80
 #define DIR_WRITE			0x00
 
+#define MPU_DEVICE_PATH_ACCEL		"/dev/mpu6000_accel"
+#define MPU_DEVICE_PATH_GYRO		"/dev/mpu6000_gyro"
+
 // MPU 6000 registers
 #define MPUREG_WHOAMI			0x75
 #define MPUREG_SMPLRT_DIV		0x19
@@ -161,6 +164,14 @@
 
 #define MPU6000_ONE_G					9.80665f
 
+/*
+  the MPU6000 can only handle high SPI bus speeds on the sensor and
+  interrupt status registers. All other registers have a maximum 1MHz
+  SPI speed
+ */
+#define MPU6000_LOW_BUS_SPEED				1000*1000
+#define MPU6000_HIGH_BUS_SPEED				11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
+
 class MPU6000_gyro;
 
 class MPU6000 : public device::SPI
@@ -200,17 +211,19 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
+	int			_accel_class_instance;
 
 	RingBuffer		*_gyro_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
-	orb_advert_t		_gyro_topic;
 
-	unsigned		_reads;
 	unsigned		_sample_rate;
+	perf_counter_t		_accel_reads;
+	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_bad_transfers;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
@@ -338,12 +351,17 @@ public:
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
 	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
 
+	virtual int		init();
+
 protected:
 	friend class MPU6000;
 
 	void			parent_poll_notify();
+
 private:
 	MPU6000			*_parent;
+	orb_advert_t		_gyro_topic;
+	int			_gyro_class_instance;
 
 };
 
@@ -351,7 +369,7 @@ private:
 extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
 
 MPU6000::MPU6000(int bus, spi_dev_e device) :
-	SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000),
+	SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
 	_gyro(new MPU6000_gyro(this)),
 	_product(0),
 	_call_interval(0),
@@ -359,13 +377,15 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_accel_class_instance(-1),
 	_gyro_reports(nullptr),
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
-	_gyro_topic(-1),
-	_reads(0),
 	_sample_rate(1000),
+	_accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")),
+	_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")),
 	_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),
@@ -409,8 +429,14 @@ MPU6000::~MPU6000()
 	if (_gyro_reports != nullptr)
 		delete _gyro_reports;
 
+	if (_accel_class_instance != -1)
+		unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
+
 	/* delete the perf counter */
 	perf_free(_sample_perf);
+	perf_free(_accel_reads);
+	perf_free(_gyro_reads);
+	perf_free(_bad_transfers);
 }
 
 int
@@ -455,24 +481,23 @@ MPU6000::init()
 	_gyro_scale.z_scale  = 1.0f;
 
 	/* do CDev init for the gyro device node, keep it optional */
-	gyro_ret = _gyro->init();
+	ret = _gyro->init();
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		debug("gyro init failed");
+		return ret;
+	}
 
 	/* fetch an initial set of measurements for advertisement */
 	measure();
 
-	if (gyro_ret != OK) {
-		_gyro_topic = -1;
-	} else {
-		gyro_report gr;
-		_gyro_reports->get(&gr);
-
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
-	}
-
-	/* advertise accel topic */
-	accel_report ar;
-	_accel_reports->get(&ar);
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
+	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
+	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+		/* advertise accel topic */
+		accel_report ar;
+		_accel_reports->get(&ar);
+		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
+	}		
 
 out:
 	return ret;
@@ -480,17 +505,26 @@ out:
 
 void MPU6000::reset()
 {
+	// if the mpu6000 is initialised after the l3gd20 and lsm303d
+	// then if we don't do an irqsave/irqrestore here the mpu6000
+	// frequenctly comes up in a bad state where all transfers
+	// come as zero
+	irqstate_t state;
+	state = irqsave();
 
-	// Chip reset
 	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
 	up_udelay(10000);
 
-	// Wake up device and select GyroZ clock (better performance)
+	// Wake up device and select GyroZ clock. Note that the
+	// MPU6000 starts up in sleep mode, and it can take some time
+	// for it to come out of sleep
 	write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
 	up_udelay(1000);
 
 	// Disable I2C bus (recommended on datasheet)
 	write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
+        irqrestore(state);
+
 	up_udelay(1000);
 
 	// SAMPLE RATE
@@ -652,6 +686,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 	if (_accel_reports->empty())
 		return -EAGAIN;
 
+	perf_count(_accel_reads);
+
 	/* copy reports out of our buffer to the caller */
 	accel_report *arp = reinterpret_cast<accel_report *>(buffer);
 	int transferred = 0;
@@ -669,12 +705,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 int
 MPU6000::self_test()
 {
-	if (_reads == 0) {
+	if (perf_event_count(_sample_perf) == 0) {
 		measure();
 	}
 
 	/* return 0 on success, 1 else */
-	return (_reads > 0) ? 0 : 1;
+	return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
 }
 
 int
@@ -746,6 +782,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 	if (_gyro_reports->empty())
 		return -EAGAIN;
 
+	perf_count(_gyro_reads);
+
 	/* copy reports out of our buffer to the caller */
 	gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
 	int transferred = 0;
@@ -987,9 +1025,10 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 uint8_t
 MPU6000::read_reg(unsigned reg)
 {
-	uint8_t cmd[2];
+	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
 
-	cmd[0] = reg | DIR_READ;
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -999,9 +1038,10 @@ MPU6000::read_reg(unsigned reg)
 uint16_t
 MPU6000::read_reg16(unsigned reg)
 {
-	uint8_t cmd[3];
+	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
 
-	cmd[0] = reg | DIR_READ;
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
 
 	transfer(cmd, cmd, sizeof(cmd));
 
@@ -1016,6 +1056,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value)
 	cmd[0] = reg | DIR_WRITE;
 	cmd[1] = value;
 
+        // general register transfer at low clock speed
+        set_frequency(MPU6000_LOW_BUS_SPEED);
+
 	transfer(cmd, nullptr, sizeof(cmd));
 }
 
@@ -1139,12 +1182,13 @@ MPU6000::measure()
 	 * Fetch the full set of measurements from the MPU6000 in one pass.
 	 */
 	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+
+        // sensor transfer at high clock speed
+        set_frequency(MPU6000_HIGH_BUS_SPEED);
+
 	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
 		return;
 
-	/* count measurement */
-	_reads++;
-
 	/*
 	 * Convert from big to little endian
 	 */
@@ -1159,6 +1203,20 @@ MPU6000::measure()
 	report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
 	report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
 
+	if (report.accel_x == 0 &&
+	    report.accel_y == 0 &&
+	    report.accel_z == 0 &&
+	    report.temp == 0 &&
+	    report.gyro_x == 0 &&
+	    report.gyro_y == 0 &&
+	    report.gyro_z == 0) {
+		// all zero data - probably a SPI bus error
+		perf_count(_bad_transfers);
+		perf_end(_sample_perf);
+		return;
+	}
+	    
+
 	/*
 	 * Swap axes and negate y
 	 */
@@ -1249,10 +1307,11 @@ MPU6000::measure()
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	/* and publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
-	if (_gyro_topic != -1) {
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
+	if (_accel_topic != -1) {
+		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
+	}
+	if (_gyro->_gyro_topic != -1) {
+		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -1263,19 +1322,48 @@ void
 MPU6000::print_info()
 {
 	perf_print_counter(_sample_perf);
-	printf("reads:          %u\n", _reads);
+	perf_print_counter(_accel_reads);
+	perf_print_counter(_gyro_reads);
 	_accel_reports->print_info("accel queue");
 	_gyro_reports->print_info("gyro queue");
 }
 
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
-	CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
-	_parent(parent)
+	CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
+	_parent(parent),
+	_gyro_class_instance(-1)
 {
 }
 
 MPU6000_gyro::~MPU6000_gyro()
 {
+	if (_gyro_class_instance != -1)
+		unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance);
+}
+
+int
+MPU6000_gyro::init()
+{
+	int ret;
+
+	// do base class init
+	ret = CDev::init();
+
+	/* if probe/setup failed, bail now */
+	if (ret != OK) {
+		debug("gyro init failed");
+		return ret;
+	}
+
+	_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
+	if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
+		gyro_report gr;
+		memset(&gr, 0, sizeof(gr));
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
+	}
+
+out:
+	return ret;
 }
 
 void
@@ -1331,7 +1419,7 @@ start()
 		goto fail;
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		goto fail;
@@ -1339,6 +1427,8 @@ start()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
+        close(fd);
+
 	exit(0);
 fail:
 
@@ -1363,17 +1453,17 @@ test()
 	ssize_t sz;
 
 	/* get the driver */
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
-		    ACCEL_DEVICE_PATH);
+		    MPU_DEVICE_PATH_ACCEL);
 
 	/* get the driver */
-	int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+	int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
 
 	if (fd_gyro < 0)
-		err(1, "%s open failed", GYRO_DEVICE_PATH);
+		err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
 
 	/* reset to manual polling */
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@@ -1431,7 +1521,7 @@ test()
 void
 reset()
 {
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
 
 	if (fd < 0)
 		err(1, "failed ");
@@ -1442,6 +1532,8 @@ reset()
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 
+        close(fd);
+
 	exit(0);
 }
 

src/drivers/ms5611/ms5611.cpp

@@ -420,8 +420,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _reports->size();
 
 	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
+		/*
+		 * Since we are initialized, we do not need to do anything, since the
+		 * PROM is correctly read and the part does not need to be configured.
+		 */
+		return OK;
 
 	case BAROIOCSMSLPRESSURE:
 

src/drivers/ms5611/ms5611_spi.cpp

@@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf)
 }
 
 MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
-	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000),
+	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
 	_prom(prom_buf)
 {
 }
@@ -134,7 +134,6 @@ int
 MS5611_SPI::init()
 {
 	int ret;
-	irqstate_t flags;
 
 	ret = SPI::init();
 	if (ret != OK) {
@@ -167,10 +166,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count)
 		uint8_t	b[4];
 		uint32_t w;
 	} *cvt = (_cvt *)data;
-	uint8_t buf[4];
+	uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 };
 
 	/* read the most recent measurement */
-	buf[0] = 0 | DIR_WRITE;
 	int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
 
 	if (ret == OK) {
@@ -238,21 +236,31 @@ MS5611_SPI::_read_prom()
 	usleep(3000);
 
 	/* read and convert PROM words */
+        bool all_zero = true;
 	for (int i = 0; i < 8; i++) {
 		uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
 		_prom.c[i] = _reg16(cmd);
+                if (_prom.c[i] != 0)
+			all_zero = false;
+                //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
 	}
 
 	/* calculate CRC and return success/failure accordingly */
-	return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+	int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+        if (ret != OK) {
+		debug("crc failed");
+        }
+        if (all_zero) {
+		debug("prom all zero");
+		ret = -EIO;
+        }
+        return ret;
 }
 
 uint16_t
 MS5611_SPI::_reg16(unsigned reg)
 {
-	uint8_t cmd[3];
-
-	cmd[0] = reg | DIR_READ;
+	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
 
 	_transfer(cmd, cmd, sizeof(cmd));
 

src/drivers/px4fmu/fmu.cpp

@@ -69,7 +69,6 @@
 #include <drivers/drv_rc_input.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
@@ -123,7 +122,6 @@ private:
 	int		_t_actuators;
 	int		_t_actuator_armed;
 	orb_advert_t	_t_outputs;
-	orb_advert_t	_t_actuators_effective;
 	unsigned	_num_outputs;
 	bool		_primary_pwm_device;
 
@@ -164,6 +162,7 @@ private:
 	static const unsigned	_ngpio;
 
 	void		gpio_reset(void);
+	void		sensor_reset(int ms);
 	void		gpio_set_function(uint32_t gpios, int function);
 	void		gpio_write(uint32_t gpios, int function);
 	uint32_t	gpio_read(void);
@@ -219,17 +218,16 @@ PX4FMU::PX4FMU() :
 	_t_actuators(-1),
 	_t_actuator_armed(-1),
 	_t_outputs(0),
-	_t_actuators_effective(0),
 	_num_outputs(0),
 	_primary_pwm_device(false),
 	_task_should_exit(false),
 	_armed(false),
 	_pwm_on(false),
 	_mixers(nullptr),
-	_failsafe_pwm({0}),
-	_disarmed_pwm({0}),
-	_num_failsafe_set(0),
-	_num_disarmed_set(0)
+	_failsafe_pwm( {0}),
+	       _disarmed_pwm( {0}),
+	       _num_failsafe_set(0),
+	       _num_disarmed_set(0)
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		_min_pwm[i] = PWM_DEFAULT_MIN;
@@ -293,11 +291,11 @@ PX4FMU::init()
 
 	/* start the IO interface task */
 	_task = task_spawn_cmd("fmuservo",
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_DEFAULT,
-			   2048,
-			   (main_t)&PX4FMU::task_main_trampoline,
-			   nullptr);
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_DEFAULT,
+			       2048,
+			       (main_t)&PX4FMU::task_main_trampoline,
+			       nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -326,7 +324,7 @@ PX4FMU::set_mode(Mode mode)
 	switch (mode) {
 	case MODE_2PWM:	// v1 multi-port with flow control lines as PWM
 		debug("MODE_2PWM");
-		
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
@@ -340,7 +338,7 @@ PX4FMU::set_mode(Mode mode)
 
 	case MODE_4PWM: // v1 multi-port as 4 PWM outs
 		debug("MODE_4PWM");
-		
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
@@ -354,7 +352,7 @@ PX4FMU::set_mode(Mode mode)
 
 	case MODE_6PWM: // v2 PWMs as 6 PWM outs
 		debug("MODE_6PWM");
-		
+
 		/* default output rates */
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
@@ -396,6 +394,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 
 			// get the channel mask for this rate group
 			uint32_t mask = up_pwm_servo_get_rate_group(group);
+
 			if (mask == 0)
 				continue;
 
@@ -409,6 +408,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 					// not a legal map, bail
 					return -EINVAL;
 				}
+
 			} else {
 				// set it - errors here are unexpected
 				if (alt != 0) {
@@ -416,6 +416,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 						warn("rate group set alt failed");
 						return -EINVAL;
 					}
+
 				} else {
 					if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
 						warn("rate group set default failed");
@@ -425,6 +426,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
 			}
 		}
 	}
+
 	_pwm_alt_rate_channels = rate_map;
 	_pwm_default_rate = default_rate;
 	_pwm_alt_rate = alt_rate;
@@ -466,13 +468,6 @@ PX4FMU::task_main()
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &outputs);
 
-	/* advertise the effective control inputs */
-	actuator_controls_effective_s controls_effective;
-	memset(&controls_effective, 0, sizeof(controls_effective));
-	/* advertise the effective control inputs */
-	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
-				   &controls_effective);
-
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
@@ -503,6 +498,7 @@ PX4FMU::task_main()
 		 * We always mix at max rate; some channels may update slower.
 		 */
 		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
+
 		if (_current_update_rate != max_rate) {
 			_current_update_rate = max_rate;
 			int update_rate_in_ms = int(1000 / _current_update_rate);
@@ -511,6 +507,7 @@ PX4FMU::task_main()
 			if (update_rate_in_ms < 2) {
 				update_rate_in_ms = 2;
 			}
+
 			/* reject slower than 10 Hz updates */
 			if (update_rate_in_ms > 100) {
 				update_rate_in_ms = 100;
@@ -532,6 +529,7 @@ PX4FMU::task_main()
 			log("poll error %d", errno);
 			usleep(1000000);
 			continue;
+
 		} else if (ret == 0) {
 			/* timeout: no control data, switch to failsafe values */
 //			warnx("no PWM: failsafe");
@@ -554,12 +552,15 @@ PX4FMU::task_main()
 					case MODE_2PWM:
 						num_outputs = 2;
 						break;
+
 					case MODE_4PWM:
 						num_outputs = 4;
 						break;
+
 					case MODE_6PWM:
 						num_outputs = 6;
 						break;
+
 					default:
 						num_outputs = 0;
 						break;
@@ -573,9 +574,9 @@ PX4FMU::task_main()
 					for (unsigned i = 0; i < num_outputs; i++) {
 						/* last resort: catch NaN, INF and out-of-band errors */
 						if (i >= outputs.noutputs ||
-							!isfinite(outputs.output[i]) ||
-							outputs.output[i] < -1.0f ||
-							outputs.output[i] > 1.0f) {
+						    !isfinite(outputs.output[i]) ||
+						    outputs.output[i] < -1.0f ||
+						outputs.output[i] > 1.0f) {
 							/*
 							 * Value is NaN, INF or out of band - set to the minimum value.
 							 * This will be clearly visible on the servo status and will limit the risk of accidentally
@@ -592,12 +593,6 @@ PX4FMU::task_main()
 
 					pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
 
-					/* output actual limited values */
-					for (unsigned i = 0; i < num_outputs; i++) {
-						controls_effective.control_effective[i] = (float)pwm_limited[i];
-					}
-					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
-
 					/* output to the servos */
 					for (unsigned i = 0; i < num_outputs; i++) {
 						up_pwm_servo_set(i, pwm_limited[i]);
@@ -617,11 +612,13 @@ PX4FMU::task_main()
 
 				/* update the armed status and check that we're not locked down */
 				bool set_armed = aa.armed && !aa.lockdown;
+
 				if (_armed != set_armed)
 					_armed = set_armed;
 
 				/* update PWM status if armed or if disarmed PWM values are set */
 				bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+
 				if (_pwm_on != pwm_on) {
 					_pwm_on = pwm_on;
 					up_pwm_servo_arm(pwm_on);
@@ -630,31 +627,36 @@ PX4FMU::task_main()
 		}
 
 #ifdef HRT_PPM_CHANNEL
+
 		// see if we have new PPM input data
 		if (ppm_last_valid_decode != rc_in.timestamp) {
 			// we have a new PPM frame. Publish it.
 			rc_in.channel_count = ppm_decoded_channels;
+
 			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
 				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
 			}
-			for (uint8_t i=0; i<rc_in.channel_count; i++) {
+
+			for (uint8_t i = 0; i < rc_in.channel_count; i++) {
 				rc_in.values[i] = ppm_buffer[i];
 			}
+
 			rc_in.timestamp = ppm_last_valid_decode;
 
 			/* lazily advertise on first publication */
 			if (to_input_rc == 0) {
 				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
-			} else { 
+
+			} else {
 				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
 			}
 		}
+
 #endif
 
 	}
 
 	::close(_t_actuators);
-	::close(_t_actuators_effective);
 	::close(_t_actuator_armed);
 
 	/* make sure servos are off */
@@ -757,142 +759,176 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_FAILSAFE_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_failsafe_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_failsafe_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_failsafe_pwm[i] = pwm->values[i];
+				}
+			}
+
+			/*
+			 * update the counter
+			 * this is needed to decide if disarmed PWM output should be turned on or not
+			 */
+			_num_failsafe_set = 0;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				if (_failsafe_pwm[i] > 0)
+					_num_failsafe_set++;
+			}
+
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
-				_failsafe_pwm[i] = PWM_HIGHEST_MAX;
-			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
-				_failsafe_pwm[i] = PWM_LOWEST_MIN;
-			} else {
-				_failsafe_pwm[i] = pwm->values[i];
-			}
-		}
 
-		/*
-		 * update the counter
-		 * this is needed to decide if disarmed PWM output should be turned on or not
-		 */
-		_num_failsafe_set = 0;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			if (_failsafe_pwm[i] > 0)
-				_num_failsafe_set++;
-		}
-		break;
-	}
 	case PWM_SERVO_GET_FAILSAFE_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _failsafe_pwm[i];
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _failsafe_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			break;
 		}
-		pwm->channel_count = _max_actuators;
-		break;
-	}
 
 	case PWM_SERVO_SET_DISARMED_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_disarmed_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_disarmed_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_disarmed_pwm[i] = pwm->values[i];
+				}
+			}
+
+			/*
+			 * update the counter
+			 * this is needed to decide if disarmed PWM output should be turned on or not
+			 */
+			_num_disarmed_set = 0;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				if (_disarmed_pwm[i] > 0)
+					_num_disarmed_set++;
+			}
+
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
-				_disarmed_pwm[i] = PWM_HIGHEST_MAX;
-			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
-				_disarmed_pwm[i] = PWM_LOWEST_MIN;
-			} else {
-				_disarmed_pwm[i] = pwm->values[i];
-			}
-		}
 
-		/*
-		 * update the counter
-		 * this is needed to decide if disarmed PWM output should be turned on or not
-		 */
-		_num_disarmed_set = 0;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			if (_disarmed_pwm[i] > 0)
-				_num_disarmed_set++;
-		}
-		break;
-	}
 	case PWM_SERVO_GET_DISARMED_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _disarmed_pwm[i];
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _disarmed_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			break;
 		}
-		pwm->channel_count = _max_actuators;
-		break;
-	}
 
 	case PWM_SERVO_SET_MIN_PWM: {
-		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
+					_min_pwm[i] = PWM_HIGHEST_MIN;
+
+				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_min_pwm[i] = PWM_LOWEST_MIN;
+
+				} else {
+					_min_pwm[i] = pwm->values[i];
+				}
+			}
+
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
-				_min_pwm[i] = PWM_HIGHEST_MIN;
-			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
-				_min_pwm[i] = PWM_LOWEST_MIN;
-			} else {
-				_min_pwm[i] = pwm->values[i];
-			}
-		}
-		break;
-	}
+
 	case PWM_SERVO_GET_MIN_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _min_pwm[i];
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _min_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			arg = (unsigned long)&pwm;
+			break;
 		}
-		pwm->channel_count = _max_actuators;
-		arg = (unsigned long)&pwm;
-		break;
-	}
 
 	case PWM_SERVO_SET_MAX_PWM: {
-		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
-		/* discard if too many values are sent */
-		if (pwm->channel_count > _max_actuators) {
-			ret = -EINVAL;
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			/* discard if too many values are sent */
+			if (pwm->channel_count > _max_actuators) {
+				ret = -EINVAL;
+				break;
+			}
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (pwm->values[i] == 0) {
+					/* ignore 0 */
+				} else if (pwm->values[i] < PWM_LOWEST_MAX) {
+					_max_pwm[i] = PWM_LOWEST_MAX;
+
+				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+					_max_pwm[i] = PWM_HIGHEST_MAX;
+
+				} else {
+					_max_pwm[i] = pwm->values[i];
+				}
+			}
+
 			break;
 		}
-		for (unsigned i = 0; i < pwm->channel_count; i++) {
-			if (pwm->values[i] == 0) {
-				/* ignore 0 */
-			} else if (pwm->values[i] < PWM_LOWEST_MAX) {
-				_max_pwm[i] = PWM_LOWEST_MAX;
-			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
-				_max_pwm[i] = PWM_HIGHEST_MAX;
-			} else {
-				_max_pwm[i] = pwm->values[i];
-			}
-		}
-		break;
-	}
+
 	case PWM_SERVO_GET_MAX_PWM: {
-		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			pwm->values[i] = _max_pwm[i];
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				pwm->values[i] = _max_pwm[i];
+			}
+
+			pwm->channel_count = _max_actuators;
+			arg = (unsigned long)&pwm;
+			break;
 		}
-		pwm->channel_count = _max_actuators;
-		arg = (unsigned long)&pwm;
-		break;
-	}
 
 	case PWM_SERVO_SET(5):
 	case PWM_SERVO_SET(4):
@@ -914,6 +950,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(0):
 		if (arg <= 2100) {
 			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
+
 		} else {
 			ret = -EINVAL;
 		}
@@ -950,18 +987,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;
 
-	case PWM_SERVO_GET_COUNT:	
+	case PWM_SERVO_GET_COUNT:
 	case MIXERIOCGETOUTPUTCOUNT:
 		switch (_mode) {
 		case MODE_6PWM:
 			*(unsigned *)arg = 6;
 			break;
+
 		case MODE_4PWM:
 			*(unsigned *)arg = 4;
 			break;
+
 		case MODE_2PWM:
 			*(unsigned *)arg = 2;
 			break;
+
 		default:
 			ret = -EINVAL;
 			break;
@@ -1019,6 +1059,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
+
 			break;
 		}
 
@@ -1053,9 +1094,80 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	for (uint8_t i = 0; i < count; i++) {
 		up_pwm_servo_set(i, values[i]);
 	}
+
 	return count * 2;
 }
 
+void
+PX4FMU::sensor_reset(int ms)
+{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
+	if (ms < 1) {
+		ms = 1;
+	}
+
+	/* disable SPI bus */
+	stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
+	stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
+
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+
+	stm32_configgpio(GPIO_SPI1_SCK_OFF);
+	stm32_configgpio(GPIO_SPI1_MISO_OFF);
+	stm32_configgpio(GPIO_SPI1_MOSI_OFF);
+
+	stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
+
+	stm32_configgpio(GPIO_GYRO_DRDY_OFF);
+	stm32_configgpio(GPIO_MAG_DRDY_OFF);
+	stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
+
+	stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
+	stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+
+	/* set the sensor rail off */
+	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+
+	/* wait for the sensor rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the sensor rail back on */
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
+
+	/* wait a bit before starting SPI, different times didn't influence results */
+	usleep(100);
+
+	/* reconfigure the SPI pins */
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+	stm32_configgpio(GPIO_SPI_CS_MPU);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+#endif
+#endif
+}
+
+
 void
 PX4FMU::gpio_reset(void)
 {
@@ -1066,6 +1178,7 @@ PX4FMU::gpio_reset(void)
 	for (unsigned i = 0; i < _ngpio; i++) {
 		if (_gpio_tab[i].input != 0) {
 			stm32_configgpio(_gpio_tab[i].input);
+
 		} else if (_gpio_tab[i].output != 0) {
 			stm32_configgpio(_gpio_tab[i].output);
 		}
@@ -1082,6 +1195,7 @@ void
 PX4FMU::gpio_set_function(uint32_t gpios, int function)
 {
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	/*
 	 * GPIOs 0 and 1 must have the same direction as they are buffered
 	 * by a shared 2-port driver.  Any attempt to set either sets both.
@@ -1093,6 +1207,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		if (GPIO_SET_OUTPUT == function)
 			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
 	}
+
 #endif
 
 	/* configure selected GPIOs as required */
@@ -1117,9 +1232,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 	}
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	/* flip buffer to input mode if required */
 	if ((GPIO_SET_INPUT == function) && (gpios & 3))
 		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+
 #endif
 }
 
@@ -1158,6 +1275,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 		gpio_reset();
 		break;
 
+	case GPIO_SENSOR_RAIL_RESET:
+		sensor_reset(arg);
+		break;
+
 	case GPIO_SET_OUTPUT:
 	case GPIO_SET_INPUT:
 	case GPIO_SET_ALT_1:
@@ -1231,8 +1352,9 @@ fmu_new_mode(PortMode new_mode)
 #endif
 		break;
 
-	/* mixed modes supported on v1 board only */
+		/* mixed modes supported on v1 board only */
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	case PORT_FULL_SERIAL:
 		/* set all multi-GPIOs to serial mode */
 		gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
@@ -1255,6 +1377,7 @@ fmu_new_mode(PortMode new_mode)
 		servo_mode = PX4FMU::MODE_2PWM;
 		break;
 #endif
+
 	default:
 		return -1;
 	}
@@ -1308,15 +1431,31 @@ fmu_stop(void)
 	return ret;
 }
 
+void
+sensor_reset(int ms)
+{
+	int	 fd;
+	int	 ret;
+
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0)
+		errx(1, "open fail");
+
+	if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
+		err(1, "servo arm failed");
+
+}
+
 void
 test(void)
 {
 	int	 fd;
-        unsigned servo_count = 0;
+	unsigned servo_count = 0;
 	unsigned pwm_value = 1000;
 	int	 direction = 1;
 	int	 ret;
-        
+
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
 	if (fd < 0)
@@ -1324,9 +1463,9 @@ test(void)
 
 	if (ioctl(fd, PWM_SERVO_ARM, 0) < 0)       err(1, "servo arm failed");
 
-        if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
-            err(1, "Unable to get servo count\n");        
-        }
+	if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+		err(1, "Unable to get servo count\n");
+	}
 
 	warnx("Testing %u servos", (unsigned)servo_count);
 
@@ -1339,32 +1478,38 @@ test(void)
 	for (;;) {
 		/* sweep all servos between 1000..2000 */
 		servo_position_t servos[servo_count];
+
 		for (unsigned i = 0; i < servo_count; i++)
 			servos[i] = pwm_value;
 
-                if (direction == 1) {
-                    // use ioctl interface for one direction
-                    for (unsigned i=0; i < servo_count;	i++) {
-                        if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
-                            err(1, "servo %u set failed", i);
-                        }
-                    }
-                } else {
-                    // and use write interface for the other direction
-                    ret = write(fd, servos, sizeof(servos));
-                    if (ret != (int)sizeof(servos))
-			err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
-                }
+		if (direction == 1) {
+			// use ioctl interface for one direction
+			for (unsigned i = 0; i < servo_count;	i++) {
+				if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+					err(1, "servo %u set failed", i);
+				}
+			}
+
+		} else {
+			// and use write interface for the other direction
+			ret = write(fd, servos, sizeof(servos));
+
+			if (ret != (int)sizeof(servos))
+				err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+		}
 
 		if (direction > 0) {
 			if (pwm_value < 2000) {
 				pwm_value++;
+
 			} else {
 				direction = -1;
 			}
+
 		} else {
 			if (pwm_value > 1000) {
 				pwm_value--;
+
 			} else {
 				direction = 1;
 			}
@@ -1376,6 +1521,7 @@ test(void)
 
 			if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
 				err(1, "error reading PWM servo %d", i);
+
 			if (value != servos[i])
 				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
 		}
@@ -1383,12 +1529,14 @@ test(void)
 		/* Check if user wants to quit */
 		char c;
 		ret = poll(&fds, 1, 0);
+
 		if (ret > 0) {
 
 			read(0, &c, 1);
+
 			if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("User abort\n");
-                                break;
+				break;
 			}
 		}
 	}
@@ -1461,6 +1609,7 @@ fmu_main(int argc, char *argv[])
 		new_mode = PORT_FULL_PWM;
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+
 	} else if (!strcmp(verb, "mode_serial")) {
 		new_mode = PORT_FULL_SERIAL;
 
@@ -1493,11 +1642,24 @@ fmu_main(int argc, char *argv[])
 	if (!strcmp(verb, "fake"))
 		fake(argc - 1, argv + 1);
 
+	if (!strcmp(verb, "sensor_reset")) {
+		if (argc > 2) {
+			int reset_time = strtol(argv[2], 0, 0);
+			sensor_reset(reset_time);
+
+		} else {
+			sensor_reset(0);
+			warnx("resettet default time");
+		}
+		exit(0);
+	}
+
+
 	fprintf(stderr, "FMU: unrecognised command, try:\n");
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-	fprintf(stderr, "  mode_gpio, mode_pwm, test\n");
+	fprintf(stderr, "  mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
 #endif
 	exit(1);
 }

src/drivers/px4io/px4io.cpp

@@ -54,6 +54,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <math.h>
+#include <crc32.h>
 
 #include <arch/board/board.h>
 
@@ -72,7 +73,6 @@
 #include <systemlib/param/param.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
@@ -95,6 +95,8 @@ extern device::Device *PX4IO_serial_interface() weak_function;
 
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
+#define PX4IO_REBOOT_BOOTLOADER		_IOC(0xff00, 2)
+#define PX4IO_CHECK_CRC			_IOC(0xff00, 3)
 
 #define UPDATE_INTERVAL_MIN		2			// 2 ms	-> 500 Hz
 #define ORB_CHECK_INTERVAL		200000		// 200 ms -> 5 Hz
@@ -226,30 +228,36 @@ private:
 	device::Device		*_interface;
 
 	// XXX
-	unsigned		_hardware;			///< Hardware revision
-	unsigned		_max_actuators;		///<Maximum # of actuators supported by PX4IO
-	unsigned		_max_controls;		///<Maximum # of controls supported by PX4IO
-	unsigned		_max_rc_input;		///<Maximum receiver channels supported by PX4IO
-	unsigned		_max_relays;		///<Maximum relays supported by PX4IO
-	unsigned		_max_transfer;		///<Maximum number of I2C transfers supported by PX4IO
+	unsigned		_hardware;		///< Hardware revision
+	unsigned		_max_actuators;		///< Maximum # of actuators supported by PX4IO
+	unsigned		_max_controls;		///< Maximum # of controls supported by PX4IO
+	unsigned		_max_rc_input;		///< Maximum receiver channels supported by PX4IO
+	unsigned		_max_relays;		///< Maximum relays supported by PX4IO
+	unsigned		_max_transfer;		///< Maximum number of I2C transfers supported by PX4IO
 
 	unsigned 		_update_interval;	///< Subscription interval limiting send rate
 	bool			_rc_handling_disabled;	///< If set, IO does not evaluate, but only forward the RC values
+	unsigned		_rc_chan_count;		///< Internal copy of the last seen number of RC channels
 
-	volatile int		_task;			///<worker task id
-	volatile bool		_task_should_exit;	///<worker terminate flag
+	volatile int		_task;			///< worker task id
+	volatile bool		_task_should_exit;	///< worker terminate flag
 
-	int			_mavlink_fd;		///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
-	int			_thread_mavlink_fd;	///<mavlink file descriptor for thread.
+	int			_mavlink_fd;		///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
+	int			_thread_mavlink_fd;	///< mavlink file descriptor for thread.
 
-	perf_counter_t		_perf_update;		///<local performance counter
+	perf_counter_t		_perf_update;		///<local performance counter for status updates
+	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
+	perf_counter_t		_perf_chan_count;	///<local performance counter for channel number changes
 
 	/* cached IO state */
-	uint16_t		_status;		///<Various IO status flags
-	uint16_t		_alarms;		///<Various IO alarms
+	uint16_t		_status;		///< Various IO status flags
+	uint16_t		_alarms;		///< Various IO alarms
 
 	/* subscribed topics */
-	int			_t_actuators;		///< actuator controls topic
+	int			_t_actuator_controls_0;	///< actuator controls group 0 topic
+	int			_t_actuator_controls_1;	///< actuator controls group 1 topic
+	int			_t_actuator_controls_2;	///< actuator controls group 2 topic
+	int			_t_actuator_controls_3;	///< actuator controls group 3 topic
 	int			_t_actuator_armed;	///< system armed control topic
 	int 			_t_vehicle_control_mode;///< vehicle control mode topic
 	int			_t_param;		///< parameter update topic
@@ -257,24 +265,22 @@ private:
 
 	/* advertised topics */
 	orb_advert_t 		_to_input_rc;		///< rc inputs from io
-	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
 	orb_advert_t		_to_outputs;		///< mixed servo outputs topic
 	orb_advert_t		_to_battery;		///< battery status / voltage
 	orb_advert_t		_to_servorail;		///< servorail status
 	orb_advert_t		_to_safety;		///< status of safety
 
 	actuator_outputs_s	_outputs;		///<mixed outputs
-	actuator_controls_effective_s _controls_effective; ///<effective controls
 
-	bool			_primary_pwm_device;	///<true if we are the default PWM output
+	bool			_primary_pwm_device;	///< true if we are the default PWM output
 
-	float			_battery_amp_per_volt;	///<current sensor amps/volt
-	float			_battery_amp_bias;	///<current sensor bias
-	float			_battery_mamphour_total;///<amp hours consumed so far
-	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
+	float			_battery_amp_per_volt;	///< current sensor amps/volt
+	float			_battery_amp_bias;	///< current sensor bias
+	float			_battery_mamphour_total;///< amp hours consumed so far
+	uint64_t		_battery_last_timestamp;///< last amp hour calculation timestamp
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
+	bool			_dsm_vcc_ctl;		///< true if relay 1 controls DSM satellite RX power
 #endif
 
 	/**
@@ -288,9 +294,14 @@ private:
 	void			task_main();
 
 	/**
-	 * Send controls to IO
+	 * Send controls for one group to IO
 	 */
-	int			io_set_control_state();
+	int			io_set_control_state(unsigned group);
+
+	/**
+	 * Send all controls to IO
+	 */
+	int			io_set_control_groups();
 
 	/**
 	 * Update IO's arming-related state
@@ -327,11 +338,6 @@ private:
 	 */
 	int			io_publish_raw_rc();
 
-	/**
-	 * Fetch and publish the mixed control values.
-	 */
-	int			io_publish_mixed_controls();
-
 	/**
 	 * Fetch and publish the PWM servo outputs.
 	 */
@@ -459,20 +465,25 @@ PX4IO::PX4IO(device::Device *interface) :
 	_max_transfer(16),	/* sensible default */
 	_update_interval(0),
 	_rc_handling_disabled(false),
+	_rc_chan_count(0),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
 	_thread_mavlink_fd(-1),
-	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
+	_perf_update(perf_alloc(PC_ELAPSED, "io update")),
+	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
+	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
 	_status(0),
 	_alarms(0),
-	_t_actuators(-1),
+	_t_actuator_controls_0(-1),
+	_t_actuator_controls_1(-1),
+	_t_actuator_controls_2(-1),
+	_t_actuator_controls_3(-1),
 	_t_actuator_armed(-1),
 	_t_vehicle_control_mode(-1),
 	_t_param(-1),
 	_t_vehicle_command(-1),
 	_to_input_rc(0),
-	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
 	_to_servorail(0),
@@ -769,15 +780,20 @@ PX4IO::task_main()
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
 	 * primary PWM output or not.
 	 */
-	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-				     ORB_ID(actuator_controls_1));
-	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
+	_t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0));
+	orb_set_interval(_t_actuator_controls_0, 20);		/* default to 50Hz */
+	_t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1));
+	orb_set_interval(_t_actuator_controls_1, 33);		/* default to 30Hz */
+	_t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2));
+	orb_set_interval(_t_actuator_controls_2, 33);		/* default to 30Hz */
+	_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
+	orb_set_interval(_t_actuator_controls_3, 33);		/* default to 30Hz */
 	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
 	_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_t_param = orb_subscribe(ORB_ID(parameter_update));
 	_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
 
-	if ((_t_actuators < 0) ||
+	if ((_t_actuator_controls_0 < 0) ||
 	    (_t_actuator_armed < 0) ||
 	    (_t_vehicle_control_mode < 0) ||
 	    (_t_param < 0) ||
@@ -788,7 +804,7 @@ PX4IO::task_main()
 
 	/* poll descriptor */
 	pollfd fds[1];
-	fds[0].fd = _t_actuators;
+	fds[0].fd = _t_actuator_controls_0;
 	fds[0].events = POLLIN;
 
 	log("ready");
@@ -807,7 +823,11 @@ PX4IO::task_main()
 			if (_update_interval > 100)
 				_update_interval = 100;
 
-			orb_set_interval(_t_actuators, _update_interval);
+			orb_set_interval(_t_actuator_controls_0, _update_interval);
+			/*
+			 * NOT changing the rate of groups 1-3 here, because only attitude
+			 * really needs to run fast.
+			 */
 			_update_interval = 0;
 		}
 
@@ -827,7 +847,10 @@ PX4IO::task_main()
 
 		/* if we have new control data from the ORB, handle it */
 		if (fds[0].revents & POLLIN) {
-			io_set_control_state();
+
+			/* we're not nice to the lower-priority control groups and only check them
+			   when the primary group updated (which is now). */
+			io_set_control_groups();
 		}
 
 		if (now >= poll_last + IO_POLL_INTERVAL) {
@@ -840,8 +863,7 @@ PX4IO::task_main()
 			/* get raw R/C input from IO */
 			io_publish_raw_rc();
 
-			/* fetch mixed servo controls and PWM outputs from IO */
-			io_publish_mixed_controls();
+			/* fetch PWM outputs from IO */
 			io_publish_pwm_outputs();
 		}
 
@@ -871,7 +893,7 @@ PX4IO::task_main()
 				orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
 
 				// Check for a DSM pairing command
-				if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) {
+				if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
 					dsm_bind_ioctl((int)cmd.param2);
 				}
 			}
@@ -901,7 +923,23 @@ PX4IO::task_main()
 
 				/* re-upload RC input config as it may have changed */
 				io_set_rc_config();
+
+				/* re-set the battery scaling */
+				int32_t voltage_scaling_val;
+				param_t voltage_scaling_param;
+
+				/* set battery voltage scaling */
+				param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val);
+
+				/* send scaling voltage to IO */
+				uint16_t scaling = voltage_scaling_val;
+				int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
+
+				if (pret != OK) {
+					log("voltage scaling upload failed");
+				}
 			}
+
 		}
 
 		perf_end(_perf_update);
@@ -922,20 +960,74 @@ out:
 }
 
 int
-PX4IO::io_set_control_state()
+PX4IO::io_set_control_groups()
+{
+	bool attitude_ok = io_set_control_state(0);
+
+	/* send auxiliary control groups */
+	(void)io_set_control_state(1);
+	(void)io_set_control_state(2);
+	(void)io_set_control_state(3);
+
+	return attitude_ok;
+}
+
+int
+PX4IO::io_set_control_state(unsigned group)
 {
 	actuator_controls_s	controls;	///< actuator outputs
 	uint16_t 		regs[_max_actuators];
 
 	/* get controls */
-	orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-		 ORB_ID(actuator_controls_1), _t_actuators, &controls);
+	bool changed;
+
+	switch (group) {
+		case 0:
+		{
+			orb_check(_t_actuator_controls_0, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
+			}
+		}
+		break;
+		case 1:
+		{
+			orb_check(_t_actuator_controls_1, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls);
+			}
+		}
+		break;
+		case 2:
+		{
+			orb_check(_t_actuator_controls_2, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls);
+			}
+		}
+		break;
+		case 3:
+		{
+			orb_check(_t_actuator_controls_3, &changed);
+
+			if (changed) {
+				orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls);
+			}
+		}
+		break;
+	}
+
+	if (!changed)
+		return -1;
 
 	for (unsigned i = 0; i < _max_controls; i++)
 		regs[i] = FLOAT_TO_REG(controls.control[i]);
 
 	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
+	return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
 }
 
 
@@ -1031,7 +1123,12 @@ PX4IO::io_set_rc_config()
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 3;
 
-	ichan = 4;
+	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
+
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+		input_map[ichan - 1] = 4;
+
+	ichan = 5;
 
 	for (unsigned i = 0; i < _max_rc_input; i++)
 		if (input_map[i] == -1)
@@ -1309,6 +1406,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 */
 	channel_count = regs[0];
 
+	if (channel_count != _rc_chan_count)
+		perf_count(_perf_chan_count);
+
+	_rc_chan_count = channel_count;
+
 	if (channel_count > 9) {
 		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
 
@@ -1363,50 +1465,6 @@ PX4IO::io_publish_raw_rc()
 	return OK;
 }
 
-int
-PX4IO::io_publish_mixed_controls()
-{
-	/* if no FMU comms(!) just don't publish */
-	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
-		return OK;
-
-	/* if not taking raw PPM from us, must be mixing */
-	if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM)
-		return OK;
-
-	/* data we are going to fetch */
-	actuator_controls_effective_s controls_effective;
-	controls_effective.timestamp = hrt_absolute_time();
-
-	/* get actuator controls from IO */
-	uint16_t act[_max_actuators];
-	int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
-
-	if (ret != OK)
-		return ret;
-
-	/* convert from register format to float */
-	for (unsigned i = 0; i < _max_actuators; i++)
-		controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
-
-	/* laxily advertise on first publication */
-	if (_to_actuators_effective == 0) {
-		_to_actuators_effective =
-			orb_advertise((_primary_pwm_device ?
-				       ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
-				       ORB_ID(actuator_controls_effective_1)),
-				      &controls_effective);
-
-	} else {
-		orb_publish((_primary_pwm_device ?
-			     ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
-			     ORB_ID(actuator_controls_effective_1)),
-			    _to_actuators_effective, &controls_effective);
-	}
-
-	return OK;
-}
-
 int
 PX4IO::io_publish_pwm_outputs()
 {
@@ -1659,11 +1717,13 @@ void
 PX4IO::print_status()
 {
 	/* basic configuration */
-	printf("protocol %u hardware %u bootloader %u buffer %uB\n",
+	printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
-	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
+	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER),
+	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC),
+	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC+1));
 	printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
@@ -2146,6 +2206,29 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
 		break;
 
+	case PX4IO_REBOOT_BOOTLOADER:
+		if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+			return -EINVAL;
+
+		/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
+		// we don't expect a reply from this operation
+		ret = OK;
+		break;
+
+	case PX4IO_CHECK_CRC: {
+		/* check IO firmware CRC against passed value */		
+		uint32_t io_crc = 0;
+		ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
+		if (ret != OK)
+			return ret;
+		if (io_crc != arg) {
+			debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg);
+			return -EINVAL;
+		}
+		break;
+	}
+
 	case PX4IO_INAIR_RESTART_ENABLE:
 
 		/* set/clear the 'in-air restart' bit */
@@ -2176,7 +2259,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
 		count = _max_actuators;
 
 	if (count > 0) {
+
+		perf_begin(_perf_write);
 		int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+		perf_end(_perf_write);
 
 		if (ret != OK)
 			return ret;
@@ -2255,7 +2341,7 @@ void
 start(int argc, char *argv[])
 {
 	if (g_dev != nullptr)
-		errx(1, "already loaded");
+		errx(0, "already loaded");
 
 	/* allocate the interface */
 	device::Device *interface = get_interface();
@@ -2588,6 +2674,39 @@ px4io_main(int argc, char *argv[])
 		if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
 	}
 
+	if (!strcmp(argv[1], "forceupdate")) {
+		/*
+		  force update of the IO firmware without requiring
+		  the user to hold the safety switch down
+		 */
+		if (argc <= 3) {
+			warnx("usage: px4io forceupdate MAGIC filename");
+			exit(1);
+		}
+		if (g_dev == nullptr) {
+			warnx("px4io is not started, still attempting upgrade");
+		} else {
+			uint16_t arg = atol(argv[2]);
+			int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
+			if (ret != OK) {
+				printf("reboot failed - %d\n", ret);
+				exit(1);
+			}
+
+			// tear down the px4io instance
+			delete g_dev;
+		}
+
+		// upload the specified firmware
+		const char *fn[2];
+		fn[0] = argv[3];
+		fn[1] = nullptr;
+		PX4IO_Uploader *up = new PX4IO_Uploader;
+		up->upload(&fn[0]);
+		delete up;
+		exit(0);
+	}
+
 	/* commands below here require a started driver */
 
 	if (g_dev == nullptr)
@@ -2673,6 +2792,49 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "checkcrc")) {
+		/*
+		  check IO CRC against CRC of a file
+		 */
+		if (argc <= 2) {
+			printf("usage: px4io checkcrc filename\n");
+			exit(1);
+		}
+		if (g_dev == nullptr) {
+			printf("px4io is not started\n");
+			exit(1);
+		}
+		int fd = open(argv[2], O_RDONLY);
+		if (fd == -1) {
+			printf("open of %s failed - %d\n", argv[2], errno);
+			exit(1);			
+		}
+		const uint32_t app_size_max = 0xf000;
+		uint32_t fw_crc = 0;
+		uint32_t nbytes = 0;
+		while (true) {
+			uint8_t buf[16];
+			int n = read(fd, buf, sizeof(buf));
+			if (n <= 0) break;
+			fw_crc = crc32part(buf, n, fw_crc);
+			nbytes += n;
+		}
+		close(fd);
+		while (nbytes < app_size_max) {
+			uint8_t b = 0xff;
+			fw_crc = crc32part(&b, 1, fw_crc);			
+			nbytes++;
+		}
+
+		int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
+		if (ret != OK) {
+			printf("check CRC failed - %d\n", ret);
+			exit(1);			
+		}
+		printf("CRCs match\n");
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "rx_dsm") ||
 	    !strcmp(argv[1], "rx_dsm_10bit") ||
 	    !strcmp(argv[1], "rx_dsm_11bit") ||
@@ -2690,5 +2852,5 @@ px4io_main(int argc, char *argv[])
 		bind(argc, argv);
 
 out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
 }

src/drivers/px4io/px4io_uploader.cpp

@@ -274,7 +274,10 @@ PX4IO_Uploader::drain()
 	int ret;
 
 	do {
-		ret = recv(c, 1000);
+		// the small recv timeout here is to allow for fast
+		// drain when rebooting the io board for a forced
+		// update of the fw without using the safety switch
+		ret = recv(c, 40);
 
 #ifdef UDEBUG
 		if (ret == OK) {

src/drivers/stm32/drv_hrt.c

@@ -338,7 +338,12 @@ static void		hrt_call_invoke(void);
 # define PPM_MIN_START		2500		/* shortest valid start gap */
 
 /* decoded PPM buffer */
-#define PPM_MAX_CHANNELS	12
+#define PPM_MIN_CHANNELS	5
+#define PPM_MAX_CHANNELS	20
+
+/* Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	4		/* should be less than the input timeout */
+
 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
 __EXPORT unsigned ppm_decoded_channels = 0;
 __EXPORT uint64_t ppm_last_valid_decode = 0;
@@ -440,7 +445,7 @@ hrt_ppm_decode(uint32_t status)
 	if (status & SR_OVF_PPM)
 		goto error;
 
-	/* how long since the last edge? */
+	/* how long since the last edge? - this handles counter wrapping implicitely. */
 	width = count - ppm.last_edge;
 	ppm.last_edge = count;
 
@@ -455,14 +460,38 @@ hrt_ppm_decode(uint32_t status)
 	 */
 	if (width >= PPM_MIN_START) {
 
-		/* export the last set of samples if we got something sensible */
-		if (ppm.next_channel > 4) {
-			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
-				ppm_buffer[i] = ppm_temp_buffer[i];
+		/*
+		 * If the number of channels changes unexpectedly, we don't want
+		 * to just immediately jump on the new count as it may be a result
+		 * of noise or dropped edges.  Instead, take a few frames to settle.
+		 */
+		if (ppm.next_channel != ppm_decoded_channels) {
+			static unsigned new_channel_count;
+			static unsigned new_channel_holdoff;
 
-			ppm_decoded_channels = i;
-			ppm_last_valid_decode = hrt_absolute_time();
+			if (new_channel_count != ppm.next_channel) {
+				/* start the lock counter for the new channel count */
+				new_channel_count = ppm.next_channel;
+				new_channel_holdoff = PPM_CHANNEL_LOCK;
 
+			} else if (new_channel_holdoff > 0) {
+				/* this frame matched the last one, decrement the lock counter */
+				new_channel_holdoff--;
+
+			} else {
+				/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+				ppm_decoded_channels = new_channel_count;
+				new_channel_count = 0;
+			}
+
+		} else {
+			/* frame channel count matches expected, let's use it */
+			if (ppm.next_channel > PPM_MIN_CHANNELS) {
+				for (i = 0; i < ppm.next_channel; i++)
+					ppm_buffer[i] = ppm_temp_buffer[i];
+
+				ppm_last_valid_decode = hrt_absolute_time();
+			}
 		}
 
 		/* reset for the next frame */
@@ -733,6 +762,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	irqstate_t flags = irqsave();
 
 	/* if the entry is currently queued, remove it */
+        /* note that we are using a potentially uninitialised
+           entry->link here, but it is safe as sq_rem() doesn't
+           dereference the passed node unless it is found in the
+           list. So we potentially waste a bit of time searching the
+           queue for the uninitialised entry->link but we don't do
+           anything actually unsafe.
+        */
 	if (entry->deadline != 0)
 		sq_rem(&entry->link, &callout_queue);
 
@@ -839,7 +875,12 @@ hrt_call_invoke(void)
 
 		/* if the callout has a non-zero period, it has to be re-entered */
 		if (call->period != 0) {
-			call->deadline = deadline + call->period;
+			// re-check call->deadline to allow for
+			// callouts to re-schedule themselves 
+			// using hrt_call_delay()
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
 			hrt_call_enter(call);
 		}
 	}
@@ -906,5 +947,16 @@ hrt_latency_update(void)
 	latency_counters[index]++;
 }
 
+void
+hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+void
+hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
 
 #endif /* HRT_TIMER */

src/examples/flow_position_control/flow_position_control_main.c

@@ -61,7 +61,6 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/filtered_bottom_flow.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>

src/lib/external_lgpl/tecs/tecs.cpp

@@ -433,10 +433,18 @@ void TECS::_update_pitch(void)
 	// Apply max and min values for integrator state that will allow for no more than
 	// 5deg of saturation. This allows for some pitch variation due to gusts before the
 	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
+	// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
+	// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
+	// integrator has to catch up before the nose can be raised to reduce speed during climbout.
 	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
 	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	if (_climbOutDem)
+	{
+		temp += _PITCHminf * gainInv;
+	}
 	_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
 
+
 	// Calculate pitch demand from specific energy balance signals
 	_pitch_dem_unc = (temp + _integ7_state) / gainInv;
 

src/lib/mathlib/math/filter/LowPassFilter2p.cpp

@@ -46,6 +46,10 @@ namespace math
 void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 {
     _cutoff_freq = cutoff_freq;
+    if (_cutoff_freq <= 0.0f) {
+        // no filtering
+        return;
+    }
     float fr = sample_freq/_cutoff_freq;
     float ohm = tanf(M_PI_F/fr);
     float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
@@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 
 float LowPassFilter2p::apply(float sample)
 {
+    if (_cutoff_freq <= 0.0f) {
+        // no filtering
+        return sample;
+    }
     // do the filtering
     float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
     if (isnan(delay_element_0) || isinf(delay_element_0)) {

src/modules/attitude_estimator_so3/README

@@ -0,0 +1,3 @@
+Synopsis
+
+ nsh> attitude_estimator_so3_comp start

src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -1,16 +1,49 @@
-/*
- * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
+/****************************************************************************
  *
- * @file attitude_estimator_so3_comp_main.c
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Hyon Lim <limhyon@gmail.com>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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 attitude_estimator_so3_main.cpp
  *
  * Implementation of nonlinear complementary filters on the SO(3).
  * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
  * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
- * 
+ *
  * Theory of nonlinear complementary filters on the SO(3) is based on [1].
  * Quaternion realization of [1] is based on [2].
  * Optmized quaternion update code is based on Sebastian Madgwick's implementation.
- * 
+ *
  * References
  *  [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
  *  [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
@@ -46,94 +79,91 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
-#include "attitude_estimator_so3_comp_params.h"
+#include "attitude_estimator_so3_params.h"
 #ifdef __cplusplus
 }
 #endif
 
-extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]);
+extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]);
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
-static int attitude_estimator_so3_comp_task;				/**< Handle of deamon task / thread */
+static int attitude_estimator_so3_task;				/**< Handle of deamon task / thread */
+
+//! Auxiliary variables to reduce number of repeated operations
 static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	/** quaternion of sensor frame relative to auxiliary frame */
 static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f;	/** quaternion of sensor frame relative to auxiliary frame */
 static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
-static bool bFilterInit = false;
-
-//! Auxiliary variables to reduce number of repeated operations
 static float q0q0, q0q1, q0q2, q0q3;
 static float q1q1, q1q2, q1q3;
 static float q2q2, q2q3;
 static float q3q3;
-
-//! Serial packet related
-static int uart;
-static int baudrate;
+static bool bFilterInit = false;
 
 /**
- * Mainloop of attitude_estimator_so3_comp.
+ * Mainloop of attitude_estimator_so3.
  */
-int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]);
+int attitude_estimator_so3_thread_main(int argc, char *argv[]);
 
 /**
  * Print the correct usage.
  */
 static void usage(const char *reason);
 
+/* Function prototypes */
+float invSqrt(float number);
+void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz);
+void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt);
+
 static void
 usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
 
-	fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n"
-		"-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n"
-		"ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n");
+	fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n");
 	exit(1);
 }
 
 /**
- * The attitude_estimator_so3_comp app only briefly exists to start
+ * The attitude_estimator_so3 app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
  * to task_create().
  */
-int attitude_estimator_so3_comp_main(int argc, char *argv[])
+int attitude_estimator_so3_main(int argc, char *argv[])
 {
 	if (argc < 1)
 		usage("missing command");
 
-	
-
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("attitude_estimator_so3_comp already running\n");
+			warnx("already running\n");
 			/* this is not an error */
 			exit(0);
 		}
 
 		thread_should_exit = false;
-		attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp",
+		attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      12400,
-					      attitude_estimator_so3_comp_thread_main,
-					      (const char **)argv);
+					      14000,
+					      attitude_estimator_so3_thread_main,
+					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 		thread_should_exit = true;
 
-		while(thread_running){
+		while (thread_running){
 			usleep(200000);
-			printf(".");
 		}
-		printf("terminated.");
+		
+		warnx("stopped");
 		exit(0);
 	}
 
@@ -157,7 +187,8 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
 //---------------------------------------------------------------------------------------------------
 // Fast inverse square-root
 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-float invSqrt(float number) {
+float invSqrt(float number) 
+{
     volatile long i;
     volatile float x, y;
     volatile const float f = 1.5F;
@@ -221,48 +252,47 @@ void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, floa
     q3q3 = q3 * q3;
 }
 
-void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) {
+void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) 
+{
 	float recipNorm;
 	float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
 
-	//! Make filter converge to initial solution faster
-	//! This function assumes you are in static position.
-	//! WARNING : in case air reboot, this can cause problem. But this is very
-	//!	      unlikely happen.
-	if(bFilterInit == false)
-	{
+	// Make filter converge to initial solution faster
+	// This function assumes you are in static position.
+	// WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
+	if(bFilterInit == false) {
 		NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
 		bFilterInit = true;
 	}
         	
 	//! If magnetometer measurement is available, use it.
-	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+	if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
 		float hx, hy, hz, bx, bz;
 		float halfwx, halfwy, halfwz;
 	
 		// Normalise magnetometer measurement
 		// Will sqrt work better? PX4 system is powerful enough?
-    		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
-    		mx *= recipNorm;
-    		my *= recipNorm;
-    		mz *= recipNorm;
+    	recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+    	mx *= recipNorm;
+    	my *= recipNorm;
+    	mz *= recipNorm;
     
-    		// Reference direction of Earth's magnetic field
-    		hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
-    		hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
-		hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2);
-    		bx = sqrt(hx * hx + hy * hy);
-    		bz = hz;
+    	// Reference direction of Earth's magnetic field
+    	hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+    	hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+		hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
+    	bx = sqrt(hx * hx + hy * hy);
+    	bz = hz;
     
-    		// Estimated direction of magnetic field
-    		halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
-    		halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
-    		halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+    	// Estimated direction of magnetic field
+    	halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+    	halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+    	halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
     
-    		// Error is sum of cross product between estimated direction and measured direction of field vectors
-    		halfex += (my * halfwz - mz * halfwy);
-    		halfey += (mz * halfwx - mx * halfwz);
-    		halfez += (mx * halfwy - my * halfwx);
+    	// Error is sum of cross product between estimated direction and measured direction of field vectors
+    	halfex += (my * halfwz - mz * halfwy);
+    	halfey += (mz * halfwx - mx * halfwz);
+    	halfez += (mx * halfwy - my * halfwx);
 	}
 
 	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
@@ -293,7 +323,9 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
 			gyro_bias[0] += twoKi * halfex * dt;	// integral error scaled by Ki
 			gyro_bias[1] += twoKi * halfey * dt;
 			gyro_bias[2] += twoKi * halfez * dt;
-			gx += gyro_bias[0];	// apply integral feedback
+			
+			// apply integral feedback
+			gx += gyro_bias[0];
 			gy += gyro_bias[1];
 			gz += gyro_bias[2];
 		}
@@ -337,208 +369,43 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
 	q3 *= recipNorm;
 
 	// Auxiliary variables to avoid repeated arithmetic
-        q0q0 = q0 * q0;
-       	q0q1 = q0 * q1;
-       	q0q2 = q0 * q2;
-       	q0q3 = q0 * q3;
-       	q1q1 = q1 * q1;
-       	q1q2 = q1 * q2;
+    q0q0 = q0 * q0;
+    q0q1 = q0 * q1;
+    q0q2 = q0 * q2;
+    q0q3 = q0 * q3;
+    q1q1 = q1 * q1;
+    q1q2 = q1 * q2;
    	q1q3 = q1 * q3;
-       	q2q2 = q2 * q2;
-       	q2q3 = q2 * q3;
-       	q3q3 = q3 * q3;   
-}
-
-void send_uart_byte(char c)
-{
-	write(uart,&c,1);
-}
-
-void send_uart_bytes(uint8_t *data, int length)
-{
-	write(uart,data,(size_t)(sizeof(uint8_t)*length));
-}
-
-void send_uart_float(float f) {
-  uint8_t * b = (uint8_t *) &f;
-
-  //! Assume float is 4-bytes
-  for(int i=0; i<4; i++) {
-    
-    uint8_t b1 = (b[i] >> 4) & 0x0f;
-    uint8_t b2 = (b[i] & 0x0f);
-    
-    uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
-    uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
-    
-    send_uart_bytes(&c1,1);
-    send_uart_bytes(&c2,1);
-  }
-}
-
-void send_uart_float_arr(float *arr, int length)
-{
-	for(int i=0;i<length;++i)
-	{
-		send_uart_float(arr[i]);
-		send_uart_byte(',');
-	}
-}
-
-int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
-{
-	int speed;
-	
-	switch (baud) {
-        case 0:      speed = B0;      break;
-        case 50:     speed = B50;     break;
-        case 75:     speed = B75;     break;
-        case 110:    speed = B110;    break;
-        case 134:    speed = B134;    break;
-        case 150:    speed = B150;    break;
-        case 200:    speed = B200;    break;
-        case 300:    speed = B300;    break;
-        case 600:    speed = B600;    break;
-        case 1200:   speed = B1200;   break;
-        case 1800:   speed = B1800;   break;
-        case 2400:   speed = B2400;   break;
-        case 4800:   speed = B4800;   break;
-        case 9600:   speed = B9600;   break;
-        case 19200:  speed = B19200;  break;
-        case 38400:  speed = B38400;  break;
-        case 57600:  speed = B57600;  break;
-        case 115200: speed = B115200; break;
-        case 230400: speed = B230400; break;
-        case 460800: speed = B460800; break;
-        case 921600: speed = B921600; break;
-        default:
-                printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
-                return -EINVAL;
-        }
-
-	printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
-        uart = open(uart_name, O_RDWR | O_NOCTTY);
-
-	/* Try to set baud rate */
-        struct termios uart_config;
-        int termios_state;
-        *is_usb = false;
-
-	/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
-        if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
-                /* Back up the original uart configuration to restore it after exit */
-                if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-                        printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
-                        close(uart);
-                        return -1;
-                }
-
-                /* Fill the struct for the new configuration */
-                tcgetattr(uart, &uart_config);
-
-                /* Clear ONLCR flag (which appends a CR for every LF) */
-                uart_config.c_oflag &= ~ONLCR;
-
-                /* Set baud rate */
-                if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-                        printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
-                        close(uart);
-                        return -1;
-                }
-
-
-                if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-                        printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-                        close(uart);
-                        return -1;
-                }
-
-        } else {
-                *is_usb = true;
-        }
-
-        return uart;
+    q2q2 = q2 * q2;
+    q2q3 = q2 * q3;
+    q3q3 = q3 * q3;   
 }
 
 /*
- * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
- */
-
-/*
- * EKF Attitude Estimator main function.
+ * Nonliner complementary filter on SO(3), attitude estimator main function.
  *
- * Estimates the attitude recursively once started.
+ * Estimates the attitude once started.
  *
  * @param argc number of commandline arguments (plus command name)
  * @param argv strings containing the arguments
  */
-int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
+int attitude_estimator_so3_thread_main(int argc, char *argv[])
 {
-
-const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
-
-	//! Serial debug related
-	int ch;
-	struct termios uart_config_original;
-	bool usb_uart;
-	bool debug_mode = false;
-	char *device_name = "/dev/ttyS2";
-	baudrate = 115200;
+	const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	//! Time constant
 	float dt = 0.005f;
 	
 	/* output euler angles */
 	float euler[3] = {0.0f, 0.0f, 0.0f};
-
-	float Rot_matrix[9] = {1.f,  0,  0,
-			      0,  1.f,  0,
-			      0,  0,  1.f
-			     };		/**< init: identity matrix */
-
+	
+	/* Initialization */
+	float Rot_matrix[9] = {1.f,  0.0f,  0.0f, 0.0f,  1.f,  0.0f, 0.0f,  0.0f,  1.f };		/**< init: identity matrix */
 	float acc[3] = {0.0f, 0.0f, 0.0f};
 	float gyro[3] = {0.0f, 0.0f, 0.0f};
 	float mag[3] = {0.0f, 0.0f, 0.0f};
 
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-
-	//! -d <device_name>, default : /dev/ttyS2
-	//! -b <baud_rate>,   default : 115200
-	while ((ch = getopt(argc,argv,"d:b:")) != EOF){
-		switch(ch){
-			case 'b':
-				baudrate = strtoul(optarg, NULL, 10);
-				if(baudrate == 0)
-					printf("invalid baud rate '%s'",optarg);
-				break;
-			case 'd':
-				device_name = optarg;
-				debug_mode = true;
-				break;
-			default:
-				usage("invalid argument");
-		}
-	}
-
-	if(debug_mode){
-		printf("Opening debugging port for 3D visualization\n");
-		uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
-		if (uart < 0)
-                	printf("could not open %s", device_name);
-		else
-			printf("Open port success\n");
-	}
-
-	// print text
-	printf("Nonlinear SO3 Attitude Estimator initialized..\n\n");
-	fflush(stdout);
-
-	int overloadcounter = 19;
-
-	/* store start time to guard against too slow update rates */
-	uint64_t last_run = hrt_absolute_time();
+	warnx("main thread started");
 
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
@@ -555,8 +422,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate-limit raw data updates to 200Hz */
-	orb_set_interval(sub_raw, 4);
+	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
+	orb_set_interval(sub_raw, 3);
 
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
@@ -565,17 +432,15 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* advertise attitude */
-	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	//orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	//orb_advert_t att_pub = -1;
+	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 	int loopcounter = 0;
 	int printcounter = 0;
 
 	thread_running = true;
 
-	/* advertise debug value */
-	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
-	// orb_advert_t pub_dbg = -1;
-
 	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
 	// XXX write this out to perf regs
 
@@ -583,20 +448,22 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
-	struct attitude_estimator_so3_comp_params so3_comp_params;
-	struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles;
+	struct attitude_estimator_so3_params so3_comp_params;
+	struct attitude_estimator_so3_param_handles so3_comp_param_handles;
 
 	/* initialize parameter handles */
 	parameters_init(&so3_comp_param_handles);
+	parameters_update(&so3_comp_param_handles, &so3_comp_params);
 
 	uint64_t start_time = hrt_absolute_time();
 	bool initialized = false;
+	bool state_initialized = false;
 
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
 	unsigned offset_count = 0;
 
 	/* register the perf counter */
-	perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp");
+	perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3");
 
 	/* Main loop*/
 	while (!thread_should_exit) {
@@ -615,12 +482,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
 			if (!control_mode.flag_system_hil_enabled) {
-				fprintf(stderr,
-					"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
+				warnx("WARNING: Not getting sensors - sensor app running?");
 			}
-
 		} else {
-
 			/* only update parameters if they changed */
 			if (fds[1].revents & POLLIN) {
 				/* read from param to clear updated flag */
@@ -644,11 +508,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					gyro_offsets[2] += raw.gyro_rad_s[2];
 					offset_count++;
 
-					if (hrt_absolute_time() - start_time > 3000000LL) {
+					if (hrt_absolute_time() > start_time + 3000000l) {
 						initialized = true;
 						gyro_offsets[0] /= offset_count;
 						gyro_offsets[1] /= offset_count;
 						gyro_offsets[2] /= offset_count;
+						warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
 					}
 
 				} else {
@@ -668,9 +533,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
-					gyro[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
-					gyro[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
-					gyro[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
+					gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
+					gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
+					gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
 					if (sensor_last_count[1] != raw.accelerometer_counter) {
@@ -696,31 +561,14 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					mag[1] = raw.magnetometer_ga[1];
 					mag[2] = raw.magnetometer_ga[2];
 
-					uint64_t now = hrt_absolute_time();
-					unsigned int time_elapsed = now - last_run;
-					last_run = now;
-
-					if (time_elapsed > loop_interval_alarm) {
-						//TODO: add warning, cpu overload here
-						// if (overloadcounter == 20) {
-						// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
-						// 	overloadcounter = 0;
-						// }
-
-						overloadcounter++;
-					}
-
-					static bool const_initialized = false;
-
 					/* initialize with good values once we have a reasonable dt estimate */
-					if (!const_initialized && dt < 0.05f && dt > 0.005f) {
-						dt = 0.005f;
-						parameters_update(&so3_comp_param_handles, &so3_comp_params);
-						const_initialized = true;
+					if (!state_initialized && dt < 0.05f && dt > 0.001f) {
+						state_initialized = true;
+						warnx("state initialized");
 					}
 
 					/* do not execute the filter if not initialized */
-					if (!const_initialized) {
+					if (!state_initialized) {
 						continue;
 					}
 
@@ -728,18 +576,23 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					// NOTE : Accelerometer is reversed.
 					// Because proper mount of PX4 will give you a reversed accelerometer readings.
-					NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
+					NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
+										-acc[0], -acc[1], -acc[2],
+										mag[0], mag[1], mag[2],
+										so3_comp_params.Kp,
+										so3_comp_params.Ki, 
+										dt);
 
 					// Convert q->R, This R converts inertial frame to body frame.
 					Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
-        				Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3);	// 12
-        				Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2);	// 13
-        				Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3);	// 21
-        				Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
-        				Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1);	// 23
-        				Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2);	// 31
-        				Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1);	// 32
-        				Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
+					Rot_matrix[1] = 2.f * (q1*q2 + q0*q3);	// 12
+					Rot_matrix[2] = 2.f * (q1*q3 - q0*q2);	// 13
+					Rot_matrix[3] = 2.f * (q1*q2 - q0*q3);	// 21
+					Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
+					Rot_matrix[5] = 2.f * (q2*q3 + q0*q1);	// 23
+					Rot_matrix[6] = 2.f * (q1*q3 + q0*q2);	// 31
+					Rot_matrix[7] = 2.f * (q2*q3 - q0*q1);	// 32
+					Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
 
 					//1-2-3 Representation.
 					//Equation (290) 
@@ -747,29 +600,42 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
 					euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]);	//! Roll
 					euler[1] = -asinf(Rot_matrix[2]);	//! Pitch
-					euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]);		//! Yaw
+					euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]);		//! Yaw
 					
 					/* swap values for next iteration, check for fatal inputs */
 					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
-						/* Do something */
+						// Publish only finite euler angles
+						att.roll = euler[0] - so3_comp_params.roll_off;
+						att.pitch = euler[1] - so3_comp_params.pitch_off;
+						att.yaw = euler[2] - so3_comp_params.yaw_off;
 					} else {
 						/* due to inputs or numerical failure the output is invalid, skip it */
+						// Due to inputs or numerical failure the output is invalid
+						warnx("infinite euler angles, rotation matrix:");
+						warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
+						warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
+						warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
+						// Don't publish anything
 						continue;
 					}
 
-					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+					if (last_data > 0 && raw.timestamp > last_data + 12000) {
+						warnx("sensor data missed");
+					}
 
 					last_data = raw.timestamp;
 
 					/* send out */
 					att.timestamp = raw.timestamp;
+					
+					// Quaternion
+					att.q[0] = q0;
+					att.q[1] = q1;
+					att.q[2] = q2;
+					att.q[3] = q3;
+					att.q_valid = true;
 
-					// XXX Apply the same transformation to the rotation matrix
-					att.roll = euler[0] - so3_comp_params.roll_off;
-					att.pitch = euler[1] - so3_comp_params.pitch_off;
-					att.yaw = euler[2] - so3_comp_params.yaw_off;
-
-					//! Euler angle rate. But it needs to be investigated again.
+					// Euler angle rate. But it needs to be investigated again.
 					/*
 					att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
 					att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
@@ -783,53 +649,30 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					att.pitchacc = 0;
 					att.yawacc = 0;
 
-					//! Quaternion
-					att.q[0] = q0;
-					att.q[1] = q1;
-					att.q[2] = q2;
-					att.q[3] = q3;
-					att.q_valid = true;
-
 					/* TODO: Bias estimation required */
 					memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
 
 					/* copy rotation matrix */
 					memcpy(&att.R, Rot_matrix, sizeof(float)*9);
 					att.R_valid = true;
-
-					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
-						// Broadcast
-						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
-
+					
+					// Publish
+					if (att_pub > 0) {
+						orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
 					} else {
 						warnx("NaN in roll/pitch/yaw estimate!");
+						 orb_advertise(ORB_ID(vehicle_attitude), &att);
 					}
 
 					perf_end(so3_comp_loop_perf);
-
-					//! This will print out debug packet to visualization software
-					if(debug_mode)
-					{
-						float quat[4];
-						quat[0] = q0;
-						quat[1] = q1;
-						quat[2] = q2;
-						quat[3] = q3;
-						send_uart_float_arr(quat,4);
-						send_uart_byte('\n');
-					}
 				}
 			}
 		}
 
 		loopcounter++;
-	}// while
+	}
 
 	thread_running = false;
 
-	/* Reset the UART flags to original state */
-        if (!usb_uart)
-                tcsetattr(uart, TCSANOW, &uart_config_original);
-
 	return 0;
 }

src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Hyon Lim <limhyon@gmail.com>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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 attitude_estimator_so3_params.c
+ *
+ * Parameters for nonlinear complementary filters on the SO(3).
+ */
+
+#include "attitude_estimator_so3_params.h"
+
+/* This is filter gain for nonlinear SO3 complementary filter */
+/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
+   Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
+   If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
+   will compensate gyro bias which depends on temperature and vibration of your vehicle */
+PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
+                                       //! You can set this gain higher if you want more fast response.
+                                       //! But note that higher gain will give you also higher overshoot.
+PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
+					//! This gain is depend on your vehicle status.
+
+/* offsets in roll, pitch and yaw of sensor plane and body */
+PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f);
+
+int parameters_init(struct attitude_estimator_so3_param_handles *h)
+{
+	/* Filter gain parameters */
+	h->Kp = 	param_find("SO3_COMP_KP");
+	h->Ki = 	param_find("SO3_COMP_KI");
+
+	/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
+	h->roll_off  =	param_find("SO3_ROLL_OFFS");
+	h->pitch_off =	param_find("SO3_PITCH_OFFS");
+	h->yaw_off   =	param_find("SO3_YAW_OFFS");
+
+	return OK;
+}
+
+int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p)
+{
+	/* Update filter gain */
+	param_get(h->Kp, &(p->Kp));
+	param_get(h->Ki, &(p->Ki));
+
+	/* Update attitude offset */
+	param_get(h->roll_off, &(p->roll_off));
+	param_get(h->pitch_off, &(p->pitch_off));
+	param_get(h->yaw_off, &(p->yaw_off));
+
+	return OK;
+}

src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Hyon Lim <limhyon@gmail.com>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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 attitude_estimator_so3_params.h
+ *
+ * Parameters for nonlinear complementary filters on the SO(3).
+ */
+
+#include <systemlib/param/param.h>
+
+struct attitude_estimator_so3_params {
+	float Kp;
+	float Ki;
+	float roll_off;
+	float pitch_off;
+	float yaw_off;
+};
+
+struct attitude_estimator_so3_param_handles {
+	param_t Kp, Ki;
+	param_t roll_off, pitch_off, yaw_off;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct attitude_estimator_so3_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p);

src/modules/attitude_estimator_so3/module.mk

@@ -0,0 +1,8 @@
+#
+# Attitude estimator (Nonlinear SO(3) complementary Filter)
+#
+
+MODULE_COMMAND	 = attitude_estimator_so3
+
+SRCS		 = attitude_estimator_so3_main.cpp \
+		   attitude_estimator_so3_params.c

src/modules/attitude_estimator_so3_comp/README

@@ -1,5 +0,0 @@
-Synopsis
-
- nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
-
-Option -d is for debugging packet. See code for detailed packet structure.

src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c

@@ -1,63 +0,0 @@
-/*
- * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
- *
- * @file attitude_estimator_so3_comp_params.c
- *
- * Implementation of nonlinear complementary filters on the SO(3).
- * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
- * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
- * 
- * Theory of nonlinear complementary filters on the SO(3) is based on [1].
- * Quaternion realization of [1] is based on [2].
- * Optmized quaternion update code is based on Sebastian Madgwick's implementation.
- * 
- * References
- *  [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
- *  [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
- */
-
-#include "attitude_estimator_so3_comp_params.h"
-
-/* This is filter gain for nonlinear SO3 complementary filter */
-/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
-   Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
-   If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
-   will compensate gyro bias which depends on temperature and vibration of your vehicle */
-PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
-                                       //! You can set this gain higher if you want more fast response.
-                                       //! But note that higher gain will give you also higher overshoot.
-PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
-					//! This gain is depend on your vehicle status.
-
-/* offsets in roll, pitch and yaw of sensor plane and body */
-PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
-
-int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
-{
-	/* Filter gain parameters */
-	h->Kp = 	param_find("SO3_COMP_KP");
-	h->Ki = 	param_find("SO3_COMP_KI");
-
-	/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
-	h->roll_off  =	param_find("ATT_ROLL_OFFS");
-	h->pitch_off =	param_find("ATT_PITCH_OFFS");
-	h->yaw_off   =	param_find("ATT_YAW_OFFS");
-
-	return OK;
-}
-
-int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
-{
-	/* Update filter gain */
-	param_get(h->Kp, &(p->Kp));
-	param_get(h->Ki, &(p->Ki));
-
-	/* Update attitude offset */
-	param_get(h->roll_off, &(p->roll_off));
-	param_get(h->pitch_off, &(p->pitch_off));
-	param_get(h->yaw_off, &(p->yaw_off));
-
-	return OK;
-}

src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h

@@ -1,44 +0,0 @@
-/*
- * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
- *
- * @file attitude_estimator_so3_comp_params.h
- *
- * Implementation of nonlinear complementary filters on the SO(3).
- * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
- * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
- * 
- * Theory of nonlinear complementary filters on the SO(3) is based on [1].
- * Quaternion realization of [1] is based on [2].
- * Optmized quaternion update code is based on Sebastian Madgwick's implementation.
- * 
- * References
- *  [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
- *  [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
- */
-
-#include <systemlib/param/param.h>
-
-struct attitude_estimator_so3_comp_params {
-	float Kp;
-	float Ki;
-	float roll_off;
-	float pitch_off;
-	float yaw_off;
-};
-
-struct attitude_estimator_so3_comp_param_handles {
-	param_t Kp, Ki;
-	param_t roll_off, pitch_off, yaw_off;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);

src/modules/attitude_estimator_so3_comp/module.mk

@@ -1,8 +0,0 @@
-#
-# Attitude estimator (Nonlinear SO3 complementary Filter)
-#
-
-MODULE_COMMAND	 = attitude_estimator_so3_comp
-
-SRCS		 = attitude_estimator_so3_comp_main.cpp \
-		   attitude_estimator_so3_comp_params.c

src/modules/commander/commander.cpp

@@ -509,6 +509,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
+	/* Flight termination */
+	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
+
+			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+				transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode);
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				/* reject parachute depoyment not armed */
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+		}
+		break;
+
 	default:
 		break;
 	}
@@ -1174,6 +1189,16 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/*  Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
+		if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+			transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode);
+			if (flighttermination_res == TRANSITION_CHANGED) {
+				tune_positive();
+			}
+		} else {
+			flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode);
+		}
+
 
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
@@ -1199,6 +1224,7 @@ int commander_thread_main(int argc, char *argv[])
 		bool arming_state_changed = check_arming_state_changed();
 		bool main_state_changed = check_main_state_changed();
 		bool navigation_state_changed = check_navigation_state_changed();
+		bool flighttermination_state_changed = check_flighttermination_state_changed();
 
 		hrt_abstime t1 = hrt_absolute_time();
 
@@ -1598,8 +1624,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 			/* switch to failsafe mode */
 			bool manual_control_old = control_mode->flag_control_manual_enabled;
 
-			if (!status->condition_landed) {
-				/* in air: try to hold position */
+			if (!status->condition_landed && status->condition_local_position_valid) {
+				/* in air: try to hold position if possible */
 				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
 
 			} else {
@@ -1725,7 +1751,8 @@ void *commander_low_prio_loop(void *arg)
 		/* ignore commands the high-prio loop handles */
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
 		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
-		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
+		    cmd.command == VEHICLE_CMD_DO_SET_SERVO)
 			continue;
 
 		/* only handle low-priority commands here */

src/modules/commander/state_machine_helper.cpp

@@ -65,6 +65,7 @@ static const int ERROR = -1;
 static bool arming_state_changed = true;
 static bool main_state_changed = true;
 static bool navigation_state_changed = true;
+static bool flighttermination_state_changed = true;
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -451,6 +452,18 @@ check_navigation_state_changed()
 	}
 }
 
+bool
+check_flighttermination_state_changed()
+{
+	if (flighttermination_state_changed) {
+		flighttermination_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
 void
 set_navigation_state_changed()
 {
@@ -523,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 }
 
 
+/**
+* Transition from one flightermination state to another
+*/
+transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+		/* only check transition if the new state is actually different from the current one */
+		if (new_flighttermination_state == status->flighttermination_state) {
+			ret = TRANSITION_NOT_CHANGED;
+
+		} else {
+
+			switch (new_flighttermination_state) {
+			case FLIGHTTERMINATION_STATE_ON:
+				ret = TRANSITION_CHANGED;
+				status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
+				warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
+				break;
+			case FLIGHTTERMINATION_STATE_OFF:
+				ret = TRANSITION_CHANGED;
+				status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
+				break;
+
+			default:
+				break;
+			}
+
+			if (ret == TRANSITION_CHANGED) {
+				flighttermination_state_changed = true;
+				control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
+			}
+		}
+
+		return ret;
+}
+
+
 
 // /*
 //  * Wrapper functions (to be used in the commander), all functions assume lock on current_status

src/modules/commander/state_machine_helper.h

@@ -70,8 +70,12 @@ bool check_main_state_changed();
 
 transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
 
+transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode);
+
 bool check_navigation_state_changed();
 
+bool check_flighttermination_state_changed();
+
 void set_navigation_state_changed();
 
 int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);

src/modules/controllib/uorb/blocks.hpp

@@ -43,7 +43,6 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>

src/modules/dataman/dataman.c

@@ -111,7 +111,8 @@ static unsigned g_func_counts[dm_number_of_funcs];
 static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
 	DM_KEY_SAFE_POINTS_MAX,
 	DM_KEY_FENCE_POINTS_MAX,
-	DM_KEY_WAYPOINTS_OFFBOARD_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
 	DM_KEY_WAYPOINTS_ONBOARD_MAX
 };
 
@@ -572,11 +573,13 @@ task_main(int argc, char *argv[])
 	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
 	if (g_task_fd < 0) {
 		warnx("Could not open data manager file %s", k_data_manager_device_path);
+		sem_post(&g_init_sema);
 		return -1;
 	}
 	if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
 		close(g_task_fd);
 		warnx("Could not seek data manager file %s", k_data_manager_device_path);
+		sem_post(&g_init_sema);
 		return -1;
 	}
 	fsync(g_task_fd);
@@ -720,7 +723,7 @@ dataman_main(int argc, char *argv[])
 		if (g_fd < 0)
 			errx(1, "start failed");
 
-		return 0;
+		exit(0);
 	}
 
 	if (g_fd < 0)
@@ -733,6 +736,6 @@ dataman_main(int argc, char *argv[])
 	else
 		usage();
 
-	return 0;
+	exit(1);
 }
 

src/modules/dataman/dataman.h

@@ -50,7 +50,8 @@ extern "C" {
 	typedef enum {
 		DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
 		DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
-		DM_KEY_WAYPOINTS_OFFBOARD,		/* Mission way point coordinates sent over mavlink */
+		DM_KEY_WAYPOINTS_OFFBOARD_0,	/* Mission way point coordinates sent over mavlink */
+		DM_KEY_WAYPOINTS_OFFBOARD_1,	/* (alernate between 0 and 1) */
 		DM_KEY_WAYPOINTS_ONBOARD,	/* Mission way point coordinates generated onboard */
 		DM_KEY_NUM_KEYS			/* Total number of item types defined */
 	} dm_item_t;
@@ -59,7 +60,8 @@ extern "C" {
 	enum {
 		DM_KEY_SAFE_POINTS_MAX = 8,
 		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
-		DM_KEY_WAYPOINTS_OFFBOARD_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
 		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
 	};
 

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -121,6 +121,7 @@ private:
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
+	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
 
 	struct vehicle_attitude_s				_att;			/**< vehicle attitude */
 	struct accel_report						_accel;			/**< body frame accelerations */
@@ -128,7 +129,8 @@ private:
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
 	struct airspeed_s						_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s			_vcontrol_mode;	/**< vehicle control mode */
-	struct actuator_controls_s				_actuators;		/**< actuator control inputs */
+	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+	struct actuator_controls_s			_actuators_airframe;		/**< actuator control inputs */
 	struct vehicle_global_position_s		_global_pos;	/**< global position */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
@@ -286,6 +288,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_rate_sp_pub(-1),
 	_actuators_0_pub(-1),
 	_attitude_sp_pub(-1),
+	_actuators_1_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -294,14 +297,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_airspeed_valid(false)
 {
 	/* safely initialize structs */
-	vehicle_attitude_s			_att = {0};
-	accel_report				_accel = {0};
-	vehicle_attitude_setpoint_s		_att_sp = {0};
-	manual_control_setpoint_s		_manual = {0};
-	airspeed_s				_airspeed = {0};
-	vehicle_control_mode_s			_vcontrol_mode = {0};
-	actuator_controls_s			_actuators = {0};
-	vehicle_global_position_s		_global_pos = {0};
+	_att = {0};
+	_accel = {0};
+	_att_sp = {0};
+	_manual = {0};
+	_airspeed = {0};
+	_vcontrol_mode = {0};
+	_actuators = {0};
+	_actuators_airframe = {0};
+	_global_pos = {0};
 
 
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
@@ -625,6 +629,15 @@ FixedwingAttitudeControl::task_main()
 				lock_integrator = true;
 			}
 
+			/* Simple handling of failsafe: deploy parachute if failsafe is on */
+			if (_vcontrol_mode.flag_control_flighttermination_enabled) {
+				_actuators_airframe.control[1] = 1.0f;
+//				warnx("_actuators_airframe.control[1] = 1.0f;");
+			} else {
+				_actuators_airframe.control[1] = 0.0f;
+//				warnx("_actuators_airframe.control[1] = -1.0f;");
+			}
+
 			/* decide if in stabilized or full manual control */
 
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
@@ -794,6 +807,7 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
+			_actuators_airframe.timestamp = hrt_absolute_time();
 
 			if (_actuators_0_pub > 0) {
 				/* publish the attitude setpoint */
@@ -804,6 +818,19 @@ FixedwingAttitudeControl::task_main()
 				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
 			}
 
+			if (_actuators_1_pub > 0) {
+				/* publish the attitude setpoint */
+				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
+//				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
+//						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
+//						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
+//						(double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
+
+			} else {
+				/* advertise and publish */
+				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
+			}
+
 		}
 
 		perf_end(_loop_perf);

src/modules/fw_att_control/fw_att_control_params.c

@@ -33,9 +33,9 @@ f *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  ****************************************************************************/
 
 /**
- * @file fw_pos_control_l1_params.c
+ * @file fw_att_control_params.c
  *
- * Parameters defined by the L1 position control task
+ * Parameters defined by the fixed-wing attitude control task
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -87,6 +87,7 @@
 
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
+#include "landingslope.h"
 
 /**
  * L1 control app start / stop handling function
@@ -168,6 +169,9 @@ private:
 	bool land_motor_lim;
 	bool land_onslope;
 
+	/* Landingslope object */
+	Landingslope landingslope;
+
 	float flare_curve_alt_last;
 	/* heading hold */
 	float target_bearing;
@@ -307,11 +311,6 @@ private:
 	 */
 	void		vehicle_setpoint_poll();
 
-	/**
-	 * Get Altitude on the landing glide slope
-	 */
-	float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement);
-
 	/**
 	 * Control position.
 	 */
@@ -536,6 +535,9 @@ FixedwingPositionControl::parameters_update()
 		return 1;
 	}
 
+	/* Update the landing slope */
+	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
+
 	return OK;
 }
 
@@ -707,11 +709,6 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &cu
 	}
 }
 
-float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement)
-{
-	return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative
-}
-
 bool
 FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
 		const struct mission_item_triplet_s &mission_item_triplet)
@@ -854,20 +851,13 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-			float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle);
-			float flare_relative_alt = _parameters.land_flare_alt_relative;
-			float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint
 			float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length;
-			float H1 = _parameters.land_H1_virt;
-			float H0 = flare_relative_alt + H1;
-			float d1 = flare_relative_alt/tanf(landing_slope_angle_rad);
-			float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0));
-			float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt);
-			float horizontal_slope_displacement = (flare_length - d1);
-			float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
-			float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
+			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
+			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
 
-			if ( (_global_pos.alt < _mission_item_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
+
+
+			if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
 				/* land with minimal speed */
 
@@ -877,16 +867,16 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;
 
-				 if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) {
+				 if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 					if (!land_motor_lim) {
 						land_motor_lim  = true;
-						mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, limit throttle");
+						mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle");
 					}
 
 				 }
 
-				float flare_curve_alt =   _mission_item_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1;
+				float flare_curve_alt =   _mission_item_triplet.current.altitude + landingslope.H0() * expf(-math::max(0.0f, landingslope.flare_length() - wp_distance)/landingslope.flare_constant()) - landingslope.H1_virt();
 
 				/* avoid climbout */
 				if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
@@ -902,7 +892,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 												    flare_angle_rad,  math::radians(15.0f));
 
 				if (!land_noreturn_vertical) {
-					mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, flare");
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
 					land_noreturn_vertical = true;
 				}
 				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
@@ -921,7 +911,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 				if (!land_onslope) {
 
-					mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, on slope");
+					mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
 					land_onslope = true;
 				}
 
@@ -961,7 +951,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
 				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
 							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    true, math::max(math::radians(mission_item_triplet.current.radius), math::radians(10.0f)),
+							    true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)),
 							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 

src/modules/fw_pos_control_l1/landingslope.cpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *
+ * 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: landingslope.cpp
+ *
+ * @author: Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "landingslope.h"
+
+#include <nuttx/config.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+
+void Landingslope::update(float landing_slope_angle_rad,
+		float flare_relative_alt,
+		float motor_lim_horizontal_distance,
+		float H1_virt)
+{
+
+	_landing_slope_angle_rad = landing_slope_angle_rad;
+	_flare_relative_alt = flare_relative_alt;
+	_motor_lim_horizontal_distance = motor_lim_horizontal_distance;
+	_H1_virt = H1_virt;
+
+	calculateSlopeValues();
+}
+
+void Landingslope::calculateSlopeValues()
+{
+	_H0 =  _flare_relative_alt + _H1_virt;
+	_d1 = _flare_relative_alt/tanf(_landing_slope_angle_rad);
+	_flare_constant = (_H0 * _d1)/_flare_relative_alt;
+	_flare_length = - logf(_H1_virt/_H0) * _flare_constant;
+	_horizontal_slope_displacement = (_flare_length - _d1);
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
+{
+	return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative
+}
+

src/modules/fw_pos_control_l1/landingslope.h

@@ -1,9 +1,9 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
  *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,53 +34,53 @@
  *
  ****************************************************************************/
 
-/**
- * @file vehicle_global_position_setpoint.h
- * Definition of the global WGS84 position setpoint uORB topic.
- */
-
-#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
-#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-#include "mission.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Global position setpoint in WGS84 coordinates.
+/*
+ * @file: landingslope.h
  *
- * This is the position the MAV is heading towards. If it of type loiter,
- * the MAV is circling around it with the given loiter radius in meters.
+ * @author: Thomas Gubler <thomasgubler@gmail.com>
  */
-struct vehicle_global_position_setpoint_s
+
+#ifndef LANDINGSLOPE_H_
+#define LANDINGSLOPE_H_
+
+class Landingslope
 {
-	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	int32_t lat;			/**< latitude in degrees * 1E7				*/
-	int32_t lon;			/**< longitude in degrees * 1E7				*/
-	float altitude;			/**< altitude in meters					*/
-	float yaw;			/**< in radians NED -PI..+PI 				*/
-	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
-	float param1;
-	float param2;
-	float param3;
-	float param4;
-	float turn_distance_xy;		/**< The distance on the plane which will mark this as reached */
-	float turn_distance_z;		/**< The distance in Z direction which will mark this as reached */
+private:
+	float _landing_slope_angle_rad;
+	float _flare_relative_alt;
+	float _motor_lim_horizontal_distance;
+	float _H1_virt;
+	float _H0;
+	float _d1;
+	float _flare_constant;
+	float _flare_length;
+	float _horizontal_slope_displacement;
+
+	void calculateSlopeValues();
+
+public:
+	Landingslope() {}
+	~Landingslope() {}
+
+	float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
+
+	void update(float landing_slope_angle_rad,
+			float flare_relative_alt,
+			float motor_lim_horizontal_distance,
+			float H1_virt);
+
+
+	inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
+	inline float flare_relative_alt() {return _flare_relative_alt;}
+	inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;}
+	inline float H1_virt() {return _H1_virt;}
+	inline float H0() {return _H0;}
+	inline float d1() {return _d1;}
+	inline float flare_constant() {return _flare_constant;}
+	inline float flare_length() {return _flare_length;}
+	inline float horizontal_slope_displacement() {return _horizontal_slope_displacement;}
+
 };
 
-/**
- * @}
- */
 
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_position_setpoint);
-
-#endif
+#endif /* LANDINGSLOPE_H_ */

src/modules/fw_pos_control_l1/module.mk

@@ -38,4 +38,5 @@
 MODULE_COMMAND	= fw_pos_control_l1
 
 SRCS		= fw_pos_control_l1_main.cpp \
-		  fw_pos_control_l1_params.c
+		  fw_pos_control_l1_params.c \
+		  landingslope.cpp

src/modules/mavlink/mavlink_receiver.cpp

@@ -382,16 +382,15 @@ handle_message(mavlink_message_t *msg)
 
 			/* hil gyro */
 			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.gyro_counter = hil_counter;
 			hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
 			hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
 			hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
 			hil_sensors.gyro_rad_s[0] = imu.xgyro;
 			hil_sensors.gyro_rad_s[1] = imu.ygyro;
 			hil_sensors.gyro_rad_s[2] = imu.zgyro;
+			hil_sensors.gyro_counter = hil_counter;
 
 			/* accelerometer */
-			hil_sensors.accelerometer_counter = hil_counter;
 			static const float mg2ms2 = 9.8f / 1000.0f;
 			hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
 			hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
@@ -401,6 +400,7 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.accelerometer_m_s2[2] = imu.zacc;
 			hil_sensors.accelerometer_mode = 0; // TODO what is this?
 			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
+			hil_sensors.accelerometer_counter = hil_counter;
 
 			/* adc */
 			hil_sensors.adc_voltage_v[0] = 0.0f;
@@ -409,7 +409,6 @@ handle_message(mavlink_message_t *msg)
 
 			/* magnetometer */
 			float mga2ga = 1.0e-3f;
-			hil_sensors.magnetometer_counter = hil_counter;
 			hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
 			hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
 			hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
@@ -419,15 +418,13 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.magnetometer_range_ga = 32.7f; // int16
 			hil_sensors.magnetometer_mode = 0; // TODO what is this
 			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
+			hil_sensors.magnetometer_counter = hil_counter;
 
 			/* baro */
 			hil_sensors.baro_pres_mbar = imu.abs_pressure;
 			hil_sensors.baro_alt_meter = imu.pressure_alt;
 			hil_sensors.baro_temp_celcius = imu.temperature;
-
-			hil_sensors.gyro_counter = hil_counter;
-			hil_sensors.magnetometer_counter = hil_counter;
-			hil_sensors.accelerometer_counter = hil_counter;
+			hil_sensors.baro_counter = hil_counter;
 
 			/* differential pressure */
 			hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa

src/modules/mavlink/missionlib.c

@@ -73,11 +73,15 @@
 #include "waypoints.h"
 #include "mavlink_parameters.h"
 
+
+
 static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
 static uint64_t loiter_start_time;
 
+#if 0
 static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
 	struct vehicle_global_position_setpoint_s *sp);
+#endif
 
 int
 mavlink_missionlib_send_message(mavlink_message_t *msg)
@@ -88,6 +92,8 @@ mavlink_missionlib_send_message(mavlink_message_t *msg)
 	return 0;
 }
 
+
+
 int
 mavlink_missionlib_send_gcs_string(const char *string)
 {
@@ -127,6 +133,7 @@ uint64_t mavlink_missionlib_get_system_timestamp()
 	return hrt_absolute_time();
 }
 
+#if 0
 /**
  * Set special vehicle setpoint fields based on current mission item.
  *
@@ -301,7 +308,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		// 	sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
 		// 	sp.altitude = wpm->waypoints[last_setpoint_index].z;
 		// 	sp.altitude_is_relative = false;
-		// 	sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+		// 	sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
 		// 	set_special_fields(wpm->waypoints[last_setpoint_index].param1,
 		// 		wpm->waypoints[last_setpoint_index].param2,
 		// 		wpm->waypoints[last_setpoint_index].param3,
@@ -317,7 +324,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		// 	sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
 		// 	sp.altitude = wpm->waypoints[next_setpoint_index].z;
 		// 	sp.altitude_is_relative = false;
-		// 	sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+		// 	sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
 		// 	set_special_fields(wpm->waypoints[next_setpoint_index].param1,
 		// 		wpm->waypoints[next_setpoint_index].param2,
 		// 		wpm->waypoints[next_setpoint_index].param3,
@@ -343,7 +350,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.lon = param6_lon_y * 1e7f;
 		sp.altitude = param7_alt_z;
 		sp.altitude_is_relative = true;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
 		set_special_fields(param1, param2, param3, param4, command, &sp);
 
 		/* Initialize publication if necessary */
@@ -364,7 +371,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.x = param5_lat_x;
 		sp.y = param6_lon_y;
 		sp.z = param7_alt_z;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
 
 		/* Initialize publication if necessary */
 		if (local_position_setpoint_pub < 0) {
@@ -388,3 +395,5 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 	printf("%s\n", buf);
 	//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
 }
+
+#endif

src/modules/mavlink/orb_listener.c

@@ -54,6 +54,7 @@
 #include <sys/prctl.h>
 #include <stdlib.h>
 #include <poll.h>
+#include <lib/geo/geo.h>
 
 #include <mavlink/mavlink_log.h>
 
@@ -72,7 +73,6 @@ struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_effective_0;
 struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
 struct airspeed_s airspeed;
@@ -119,7 +119,6 @@ static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
 static void	l_actuator_armed(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
-static void	l_vehicle_attitude_controls_effective(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
@@ -137,7 +136,7 @@ static const struct listener listeners[] = {
 	{l_input_rc,			&mavlink_subs.input_rc_sub,	0},
 	{l_global_position,		&mavlink_subs.global_pos_sub,	0},
 	{l_local_position,		&mavlink_subs.local_pos_sub,	0},
-	{l_global_position_setpoint,	&mavlink_subs.spg_sub,		0},
+	{l_global_position_setpoint,	&mavlink_subs.triplet_sub,	0},
 	{l_local_position_setpoint,	&mavlink_subs.spl_sub,		0},
 	{l_attitude_setpoint,		&mavlink_subs.spa_sub,		0},
 	{l_actuator_outputs,		&mavlink_subs.act_0_sub,	0},
@@ -147,7 +146,6 @@ static const struct listener listeners[] = {
 	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
-	{l_vehicle_attitude_controls_effective,	&mavlink_subs.actuators_effective_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
@@ -242,16 +240,29 @@ l_vehicle_attitude(const struct listener *l)
 					  att.rollspeed,
 					  att.pitchspeed,
 					  att.yawspeed);
-
+					  	
 		/* limit VFR message rate to 10Hz */
 		hrt_abstime t = hrt_absolute_time();
 		if (t >= last_sent_vfr + 100000) {
 			last_sent_vfr = t;
 			float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-			uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
-			float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
+			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
+			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
 			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
 		}
+		
+		/* send quaternion values if it exists */
+		if(att.q_valid) {
+			mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
+												last_sensor_timestamp / 1000,
+												att.q[0],
+												att.q[1],
+												att.q[2],
+												att.q[3],
+												att.rollspeed,
+												att.pitchspeed,
+												att.yawspeed);
+		}
 	}
 
 	attitude_counter++;
@@ -266,13 +277,7 @@ l_vehicle_gps_position(const struct listener *l)
 	orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
 
 	/* GPS COG is 0..2PI in degrees * 1e2 */
-	float cog_deg = gps.cog_rad;
-
-	if (cog_deg > M_PI_F)
-		cog_deg -= 2.0f * M_PI_F;
-
-	cog_deg *= M_RAD_TO_DEG_F;
-
+	float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
 
 	/* GPS position */
 	mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
@@ -365,28 +370,16 @@ l_global_position(const struct listener *l)
 	/* copy global position data into local buffer */
 	orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
 
-	uint64_t timestamp = global_pos.timestamp;
-	int32_t lat = global_pos.lat;
-	int32_t lon = global_pos.lon;
-	int32_t alt = (int32_t)(global_pos.alt * 1000);
-	int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
-	int16_t vx = (int16_t)(global_pos.vx * 100.0f);
-	int16_t vy = (int16_t)(global_pos.vy * 100.0f);
-	int16_t vz = (int16_t)(global_pos.vz * 100.0f);
-
-	/* heading in degrees * 10, from 0 to 36.000) */
-	uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
-
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
-					     timestamp / 1000,
-					     lat,
-					     lon,
-					     alt,
-					     relative_alt,
-					     vx,
-					     vy,
-					     vz,
-					     hdg);
+						 global_pos.timestamp / 1000,
+					     global_pos.lat,
+					     global_pos.lon,
+					     global_pos.alt * 1000.0f,
+					     global_pos.relative_alt * 1000.0f,
+					     global_pos.vx * 100.0f,
+					     global_pos.vy * 100.0f,
+					     global_pos.vz * 100.0f,
+					     _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
 }
 
 void
@@ -409,23 +402,24 @@ l_local_position(const struct listener *l)
 void
 l_global_position_setpoint(const struct listener *l)
 {
-	struct vehicle_global_position_setpoint_s global_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
+	struct mission_item_triplet_s triplet;
+	orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
 
 	uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
+	
+	if (!triplet.current_valid)
+		return;
 
-	if (global_sp.altitude_is_relative)
+	if (triplet.current.altitude_is_relative)
 		coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
 
 	if (gcs_link)
 		mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
 				coordinate_frame,
-				global_sp.lat,
-				global_sp.lon,
-				global_sp.altitude,
-				global_sp.yaw);
+				(int32_t)(triplet.current.lat * 1e7f),
+				(int32_t)(triplet.current.lon * 1e7f),
+				(int32_t)(triplet.current.altitude * 1e3f),
+				(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
 }
 
 void
@@ -506,8 +500,8 @@ l_actuator_outputs(const struct listener *l)
 						  act_outputs.output[6],
 						  act_outputs.output[7]);
 
-		/* only send in HIL mode */
-		if (mavlink_hil_enabled && armed.armed && l->arg == 0) {
+		/* only send in HIL mode and only send first group for HIL */
+		if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
@@ -603,32 +597,6 @@ l_manual_control_setpoint(const struct listener *l)
 						0);
 }
 
-void
-l_vehicle_attitude_controls_effective(const struct listener *l)
-{
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
-
-	if (gcs_link) {
-		/* send, add spaces so that string buffer is at least 10 chars long */
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl0    ",
-						   actuators_effective_0.control_effective[0]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl1    ",
-						   actuators_effective_0.control_effective[1]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl2     ",
-						   actuators_effective_0.control_effective[2]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl3     ",
-						   actuators_effective_0.control_effective[3]);
-	}
-}
-
 void
 l_vehicle_attitude_controls(const struct listener *l)
 {
@@ -803,9 +771,9 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */
 
 	/* --- GLOBAL SETPOINT VALUE --- */
-	mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	orb_set_interval(mavlink_subs.spg_sub, 2000);	/* 0.5 Hz updates */
-
+	mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+	orb_set_interval(mavlink_subs.triplet_sub, 2000);	/* 0.5 Hz updates */
+	
 	/* --- LOCAL SETPOINT VALUE --- */
 	mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 	orb_set_interval(mavlink_subs.spl_sub, 2000);	/* 0.5 Hz updates */
@@ -839,9 +807,6 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
-	orb_set_interval(mavlink_subs.actuators_effective_sub, 100);	/* 10Hz updates */
-
 	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 

src/modules/mavlink/orb_topics.h

@@ -50,8 +50,8 @@
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
@@ -86,7 +86,7 @@ struct mavlink_subscriptions {
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
-	int spg_sub;
+	int triplet_sub;
 	int debug_key_value;
 	int input_rc_sub;
 	int optical_flow;

src/modules/mavlink/waypoints.c

@@ -103,12 +103,21 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
 			return MAV_MISSION_ERROR;
 	}
 
+	switch (mavlink_mission_item->command) {
+		case MAV_CMD_NAV_TAKEOFF:
+			mission_item->pitch_min = mavlink_mission_item->param2;
+			break;
+		default:
+			mission_item->radius = mavlink_mission_item->param2;
+			break;
+	}
+
 	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
 	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
 	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
 	mission_item->nav_cmd = mavlink_mission_item->command;
-	mission_item->radius = mavlink_mission_item->param1;
-	mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
+
+	mission_item->time_inside = mavlink_mission_item->param1 / 1e3f; /* from milliseconds to seconds */
 	mission_item->autocontinue = mavlink_mission_item->autocontinue;
 	mission_item->index = mavlink_mission_item->seq;
 	mission_item->origin = ORIGIN_MAVLINK;
@@ -124,6 +133,15 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio
 		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
 	}
 	
+	switch (mission_item->nav_cmd) {
+		case NAV_CMD_TAKEOFF:
+			mavlink_mission_item->param2 = mission_item->pitch_min;
+			break;
+		default:
+			mavlink_mission_item->param2 = mission_item->radius;
+			break;
+	}
+
 	mavlink_mission_item->x = (float)mission_item->lat;
 	mavlink_mission_item->y = (float)mission_item->lon;
 	mavlink_mission_item->z = mission_item->altitude;
@@ -131,8 +149,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio
 	mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F;
 	mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction;
 	mavlink_mission_item->command = mission_item->nav_cmd;
-	mavlink_mission_item->param1 = mission_item->radius;
-	mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */
+	mavlink_mission_item->param1 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */
 	mavlink_mission_item->autocontinue = mission_item->autocontinue;
 	mavlink_mission_item->seq = mission_item->index;
 
@@ -152,6 +169,8 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->timestamp_lastaction = 0;
 	// state->timestamp_last_send_setpoint = 0;
 	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
+
+	state->current_dataman_id = 0;
 	// state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
 	// state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
 	// state->current_active_wp_id = -1;		///< id of current waypoint
@@ -595,6 +614,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						// wpm->yaw_reached = false;
 						// wpm->pos_reached = false;
 						
+
 						mission.current_index = wpc.seq;
 
 						publish_mission();
@@ -701,7 +721,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 					struct mission_item_s mission_item;
 					ssize_t len = sizeof(struct mission_item_s);
 					
-					if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, wpr.seq, &mission_item, len) == len) {
+					dm_item_t dm_current;
+
+					if (wpm->current_dataman_id == 0) {
+						dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+					} else {
+						dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+					}
+
+					if (dm_read(dm_current, wpr.seq, &mission_item, len) == len) {
 					
 						if (mission.current_index == wpr.seq) {
 							wp.current = true;
@@ -838,10 +866,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 						if (wpc.count > NUM_MISSIONS_SUPPORTED) {
 							warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
-						} else {
-							/* set count to 0 while copying */
-							mission.count = 0;
-							publish_mission();
 						}
 
 #ifdef MAVLINK_WPM_NO_PRINTF
@@ -975,7 +999,17 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 					size_t len = sizeof(struct mission_item_s);
 
-					if (dm_write(DM_KEY_WAYPOINTS_OFFBOARD, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
+					dm_item_t dm_next;
+
+					if (wpm->current_dataman_id == 0) {
+						dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
+						mission.dataman_id = 1;
+					} else {
+						dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
+						mission.dataman_id = 0;
+					}
+
+					if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
 						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						break;
@@ -1021,6 +1055,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						
 						publish_mission();
 
+						wpm->current_dataman_id = mission.dataman_id;
 						wpm->size = wpm->current_count;
 
 						//get the new current waypoint
@@ -1115,9 +1150,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				// wpm->pos_reached = false;
 
 				/* prepare mission topic */
+				mission.dataman_id = -1;
 				mission.count = 0;
+				mission.current_index = -1;
 
-				if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD) == OK) {
+				if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
 					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
 				} else {
 					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);

src/modules/mavlink/waypoints.h

@@ -110,6 +110,7 @@ struct mavlink_wpm_storage {
 	// uint64_t timestamp_firstinside_orbit;
 	// uint64_t timestamp_lastoutside_orbit;
 	uint32_t timeout;
+	int current_dataman_id;
 	// uint32_t delay_setpoint;
 	// float accept_range_yaw;
 	// float accept_range_distance;

src/modules/mavlink_onboard/orb_topics.h

@@ -50,7 +50,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>

src/modules/multirotor_pos_control/multirotor_pos_control.c

@@ -61,8 +61,8 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/pid/pid.h>
 #include <mavlink/mavlink_log.h>
@@ -190,12 +190,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	memset(&manual, 0, sizeof(manual));
 	struct vehicle_local_position_s local_pos;
 	memset(&local_pos, 0, sizeof(local_pos));
-	struct vehicle_local_position_setpoint_s local_pos_sp;
-	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
-	struct vehicle_global_position_setpoint_s global_pos_sp;
-	memset(&global_pos_sp, 0, sizeof(global_pos_sp));
+	struct mission_item_triplet_s triplet;
+	memset(&triplet, 0, sizeof(triplet));
 	struct vehicle_global_velocity_setpoint_s global_vel_sp;
 	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
+	struct vehicle_local_position_setpoint_s local_pos_sp;
+	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int param_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -203,9 +203,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+	int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
 
 	/* publish setpoint */
 	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
@@ -292,11 +290,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 		}
 
-		orb_check(global_pos_sp_sub, &updated);
+		orb_check(mission_triplet_sub, &updated);
 
 		if (updated) {
-			orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
-			global_pos_sp_valid = true;
+			orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet);
+			global_pos_sp_valid = triplet.current_valid;
 			reset_mission_sp = true;
 		}
 
@@ -329,7 +327,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
 			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
-			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
 
 			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
 			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
@@ -459,24 +456,22 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						/* update global setpoint projection */
 
 						if (global_pos_sp_valid) {
-							/* global position setpoint valid, use it */
-							double sp_lat = global_pos_sp.lat * 1e-7;
-							double sp_lon = global_pos_sp.lon * 1e-7;
-							/* project global setpoint to local setpoint */
-							map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
 
-							if (global_pos_sp.altitude_is_relative) {
-								local_pos_sp.z = -global_pos_sp.altitude;
+							/* project global setpoint to local setpoint */
+							map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
+							if (triplet.current.altitude_is_relative) {
+								local_pos_sp.z = -triplet.current.altitude;
 
 							} else {
-								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
+								local_pos_sp.z = local_pos.ref_alt - triplet.current.lat;
 							}
 							/* update yaw setpoint only if value is valid */
-							if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
-								att_sp.yaw_body = global_pos_sp.yaw;
+							if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) {
+								att_sp.yaw_body = triplet.current.yaw;
 							}
 
-							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
+							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
 
 						} else {
 							if (reset_auto_sp_xy) {

src/modules/navigator/navigator_main.cpp

@@ -440,6 +440,7 @@ Navigator::offboard_mission_update()
 	struct mission_s offboard_mission;
 	if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
 
+		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
 		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
 		_mission.set_offboard_mission_count(offboard_mission.count);
 

src/modules/navigator/navigator_mission.cpp

@@ -53,7 +53,8 @@ static const int ERROR = -1;
 
 
 Mission::Mission() : 
-
+	
+	_offboard_dataman_id(-1),
 	_current_offboard_mission_index(0),
 	_current_onboard_mission_index(0),
 	_offboard_mission_item_count(0),
@@ -67,6 +68,12 @@ Mission::~Mission()
 	
 }
 
+void
+Mission::set_offboard_dataman_id(int new_id)
+{
+	_offboard_dataman_id = new_id;
+}
+
 void
 Mission::set_current_offboard_mission_index(int new_index)
 {
@@ -132,8 +139,16 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool
 	/* otherwise fallback to offboard */
 	} else if (current_offboard_mission_available()) {
 
+		dm_item_t dm_current;
+
+		if (_offboard_dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
 		const ssize_t len = sizeof(struct mission_item_s);
-		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index, new_mission_item, len) != len) {
+		if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			_current_mission_type = MISSION_TYPE_NONE;
 			return ERROR;
@@ -166,8 +181,16 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
 	/* otherwise fallback to offboard */
 	} else if (next_offboard_mission_available()) {
 
+		dm_item_t dm_current;
+
+		if (_offboard_dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
 		const ssize_t len = sizeof(struct mission_item_s);
-		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
+		if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return ERROR;
 		}

src/modules/navigator/navigator_mission.h

@@ -55,6 +55,7 @@ public:
 	 */
 	~Mission();
 
+	void		set_offboard_dataman_id(int new_id);
 	void		set_current_offboard_mission_index(int new_index);
 	void		set_current_onboard_mission_index(int new_index);
 	void		set_offboard_mission_count(unsigned new_count);
@@ -78,6 +79,7 @@ private:
 	bool		next_onboard_mission_available();
 	bool		next_offboard_mission_available();
 
+	int 		_offboard_dataman_id;
 	unsigned	_current_offboard_mission_index;
 	unsigned	_current_onboard_mission_index;
 	unsigned	_offboard_mission_item_count;		/** number of offboard mission items available */

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -236,13 +236,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		if (ret < 0) {
 			/* poll error */
-			errx(1, "subscriptions poll error on init.");
+			mavlink_log_info(mavlink_fd, "[inav] poll error on init");
 
 		} else if (ret > 0) {
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (wait_baro && sensor.baro_counter > baro_counter) {
+				if (wait_baro && sensor.baro_counter != baro_counter) {
 					baro_counter = sensor.baro_counter;
 
 					/* mean calculation over several measurements */
@@ -320,8 +320,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		if (ret < 0) {
 			/* poll error */
-			warnx("subscriptions poll error.");
-			thread_should_exit = true;
+			mavlink_log_info(mavlink_fd, "[inav] poll error on init");
 			continue;
 
 		} else if (ret > 0) {
@@ -355,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds[4].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (sensor.accelerometer_counter > accel_counter) {
+				if (sensor.accelerometer_counter != accel_counter) {
 					if (att.R_valid) {
 						/* correct accel bias, now only for Z */
 						sensor.accelerometer_m_s2[2] -= accel_bias[2];
@@ -381,7 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					accel_updates++;
 				}
 
-				if (sensor.baro_counter > baro_counter) {
+				if (sensor.baro_counter != baro_counter) {
 					baro_corr = - sensor.baro_alt_meter - z_est[0];
 					baro_counter = sensor.baro_counter;
 					baro_updates++;

src/modules/px4iofirmware/controls.c

@@ -66,7 +66,7 @@ controls_init(void)
 	sbus_init("/dev/ttyS2");
 
 	/* default to a 1:1 input map, all enabled */
-	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+	for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
 
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = 0;
@@ -94,6 +94,9 @@ controls_tick() {
 	 * other.  Don't do that.
 	 */
 
+	/* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */
+	uint16_t rssi = 0;
+
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
 	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
@@ -104,14 +107,15 @@ controls_tick() {
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
 		else
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+
+		rssi = 1000;
 	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
-		r_raw_rc_count = 8;
 	}
 	perf_end(c_gather_sbus);
 
@@ -122,10 +126,19 @@ controls_tick() {
 	 */
 	perf_begin(c_gather_ppm);
 	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
-	if (ppm_updated)
+	if (ppm_updated) {
+
+		/* XXX sample RSSI properly here */
+		rssi = 1000;
+
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+	}
 	perf_end(c_gather_ppm);
 
+	/* limit number of channels to allowable data size */
+	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
+		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
+
 	/*
 	 * In some cases we may have received a frame, but input has still
 	 * been lost.
@@ -197,14 +210,16 @@ controls_tick() {
 
 				/* and update the scaled/mapped version */
 				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
+				if (mapped < PX4IO_CONTROL_CHANNELS) {
 
-				/* invert channel if pitch - pulling the lever down means pitching up by convention */
-				if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
-					scaled = -scaled;
+					/* invert channel if pitch - pulling the lever down means pitching up by convention */
+					if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
+						scaled = -scaled;
 
-				r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-				assigned_channels |= (1 << mapped);
+					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+					assigned_channels |= (1 << mapped);
+
+				}
 			}
 		}
 
@@ -221,7 +236,7 @@ controls_tick() {
 		 * This might happen if a protocol-based receiver returns an update
 		 * that contains no channels that we have mapped.
 		 */
-		if (assigned_channels == 0) {
+		if (assigned_channels == 0 || rssi == 0) {
 			rc_input_lost = true;
 		} else {
 			/* set RC OK flag */
@@ -321,8 +336,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
 
 		/* PPM data exists, copy it */
 		*num_values = ppm_decoded_channels;
-		if (*num_values > PX4IO_CONTROL_CHANNELS)
-			*num_values = PX4IO_CONTROL_CHANNELS;
+		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
+			*num_values = PX4IO_RC_INPUT_CHANNELS;
 
 		for (unsigned i = 0; i < *num_values; i++)
 			values[i] = ppm_buffer[i];

src/modules/px4iofirmware/dsm.c

@@ -355,7 +355,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 			continue;
 
 		/* ignore channels out of range */
-		if (channel >= PX4IO_INPUT_CHANNELS)
+		if (channel >= PX4IO_RC_INPUT_CHANNELS)
 			continue;
 
 		/* update the decoded channel count */

src/modules/px4iofirmware/mixer.cpp

@@ -77,7 +77,8 @@ enum mixer_source {
 	MIX_NONE,
 	MIX_FMU,
 	MIX_OVERRIDE,
-	MIX_FAILSAFE
+	MIX_FAILSAFE,
+	MIX_OVERRIDE_FMU_OK
 };
 static mixer_source source;
 
@@ -135,10 +136,19 @@ mixer_tick(void)
 		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
-		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+		     !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
+		} else 	if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
+
+			/* if allowed, mix from RC inputs directly up to available rc channels */
+			source = MIX_OVERRIDE_FMU_OK;
 		}
 	}
 
@@ -185,7 +195,7 @@ mixer_tick(void)
 			r_page_servos[i] = r_page_servo_failsafe[i];
 
 			/* safe actuators for FMU feedback */
-			r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
+			r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
 		}
 
 
@@ -201,6 +211,10 @@ mixer_tick(void)
 
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 			r_page_servos[i] = 0;
+
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
+			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
+		}
 	}
 
 	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
@@ -236,24 +250,35 @@ mixer_callback(uintptr_t handle,
 	       uint8_t control_index,
 	       float &control)
 {
-	if (control_group != 0)
+	if (control_group > 3)
 		return -1;
 
 	switch (source) {
 	case MIX_FMU:
-		if (control_index < PX4IO_CONTROL_CHANNELS) {
-			control = REG_TO_FLOAT(r_page_controls[control_index]);
+		if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) {
+			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
 			break;
 		}
 		return -1;
 
 	case MIX_OVERRIDE:
-		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
+		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
 			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
 			break;
 		}
 		return -1;
 
+	case MIX_OVERRIDE_FMU_OK:
+		/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
+		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
+			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
+			break;
+		} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
+			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
+			break;
+		}
+		return -1;
+
 	case MIX_FAILSAFE:
 	case MIX_NONE:
 		control = 0.0f;

src/modules/px4iofirmware/protocol.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 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
@@ -63,7 +63,7 @@
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
  *
- * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, 
+ * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, 
  * [2] denotes definitions specific to the PX4IOv2 board.
  */
 
@@ -76,6 +76,9 @@
 
 #define PX4IO_PROTOCOL_VERSION		4
 
+/* maximum allowable sizes on this protocol version */
+#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT	8	/**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
+
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
 #define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* PX4IO_PROTOCOL_VERSION */
@@ -87,6 +90,7 @@
 #define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
 #define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
 #define PX4IO_P_CONFIG_RELAY_COUNT		8	/* hardcoded # of relay outputs */
+#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT	8	/**< hardcoded # of control groups*/
 
 /* dynamic status page */
 #define PX4IO_PAGE_STATUS		1
@@ -124,6 +128,7 @@
 #define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
 #define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
 #define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
+#define PX4IO_P_STATUS_NRSSI			9	/* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -184,44 +189,59 @@ enum {							/* DSM bind states */
 	dsm_bind_reinit_uart
 };
  					     /*	8 */
-#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
+#define PX4IO_P_SETUP_SET_DEBUG			9		/* debug level for IO board */
+
+#define PX4IO_P_SETUP_REBOOT_BL		       10	/* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC               14662       /* required argument for reboot (random) */
+
+#define PX4IO_P_SETUP_CRC		       11	/* get CRC of IO firmware */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS		51		/* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS			51		/**< actuator control groups, one after the other, 8 wide */
+#define PX4IO_P_CONTROLS_GROUP_0		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_1		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_2		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_3		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+
+#define PX4IO_P_CONTROLS_GROUP_VALID		64
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /* group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /* group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /* group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /* group 3 is valid / received */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD		52
+#define PX4IO_PAGE_MIXERLOAD			52
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG		53		/* R/C input configuration */
-#define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
-#define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
-#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_OPTIONS		5	/* channel options bitmask */
+#define PX4IO_PAGE_RC_CONFIG			53		/**< R/C input configuration */
+#define PX4IO_P_RC_CONFIG_MIN			0		/**< lowest input value */
+#define PX4IO_P_RC_CONFIG_CENTER		1		/**< center input value */
+#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_OPTIONS		5		/**< channel options bitmask */
 #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED	(1 << 0)
 #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE	(1 << 1)
-#define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
+#define PX4IO_P_RC_CONFIG_STRIDE		6		/**< spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM		54		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM			54		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM		55		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM			55		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* Debug and test page - not used in normal operation */
-#define PX4IO_PAGE_TEST			127
-#define PX4IO_P_TEST_LED			0	/* set the amber LED on/off */
+#define PX4IO_PAGE_TEST				127
+#define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */
 
 /* PWM minimum values for certain ESCs */
-#define PX4IO_PAGE_CONTROL_MIN_PWM		106	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM maximum values for certain ESCs */
-#define PX4IO_PAGE_CONTROL_MAX_PWM		107	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM disarmed values that are active, even when SAFETY_SAFE */
-#define PX4IO_PAGE_DISARMED_PWM		108	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DISARMED_PWM		108			/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.

src/modules/px4iofirmware/px4io.c

@@ -45,6 +45,7 @@
 #include <string.h>
 #include <poll.h>
 #include <signal.h>
+#include <crc32.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
@@ -117,6 +118,29 @@ show_debug_messages(void)
 	}
 }
 
+static void
+heartbeat_blink(void)
+{
+	static bool heartbeat = false;
+	LED_BLUE(heartbeat = !heartbeat);
+}
+
+
+static void
+calculate_fw_crc(void)
+{
+#define APP_SIZE_MAX 0xf000
+#define APP_LOAD_ADDRESS 0x08001000
+	// compute CRC of the current firmware
+	uint32_t sum = 0;
+	for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
+		uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
+		sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
+	}
+	r_page_setup[PX4IO_P_SETUP_CRC]   = sum & 0xFFFF;
+	r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
+}
+
 int
 user_start(int argc, char *argv[])
 {
@@ -129,6 +153,9 @@ user_start(int argc, char *argv[])
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
 
+	/* calculate our fw CRC so FMU can decide if we need to update */
+	calculate_fw_crc();
+
 	/*
 	 * Poll at 1ms intervals for received bytes that have not triggered
 	 * a DMA event.
@@ -201,6 +228,7 @@ user_start(int argc, char *argv[])
 	 */
 
 	uint64_t last_debug_time = 0;
+        uint64_t last_heartbeat_time = 0;
 	for (;;) {
 
 		/* track the rate at which the loop is running */
@@ -216,6 +244,11 @@ user_start(int argc, char *argv[])
 		controls_tick();
 		perf_end(controls_perf);
 
+                if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
+                    last_heartbeat_time = hrt_absolute_time();
+                    heartbeat_blink();
+                }
+
 #if 0
 		/* check for debug activity */
 		show_debug_messages();

src/modules/px4iofirmware/px4io.h

@@ -53,7 +53,9 @@
  */
 #define PX4IO_SERVO_COUNT		8
 #define PX4IO_CONTROL_CHANNELS		8
-#define PX4IO_INPUT_CHANNELS		8 // XXX this should be 18 channels
+#define PX4IO_CONTROL_GROUPS		2
+#define PX4IO_RC_INPUT_CHANNELS		18
+#define PX4IO_RC_MAPPED_CONTROL_CHANNELS		8 /**< This is the maximum number of channels mapped/used */
 
 /*
  * Debug logging
@@ -169,6 +171,8 @@ extern pwm_limit_t pwm_limit;
 
 #define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
 
+#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
+
 /*
  * Mixer
  */
@@ -209,7 +213,7 @@ extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
-extern bool	sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
+extern bool	sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;

src/modules/px4iofirmware/registers.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 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
@@ -45,6 +45,8 @@
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
+#include <systemlib/systemlib.h>
+#include <stm32_pwr.h>
 
 #include "px4io.h"
 #include "protocol.h"
@@ -68,7 +70,7 @@ static const uint16_t	r_page_config[] = {
 	[PX4IO_P_CONFIG_MAX_TRANSFER]		= 64,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
 	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= PX4IO_SERVO_COUNT,
-	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= PX4IO_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= PX4IO_RC_INPUT_CHANNELS,
 	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= PX4IO_ADC_CHANNEL_COUNT,
 	[PX4IO_P_CONFIG_RELAY_COUNT]		= PX4IO_RELAY_CHANNELS,
 };
@@ -112,7 +114,7 @@ uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
-	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
 };
 
 /**
@@ -122,7 +124,7 @@ uint16_t		r_page_raw_rc_input[] =
  */
 uint16_t		r_page_rc_input[] = {
 	[PX4IO_P_RC_VALID]			= 0,
-	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
 };
 
 /**
@@ -154,6 +156,8 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
 #endif
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
+	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
+	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
 };
 
 #define PX4IO_P_SETUP_FEATURES_VALID	(0)
@@ -172,7 +176,7 @@ volatile uint16_t	r_page_setup[] =
  *
  * Control values from the FMU.
  */
-volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
+volatile uint16_t	r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
 
 /*
  * PAGE 102 does not have a buffer.
@@ -183,7 +187,7 @@ volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
  *
  * R/C channel input configuration.
  */
-uint16_t		r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+uint16_t		r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
 
 /* valid options */
 #define PX4IO_P_RC_CONFIG_OPTIONS_VALID	(PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
@@ -235,7 +239,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_CONTROLS:
 
 		/* copy channel data */
-		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+		while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			r_page_controls[offset] = *values;
@@ -501,6 +505,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
 			break;
 
+		case PX4IO_P_SETUP_REBOOT_BL:
+			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
+			    (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+				// don't allow reboot while armed
+				break;
+			}
+
+			// check the magic value
+			if (value != PX4IO_REBOOT_BL_MAGIC)
+				break;
+                        
+                        // note that we don't set BL_WAIT_MAGIC in
+                        // BKP_DR1 as that is not necessary given the
+                        // timing of the forceupdate command. The
+                        // bootloader on px4io waits for enough time
+                        // anyway, and this method works with older
+                        // bootloader versions (tested with both
+                        // revision 3 and revision 4).
+
+			up_systemreset();
+			break;
+
 		case PX4IO_P_SETUP_DSM:
 			dsm_bind(value & 0x0f, (value >> 4) & 7);
 			break;
@@ -525,7 +552,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
 		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
 
-		if (channel >= PX4IO_CONTROL_CHANNELS)
+		if (channel >= PX4IO_RC_INPUT_CHANNELS)
 			return -1;
 
 		/* disable the channel until we have a chance to sanity-check it */
@@ -573,7 +600,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
 					count++;
 				}
-				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
+				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
 					count++;
 				}
 

src/modules/px4iofirmware/safety.c

@@ -77,7 +77,6 @@ static unsigned blink_counter = 0;
 static bool safety_button_pressed;
 
 static void safety_check_button(void *arg);
-static void heartbeat_blink(void *arg);
 static void failsafe_blink(void *arg);
 
 void
@@ -86,9 +85,6 @@ safety_init(void)
 	/* arrange for the button handler to be called at 10Hz */
 	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
 
-	/* arrange for the heartbeat handler to be called at 4Hz */
-	hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
-
 	/* arrange for the failsafe blinker to be called at 8Hz */
 	hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
 }
@@ -163,16 +159,6 @@ safety_check_button(void *arg)
 	}
 }
 
-static void
-heartbeat_blink(void *arg)
-{
-	static bool heartbeat = false;
-
-	/* XXX add flags here that need to be frobbed by various loops */
-
-	LED_BLUE(heartbeat = !heartbeat);
-}
-
 static void
 failsafe_blink(void *arg)
 {
@@ -192,4 +178,4 @@ failsafe_blink(void *arg)
 	}
 
 	LED_AMBER(failsafe);
-}
+}

src/modules/px4iofirmware/sbus.c

@@ -54,6 +54,27 @@
 
 #define SBUS_FRAME_SIZE		25
 #define SBUS_INPUT_CHANNELS	16
+#define SBUS_FLAGS_BYTE		23
+#define SBUS_FAILSAFE_BIT	3
+#define SBUS_FRAMELOST_BIT	2
+
+/*
+  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)  
+    -+152% on TX:  PCM 0.884/1.520/2.160ms -> SBus raw values:   1/1024/2047  (140% ATV plus dirty tricks)
+*/
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define SBUS_RANGE_MIN 200.0f
+#define SBUS_RANGE_MAX 1800.0f
+
+#define SBUS_TARGET_MIN 1000.0f
+#define SBUS_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
+#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
 
 static int sbus_fd = -1;
 
@@ -66,7 +87,7 @@ static unsigned partial_frame_count;
 
 unsigned sbus_frame_drops;
 
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
 
 int
 sbus_init(const char *device)
@@ -97,7 +118,7 @@ sbus_init(const char *device)
 }
 
 bool
-sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
+sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -154,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
 	 * decode it.
 	 */
 	partial_frame_count = 0;
-	return sbus_decode(now, values, num_values, max_channels);
+	return sbus_decode(now, values, num_values, rssi, max_channels);
 }
 
 /*
@@ -194,7 +215,7 @@ 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, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
 {
 	/* check frame boundary markers to avoid out-of-sync cases */
 	if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
@@ -202,15 +223,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 		return false;
 	}
 
-	/* if the failsafe or connection lost bit is set, we consider the frame invalid */
-	if ((frame[23] & (1 << 2)) && /* signal lost */
-	    (frame[23] & (1 << 3))) { /* failsafe */
-
-		/* actively announce signal loss */
-		*values = 0;
-		return false;
-	}
-
 	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
 
@@ -234,22 +246,41 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 			}
 		}
 
-		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		values[channel] = (value / 2) + 998;
+
+		/* 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;
 	}
 
 	/* decode switch channels if data fields are wide enough */
-	if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
+	if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) {
 		chancount = 18;
 
 		/* channel 17 (index 16) */
-		values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+		values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
 		/* channel 18 (index 17) */
-		values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
+		values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
 	}
 
 	/* note the number of channels decoded */
 	*num_values = chancount;
 
+	/* decode and handle failsafe and frame-lost flags */
+	if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
+		/* report that we failed to read anything valid off the receiver */
+		*rssi = 0;
+		return false;
+	}
+	else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
+		/* set a special warning flag or try to calculate some kind of RSSI information - to be implemented 
+		 * 
+		 * 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!!! */
+
+		*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
+	}
+
+	*rssi = 1000;
+
 	return true;
 }

src/modules/sdlog2/sdlog2.c

@@ -68,12 +68,11 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -691,11 +690,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_rates_setpoint_s rates_sp;
 		struct actuator_outputs_s act_outputs;
 		struct actuator_controls_s act_controls;
-		struct actuator_controls_effective_s act_controls_effective;
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
-		struct vehicle_global_position_setpoint_s global_pos_sp;
+		struct mission_item_triplet_s triplet;
 		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
@@ -717,11 +715,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int rates_sp_sub;
 		int act_outputs_sub;
 		int act_controls_sub;
-		int act_controls_effective_sub;
 		int local_pos_sub;
 		int local_pos_sp_sub;
 		int global_pos_sub;
-		int global_pos_sp_sub;
+		int triplet_sub;
 		int gps_pos_sub;
 		int vicon_pos_sub;
 		int flow_sub;
@@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	memset(&log_msg.body, 0, sizeof(log_msg.body));
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of messages */
-	const ssize_t fdsc = 20;
-	/* Sanity check variable and index */
+	/* number of subscriptions */
+	const ssize_t fdsc = 19;
+	/* sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
 	struct pollfd fds[fdsc];
@@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
-	/* --- ACTUATOR CONTROL EFFECTIVE --- */
-	subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
-	fds[fdsc_count].fd = subs.act_controls_effective_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
 	/* --- LOCAL POSITION --- */
 	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	fds[fdsc_count].fd = subs.local_pos_sub;
@@ -849,8 +840,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fdsc_count++;
 
 	/* --- GLOBAL POSITION SETPOINT--- */
-	subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	fds[fdsc_count].fd = subs.global_pos_sp_sub;
+	subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+	fds[fdsc_count].fd = subs.triplet_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -1114,12 +1105,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(ATTC);
 			}
 
-			/* --- ACTUATOR CONTROL EFFECTIVE --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective);
-				// TODO not implemented yet
-			}
-
 			/* --- LOCAL POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
@@ -1165,20 +1150,21 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 			/* --- GLOBAL POSITION SETPOINT --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
+				orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet);
 				log_msg.msg_type = LOG_GPSP_MSG;
-				log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
-				log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
-				log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
-				log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
-				log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
-				log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
-				log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
-				log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
-				log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
-				log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
-				log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
-				log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
+				log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative;
+				log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7);
+				log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7);
+				log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude;
+				log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
+				log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd;				
+				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+				log_msg.body.log_GPSP.radius = buf.triplet.current.radius;
+				log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside;
+				log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
 				LOGBUFFER_WRITE_AND_COUNT(GPSP);
 			}
 
@@ -1208,6 +1194,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.msg_type = LOG_RC_MSG;
 				/* Copy only the first 8 channels of 14 */
 				memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
+				log_msg.body.log_RC.channel_count = buf.rc.chan_count;
 				LOGBUFFER_WRITE_AND_COUNT(RC);
 			}
 

src/modules/sdlog2/sdlog2_messages.h

@@ -159,6 +159,7 @@ struct log_STAT_s {
 #define LOG_RC_MSG 11
 struct log_RC_s {
 	float channel[8];
+	uint8_t channel_count;
 };
 
 /* --- OUT0 - ACTUATOR_0 OUTPUT --- */
@@ -213,13 +214,12 @@ struct log_GPSP_s {
 	int32_t lon;
 	float altitude;
 	float yaw;
+	uint8_t nav_cmd;
 	float loiter_radius;
 	int8_t loiter_direction;
-	uint8_t nav_cmd;
-	float param1;
-	float param2;
-	float param3;
-	float param4;
+	float radius;
+	float time_inside;
+	float pitch_min;
 };
 
 /* --- ESC - ESC STATE --- */
@@ -281,13 +281,13 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
-	LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
+	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
-	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
+	LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitRad,LoitDir,Rad,TimeIn,pitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 	/* system-level messages, ID >= 0x80 */

src/modules/sensors/sensor_params.c

@@ -190,11 +190,30 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
 
+PARAM_DEFINE_FLOAT(RC16_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC16_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC16_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC16_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC17_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC17_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC17_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC17_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC18_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC18_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC18_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC18_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 #endif
 PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
+PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 #else
@@ -226,3 +245,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera azimuth / yaw
 PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
 PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
 PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
+
+PARAM_DEFINE_INT32(RC_FS_CH, 0);	/**< RC failsafe channel, 0 = disable */
+PARAM_DEFINE_INT32(RC_FS_MODE, 0);	/**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */
+PARAM_DEFINE_FLOAT(RC_FS_THR, 800);	/**< RC failsafe PWM threshold */

src/modules/sensors/sensors.cpp

@@ -165,7 +165,7 @@ public:
 	int		start();
 
 private:
-	static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX;	/**< maximum number of r/c channels we handle */
+	static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
 
 	hrt_abstime	_rc_last_valid;		/**< last time we got a valid RC signal */
 
@@ -260,6 +260,10 @@ private:
 		float rc_scale_yaw;
 		float rc_scale_flaps;
 
+		int rc_fs_ch;
+		int rc_fs_mode;
+		float rc_fs_thr;
+
 		float battery_voltage_scaling;
 
 	}		_parameters;			/**< local copies of interesting parameters */
@@ -305,6 +309,10 @@ private:
 		param_t rc_scale_yaw;
 		param_t rc_scale_flaps;
 
+		param_t rc_fs_ch;
+		param_t rc_fs_mode;
+		param_t rc_fs_thr;
+
 		param_t battery_voltage_scaling;
 
 		param_t board_rotation;
@@ -517,6 +525,11 @@ Sensors::Sensors() :
 	_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
 	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
 
+	/* RC failsafe */
+	_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
+	_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
+	_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
+
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
 	_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
@@ -589,7 +602,7 @@ Sensors::parameters_update()
 	float tmpRevFactor = 0.0f;
 
 	/* rc values */
-	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
+	for (unsigned int i = 0; i < _rc_max_chan_count; i++) {
 
 		param_get(_parameter_handles.min[i], &(_parameters.min[i]));
 		param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
@@ -668,6 +681,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
 	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
 	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
+	param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
+	param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
+	param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -1256,6 +1272,18 @@ Sensors::rc_poll()
 		if (rc_input.channel_count < 4)
 			return;
 
+		/* failsafe check */
+		if (_parameters.rc_fs_ch != 0) {
+			if (_parameters.rc_fs_mode == 0) {
+				if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
+					return;
+
+			} else if (_parameters.rc_fs_mode == 1) {
+				if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
+					return;
+			}
+		}
+
 		unsigned channel_limit = rc_input.channel_count;
 
 		if (channel_limit > _rc_max_chan_count)

src/modules/systemlib/perf_counter.c

@@ -295,10 +295,11 @@ perf_print_counter(perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
+		printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
+		       pce->time_total / pce->event_count,
 		       pce->time_least,
 		       pce->time_most);
 		break;

src/modules/systemlib/rc_check.c

@@ -45,7 +45,7 @@
 #include <systemlib/rc_check.h>
 #include <systemlib/param/param.h>
 #include <mavlink/mavlink_log.h>
-#include <uORB/topics/rc_channels.h>
+#include <drivers/drv_rc_input.h>
 
 int rc_calibration_check(int mavlink_fd) {
 
@@ -66,7 +66,7 @@ int rc_calibration_check(int mavlink_fd) {
 
 	int channel_fail_count = 0;
 
-	for (int i = 0; i < RC_CHANNELS_MAX; i++) {
+	for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
 		/* should the channel be enabled? */
 		uint8_t count = 0;
 

src/modules/uORB/objects_common.cpp

@@ -117,9 +117,6 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
 #include "topics/vehicle_bodyframe_speed_setpoint.h"
 ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
 
-#include "topics/vehicle_global_position_setpoint.h"
-ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
-
 #include "topics/mission_item_triplet.h"
 ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);
 
@@ -176,13 +173,6 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
 #include "topics/actuator_armed.h"
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
-/* actuator controls, as set by actuators / mixers after limiting */
-#include "topics/actuator_controls_effective.h"
-ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s);
-ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s);
-ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s);
-ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s);
-
 #include "topics/actuator_outputs.h"
 ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);

src/modules/uORB/topics/actuator_controls_effective.h

@@ -46,34 +46,34 @@
 #ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
 #define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
 
-#include <stdint.h>
-#include "../uORB.h"
-#include "actuator_controls.h"
+//#include <stdint.h>
+//#include "../uORB.h"
+//#include "actuator_controls.h"
+//
+//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE		NUM_ACTUATOR_CONTROLS
+//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE	NUM_ACTUATOR_CONTROL_GROUPS	/**< for sanity checking */
+//
+///**
+// * @addtogroup topics
+// * @{
+// */
+//
+//struct actuator_controls_effective_s {
+//	uint64_t timestamp;
+//	float	control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
+//};
+//
+///**
+// * @}
+// */
+//
+///* actuator control sets; this list can be expanded as more controllers emerge */
+//ORB_DECLARE(actuator_controls_effective_0);
+//ORB_DECLARE(actuator_controls_effective_1);
+//ORB_DECLARE(actuator_controls_effective_2);
+//ORB_DECLARE(actuator_controls_effective_3);
+//
+///* control sets with pre-defined applications */
+//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE	ORB_ID(actuator_controls_effective_0)
 
-#define NUM_ACTUATOR_CONTROLS_EFFECTIVE		NUM_ACTUATOR_CONTROLS
-#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE	NUM_ACTUATOR_CONTROL_GROUPS	/**< for sanity checking */
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct actuator_controls_effective_s {
-	uint64_t timestamp;
-	float	control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
-};
-
-/**
- * @}
- */
-
-/* actuator control sets; this list can be expanded as more controllers emerge */
-ORB_DECLARE(actuator_controls_effective_0);
-ORB_DECLARE(actuator_controls_effective_1);
-ORB_DECLARE(actuator_controls_effective_2);
-ORB_DECLARE(actuator_controls_effective_3);
-
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE	ORB_ID(actuator_controls_effective_0)
-
-#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
+#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */

src/modules/uORB/topics/mission.h

@@ -80,8 +80,8 @@ enum ORIGIN {
 struct mission_item_s
 {
 	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	double lat;			/**< latitude in degrees * 1E7				*/
-	double lon;			/**< longitude in degrees * 1E7				*/
+	double lat;			/**< latitude in degrees				*/
+	double lon;			/**< longitude in degrees				*/
 	float altitude;			/**< altitude in meters					*/
 	float yaw;			/**< in radians NED -PI..+PI 				*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
@@ -89,6 +89,7 @@ struct mission_item_s
 	enum NAV_CMD nav_cmd;		/**< navigation command					*/
 	float radius;			/**< radius in which the mission is accepted as reached in meters */
 	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
+	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 	bool autocontinue;		/**< true if next waypoint should follow after this one */
 	int index;			/**< index matching the mavlink waypoint                */
 	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
@@ -96,6 +97,7 @@ struct mission_item_s
 
 struct mission_s
 {
+	int dataman_id;			/**< default -1, there are two offboard storage places in the dataman: 0 or 1 */
 	unsigned count;			/**< count of the missions stored in the datamanager */
 	int current_index;		/**< default -1, start at the one changed latest */
 };

src/modules/uORB/topics/rc_channels.h

@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Nils Wenzler <wenzlern@student.ethz.ch>
- *           @author Ivan Ovinnikov <oivan@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012, 2013 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
@@ -47,13 +44,13 @@
 
 /**
  * The number of RC channel inputs supported.
- * Current (Q1/2013) radios support up to 18 channels,
+ * Current (Q4/2013) radios support up to 18 channels,
  * leaving at a sane value of 15.
  * This number can be greater then number of RC channels,
  * because single RC channel can be mapped to multiple
  * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAX   15
+#define RC_CHANNELS_MAPPED_MAX   15
 
 /** 
  * This defines the mapping of the RC functions.
@@ -91,7 +88,7 @@ struct rc_channels_s {
   uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
   struct {
     float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
-  } chan[RC_CHANNELS_MAX];
+  } chan[RC_CHANNELS_MAPPED_MAX];
   uint8_t chan_count;                 /**< number of valid channels */
 
   /*String array to store the names of the functions*/

src/modules/uORB/topics/vehicle_control_mode.h

@@ -78,6 +78,7 @@ struct vehicle_control_mode_s
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
 	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
+	bool flag_control_flighttermination_enabled;   /**< true if flighttermination is enabled */
 
 	bool flag_control_auto_enabled;		// TEMP
 	uint8_t auto_state;	// TEMP navigation state for AUTO modes

src/modules/uORB/topics/vehicle_status.h

@@ -95,6 +95,12 @@ typedef enum {
 	HIL_STATE_ON
 } hil_state_t;
 
+
+typedef enum {
+	FLIGHTTERMINATION_STATE_OFF = 0,
+	FLIGHTTERMINATION_STATE_ON
+} flighttermination_state_t;
+
 typedef enum {
 	MODE_SWITCH_MANUAL = 0,
 	MODE_SWITCH_ASSISTED,
@@ -229,6 +235,8 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
+
+	flighttermination_state_t flighttermination_state;
 };
 
 /**

src/systemcmds/preflight_check/preflight_check.c

@@ -200,7 +200,7 @@ system_eval:
 		led_toggle(leds, LED_BLUE);
 
 		/* display and sound error */
-		for (int i = 0; i < 50; i++)
+		for (int i = 0; i < 14; i++)
 		{
 			led_toggle(leds, LED_BLUE);
 			led_toggle(leds, LED_AMBER);

src/systemcmds/pwm/pwm.c

@@ -210,7 +210,7 @@ pwm_main(int argc, char *argv[])
 		err(1, "PWM_SERVO_GET_COUNT");
 
 	if (!strcmp(argv[1], "arm")) {
-		/* tell IO that its ok to disable its safety with the switch */
+		/* tell safety that its ok to disable it with the switch */
 		ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
 		if (ret != OK)
 			err(1, "PWM_SERVO_SET_ARM_OK");

src/systemcmds/tests/module.mk

@@ -24,9 +24,8 @@ SRCS			 = test_adc.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
 			   test_mixer.cpp \
-			   test_dataman.c \
-			   tests_file.c \
+			   test_file.c \
 			   tests_main.c \
-			   tests_param.c
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+			   test_param.c \
+			   test_ppm_loopback.c \
+			   test_rc.c

src/systemcmds/tests/test_file.c

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file tests_file.c
+ * @file test_file.c
  *
  * File write test.
  */

src/systemcmds/tests/test_param.c

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file tests_param.c
+ * @file test_param.c
  *
  * Tests related to the parameter system.
  */

src/systemcmds/tests/test_ppm_loopback.c

@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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_ppm_loopback.c
+ * Tests the PWM outputs and PPM input
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_rc_input.h>
+#include <uORB/topics/rc_channels.h>
+#include <systemlib/err.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+int test_ppm_loopback(int argc, char *argv[])
+{
+
+	int _rc_sub = orb_subscribe(ORB_ID(input_rc));
+
+	int servo_fd, result;
+	servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
+	servo_position_t pos;
+
+	servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
+
+	if (servo_fd < 0) {
+		printf("failed opening /dev/pwm_servo\n");
+	}
+
+	printf("Servo readback, pairs of values should match defaults\n");
+
+	unsigned servo_count;
+	result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	if (result != OK) {
+		warnx("PWM_SERVO_GET_COUNT");
+		return ERROR;
+	}
+
+	for (unsigned i = 0; i < servo_count; i++) {
+		result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos);
+
+		if (result < 0) {
+			printf("failed reading channel %u\n", i);
+		}
+
+		//printf("%u: %u %u\n", i, pos, data[i]);
+
+	}
+
+	// /* tell safety that its ok to disable it with the switch */
+	// result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0);
+	// if (result != OK)
+	// 	warnx("FAIL: PWM_SERVO_SET_ARM_OK");
+	//  tell output device that the system is armed (it will output values if safety is off) 
+	// result = ioctl(servo_fd, PWM_SERVO_ARM, 0);
+	// if (result != OK)
+	// 	warnx("FAIL: PWM_SERVO_ARM");
+
+	int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400};
+
+
+	// for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+	// 	result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]);
+
+	// 	if (result) {
+	// 		(void)close(servo_fd);
+	// 		return ERROR;
+	// 	} else {
+	// 		warnx("channel %d set to %d", i, pwm_values[i]);
+	// 	}
+	// }
+
+	warnx("servo count: %d", servo_count);
+
+	struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0};
+
+	for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+		pwm_out.values[i] = pwm_values[i];
+		//warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]);
+		pwm_out.channel_count++;
+	}
+
+	result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out);
+
+	/* give driver 10 ms to propagate */
+
+	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
+	struct rc_input_values	rc_input;
+	orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+	usleep(100000);
+
+	/* open PPM input and expect values close to the output values */
+
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
+
+	if (rc_updated) {
+
+		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+		// int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY);
+
+
+
+		// struct rc_input_values rc;
+		// result = read(ppm_fd, &rc, sizeof(rc));
+
+		// if (result != sizeof(rc)) {
+		// 	warnx("Error reading RC output");
+		// 	(void)close(servo_fd);
+		// 	(void)close(ppm_fd);
+		// 	return ERROR;
+		// }
+
+		/* go and check values */
+		for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) {
+			if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) {
+				warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]);
+				(void)close(servo_fd);
+				return ERROR;
+			}
+		}
+	} else {
+		warnx("failed reading RC input data");
+		return ERROR;
+	}
+
+	warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!");
+
+	return 0;
+}

src/systemcmds/tests/test_rc.c

@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 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_ppm_loopback.c
+ * Tests the PWM outputs and PPM input
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_rc_input.h>
+#include <systemlib/err.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+int test_rc(int argc, char *argv[])
+{
+
+	int _rc_sub = orb_subscribe(ORB_ID(input_rc));
+
+	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
+	struct rc_input_values	rc_input;
+	struct rc_input_values	rc_last;
+	orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+	usleep(100000);
+
+	/* open PPM input and expect values close to the output values */
+
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
+
+	warnx("Reading PPM values - press any key to abort");
+	warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes.");
+
+	if (rc_updated) {
+
+		/* copy initial set */
+		for (unsigned i = 0; i < rc_input.channel_count; i++) {
+			rc_last.values[i] = rc_input.values[i];
+		}
+
+		rc_last.channel_count = rc_input.channel_count;
+
+		/* poll descriptor */
+		struct pollfd fds[2];
+		fds[0].fd = _rc_sub;
+		fds[0].events = POLLIN;
+		fds[1].fd = 0;
+		fds[1].events = POLLIN;
+
+		while (true) {
+
+			int ret = poll(fds, 2, 200);
+
+			if (ret > 0) {
+
+				if (fds[0].revents & POLLIN) {
+
+					orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+					/* go and check values */
+					for (unsigned i = 0; i < rc_input.channel_count; i++) {
+						if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) {
+							warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]);
+							(void)close(_rc_sub);
+							return ERROR;
+						}
+
+						rc_last.values[i] = rc_input.values[i];
+					}
+
+					if (rc_last.channel_count != rc_input.channel_count) {
+						warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count);
+						(void)close(_rc_sub);
+						return ERROR;
+					}
+
+					if (hrt_absolute_time() - rc_input.timestamp > 100000) {
+						warnx("TIMEOUT, less than 10 Hz updates");
+						(void)close(_rc_sub);
+						return ERROR;
+					} 
+
+				} else {
+					/* key pressed, bye bye */
+					return 0;
+				}
+
+			}
+		}
+
+	} else {
+		warnx("failed reading RC input data");
+		return ERROR;
+	}
+
+	warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!");
+
+	return 0;
+}

src/systemcmds/tests/test_sensors.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012, 2013 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
@@ -49,6 +48,8 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <debug.h>
+#include <math.h>
+#include <systemlib/err.h>
 
 #include <arch/board/board.h>
 
@@ -77,6 +78,8 @@ static int accel(int argc, char *argv[]);
 static int gyro(int argc, char *argv[]);
 static int mag(int argc, char *argv[]);
 static int baro(int argc, char *argv[]);
+static int accel1(int argc, char *argv[]);
+static int gyro1(int argc, char *argv[]);
 
 /****************************************************************************
  * Private Data
@@ -91,6 +94,8 @@ struct {
 	{"gyro",	"/dev/gyro",	gyro},
 	{"mag",		"/dev/mag",	mag},
 	{"baro",	"/dev/baro",	baro},
+	{"accel1",	"/dev/accel1",	accel1},
+	{"gyro1",	"/dev/gyro1",	gyro1},
 	{NULL, NULL, NULL}
 };
 
@@ -133,23 +138,58 @@ accel(int argc, char *argv[])
 		printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
-	// /* wait at least 10ms, sensor should have data after no more than 2ms */
-	// usleep(100000);
-
-	// ret = read(fd, buf, sizeof(buf));
-
-	// if (ret != sizeof(buf)) {
-	// 	printf("\tMPU-6000: read2 fail (%d)\n", ret);
-	// 	return ERROR;
-
-	// } else {
-	// 	printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
-	// }
-
-	/* XXX more tests here */
+	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
+		warnx("ACCEL1 acceleration values out of range!");
+		return ERROR;
+	}
 
 	/* Let user know everything is ok */
 	printf("\tOK: ACCEL passed all tests successfully\n");
+	close(fd);
+
+	return OK;
+}
+
+static int
+accel1(int argc, char *argv[])
+{
+	printf("\tACCEL1: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct accel_report buf;
+	int		ret;
+
+	fd = open("/dev/accel1", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tACCEL1: open fail, run <mpu6000 start> or <lsm303d start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 100ms, sensor should have data after no more than 20ms */
+	usleep(100000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tACCEL1: read1 fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tACCEL1 accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
+		warnx("ACCEL1 acceleration values out of range!");
+		return ERROR;
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: ACCEL1 passed all tests successfully\n");
+
+	close(fd);
 
 	return OK;
 }
@@ -187,6 +227,45 @@ gyro(int argc, char *argv[])
 
 	/* Let user know everything is ok */
 	printf("\tOK: GYRO passed all tests successfully\n");
+	close(fd);
+
+	return OK;
+}
+
+static int
+gyro1(int argc, char *argv[])
+{
+	printf("\tGYRO1: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct gyro_report buf;
+	int		ret;
+
+	fd = open("/dev/gyro1", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tGYRO1: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tGYRO1: read fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: GYRO1 passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }
@@ -224,6 +303,7 @@ mag(int argc, char *argv[])
 
 	/* Let user know everything is ok */
 	printf("\tOK: MAG passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }
@@ -261,6 +341,7 @@ baro(int argc, char *argv[])
 
 	/* Let user know everything is ok */
 	printf("\tOK: BARO passed all tests successfully\n");
+	close(fd);
 
 	return OK;
 }

src/systemcmds/tests/test_servo.c

@@ -1,7 +1,6 @@
 /****************************************************************************
- * px4/sensors/test_hrt.c
  *
- *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 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
@@ -13,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -32,9 +31,11 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
+/**
+ * @file test_servo.c
+ * Tests the servo outputs
+ *
+ */
 
 #include <nuttx/config.h>
 
@@ -55,39 +56,6 @@
 
 #include "tests.h"
 
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_servo
- ****************************************************************************/
-
 int test_servo(int argc, char *argv[])
 {
 	int fd, result;
@@ -110,7 +78,14 @@ int test_servo(int argc, char *argv[])
 
 	printf("Servo readback, pairs of values should match defaults\n");
 
-	for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
+	unsigned servo_count;
+	result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	if (result != OK) {
+		warnx("PWM_SERVO_GET_COUNT");
+		return ERROR;
+	}
+
+	for (unsigned i = 0; i < servo_count; i++) {
 		result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos);
 
 		if (result < 0) {
@@ -122,11 +97,20 @@ int test_servo(int argc, char *argv[])
 
 	}
 
-	printf("Servos arming at default values\n");
+	/* tell safety that its ok to disable it with the switch */
+	result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (result != OK)
+		warnx("FAIL: PWM_SERVO_SET_ARM_OK");
+	/* tell output device that the system is armed (it will output values if safety is off) */
 	result = ioctl(fd, PWM_SERVO_ARM, 0);
+	if (result != OK)
+		warnx("FAIL: PWM_SERVO_ARM");
+
 	usleep(5000000);
 	printf("Advancing channel 0 to 1500\n");
 	result = ioctl(fd, PWM_SERVO_SET(0), 1500);
+	printf("Advancing channel 1 to 1800\n");
+	result = ioctl(fd, PWM_SERVO_SET(1), 1800);
 out:
 	return 0;
 }

src/systemcmds/tests/test_uart_loopback.c

@@ -1,8 +1,6 @@
 /****************************************************************************
- * px4/sensors/test_gpio.c
  *
- *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *            Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012, 2013 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
@@ -14,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -33,9 +31,11 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
+/**
+ * @file test_uart_loopback.c
+ * Tests the uart outputs
+ *
+ */
 
 #include <nuttx/config.h>
 
@@ -55,40 +55,6 @@
 #include <math.h>
 #include <float.h>
 
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
 int test_uart_loopback(int argc, char *argv[])
 {
 
@@ -97,11 +63,11 @@ int test_uart_loopback(int argc, char *argv[])
 	int uart5_nwrite = 0;
 	int uart2_nwrite = 0;
 
-	int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); //
+	/* opening stdout */
+	int stdout_fd = 1;
 
-	/* assuming NuttShell is on UART1 (/dev/ttyS0) */
-	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); //
-	int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); //
+	int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY);
+	int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY);
 
 	if (uart2 < 0) {
 		printf("ERROR opening UART2, aborting..\n");
@@ -113,7 +79,7 @@ int test_uart_loopback(int argc, char *argv[])
 		exit(uart5);
 	}
 
-	uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
+	uint8_t sample_stdout_fd[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'};
 	uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
 	uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0};
 
@@ -121,7 +87,7 @@ int test_uart_loopback(int argc, char *argv[])
 
 	for (i = 0; i < 1000; i++) {
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		/* uart2 -> uart5 */
 		r = write(uart2, sample_uart2, sizeof(sample_uart2));
@@ -130,7 +96,7 @@ int test_uart_loopback(int argc, char *argv[])
 			uart2_nwrite += r;
 
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		/* uart2 -> uart5 */
 		r = write(uart5, sample_uart5, sizeof(sample_uart5));
@@ -139,7 +105,7 @@ int test_uart_loopback(int argc, char *argv[])
 			uart5_nwrite += r;
 
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		/* try to read back values */
 		do {
@@ -150,7 +116,7 @@ int test_uart_loopback(int argc, char *argv[])
 		} while (r > 0);
 
 //		printf("TEST #%d\n",i);
-		write(uart1, sample_uart1, sizeof(sample_uart1));
+		write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd));
 
 		do {
 			r = read(uart2, sample_uart5, sizeof(sample_uart5));
@@ -160,7 +126,7 @@ int test_uart_loopback(int argc, char *argv[])
 		} while (r > 0);
 
 //		printf("TEST #%d\n",i);
-//		write(uart1, sample_uart1, sizeof(sample_uart5));
+//		write(stdout_fd, sample_stdout_fd, sizeof(sample_uart5));
 	}
 
 	for (i = 0; i < 200000; i++) {
@@ -181,7 +147,7 @@ int test_uart_loopback(int argc, char *argv[])
 	}
 
 
-	close(uart1);
+	close(stdout_fd);
 	close(uart2);
 	close(uart5);
 

src/systemcmds/tests/tests.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 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
@@ -34,6 +34,12 @@
 #ifndef __APPS_PX4_TESTS_H
 #define __APPS_PX4_TESTS_H
 
+/**
+ * @file tests.h
+ * Tests declaration file.
+ *
+ */
+
 /****************************************************************************
  * Included Files
  ****************************************************************************/
@@ -88,6 +94,7 @@ extern int	test_int(int argc, char *argv[]);
 extern int	test_float(int argc, char *argv[]);
 extern int	test_ppm(int argc, char *argv[]);
 extern int	test_servo(int argc, char *argv[]);
+extern int	test_ppm_loopback(int argc, char *argv[]);
 extern int	test_uart_loopback(int argc, char *argv[]);
 extern int	test_uart_baudchange(int argc, char *argv[]);
 extern int	test_cpuload(int argc, char *argv[]);
@@ -101,7 +108,7 @@ extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
-extern int	test_dataman(int argc, char *argv[]);
+extern int	test_rc(int argc, char *argv[]);
 
 __END_DECLS
 

src/systemcmds/tests/tests_main.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012, 2013 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
@@ -35,6 +34,8 @@
 /**
  * @file tests_main.c
  * Tests main file, loads individual tests.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>
@@ -57,14 +58,6 @@
 
 #include "tests.h"
 
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
 /****************************************************************************
  * Private Function Prototypes
  ****************************************************************************/
@@ -94,6 +87,7 @@ const struct {
 	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"ppm_loopback",	test_ppm_loopback,	OPT_NOALLTEST},
 	{"adc",			test_adc,	OPT_NOJIGTEST},
 	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
 	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
@@ -111,8 +105,8 @@ const struct {
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
-	{"dataman",		test_dataman,	OPT_NOALLTEST},
 	{NULL,			NULL, 		0}
 };