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

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	src/modules/commander/gyro_calibration.cpp
	src/modules/mavlink/mavlink_ftp.cpp

ROMFS/px4fmu_common/init.d/10016_3dr_iris

@@ -12,11 +12,11 @@ then
 	# TODO tune roll/pitch separately
 	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.13
-	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.004
 	param set MC_PITCH_P 7.0
 	param set MC_PITCHRATE_P 0.13
-	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_I 0.05
 	param set MC_PITCHRATE_D 0.004
 	param set MC_YAW_P 2.5
 	param set MC_YAWRATE_P 0.25

ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d

@@ -12,11 +12,11 @@ then
 	# TODO tune roll/pitch separately
 	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.13
-	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.004
 	param set MC_PITCH_P 7.0
 	param set MC_PITCHRATE_P 0.13
-	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_I 0.05
 	param set MC_PITCHRATE_D 0.004
 	param set MC_YAW_P 0.5
 	param set MC_YAWRATE_P 0.2

ROMFS/px4fmu_common/init.d/13002_firefly6

@@ -7,6 +7,26 @@
 
 sh /etc/init.d/rc.vtol_defaults
 
+if [ $AUTOCNF == yes ]
+then
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.15
+	param set MC_ROLLRATE_I 0.002
+	param set MC_ROLLRATE_D 0.003
+	param set MC_ROLLRATE_FF 0.0
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.12
+	param set MC_PITCHRATE_I 0.002
+	param set MC_PITCHRATE_D 0.003
+	param set MC_PITCHRATE_FF 0.0
+	param set MC_YAW_P 2.8
+	param set MC_YAW_FF 0.5
+	param set MC_YAWRATE_P 0.22
+	param set MC_YAWRATE_I 0.02
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAWRATE_FF 0.0
+fi
+
 set MIXER firefly6
 set PWM_OUT 12345678
 

ROMFS/px4fmu_common/init.d/4010_dji_f330

@@ -11,11 +11,11 @@ if [ $AUTOCNF == yes ]
 then
 	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.1
-	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.003
 	param set MC_PITCH_P 7.0
 	param set MC_PITCHRATE_P 0.1
-	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_I 0.05
 	param set MC_PITCHRATE_D 0.003
 	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.2

ROMFS/px4fmu_common/init.d/4011_dji_f450

@@ -12,11 +12,11 @@ then
 	# TODO REVIEW
 	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.1
-	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.003
 	param set MC_PITCH_P 7.0
 	param set MC_PITCHRATE_P 0.1
-	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_I 0.05
 	param set MC_PITCHRATE_D 0.003
 	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.2

ROMFS/px4fmu_common/init.d/4012_quad_x_can

@@ -12,11 +12,11 @@ then
 	# TODO REVIEW
 	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.1
-	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.003
 	param set MC_PITCH_P 7.0
 	param set MC_PITCHRATE_P 0.1
-	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_I 0.05
 	param set MC_PITCHRATE_D 0.003
 	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.2

ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb

@@ -13,15 +13,15 @@ if [ $AUTOCNF == yes ]
 then
 	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.1
-	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.003
 	param set MC_PITCH_P 7.0
 	param set MC_PITCHRATE_P 0.1
-	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_I 0.05
 	param set MC_PITCHRATE_D 0.003
 	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
 fi
 

ROMFS/px4fmu_common/init.d/rc.logging

@@ -11,7 +11,7 @@ then
 		then
 		fi
 	else
-		if sdlog2 start -r 200 -a -b 22 -t
+		if sdlog2 start -r 100 -a -b 22 -t
 		then
 		fi
 	fi

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -3,8 +3,13 @@
 # Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers.
 #
 
-ms5611 start
+if ms5611 start
-adc start
+then
+fi
+
+if adc start
+then
+fi
 
 if ver hwcmp PX4FMU_V2
 then

ROMFS/px4fmu_common/init.d/rc.usb

@@ -3,9 +3,9 @@
 # USB MAVLink start
 #
 
-mavlink start -r 30000 -d /dev/ttyACM0 -x
+mavlink start -r 80000 -d /dev/ttyACM0 -x
 # Enable a number of interesting streams we want via USB
-mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200
+mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300
 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10

launch/ardrone.launch

@@ -6,14 +6,14 @@
 </include>
 
 <group ns="$(arg ns)">
-	<param name="MPC_XY_P" type="double" value="1.0" />
+	<param name="MPP_XY_P" type="double" value="1.0" />
-	<param name="MPC_XY_FF" type="double" value="0.1" />
+	<param name="MPP_XY_FF" type="double" value="0.1" />
-	<param name="MPC_XY_VEL_P" type="double" value="0.01" />
+	<param name="MPP_XY_VEL_P" type="double" value="0.01" />
-	<param name="MPC_XY_VEL_I" type="double" value="0.0" />
+	<param name="MPP_XY_VEL_I" type="double" value="0.0" />
-	<param name="MPC_XY_VEL_D" type="double" value="0.01" />
+	<param name="MPP_XY_VEL_D" type="double" value="0.01" />
-	<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
+	<param name="MPP_XY_VEL_MAX" type="double" value="2.0" />
-	<param name="MPC_Z_VEL_P" type="double" value="0.3" />
+	<param name="MPP_Z_VEL_P" type="double" value="0.3" />
-	<param name="MPC_Z_P" type="double" value="2" />
+	<param name="MPP_Z_P" type="double" value="2" />
 	<param name="vehicle_model" type="string" value="ardrone" />
 </group>
 

launch/iris.launch

@@ -7,14 +7,14 @@
 
 <group ns="$(arg ns)">
 	<param name="mixer" type="string" value="i" />
-	<param name="MPC_XY_P" type="double" value="1.0" />
+	<param name="MPP_XY_P" type="double" value="1.0" />
-	<param name="MPC_XY_FF" type="double" value="0.0" />
+	<param name="MPP_XY_FF" type="double" value="0.0" />
-	<param name="MPC_XY_VEL_P" type="double" value="0.01" />
+	<param name="MPP_XY_VEL_P" type="double" value="0.01" />
-	<param name="MPC_XY_VEL_I" type="double" value="0.0" />
+	<param name="MPP_XY_VEL_I" type="double" value="0.0" />
-	<param name="MPC_XY_VEL_D" type="double" value="0.01" />
+	<param name="MPP_XY_VEL_D" type="double" value="0.01" />
-	<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
+	<param name="MPP_XY_VEL_MAX" type="double" value="2.0" />
-	<param name="MPC_Z_VEL_P" type="double" value="0.3" />
+	<param name="MPP_Z_VEL_P" type="double" value="0.3" />
-	<param name="MPC_Z_P" type="double" value="2" />
+	<param name="MPP_Z_P" type="double" value="2" />
 	<param name="vehicle_model" type="string" value="iris" />
 </group>
 

nuttx-configs/px4-stm32f4discovery/include/board.h

@@ -34,8 +34,8 @@
  *
  ************************************************************************************/
 
-#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
+#ifndef __CONFIG_DISCOVERY_INCLUDE_BOARD_H
-#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
+#define __CONFIG_DISCOVERY_INCLUDE_BOARD_H
 
 /************************************************************************************
  * Included Files
@@ -200,6 +200,18 @@
 
 #define GPIO_USART2_RX GPIO_USART2_RX_1
 #define GPIO_USART2_TX GPIO_USART2_TX_1
+
+/* UART6:
+ *
+ * The STM32F4 Discovery has no on-board serial devices, PC6 (TX) and PC7 (RX) 
+ * for connection to an external serial device.
+ */
+
+#define GPIO_USART6_RX GPIO_USART6_RX_1
+#define GPIO_USART6_TX GPIO_USART6_TX_1
+
+/* USART6 require a RX DMA configuration */
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1
  
 /* SPI - There is a MEMS device on SPI1 using these pins: */
 
@@ -270,4 +282,4 @@ EXTERN void stm32_setleds(uint8_t ledset);
 #endif
 
 #endif /* __ASSEMBLY__ */
-#endif  /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */
+#endif  /* __CONFIG_DISCOVERY_INCLUDE_BOARD_H */

nuttx-configs/px4-stm32f4discovery/nsh/defconfig

@@ -91,6 +91,8 @@ CONFIG_ARCH_HAVE_MPU=y
 # ARMV7M Configuration Options
 #
 # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set
+# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set
 CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
 CONFIG_ARMV7M_STACKCHECK=y
 CONFIG_SERIAL_TERMIOS=y
@@ -128,6 +130,7 @@ CONFIG_SERIAL_TERMIOS=y
 # CONFIG_ARCH_CHIP_STM32F100VC is not set
 # CONFIG_ARCH_CHIP_STM32F100VD is not set
 # CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F102CB is not set
 # CONFIG_ARCH_CHIP_STM32F103C4 is not set
 # CONFIG_ARCH_CHIP_STM32F103C8 is not set
 # CONFIG_ARCH_CHIP_STM32F103RET6 is not set
@@ -173,7 +176,6 @@ CONFIG_ARCH_CHIP_STM32F407VG=y
 # CONFIG_STM32_STM32F20XX is not set
 # CONFIG_STM32_STM32F30XX is not set
 CONFIG_STM32_STM32F40XX=y
-# CONFIG_STM32_STM32F427 is not set
 # CONFIG_STM32_DFU is not set
 
 #
@@ -207,9 +209,6 @@ CONFIG_STM32_PWR=y
 CONFIG_STM32_SPI1=y
 # CONFIG_STM32_SPI2 is not set
 # CONFIG_STM32_SPI3 is not set
-# CONFIG_STM32_SPI4 is not set
-# CONFIG_STM32_SPI5 is not set
-# CONFIG_STM32_SPI6 is not set
 CONFIG_STM32_SYSCFG=y
 CONFIG_STM32_TIM1=y
 # CONFIG_STM32_TIM2 is not set
@@ -230,9 +229,7 @@ CONFIG_STM32_USART2=y
 # CONFIG_STM32_USART3 is not set
 # CONFIG_STM32_UART4 is not set
 # CONFIG_STM32_UART5 is not set
-# CONFIG_STM32_USART6 is not set
+CONFIG_STM32_USART6=y
-# CONFIG_STM32_UART7 is not set
-# CONFIG_STM32_UART8 is not set
 # CONFIG_STM32_IWDG is not set
 # CONFIG_STM32_WWDG is not set
 CONFIG_STM32_ADC=y
@@ -268,6 +265,8 @@ CONFIG_STM32_USART=y
 #
 # CONFIG_USART2_RS485 is not set
 CONFIG_USART2_RXDMA=y
+# CONFIG_USART6_RS485 is not set
+CONFIG_USART6_RXDMA=y
 CONFIG_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32_USART_SINGLEWIRE=y
 
@@ -336,8 +335,10 @@ CONFIG_BOOT_RUNFROMFLASH=y
 #
 # Board Selection
 #
-CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_STM32F4_DISCOVERY=y
-CONFIG_ARCH_BOARD=""
+# CONFIG_ARCH_BOARD_MIKROE_STM32F4 is not set
+# CONFIG_ARCH_BOARD_CUSTOM is not set
+CONFIG_ARCH_BOARD="stm32f4discovery"
 
 #
 # Common Board Options
@@ -431,7 +432,7 @@ CONFIG_SPI=y
 # CONFIG_SPI_OWNBUS is not set
 CONFIG_SPI_EXCHANGE=y
 # CONFIG_SPI_CMDDATA is not set
-# CONFIG_RTC is not set
+# CONFIG_RTC= is not set
 CONFIG_WATCHDOG=y
 # CONFIG_ANALOG is not set
 # CONFIG_AUDIO_DEVICES is not set
@@ -449,10 +450,12 @@ CONFIG_SERIAL=y
 CONFIG_SERIAL_REMOVABLE=y
 # CONFIG_16550_UART is not set
 CONFIG_ARCH_HAVE_USART2=y
+CONFIG_ARCH_HAVE_USART6=y
 CONFIG_MCU_SERIAL=y
 CONFIG_STANDARD_SERIAL=y
 CONFIG_SERIAL_NPOLLWAITERS=2
 CONFIG_USART2_SERIAL_CONSOLE=y
+# CONFIG_USART6_SERIAL_CONSOLE is not set
 # CONFIG_NO_SERIAL_CONSOLE is not set
 
 #
@@ -466,6 +469,18 @@ CONFIG_USART2_PARITY=0
 CONFIG_USART2_2STOP=0
 # CONFIG_USART2_IFLOWCONTROL is not set
 # CONFIG_USART2_OFLOWCONTROL is not set
+
+#
+# USART6 Configuration
+#
+CONFIG_USART6_RXBUFSIZE=512
+CONFIG_USART6_TXBUFSIZE=512
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_BITS=8
+CONFIG_USART6_PARITY=0
+CONFIG_USART6_2STOP=0
+# CONFIG_USART6_IFLOWCONTROL is not set
+# CONFIG_USART6_OFLOWCONTROL is not set
 # CONFIG_SERIAL_IFLOWCONTROL is not set
 # CONFIG_SERIAL_OFLOWCONTROL is not set
 CONFIG_USBDEV=y

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -663,8 +663,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512
 CONFIG_CDCACM_NWRREQS=4
 CONFIG_CDCACM_NRDREQS=4
 CONFIG_CDCACM_BULKIN_REQLEN=96
-CONFIG_CDCACM_RXBUFSIZE=512
+CONFIG_CDCACM_RXBUFSIZE=1000
-CONFIG_CDCACM_TXBUFSIZE=2048
+CONFIG_CDCACM_TXBUFSIZE=8000
 CONFIG_CDCACM_VENDORID=0x26ac
 CONFIG_CDCACM_PRODUCTID=0x0011
 CONFIG_CDCACM_VENDORSTR="3D Robotics"

src/drivers/boards/px4-stm32f4discovery/board_config.h

@@ -60,9 +60,10 @@ __BEGIN_DECLS
  ****************************************************************************************************/
 /* Configuration ************************************************************************************/
 
-/* PX4FMU GPIOs ***********************************************************************************/
+/* PX4-STM32F4Discovery GPIOs ***********************************************************************************/
-/* LEDs */
 /* LEDs */
+// LED1 green, LED2 orange, LED3 red, LED4 blue
+
 
 #define GPIO_LED1       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)

src/drivers/l3gd20/l3gd20.cpp

@@ -189,6 +189,14 @@ static const int ERROR = -1;
 #define SENSOR_BOARD_ROTATION_DEFAULT		SENSOR_BOARD_ROTATION_270_DEG
 #endif
 
+/*
+  we set the timer interrupt to run a bit faster than the desired
+  sample rate and then throw away duplicates using the data ready bit.
+  This time reduction is enough to cope with worst case timing jitter
+  due to other timers
+ */
+#define L3GD20_TIMER_REDUCTION				200
+
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 
 class L3GD20 : public device::SPI
@@ -236,9 +244,9 @@ private:
 	unsigned		_read;
 
 	perf_counter_t		_sample_perf;
-	perf_counter_t		_reschedules;
 	perf_counter_t		_errors;
 	perf_counter_t		_bad_registers;
+	perf_counter_t		_duplicates;
 
 	uint8_t			_register_wait;
 
@@ -410,9 +418,9 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
 	_read(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
-	_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
 	_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
 	_bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")),
+	_duplicates(perf_alloc(PC_COUNT, "l3gd20_duplicates")),
 	_register_wait(0),
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
@@ -449,9 +457,9 @@ L3GD20::~L3GD20()
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
-	perf_free(_reschedules);
 	perf_free(_errors);
 	perf_free(_bad_registers);
+	perf_free(_duplicates);
 }
 
 int
@@ -608,7 +616,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
-					_call.period = _call_interval = ticks;
+					_call_interval = ticks;
+
+                                        _call.period = _call_interval - L3GD20_TIMER_REDUCTION;
 
 					/* adjust filters */
 					float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
@@ -834,7 +844,10 @@ L3GD20::start()
 	_reports->flush();
 
 	/* start polling at the specified rate */
-	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
+	hrt_call_every(&_call,
+                       1000,
+                       _call_interval - L3GD20_TIMER_REDUCTION,
+                       (hrt_callout)&L3GD20::measure_trampoline, this);
 }
 
 void
@@ -899,12 +912,6 @@ 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::check_registers(void)
 {
@@ -954,33 +961,17 @@ L3GD20::measure()
 
         check_registers();
 
-#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 (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
-		perf_count(_reschedules);
-		hrt_call_delay(&_call, 100);
-                perf_end(_sample_perf);
-		return;
-	}
-#endif
-
 	/* 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 & STATUS_ZYXDA)) {
-        if (_bus == PX4_SPI_BUS_SENSORS && (raw_report.status & 0xF) != 0xF) {
+		perf_end(_sample_perf);
-            /*
+		perf_count(_duplicates);
-              we waited for DRDY, but did not see DRDY on all axes
+		return;
-              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)
@@ -1071,9 +1062,9 @@ L3GD20::print_info()
 {
 	printf("gyro reads:          %u\n", _read);
 	perf_print_counter(_sample_perf);
-	perf_print_counter(_reschedules);
 	perf_print_counter(_errors);
 	perf_print_counter(_bad_registers);
+	perf_print_counter(_duplicates);
 	_reports->print_info("report queue");
         ::printf("checked_next: %u\n", _checked_next);
         for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {

src/drivers/lsm303d/lsm303d.cpp

@@ -196,6 +196,7 @@ static const int ERROR = -1;
 
 #define REG7_CONT_MODE_M		((0<<1) | (0<<0))
 
+#define REG_STATUS_A_NEW_ZYXADA		0x08
 
 #define INT_CTRL_M              0x12
 #define INT_SRC_M               0x13
@@ -217,6 +218,14 @@ static const int ERROR = -1;
 #define EXTERNAL_BUS 0
 #endif
 
+/*
+  we set the timer interrupt to run a bit faster than the desired
+  sample rate and then throw away duplicates using the data ready bit.
+  This time reduction is enough to cope with worst case timing jitter
+  due to other timers
+ */
+#define LSM303D_TIMER_REDUCTION				200
+
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
 
@@ -289,9 +298,9 @@ private:
 
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
-	perf_counter_t		_accel_reschedules;
 	perf_counter_t		_bad_registers;
 	perf_counter_t		_bad_values;
+	perf_counter_t		_accel_duplicates;
 
 	uint8_t			_register_wait;
 
@@ -561,9 +570,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
-	_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
 	_bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")),
 	_bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")),
+	_accel_duplicates(perf_alloc(PC_COUNT, "lsm303d_accel_duplicates")),
 	_register_wait(0),
 	_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),
@@ -622,7 +631,7 @@ LSM303D::~LSM303D()
 	perf_free(_mag_sample_perf);
 	perf_free(_bad_registers);
 	perf_free(_bad_values);
-	perf_free(_accel_reschedules);
+	perf_free(_accel_duplicates);
 }
 
 int
@@ -874,7 +883,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* update interval for next measurement */
 				/* XXX this is a bit shady, but no other way to adjust... */
-				_accel_call.period = _call_accel_interval = ticks;
+				_call_accel_interval = ticks;
+
+                                _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION;
 
 				/* if we need to start the poll state machine, do it */
 				if (want_start)
@@ -1388,7 +1399,10 @@ LSM303D::start()
 	_mag_reports->flush();
 
 	/* start polling at the specified rate */
-	hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
+	hrt_call_every(&_accel_call, 
+                       1000,
+                       _call_accel_interval - LSM303D_TIMER_REDUCTION, 
+                       (hrt_callout)&LSM303D::measure_trampoline, this);
 	hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
 }
 
@@ -1466,20 +1480,6 @@ LSM303D::measure()
 
 	check_registers();
 
-	// 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.
-	// Note that DRDY is not available when the lsm303d is
-	// connected on the external bus
-#ifdef GPIO_EXTI_ACCEL_DRDY
-	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
-		perf_count(_accel_reschedules);
-		hrt_call_delay(&_accel_call, 100);
-                perf_end(_accel_sample_perf);
-		return;
-	}
-#endif
-
 	if (_register_wait != 0) {
 		// we are waiting for some good transfers before using
 		// the sensor again.
@@ -1493,6 +1493,12 @@ LSM303D::measure()
 	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));
 
+        if (!(raw_accel_report.status & REG_STATUS_A_NEW_ZYXADA)) {
+		perf_end(_accel_sample_perf);
+		perf_count(_accel_duplicates);
+		return;
+        }
+
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
 	 * 2) Subtract static offset (in SI units)
@@ -1681,7 +1687,7 @@ LSM303D::print_info()
 	perf_print_counter(_mag_sample_perf);
 	perf_print_counter(_bad_registers);
 	perf_print_counter(_bad_values);
-	perf_print_counter(_accel_reschedules);
+	perf_print_counter(_accel_duplicates);
 	_accel_reports->print_info("accel reports");
 	_mag_reports->print_info("mag reports");
         ::printf("checked_next: %u\n", _checked_next);

src/drivers/mpu6000/mpu6000.cpp

@@ -189,6 +189,14 @@
 #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 */
 
+/*
+  we set the timer interrupt to run a bit faster than the desired
+  sample rate and then throw away duplicates by comparing
+  accelerometer values. This time reduction is enough to cope with
+  worst case timing jitter due to other timers
+ */
+#define MPU6000_TIMER_REDUCTION				200
+
 class MPU6000_gyro;
 
 class MPU6000 : public device::SPI
@@ -257,6 +265,7 @@ private:
 	perf_counter_t		_bad_registers;
 	perf_counter_t		_good_transfers;
 	perf_counter_t		_reset_retries;
+	perf_counter_t		_duplicates;
 	perf_counter_t		_system_latency_perf;
 	perf_counter_t		_controller_latency_perf;
 
@@ -287,6 +296,10 @@ private:
 	// last temperature reading for print_info()
 	float			_last_temperature;
 
+	// keep last accel reading for duplicate detection
+	uint16_t		_last_accel[3];
+	bool			_got_duplicate;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -509,6 +522,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")),
 	_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
 	_reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")),
+	_duplicates(perf_alloc(PC_COUNT, "mpu6000_duplicates")),
 	_system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")),
 	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
 	_register_wait(0),
@@ -522,7 +536,9 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_rotation(rotation),
 	_checked_next(0),
 	_in_factory_test(false),
-	_last_temperature(0)
+	_last_temperature(0),
+	_last_accel{},
+	_got_duplicate(false)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -576,6 +592,8 @@ MPU6000::~MPU6000()
 	perf_free(_bad_transfers);
 	perf_free(_bad_registers);
 	perf_free(_good_transfers);
+	perf_free(_reset_retries);
+	perf_free(_duplicates);
 }
 
 int
@@ -1198,7 +1216,15 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
-					_call.period = _call_interval = ticks;
+					_call_interval = ticks;
+
+                                        /*
+                                          set call interval faster then the sample time. We
+                                          then detect when we have duplicate samples and reject
+                                          them. This prevents aliasing due to a beat between the
+                                          stm32 clock and the mpu6000 clock
+                                         */
+                                        _call.period = _call_interval - MPU6000_TIMER_REDUCTION;
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
@@ -1476,7 +1502,10 @@ MPU6000::start()
 	_gyro_reports->flush();
 
 	/* start polling at the specified rate */
-	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
+	hrt_call_every(&_call,
+                       1000,
+                       _call_interval-MPU6000_TIMER_REDUCTION,
+                       (hrt_callout)&MPU6000::measure_trampoline, this);
 }
 
 void
@@ -1578,14 +1607,32 @@ MPU6000::measure()
 	 */
 	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
 
-	check_registers();
-
         // 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;
 
+        check_registers();
+
+	/*
+	   see if this is duplicate accelerometer data. Note that we
+	   can't use the data ready interrupt status bit in the status
+	   register as that also goes high on new gyro data, and when
+	   we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
+	   sampled at 8kHz, so we would incorrectly think we have new
+	   data when we are in fact getting duplicate accelerometer data.
+	*/
+        if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) {
+		// it isn't new data - wait for next timer
+		perf_end(_sample_perf);
+		perf_count(_duplicates);
+		_got_duplicate = true;
+		return;
+	}
+	memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6);
+	_got_duplicate = false;
+
 	/*
 	 * Convert from big to little endian
 	 */
@@ -1766,6 +1813,7 @@ MPU6000::print_info()
 	perf_print_counter(_bad_registers);
 	perf_print_counter(_good_transfers);
 	perf_print_counter(_reset_retries);
+	perf_print_counter(_duplicates);
 	_accel_reports->print_info("accel queue");
 	_gyro_reports->print_info("gyro queue");
         ::printf("checked_next: %u\n", _checked_next);

src/drivers/px4io/px4io.cpp

@@ -909,6 +909,9 @@ PX4IO::task_main()
 		goto out;
 	}
 
+	/* Fetch initial flight termination circuit breaker state */
+	_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
+
 	/* poll descriptor */
 	pollfd fds[1];
 	fds[0].fd = _t_actuator_controls_0;
@@ -1079,7 +1082,7 @@ PX4IO::task_main()
 					}
 				}
 
-				/* Update Circuit breakers */
+				/* Check if the flight termination circuit breaker has been updated */
 				_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
 
 				param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan);

src/lib/ecl/attitude_fw/ecl_controller.cpp

@@ -132,9 +132,9 @@ float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float m
 	float airspeed_result = airspeed;
 	if (!isfinite(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
-		airspeed = 0.5f * (minspeed + maxspeed);
+		airspeed_result = 0.5f * (minspeed + maxspeed);
 	} else if (airspeed < minspeed) {
-		airspeed = minspeed;
+		airspeed_result = minspeed;
 	}
 	return airspeed_result;
 }

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -397,16 +397,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 					hrt_abstime vel_t = 0;
 					bool vel_valid = false;
-					if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
+					if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
-						vel_valid = true;
-						if (gps_updated) {
-							vel_t = gps.timestamp_velocity;
-							vel(0) = gps.vel_n_m_s;
-							vel(1) = gps.vel_e_m_s;
-							vel(2) = gps.vel_d_m_s;
-						}
-
-					} else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
 						vel_valid = true;
 						if (global_pos_updated) {
 							vel_t = global_pos.timestamp;
@@ -487,8 +478,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
 							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
 
-						} else {
-							mag_decl = ekf_params.mag_decl;
 						}
 
 						/* update mag declination rotation matrix */

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -108,11 +108,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
  */
 PARAM_DEFINE_INT32(EKF_ATT_ENABLED, 1);
 
-/* magnetic declination, in degrees */
-PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
-
-PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
-
 /**
  * Moment of inertia matrix diagonal entry (1, 1)
  *
@@ -160,10 +155,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->r1 	=	param_find("EKF_ATT_V4_R1");
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
 
-	h->mag_decl   =	param_find("ATT_MAG_DECL");
-
-	h->acc_comp   =	param_find("ATT_ACC_COMP");
-
 	h->moment_inertia_J[0]  =   param_find("ATT_J11");
 	h->moment_inertia_J[1]  =   param_find("ATT_J22");
 	h->moment_inertia_J[2]  =   param_find("ATT_J33");
@@ -183,11 +174,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->r1, &(p->r[1]));
 	param_get(h->r2, &(p->r[2]));
 
-	param_get(h->mag_decl, &(p->mag_decl));
-	p->mag_decl *= M_PI_F / 180.0f;
-
-	param_get(h->acc_comp, &(p->acc_comp));
-
 	for (int i = 0; i < 3; i++) {
 		param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
 	}

src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -41,10 +41,70 @@
 
 #include <systemlib/param/param.h>
 
+/**
+ * Complimentary filter accelerometer weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
 PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
+
+/**
+ * Complimentary filter magnetometer weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
 PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
+
+/**
+ * Complimentary filter gyroscope bias weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
 PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
-PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);		///< magnetic declination, in degrees
+
-PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);		///< automatic GPS based magnetic declination
+/**
-PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);		///< acceleration compensation
+ * Magnetic declination, in degrees
-PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);		///< gyro bias limit, rad/s
+ *
+ * This parameter is not used in normal operation,
+ * as the declination is looked up based on the
+ * GPS coordinates of the vehicle.
+ *
+ * @group Attitude Q estimator
+ * @unit degrees
+ */
+PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
+
+/**
+ * Enable automatic GPS based declination compensation
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
+
+/**
+ * Enable acceleration compensation based on GPS
+ * velocity.
+ *
+ * @group Attitude Q estimator
+ * @min 1
+ * @max 2
+ */
+PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
+
+/**
+ * Gyro bias limit
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 2
+ * @unit rad/s
+ */
+PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);

src/modules/commander/commander.cpp

@@ -2164,10 +2164,12 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 
 		if (set_normal_color) {
 			/* set color */
-			if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
+			if (status_local->failsafe) {
+				rgbled_set_color(RGBLED_COLOR_PURPLE);
+			} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
-				/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
+			} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
-
+				rgbled_set_color(RGBLED_COLOR_RED);
 			} else {
 				if (status_local->condition_global_position_valid) {
 					rgbled_set_color(RGBLED_COLOR_GREEN);
@@ -2801,7 +2803,7 @@ void *commander_low_prio_loop(void *arg)
 								need_param_autosave_timeout = 0;
 							}
 
-							mavlink_log_info(mavlink_fd, "settings saved");
+							/* do not spam MAVLink, but provide the answer / green led mechanism */
 							answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 						} else {

src/modules/commander/gyro_calibration.cpp

@@ -77,7 +77,7 @@ typedef struct  {
 	struct gyro_report	gyro_report_0;
 } gyro_worker_data_t;
 
-static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
+static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 {
 	gyro_worker_data_t*	worker_data = (gyro_worker_data_t*)(data);
 	unsigned		calibration_counter[max_gyros] = { 0 };
@@ -92,6 +92,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient
 	}
 	
 	memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
+	memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale));
 	
 	/* use first gyro to pace, but count correctly per-gyro for statistics */
 	while (calibration_counter[0] < calibration_count) {
@@ -152,7 +153,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient
 int do_gyro_calibration(int mavlink_fd)
 {
 	int			res = OK;
-	gyro_worker_data_t	worker_data;
+	gyro_worker_data_t	worker_data = {};
 
 	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
@@ -199,51 +200,63 @@ int do_gyro_calibration(int mavlink_fd)
 	for (unsigned s = 0; s < max_gyros; s++) {
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
 	}
+
+	int cancel_sub  = calibrate_cancel_subscribe();
+
+	unsigned try_count = 0;
+	unsigned max_tries = 20;
+	res = ERROR;
 	
-	// Calibrate right-side up
+	do {
-	
+		// Calibrate gyro and ensure user didn't move
-	bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
+		calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data);
-	
+
-	int cancel_sub  = calibrate_cancel_subscribe();	
+		if (cal_return == calibrate_return_cancelled) {
-	calibrate_return cal_return = calibrate_from_orientation(mavlink_fd,                 // Mavlink fd to write output
+			// Cancel message already sent, we are done here
-								cancel_sub,                 // Subscription to vehicle_command for cancel support
+			res = ERROR;
-								side_collected,		// Sides to calibrate
+			break;
-								gyro_calibration_worker,     // Calibration worker
+
-								&worker_data,		// Opaque data for calibration worked
+		} else if (cal_return == calibrate_return_error) {
-								true);			// true: lenient still detection
+			res = ERROR;
+
+		} else {
+			/* check offsets */
+			float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
+			float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
+			float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
+
+			/* maximum allowable calibration error in radians */
+			const float maxoff = 0.0055f;
+
+			if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
+			    !isfinite(worker_data.gyro_scale[0].y_offset) ||
+			    !isfinite(worker_data.gyro_scale[0].z_offset) ||
+			    fabsf(xdiff) > maxoff ||
+			    fabsf(ydiff) > maxoff ||
+			    fabsf(zdiff) > maxoff) {
+
+				mavlink_and_console_log_critical(mavlink_fd, "[cal] motion, retrying..");
+				res = ERROR;
+
+			} else {
+				res = OK;
+			}
+		}
+		try_count++;
+
+	} while (res == ERROR && try_count <= max_tries);
+
+	if (try_count >= max_tries) {
+		mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
+		res = ERROR;
+	}
+
 	calibrate_cancel_unsubscribe(cancel_sub);
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
 		px4_close(worker_data.gyro_sensor_sub[s]);
 	}
 
-	if (cal_return == calibrate_return_cancelled) {
-		// Cancel message already sent, we are done here
-		return ERROR;
-	} else if (cal_return == calibrate_return_error) {
-		res = ERROR;
-	}
-		
-	if (res == OK) {
-		/* check offsets */
-		float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
-		float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
-		float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
-
-		/* maximum allowable calibration error in radians */
-		const float maxoff = 0.0055f;
-
-		if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
-		    !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||
-		    !PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) ||
-		    fabsf(xdiff) > maxoff ||
-		    fabsf(ydiff) > maxoff ||
-		    fabsf(zdiff) > maxoff) {
-			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
-			res = ERROR;
-		}
-	}
-
 	if (res == OK) {
 		/* set offset parameters to new values */
 		bool failed = false;

src/modules/commander/mag_calibration.cpp

@@ -53,6 +53,7 @@
 #include <fcntl.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_mag.h>
 #include <mavlink/mavlink_log.h>
@@ -196,7 +197,71 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 	
 	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
 	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
-	sleep(2);
+
+	/*
+	 * Detect if the system is rotating.
+	 *
+	 * We're detecting this as a general rotation on any axis, not necessary on the one we
+	 * asked the user for. This is because we really just need two roughly orthogonal axes
+	 * for a good result, so we're not constraining the user more than we have to.
+	 */
+
+	hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
+	hrt_abstime last_gyro = 0;
+	float gyro_x_integral = 0.0f;
+	float gyro_y_integral = 0.0f;
+	float gyro_z_integral = 0.0f;
+
+	const float gyro_int_thresh_rad = 0.5f;
+
+	int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+
+	while (fabsf(gyro_x_integral) < gyro_int_thresh_rad &&
+		fabsf(gyro_y_integral) < gyro_int_thresh_rad &&
+		fabsf(gyro_z_integral) < gyro_int_thresh_rad) {
+
+		/* abort on request */
+		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
+			result = calibrate_return_cancelled;
+			close(sub_gyro);
+			return result;
+		}
+
+		/* abort with timeout */
+		if (hrt_absolute_time() > detection_deadline) {
+			result = calibrate_return_error;
+			warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
+			mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation.");
+			break;
+		}
+
+		/* Wait clocking for new data on all gyro */
+		struct pollfd fds[1];
+		fds[0].fd = sub_gyro;
+		fds[0].events = POLLIN;
+		size_t fd_count = 1;
+
+		int poll_ret = poll(fds, fd_count, 1000);
+
+		if (poll_ret > 0) {
+			struct gyro_report gyro;
+			orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro);
+
+			/* ensure we have a valid first timestamp */
+			if (last_gyro > 0) {
+
+				/* integrate */
+				float delta_t = (gyro.timestamp - last_gyro) / 1e6f;
+				gyro_x_integral += gyro.x * delta_t;
+				gyro_y_integral += gyro.y * delta_t;
+				gyro_z_integral += gyro.z * delta_t;
+			}
+
+			last_gyro = gyro.timestamp;
+		}
+	}
+
+	close(sub_gyro);
 	
 	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
 	unsigned poll_errcount = 0;

src/modules/mavlink/mavlink_ftp.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 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
@@ -301,23 +301,32 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workList(PayloadHeader* payload)
 {
-    char dirPath[kMaxDataLength];
+	char dirPath[kMaxDataLength];
-    strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
+	strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
-    
+
+	ErrorCode errorCode = kErrNone;
+	unsigned offset = 0;
+
 	DIR *dp = opendir(dirPath);
 
 	if (dp == nullptr) {
-		warnx("FTP: can't open path '%s'", dirPath);
+		_mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)");
-		return kErrFailErrno;
+		_mavlink->send_statustext_critical(dirPath);
+		// this is not an FTP error, abort directory read and continue
+
+		payload->data[offset++] = kDirentSkip;
+		*((char *)&payload->data[offset]) = '\0';
+		offset++;
+		payload->size = offset;
+
+		return errorCode;
 	}
-    
+
 #ifdef MAVLINK_FTP_DEBUG
 	warnx("FTP: list %s offset %d", dirPath, payload->offset);
 #endif
 
-	ErrorCode errorCode = kErrNone;
 	struct dirent entry, *result = nullptr;
-	unsigned offset = 0;
 
 	// move to the requested offset
 	seekdir(dp, payload->offset);
@@ -325,9 +334,16 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 	for (;;) {
 		// read the directory entry
 		if (readdir_r(dp, &entry, &result)) {
-			warnx("FTP: list %s readdir_r failure\n", dirPath);
+			_mavlink->send_statustext_critical("FTP: list readdir_r failure");
-			errorCode = kErrFailErrno;
+			_mavlink->send_statustext_critical(dirPath);
-			break;
+
+			payload->data[offset++] = kDirentSkip;
+			*((char *)&payload->data[offset]) = '\0';
+			offset++;
+			payload->size = offset;
+			closedir(dp);
+
+			return errorCode;
 		}
 
 		// no more entries?
@@ -365,7 +381,8 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 #else
 		case DT_DIR:
 #endif
-			if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
+			// XXX @DonLakeFlyer: Remove the first condition for the test setup
+			if ((entry.d_name[0] == '.') || strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
 				// Don't bother sending these back
 				direntType = kDirentSkip;
 			} else {
@@ -499,6 +516,7 @@ MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id)
 	// Setup for streaming sends
 	_session_info.stream_download = true;
 	_session_info.stream_offset = payload->offset;
+	_session_info.stream_chunk_transmitted = 0;
 	_session_info.stream_seq_number = payload->seq_number + 1;
 	_session_info.stream_target_system_id = target_system_id;
 
@@ -885,6 +903,7 @@ void MavlinkFTP::send(const hrt_abstime t)
 			} else {
 				payload->size = bytes_read;
 				_session_info.stream_offset += bytes_read;
+				_session_info.stream_chunk_transmitted += bytes_read;
 			}
 		}
 		
@@ -903,8 +922,12 @@ void MavlinkFTP::send(const hrt_abstime t)
 #ifndef MAVLINK_FTP_UNIT_TEST
 			if (max_bytes_to_send < (get_size()*2)) {
 				more_data = false;
-				payload->burst_complete = true;
+				/* perform transfers in 35K chunks - this is determined empirical */
-				_session_info.stream_download = false;
+				if (_session_info.stream_chunk_transmitted > 35000) {
+					payload->burst_complete = true;
+					_session_info.stream_download = false;
+					_session_info.stream_chunk_transmitted = 0;
+				}
 			} else {
 #endif
 				more_data = true;

src/modules/mavlink/mavlink_ftp.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 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
@@ -165,6 +165,7 @@ private:
 		uint32_t	stream_offset;
 		uint16_t	stream_seq_number;
 		uint8_t		stream_target_system_id;
+		unsigned	stream_chunk_transmitted;
 	};
 	struct SessionInfo _session_info;	///< Session info, fd=-1 for no active session
 	

src/modules/mavlink/mavlink_main.cpp

@@ -93,7 +93,7 @@
 static const int ERROR = -1;
 
 #define DEFAULT_DEVICE_NAME			"/dev/ttyS1"
-#define MAX_DATA_RATE				60000	///< max data rate in bytes/s
+#define MAX_DATA_RATE				1000000	///< max data rate in bytes/s
 #define MAIN_LOOP_DELAY 			10000	///< 100 Hz @ 1000 bytes/s data rate
 #define FLOW_CONTROL_DISABLE_THRESHOLD		40	///< picked so that some messages still would fit it.
 
@@ -262,7 +262,9 @@ Mavlink::~Mavlink()
 		} while (_task_running);
 	}
 
-	LL_DELETE(_mavlink_instances, this);
+	if (_mavlink_instances) {
+		LL_DELETE(_mavlink_instances, this);
+	}
 }
 
 void
@@ -1025,7 +1027,7 @@ void
 Mavlink::adjust_stream_rates(const float multiplier)
 {
 	/* do not allow to push us to zero */
-	if (multiplier < 0.01f) {
+	if (multiplier < 0.0005f) {
 		return;
 	}
 
@@ -1036,9 +1038,9 @@ Mavlink::adjust_stream_rates(const float multiplier)
 		unsigned interval = stream->get_interval();
 		interval /= multiplier;
 
-		/* allow max ~600 Hz */
+		/* allow max ~2000 Hz */
 		if (interval < 1600) {
-			interval = 1600;
+			interval = 500;
 		}
 
 		/* set new interval */
@@ -1421,7 +1423,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* MAVLINK_FTP stream */
 	_mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this);
-	_mavlink_ftp->set_interval(interval_from_rate(120.0f));
+	_mavlink_ftp->set_interval(interval_from_rate(80.0f));
 	LL_APPEND(_streams, _mavlink_ftp);
 	
 	/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
@@ -1471,7 +1473,7 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	/* set main loop delay depending on data rate to minimize CPU overhead */
-	_main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate;
+	_main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;
 
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);

src/modules/mavlink/mavlink_parameters.cpp

@@ -181,8 +181,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 void
 MavlinkParametersManager::send(const hrt_abstime t)
 {
-	/* send all parameters if requested */
+	/* send all parameters if requested, but only after the system has booted */
-	if (_send_all_index >= 0) {
+	if (_send_all_index >= 0 && t > 4 * 1000 * 1000) {
 
 		/* skip if no space is available */
 		if (_mavlink->get_free_tx_buf() < get_size()) {

src/modules/mavlink/mavlink_receiver.cpp

@@ -131,6 +131,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_hil_local_alt0(0.0f),
 	_hil_local_proj_ref{},
 	_offboard_control_mode{},
+	_att_sp{},
 	_rates_sp{},
 	_time_offset_avg_alpha(0.6),
 	_time_offset(0)
@@ -780,16 +781,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 		 * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
 		 * body rates to keep the controllers running
 		 */
-		bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
+		bool ignore_bodyrate_msg = (bool)(set_attitude_target.type_mask & 0x7);
-		bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
+		bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7));
 
-		if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) {
+		if (ignore_bodyrate_msg && ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) {
 			/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
-			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate;
+			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate;
-			_offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude;
+			_offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude;
 		} else {
-			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
+			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg;
-			_offboard_control_mode.ignore_attitude = ignore_attitude;
+			_offboard_control_mode.ignore_attitude = ignore_attitude_msg;
 		}
 
 		_offboard_control_mode.ignore_position = true;
@@ -818,22 +819,25 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 
 				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
 				if (!(_offboard_control_mode.ignore_attitude)) {
-					struct vehicle_attitude_setpoint_s att_sp = {};
+					_att_sp.timestamp = hrt_absolute_time();
-					att_sp.timestamp = hrt_absolute_time();
+					if (!ignore_attitude_msg) { // only copy att sp if message contained new data
-					mavlink_quaternion_to_euler(set_attitude_target.q,
+						mavlink_quaternion_to_euler(set_attitude_target.q,
-							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
+								&_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body);
-					mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
+						mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body);
-					att_sp.R_valid = true;
+						_att_sp.R_valid = true;
-					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
+						_att_sp.yaw_sp_move_rate = 0.0;
-						att_sp.thrust = set_attitude_target.thrust;
+						memcpy(_att_sp.q_d, set_attitude_target.q, sizeof(_att_sp.q_d));
+						_att_sp.q_d_valid = true;
 					}
-					att_sp.yaw_sp_move_rate = 0.0;
+
-					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
+					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
-					att_sp.q_d_valid = true;
+						_att_sp.thrust = set_attitude_target.thrust;
+					}
+
 					if (_att_sp_pub < 0) {
-						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
 					} else {
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
 					}
 				}
 
@@ -841,9 +845,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 				///XXX add support for ignoring individual axes
 				if (!(_offboard_control_mode.ignore_bodyrate)) {
 					_rates_sp.timestamp = hrt_absolute_time();
-					_rates_sp.roll = set_attitude_target.body_roll_rate;
+					if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data
-					_rates_sp.pitch = set_attitude_target.body_pitch_rate;
+						_rates_sp.roll = set_attitude_target.body_roll_rate;
-					_rates_sp.yaw = set_attitude_target.body_yaw_rate;
+						_rates_sp.pitch = set_attitude_target.body_pitch_rate;
+						_rates_sp.yaw = set_attitude_target.body_yaw_rate;
+					}
 					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
 						_rates_sp.thrust = set_attitude_target.thrust;
 					}
@@ -1356,8 +1362,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		memset(&hil_global_pos, 0, sizeof(hil_global_pos));
 
 		hil_global_pos.timestamp = timestamp;
-		hil_global_pos.lat = hil_state.lat;
+		hil_global_pos.lat = hil_state.lat / ((double)1e7);
-		hil_global_pos.lon = hil_state.lon;
+		hil_global_pos.lon = hil_state.lon / ((double)1e7);
 		hil_global_pos.alt = hil_state.alt / 1000.0f;
 		hil_global_pos.vel_n = hil_state.vx / 100.0f;
 		hil_global_pos.vel_e = hil_state.vy / 100.0f;
@@ -1581,7 +1587,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	param.sched_priority = SCHED_PRIORITY_MAX - 80;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
-	pthread_attr_setstacksize(&receiveloop_attr, 1800);
+	pthread_attr_setstacksize(&receiveloop_attr, 2100);
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
 

src/modules/mavlink/mavlink_receiver.h

@@ -187,6 +187,7 @@ private:
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
 	struct offboard_control_mode_s _offboard_control_mode;
+	struct vehicle_attitude_setpoint_s _att_sp;
 	struct vehicle_rates_setpoint_s _rates_sp;
 	double _time_offset_avg_alpha;
 	uint64_t _time_offset;

src/modules/mc_att_control_multiplatform/mc_att_control.cpp

@@ -75,23 +75,23 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 	/* parameters */
 	_params_handles({
-			.roll_p		= px4::ParameterFloat("MC_ROLL_P", PARAM_MC_ROLL_P_DEFAULT),
+			.roll_p		= px4::ParameterFloat("MP_ROLL_P", PARAM_MP_ROLL_P_DEFAULT),
-			.roll_rate_p	= px4::ParameterFloat("MC_ROLLRATE_P", PARAM_MC_ROLLRATE_P_DEFAULT),
+			.roll_rate_p	= px4::ParameterFloat("MP_ROLLRATE_P", PARAM_MP_ROLLRATE_P_DEFAULT),
-			.roll_rate_i	= px4::ParameterFloat("MC_ROLLRATE_I", PARAM_MC_ROLLRATE_I_DEFAULT),
+			.roll_rate_i	= px4::ParameterFloat("MP_ROLLRATE_I", PARAM_MP_ROLLRATE_I_DEFAULT),
-			.roll_rate_d	= px4::ParameterFloat("MC_ROLLRATE_D", PARAM_MC_ROLLRATE_D_DEFAULT),
+			.roll_rate_d	= px4::ParameterFloat("MP_ROLLRATE_D", PARAM_MP_ROLLRATE_D_DEFAULT),
-			.pitch_p	= px4::ParameterFloat("MC_PITCH_P", PARAM_MC_PITCH_P_DEFAULT),
+			.pitch_p	= px4::ParameterFloat("MP_PITCH_P", PARAM_MP_PITCH_P_DEFAULT),
-			.pitch_rate_p	= px4::ParameterFloat("MC_PITCHRATE_P", PARAM_MC_PITCHRATE_P_DEFAULT),
+			.pitch_rate_p	= px4::ParameterFloat("MP_PITCHRATE_P", PARAM_MP_PITCHRATE_P_DEFAULT),
-			.pitch_rate_i	= px4::ParameterFloat("MC_PITCHRATE_I", PARAM_MC_PITCHRATE_I_DEFAULT),
+			.pitch_rate_i	= px4::ParameterFloat("MP_PITCHRATE_I", PARAM_MP_PITCHRATE_I_DEFAULT),
-			.pitch_rate_d	= px4::ParameterFloat("MC_PITCHRATE_D", PARAM_MC_PITCHRATE_D_DEFAULT),
+			.pitch_rate_d	= px4::ParameterFloat("MP_PITCHRATE_D", PARAM_MP_PITCHRATE_D_DEFAULT),
-			.yaw_p		= px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT),
+			.yaw_p		= px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT),
-			.yaw_rate_p	= px4::ParameterFloat("MC_YAWRATE_P", PARAM_MC_YAWRATE_P_DEFAULT),
+			.yaw_rate_p	= px4::ParameterFloat("MP_YAWRATE_P", PARAM_MP_YAWRATE_P_DEFAULT),
-			.yaw_rate_i	= px4::ParameterFloat("MC_YAWRATE_I", PARAM_MC_YAWRATE_I_DEFAULT),
+			.yaw_rate_i	= px4::ParameterFloat("MP_YAWRATE_I", PARAM_MP_YAWRATE_I_DEFAULT),
-			.yaw_rate_d	= px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT),
+			.yaw_rate_d	= px4::ParameterFloat("MP_YAWRATE_D", PARAM_MP_YAWRATE_D_DEFAULT),
-			.yaw_ff		= px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT),
+			.yaw_ff		= px4::ParameterFloat("MP_YAW_FF", PARAM_MP_YAW_FF_DEFAULT),
-			.yaw_rate_max	= px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT),
+			.yaw_rate_max	= px4::ParameterFloat("MP_YAWRATE_MAX", PARAM_MP_YAWRATE_MAX_DEFAULT),
-			.acro_roll_max	= px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT),
+			.acro_roll_max	= px4::ParameterFloat("MP_ACRO_R_MAX", PARAM_MP_ACRO_R_MAX_DEFAULT),
-			.acro_pitch_max	= px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT),
+			.acro_pitch_max	= px4::ParameterFloat("MP_ACRO_P_MAX", PARAM_MP_ACRO_P_MAX_DEFAULT),
-			.acro_yaw_max	= px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT)
+			.acro_yaw_max	= px4::ParameterFloat("MP_ACRO_Y_MAX", PARAM_MP_ACRO_Y_MAX_DEFAULT)
 	}),
 
 	/* performance counters */

src/modules/mc_att_control_multiplatform/mc_att_control_params.c

@@ -52,7 +52,7 @@
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
+PX4_PARAM_DEFINE_FLOAT(MP_ROLL_P);
 
 /**
  * Roll rate P gain
@@ -62,7 +62,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
+PX4_PARAM_DEFINE_FLOAT(MP_ROLLRATE_P);
 
 /**
  * Roll rate I gain
@@ -72,7 +72,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
+PX4_PARAM_DEFINE_FLOAT(MP_ROLLRATE_I);
 
 /**
  * Roll rate D gain
@@ -82,7 +82,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
+PX4_PARAM_DEFINE_FLOAT(MP_ROLLRATE_D);
 
 /**
  * Pitch P gain
@@ -93,7 +93,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
+PX4_PARAM_DEFINE_FLOAT(MP_PITCH_P);
 
 /**
  * Pitch rate P gain
@@ -103,7 +103,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
+PX4_PARAM_DEFINE_FLOAT(MP_PITCHRATE_P);
 
 /**
  * Pitch rate I gain
@@ -113,7 +113,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
+PX4_PARAM_DEFINE_FLOAT(MP_PITCHRATE_I);
 
 /**
  * Pitch rate D gain
@@ -123,7 +123,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
+PX4_PARAM_DEFINE_FLOAT(MP_PITCHRATE_D);
 
 /**
  * Yaw P gain
@@ -134,7 +134,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
+PX4_PARAM_DEFINE_FLOAT(MP_YAW_P);
 
 /**
  * Yaw rate P gain
@@ -144,7 +144,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
+PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_P);
 
 /**
  * Yaw rate I gain
@@ -154,7 +154,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
+PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_I);
 
 /**
  * Yaw rate D gain
@@ -164,7 +164,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
+PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_D);
 
 /**
  * Yaw feed forward
@@ -175,7 +175,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
  * @max 1.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
+PX4_PARAM_DEFINE_FLOAT(MP_YAW_FF);
 
 /**
  * Max yaw rate
@@ -187,7 +187,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
+PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_MAX);
 
 /**
  * Max acro roll rate
@@ -197,7 +197,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
+PX4_PARAM_DEFINE_FLOAT(MP_ACRO_R_MAX);
 
 /**
  * Max acro pitch rate
@@ -207,7 +207,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
+PX4_PARAM_DEFINE_FLOAT(MP_ACRO_P_MAX);
 
 /**
  * Max acro yaw rate
@@ -216,4 +216,4 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
+PX4_PARAM_DEFINE_FLOAT(MP_ACRO_Y_MAX);

src/modules/mc_att_control_multiplatform/mc_att_control_params.h

@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file mc_att_control_params.h
+ * @file MP_att_control_params.h
  * Parameters for multicopter attitude controller.
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -43,20 +43,20 @@
  */
 #pragma once
 
-#define PARAM_MC_ROLL_P_DEFAULT 6.0f
+#define PARAM_MP_ROLL_P_DEFAULT 6.0f
-#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f
+#define PARAM_MP_ROLLRATE_P_DEFAULT 0.1f
-#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f
+#define PARAM_MP_ROLLRATE_I_DEFAULT 0.0f
-#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f
+#define PARAM_MP_ROLLRATE_D_DEFAULT 0.002f
-#define PARAM_MC_PITCH_P_DEFAULT 6.0f
+#define PARAM_MP_PITCH_P_DEFAULT 6.0f
-#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f
+#define PARAM_MP_PITCHRATE_P_DEFAULT 0.1f
-#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f
+#define PARAM_MP_PITCHRATE_I_DEFAULT 0.0f
-#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f
+#define PARAM_MP_PITCHRATE_D_DEFAULT 0.002f
-#define PARAM_MC_YAW_P_DEFAULT 2.0f
+#define PARAM_MP_YAW_P_DEFAULT 2.0f
-#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f
+#define PARAM_MP_YAWRATE_P_DEFAULT 0.3f
-#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f
+#define PARAM_MP_YAWRATE_I_DEFAULT 0.0f
-#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f
+#define PARAM_MP_YAWRATE_D_DEFAULT 0.0f
-#define PARAM_MC_YAW_FF_DEFAULT 0.5f
+#define PARAM_MP_YAW_FF_DEFAULT 0.5f
-#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f
+#define PARAM_MP_YAWRATE_MAX_DEFAULT 120.0f
-#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f
+#define PARAM_MP_ACRO_R_MAX_DEFAULT 35.0f
-#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f
+#define PARAM_MP_ACRO_P_MAX_DEFAULT 35.0f
-#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f
+#define PARAM_MP_ACRO_Y_MAX_DEFAULT 120.0f

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1105,7 +1105,6 @@ MulticopterPositionControl::task_main()
 
 					/* derivative of velocity error, not includes setpoint acceleration */
 					math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
-					_vel_prev = _vel;
 
 					/* thrust vector in NED frame */
 					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
@@ -1288,6 +1287,11 @@ MulticopterPositionControl::task_main()
 						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
+						/* copy quaternion setpoint to attitude setpoint topic */
+						math::Quaternion q_sp;
+						q_sp.from_dcm(R);
+						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
+
 						/* calculate euler angles, for logging only, must not be used for control */
 						math::Vector<3> euler = R.to_euler();
 						_att_sp.roll_body = euler(0);
@@ -1303,6 +1307,11 @@ MulticopterPositionControl::task_main()
 						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
+						/* copy quaternion setpoint to attitude setpoint topic */
+						math::Quaternion q_sp;
+						q_sp.from_dcm(R);
+						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
+
 						_att_sp.roll_body = 0.0f;
 						_att_sp.pitch_body = 0.0f;
 					}
@@ -1388,12 +1397,20 @@ MulticopterPositionControl::task_main()
 			math::Matrix<3,3> R_sp;
 			R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
 			memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
+
+			/* copy quaternion setpoint to attitude setpoint topic */
+			math::Quaternion q_sp;
+			q_sp.from_dcm(R_sp);
+			memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
 			_att_sp.timestamp = hrt_absolute_time();
 		}
 		else {
 			reset_yaw_sp = true;
 		}
 
+		/* update previous velocity for velocity controller D part */
+		_vel_prev = _vel;
+
 		/* publish attitude setpoint
 		 * Do not publish if offboard is enabled but position/velocity control is disabled,
 		 * in this case the attitude setpoint is published by the mavlink app

src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp

@@ -69,27 +69,27 @@ MulticopterPositionControl::MulticopterPositionControl() :
 
 	/* parameters */
 	_params_handles({
-		.thr_min	    = px4::ParameterFloat("MPC_THR_MIN", PARAM_MPC_THR_MIN_DEFAULT),
+		.thr_min	    = px4::ParameterFloat("MPP_THR_MIN", PARAM_MPP_THR_MIN_DEFAULT),
-		.thr_max	    = px4::ParameterFloat("MPC_THR_MAX", PARAM_MPC_THR_MAX_DEFAULT),
+		.thr_max	    = px4::ParameterFloat("MPP_THR_MAX", PARAM_MPP_THR_MAX_DEFAULT),
-		.z_p		    = px4::ParameterFloat("MPC_Z_P", PARAM_MPC_Z_P_DEFAULT),
+		.z_p		    = px4::ParameterFloat("MPP_Z_P", PARAM_MPP_Z_P_DEFAULT),
-		.z_vel_p	    = px4::ParameterFloat("MPC_Z_VEL_P", PARAM_MPC_Z_VEL_P_DEFAULT),
+		.z_vel_p	    = px4::ParameterFloat("MPP_Z_VEL_P", PARAM_MPP_Z_VEL_P_DEFAULT),
-		.z_vel_i	    = px4::ParameterFloat("MPC_Z_VEL_I", PARAM_MPC_Z_VEL_I_DEFAULT),
+		.z_vel_i	    = px4::ParameterFloat("MPP_Z_VEL_I", PARAM_MPP_Z_VEL_I_DEFAULT),
-		.z_vel_d	    = px4::ParameterFloat("MPC_Z_VEL_D", PARAM_MPC_Z_VEL_D_DEFAULT),
+		.z_vel_d	    = px4::ParameterFloat("MPP_Z_VEL_D", PARAM_MPP_Z_VEL_D_DEFAULT),
-		.z_vel_max	    = px4::ParameterFloat("MPC_Z_VEL_MAX", PARAM_MPC_Z_VEL_MAX_DEFAULT),
+		.z_vel_max	    = px4::ParameterFloat("MPP_Z_VEL_MAX", PARAM_MPP_Z_VEL_MAX_DEFAULT),
-		.z_ff		    = px4::ParameterFloat("MPC_Z_FF", PARAM_MPC_Z_FF_DEFAULT),
+		.z_ff		    = px4::ParameterFloat("MPP_Z_FF", PARAM_MPP_Z_FF_DEFAULT),
-		.xy_p		    = px4::ParameterFloat("MPC_XY_P", PARAM_MPC_XY_P_DEFAULT),
+		.xy_p		    = px4::ParameterFloat("MPP_XY_P", PARAM_MPP_XY_P_DEFAULT),
-		.xy_vel_p	    = px4::ParameterFloat("MPC_XY_VEL_P", PARAM_MPC_XY_VEL_P_DEFAULT),
+		.xy_vel_p	    = px4::ParameterFloat("MPP_XY_VEL_P", PARAM_MPP_XY_VEL_P_DEFAULT),
-		.xy_vel_i	    = px4::ParameterFloat("MPC_XY_VEL_I", PARAM_MPC_XY_VEL_I_DEFAULT),
+		.xy_vel_i	    = px4::ParameterFloat("MPP_XY_VEL_I", PARAM_MPP_XY_VEL_I_DEFAULT),
-		.xy_vel_d	    = px4::ParameterFloat("MPC_XY_VEL_D", PARAM_MPC_XY_VEL_D_DEFAULT),
+		.xy_vel_d	    = px4::ParameterFloat("MPP_XY_VEL_D", PARAM_MPP_XY_VEL_D_DEFAULT),
-		.xy_vel_max	    = px4::ParameterFloat("MPC_XY_VEL_MAX", PARAM_MPC_XY_VEL_MAX_DEFAULT),
+		.xy_vel_max	    = px4::ParameterFloat("MPP_XY_VEL_MAX", PARAM_MPP_XY_VEL_MAX_DEFAULT),
-		.xy_ff		    = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT),
+		.xy_ff		    = px4::ParameterFloat("MPP_XY_FF", PARAM_MPP_XY_FF_DEFAULT),
-		.tilt_max_air	    = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT),
+		.tilt_max_air	    = px4::ParameterFloat("MPP_TILTMAX_AIR", PARAM_MPP_TILTMAX_AIR_DEFAULT),
-		.land_speed	    = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT),
+		.land_speed	    = px4::ParameterFloat("MPP_LAND_SPEED", PARAM_MPP_LAND_SPEED_DEFAULT),
-		.tilt_max_land	    = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT),
+		.tilt_max_land	    = px4::ParameterFloat("MPP_TILTMAX_LND", PARAM_MPP_TILTMAX_LND_DEFAULT),
-		.man_roll_max	    = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT),
+		.man_roll_max	    = px4::ParameterFloat("MPP_MAN_R_MAX", PARAM_MPP_MAN_R_MAX_DEFAULT),
-		.man_pitch_max	    = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT),
+		.man_pitch_max	    = px4::ParameterFloat("MPP_MAN_P_MAX", PARAM_MPP_MAN_P_MAX_DEFAULT),
-		.man_yaw_max	    = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT),
+		.man_yaw_max	    = px4::ParameterFloat("MPP_MAN_Y_MAX", PARAM_MPP_MAN_Y_MAX_DEFAULT),
-		.mc_att_yaw_p	    = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT)
+		.mc_att_yaw_p	    = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT)
 	}),
 	_ref_alt(0.0f),
 	_ref_timestamp(0),

src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c

@@ -52,7 +52,7 @@
  * @max 1.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN);
+PX4_PARAM_DEFINE_FLOAT(MPP_THR_MIN);
 
 /**
  * Maximum thrust
@@ -63,7 +63,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN);
  * @max 1.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_THR_MAX);
 
 /**
  * Proportional gain for vertical position error
@@ -71,7 +71,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_P);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_P);
 
 /**
  * Proportional gain for vertical velocity error
@@ -79,7 +79,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_P);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_P);
 
 /**
  * Integral gain for vertical velocity error
@@ -89,7 +89,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_I);
 
 /**
  * Differential gain for vertical velocity error
@@ -97,7 +97,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_D);
 
 /**
  * Maximum vertical velocity
@@ -108,7 +108,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_MAX);
 
 /**
  * Vertical velocity feed forward
@@ -119,7 +119,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX);
  * @max 1.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_FF);
 
 /**
  * Proportional gain for horizontal position error
@@ -127,7 +127,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_P);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_P);
 
 /**
  * Proportional gain for horizontal velocity error
@@ -135,7 +135,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_P);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_P);
 
 /**
  * Integral gain for horizontal velocity error
@@ -145,7 +145,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_I);
 
 /**
  * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
@@ -153,7 +153,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_D);
 
 /**
  * Maximum horizontal velocity
@@ -164,7 +164,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_MAX);
 
 /**
  * Horizontal velocity feed forward
@@ -175,7 +175,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX);
  * @max 1.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_FF);
 
 /**
  * Maximum tilt angle in air
@@ -187,7 +187,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF);
  * @max 90.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR);
+PX4_PARAM_DEFINE_FLOAT(MPP_TILTMAX_AIR);
 
 /**
  * Maximum tilt during landing
@@ -199,7 +199,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR);
  * @max 90.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
+PX4_PARAM_DEFINE_FLOAT(MPP_TILTMAX_LND);
 
 /**
  * Landing descend rate
@@ -208,7 +208,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
+PX4_PARAM_DEFINE_FLOAT(MPP_LAND_SPEED);
 
 /**
  * Max manual roll
@@ -218,7 +218,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
  * @max 90.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_MAN_R_MAX);
 
 /**
  * Max manual pitch
@@ -228,7 +228,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX);
  * @max 90.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_MAN_P_MAX);
 
 /**
  * Max manual yaw rate
@@ -237,5 +237,5 @@ PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_MAN_Y_MAX);
 

src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h

@@ -41,24 +41,24 @@
 
 #pragma once
 
-#define PARAM_MPC_THR_MIN_DEFAULT 0.1f
+#define PARAM_MPP_THR_MIN_DEFAULT 0.1f
-#define PARAM_MPC_THR_MAX_DEFAULT 1.0f
+#define PARAM_MPP_THR_MAX_DEFAULT 1.0f
-#define PARAM_MPC_Z_P_DEFAULT 1.0f
+#define PARAM_MPP_Z_P_DEFAULT 1.0f
-#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f
+#define PARAM_MPP_Z_VEL_P_DEFAULT 0.1f
-#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f
+#define PARAM_MPP_Z_VEL_I_DEFAULT 0.02f
-#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f
+#define PARAM_MPP_Z_VEL_D_DEFAULT 0.0f
-#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPP_Z_VEL_MAX_DEFAULT 5.0f
-#define PARAM_MPC_Z_FF_DEFAULT 0.5f
+#define PARAM_MPP_Z_FF_DEFAULT 0.5f
-#define PARAM_MPC_XY_P_DEFAULT 1.0f
+#define PARAM_MPP_XY_P_DEFAULT 1.0f
-#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f
+#define PARAM_MPP_XY_VEL_P_DEFAULT 0.1f
-#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f
+#define PARAM_MPP_XY_VEL_I_DEFAULT 0.02f
-#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f
+#define PARAM_MPP_XY_VEL_D_DEFAULT 0.01f
-#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPP_XY_VEL_MAX_DEFAULT 5.0f
-#define PARAM_MPC_XY_FF_DEFAULT 0.5f
+#define PARAM_MPP_XY_FF_DEFAULT 0.5f
-#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
+#define PARAM_MPP_TILTMAX_AIR_DEFAULT 45.0f
-#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
+#define PARAM_MPP_TILTMAX_LND_DEFAULT 15.0f
-#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
+#define PARAM_MPP_LAND_SPEED_DEFAULT 1.0f
-#define PARAM_MPC_MAN_R_MAX_DEFAULT 35.0f
+#define PARAM_MPP_MAN_R_MAX_DEFAULT 35.0f
-#define PARAM_MPC_MAN_P_MAX_DEFAULT 35.0f
+#define PARAM_MPP_MAN_P_MAX_DEFAULT 35.0f
-#define PARAM_MPC_MAN_Y_MAX_DEFAULT 120.0f
+#define PARAM_MPP_MAN_Y_MAX_DEFAULT 120.0f
 

src/modules/navigator/rtl.cpp

@@ -103,6 +103,7 @@ RTL::on_activation()
 			_mission_item.altitude_is_relative = false;
 			_mission_item.altitude = _navigator->get_global_position()->alt;
 		}
+
 	}
 
 	set_rtl_item();
@@ -146,7 +147,8 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d meters above home",
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d m (%d m above home)",
+			(int)(climb_alt),
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
 	}
@@ -177,7 +179,8 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d meters above home",
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
+			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
@@ -197,7 +200,8 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = false;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d meters above home",
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d m (%d m above home)",
+			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f);
  * @max 10.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f);
 
 /**
  * Z axis weight for sonar

src/modules/sdlog2/sdlog2.c

@@ -1982,10 +1982,10 @@ void handle_command(struct vehicle_command_s *cmd)
 		if (param == 1)	{
 			sdlog2_start_log();
 
-		} else if (param == 0)	{
+		} else if (param == -1)	{
 			sdlog2_stop_log();
 		} else {
-			warnx("unknown storage cmd");
+			// Silently ignore non-matching command values, as they could be for params.
 		}
 
 		break;

src/modules/sensors/sensors.cpp

@@ -637,6 +637,7 @@ Sensors::Sensors() :
 	(void)param_find("CAL_MAG1_ROT");
 	(void)param_find("CAL_MAG2_ROT");
 	(void)param_find("SYS_PARAM_VER");
+	(void)param_find("SYS_AUTOSTART");
 	
 	/* fetch initial parameter values */
 	parameters_update();

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -200,18 +200,31 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 unsigned
 MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 {
+	/* Summary of mixing strategy:
+	1) mix roll, pitch and thrust without yaw.
+	2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation ->
+		increase or decrease total thrust (boost). The total increase or decrease of thrust is limited
+		(max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch.
+		In case there is violation at the lower and upper bound then try to shift such that violation is equal
+		on both sides.
+	3) mix in yaw and scale if it leads to limit violation.
+	4) scale all outputs to range [idle_speed,1]
+	*/
+
 	float		roll    = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
-	//lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
 	float		pitch   = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
 	float		yaw     = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
 	float		thrust  = constrain(get_control(0, 3), 0.0f, 1.0f);
-	//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
 	float		min_out = 0.0f;
 	float		max_out = 0.0f;
 
+	// clean register for saturation status flags
 	if (status_reg != NULL) {
 		(*status_reg) = 0;
 	}
+	// thrust boost parameters
+	float thrust_increase_factor = 1.5f;
+	float thrust_decrease_factor = 0.6f;
 
 	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
 	for (unsigned i = 0; i < _rotor_count; i++) {
@@ -221,14 +234,6 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 
 		out *= _rotors[i].out_scale;
 
-		/* limit yaw if it causes outputs clipping */
-		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
-			yaw = -out / _rotors[i].yaw_scale;
-			if(status_reg != NULL) {
-				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
-			}
-		}
-
 		/* calculate min and max output values */
 		if (out < min_out) {
 			min_out = out;
@@ -240,51 +245,89 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 		outputs[i] = out;
 	}
 
-	/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
+	float boost = 0.0f;				// value added to demanded thrust (can also be negative)
-	if (min_out < 0.0f) {
+	float roll_pitch_scale = 1.0f;	// scale for demanded roll and pitch
-		float scale_in = thrust / (thrust - min_out);
 
-		max_out = 0.0f;
+	if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
-
+		float max_thrust_diff = thrust * thrust_increase_factor - thrust;
-		/* mix again with adjusted controls */
+		if(max_thrust_diff >= -min_out) {
-		for (unsigned i = 0; i < _rotor_count; i++) {
+			boost = -min_out;
-			float out = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
-
-			/* update max output value */
-			if (out > max_out) {
-				max_out = out;
-			}
-
-			outputs[i] = out;
 		}
+		else {
+			boost = max_thrust_diff;
+			roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+		}
+	}
+	else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
+		float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
+		if(max_thrust_diff >= max_out - 1.0f) {
+			boost = -(max_out - 1.0f);
+		} else {
+			boost = -max_thrust_diff;
+			roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
+		}
+	}
+	else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
+		float max_thrust_diff = thrust * thrust_increase_factor - thrust;
+		boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff);
+		roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+	}
+	else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) {
+		float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
+		boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f);
+		roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
+	}
+	else if (min_out < 0.0f && max_out > 1.0f) {
+		boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust);
+		roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+	}
 
+	// notify if saturation has occurred
+	if(min_out < 0.0f) {
 		if(status_reg != NULL) {
 			(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
 		}
-
-	} else {
-		/* roll/pitch mixed without lower side limiting, add yaw control */
-		for (unsigned i = 0; i < _rotor_count; i++) {
-			outputs[i] += yaw * _rotors[i].yaw_scale;
-		}
 	}
-
+	if(max_out > 0.0f) {
-	/* scale down all outputs if some outputs are too large, reduce total thrust */
-	float scale_out;
-	if (max_out > 1.0f) {
-		scale_out = 1.0f / max_out;
-
 		if(status_reg != NULL) {
 			(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
 		}
-
-	} else {
-		scale_out = 1.0f;
 	}
 
-	/* scale outputs to range _idle_speed..1, and do final limiting */
+	// mix again but now with thrust boost, scale roll/pitch and also add yaw
+	for(unsigned i = 0; i < _rotor_count; i++) {
+		float out = (roll * _rotors[i].roll_scale +
+			    pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
+				yaw * _rotors[i].yaw_scale +
+			    thrust + boost;
+
+		out *= _rotors[i].out_scale;
+
+		// scale yaw if it violates limits. inform about yaw limit reached
+		if(out < 0.0f) {
+			yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
+				roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale;
+			if(status_reg != NULL) {
+				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
+			}
+		}
+		else if(out > 1.0f) {
+			yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
+				roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale;
+			if(status_reg != NULL) {
+				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
+			}
+		}
+	}
+
+	/* last mix, add yaw and scale outputs to range idle_speed...1 */
 	for (unsigned i = 0; i < _rotor_count; i++) {
-		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
+		outputs[i] = (roll * _rotors[i].roll_scale +
+			    pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
+				yaw * _rotors[i].yaw_scale +
+			    thrust + boost;
+
+		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
 	}
 
 	return _rotor_count;