diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
index caa8e28fc5..68428be6f6 100644
--- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
index 5e41d6957d..8ab65be7b9 100644
--- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6
index 191c58da4c..ed90dabf41 100644
--- a/ROMFS/px4fmu_common/init.d/13002_firefly6
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330
index 8e5dc76b1f..d07e926a79 100644
--- a/ROMFS/px4fmu_common/init.d/4010_dji_f330
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450
index ea35b3ba9b..9b3954be6f 100644
--- a/ROMFS/px4fmu_common/init.d/4011_dji_f450
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can
index b1db1dd9aa..05b4355138 100644
--- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
index a1de19d5df..aec0c62f8f 100644
--- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index 454af8da7a..6bfbfea396 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index b8a704b90f..474db36ef7 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -3,8 +3,13 @@
# Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers.
#
-ms5611 start
-adc start
+if ms5611 start
+then
+fi
+
+if adc start
+then
+fi
if ver hwcmp PX4FMU_V2
then
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index e456eff7f1..0442637941 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/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
diff --git a/launch/ardrone.launch b/launch/ardrone.launch
index 56030dd1f4..f43bbf4709 100644
--- a/launch/ardrone.launch
+++ b/launch/ardrone.launch
@@ -6,14 +6,14 @@
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
diff --git a/launch/iris.launch b/launch/iris.launch
index bf5b099b64..5231e3215b 100644
--- a/launch/iris.launch
+++ b/launch/iris.launch
@@ -7,14 +7,14 @@
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
diff --git a/nuttx-configs/px4-stm32f4discovery/include/board.h b/nuttx-configs/px4-stm32f4discovery/include/board.h
index 0412c3aa6d..95feca5506 100644
--- a/nuttx-configs/px4-stm32f4discovery/include/board.h
+++ b/nuttx-configs/px4-stm32f4discovery/include/board.h
@@ -34,8 +34,8 @@
*
************************************************************************************/
-#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
-#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
+#ifndef __CONFIG_DISCOVERY_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 */
diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig
index 6a2470bea9..03092256e9 100644
--- a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig
+++ b/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_UART7 is not set
-# CONFIG_STM32_UART8 is not set
+CONFIG_STM32_USART6=y
# 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=""
+CONFIG_ARCH_BOARD_STM32F4_DISCOVERY=y
+# 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
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 4ccc5dacb6..ab8f9eef62 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/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_TXBUFSIZE=2048
+CONFIG_CDCACM_RXBUFSIZE=1000
+CONFIG_CDCACM_TXBUFSIZE=8000
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0011
CONFIG_CDCACM_VENDORSTR="3D Robotics"
diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h
index dd66abc19f..c4f469caa1 100644
--- a/src/drivers/boards/px4-stm32f4discovery/board_config.h
+++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h
@@ -60,9 +60,10 @@ __BEGIN_DECLS
****************************************************************************************************/
/* Configuration ************************************************************************************/
-/* PX4FMU GPIOs ***********************************************************************************/
-/* LEDs */
+/* PX4-STM32F4Discovery GPIOs ***********************************************************************************/
/* 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)
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 2b3f4bf51b..144614d6bc 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/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 (_bus == PX4_SPI_BUS_SENSORS && (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;
+ if (!(raw_report.status & STATUS_ZYXDA)) {
+ perf_end(_sample_perf);
+ perf_count(_duplicates);
+ 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; iflush();
/* 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);
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index bf0e23da6b..9dd7cc7048 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/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);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 12a52f254b..6de4f993a8 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/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);
diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp
index 9cd08a50d4..95ccf4130f 100644
--- a/src/lib/ecl/attitude_fw/ecl_controller.cpp
+++ b/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;
}
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 4a086be5dd..6f22786a3d 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/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) {
- 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) {
+ if (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 */
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index e2ff49dae7..30eb2a96e0 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/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]));
}
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
index fa15923076..730d7a4e8b 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
@@ -41,10 +41,70 @@
#include
+/**
+ * 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
-PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); ///< gyro bias limit, rad/s
+
+/**
+ * Magnetic declination, in degrees
+ *
+ * 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);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index dcf7cdddf0..bab6bd2076 100644
--- a/src/modules/commander/commander.cpp
+++ b/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 {
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 71e44c3d1e..a0d1de3907 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/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
-
- bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
-
- int cancel_sub = calibrate_cancel_subscribe();
- calibrate_return cal_return = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
- cancel_sub, // Subscription to vehicle_command for cancel support
- side_collected, // Sides to calibrate
- gyro_calibration_worker, // Calibration worker
- &worker_data, // Opaque data for calibration worked
- true); // true: lenient still detection
+ do {
+ // Calibrate gyro and ensure user didn't move
+ calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data);
+
+ if (cal_return == calibrate_return_cancelled) {
+ // Cancel message already sent, we are done here
+ res = ERROR;
+ break;
+
+ } else if (cal_return == calibrate_return_error) {
+ 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;
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 2790d6838d..10d32b0995 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -53,6 +53,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -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;
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 6913fbb55c..3030bc1fd4 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/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];
- strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
-
+ char dirPath[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);
- return kErrFailErrno;
+ _mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)");
+ _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);
- errorCode = kErrFailErrno;
- break;
+ _mavlink->send_statustext_critical("FTP: list readdir_r failure");
+ _mavlink->send_statustext_critical(dirPath);
+
+ 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;
- _session_info.stream_download = false;
+ /* perform transfers in 35K chunks - this is determined empirical */
+ 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;
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index af8740e481..33b8996f71 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/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
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 57abf4f09a..30d2a64164 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/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);
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 9c8794017b..524effb205 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/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 */
- if (_send_all_index >= 0) {
+ /* send all parameters if requested, but only after the system has booted */
+ if (_send_all_index >= 0 && t > 4 * 1000 * 1000) {
/* skip if no space is available */
if (_mavlink->get_free_tx_buf() < get_size()) {
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 77b2bfb0be..ca6b226934 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/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_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
+ bool ignore_bodyrate_msg = (bool)(set_attitude_target.type_mask & 0x7);
+ 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_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude;
+ _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate;
+ _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_attitude = ignore_attitude;
+ _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg;
+ _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();
- mavlink_quaternion_to_euler(set_attitude_target.q,
- &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);
- att_sp.R_valid = true;
- if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
- att_sp.thrust = set_attitude_target.thrust;
+ _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,
+ &_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);
+ _att_sp.R_valid = true;
+ _att_sp.yaw_sp_move_rate = 0.0;
+ 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));
- att_sp.q_d_valid = true;
+
+ if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
+ _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;
- _rates_sp.pitch = set_attitude_target.body_pitch_rate;
- _rates_sp.yaw = set_attitude_target.body_yaw_rate;
+ if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data
+ _rates_sp.roll = set_attitude_target.body_roll_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.lon = hil_state.lon;
+ hil_global_pos.lat = hil_state.lat / ((double)1e7);
+ 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, ¶m);
- 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);
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 2b6174f8fe..887d2f88ed 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/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;
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
index e478a92e9e..2ad89e606c 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+++ b/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_rate_p = px4::ParameterFloat("MC_ROLLRATE_P", PARAM_MC_ROLLRATE_P_DEFAULT),
- .roll_rate_i = px4::ParameterFloat("MC_ROLLRATE_I", PARAM_MC_ROLLRATE_I_DEFAULT),
- .roll_rate_d = px4::ParameterFloat("MC_ROLLRATE_D", PARAM_MC_ROLLRATE_D_DEFAULT),
- .pitch_p = px4::ParameterFloat("MC_PITCH_P", PARAM_MC_PITCH_P_DEFAULT),
- .pitch_rate_p = px4::ParameterFloat("MC_PITCHRATE_P", PARAM_MC_PITCHRATE_P_DEFAULT),
- .pitch_rate_i = px4::ParameterFloat("MC_PITCHRATE_I", PARAM_MC_PITCHRATE_I_DEFAULT),
- .pitch_rate_d = px4::ParameterFloat("MC_PITCHRATE_D", PARAM_MC_PITCHRATE_D_DEFAULT),
- .yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT),
- .yaw_rate_p = px4::ParameterFloat("MC_YAWRATE_P", PARAM_MC_YAWRATE_P_DEFAULT),
- .yaw_rate_i = px4::ParameterFloat("MC_YAWRATE_I", PARAM_MC_YAWRATE_I_DEFAULT),
- .yaw_rate_d = px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT),
- .yaw_ff = px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT),
- .yaw_rate_max = px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT),
- .acro_roll_max = px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT),
- .acro_pitch_max = px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT),
- .acro_yaw_max = px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT)
+ .roll_p = px4::ParameterFloat("MP_ROLL_P", PARAM_MP_ROLL_P_DEFAULT),
+ .roll_rate_p = px4::ParameterFloat("MP_ROLLRATE_P", PARAM_MP_ROLLRATE_P_DEFAULT),
+ .roll_rate_i = px4::ParameterFloat("MP_ROLLRATE_I", PARAM_MP_ROLLRATE_I_DEFAULT),
+ .roll_rate_d = px4::ParameterFloat("MP_ROLLRATE_D", PARAM_MP_ROLLRATE_D_DEFAULT),
+ .pitch_p = px4::ParameterFloat("MP_PITCH_P", PARAM_MP_PITCH_P_DEFAULT),
+ .pitch_rate_p = px4::ParameterFloat("MP_PITCHRATE_P", PARAM_MP_PITCHRATE_P_DEFAULT),
+ .pitch_rate_i = px4::ParameterFloat("MP_PITCHRATE_I", PARAM_MP_PITCHRATE_I_DEFAULT),
+ .pitch_rate_d = px4::ParameterFloat("MP_PITCHRATE_D", PARAM_MP_PITCHRATE_D_DEFAULT),
+ .yaw_p = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT),
+ .yaw_rate_p = px4::ParameterFloat("MP_YAWRATE_P", PARAM_MP_YAWRATE_P_DEFAULT),
+ .yaw_rate_i = px4::ParameterFloat("MP_YAWRATE_I", PARAM_MP_YAWRATE_I_DEFAULT),
+ .yaw_rate_d = px4::ParameterFloat("MP_YAWRATE_D", PARAM_MP_YAWRATE_D_DEFAULT),
+ .yaw_ff = px4::ParameterFloat("MP_YAW_FF", PARAM_MP_YAW_FF_DEFAULT),
+ .yaw_rate_max = px4::ParameterFloat("MP_YAWRATE_MAX", PARAM_MP_YAWRATE_MAX_DEFAULT),
+ .acro_roll_max = px4::ParameterFloat("MP_ACRO_R_MAX", PARAM_MP_ACRO_R_MAX_DEFAULT),
+ .acro_pitch_max = px4::ParameterFloat("MP_ACRO_P_MAX", PARAM_MP_ACRO_P_MAX_DEFAULT),
+ .acro_yaw_max = px4::ParameterFloat("MP_ACRO_Y_MAX", PARAM_MP_ACRO_Y_MAX_DEFAULT)
}),
/* performance counters */
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
index 395ae83b02..9c6ac5dc73 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
+++ b/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);
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
index ff962dbf1f..c3b71715bb 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
+++ b/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
@@ -43,20 +43,20 @@
*/
#pragma once
-#define PARAM_MC_ROLL_P_DEFAULT 6.0f
-#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f
-#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f
-#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f
-#define PARAM_MC_PITCH_P_DEFAULT 6.0f
-#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f
-#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f
-#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f
-#define PARAM_MC_YAW_P_DEFAULT 2.0f
-#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f
-#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f
-#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f
-#define PARAM_MC_YAW_FF_DEFAULT 0.5f
-#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f
-#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f
-#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f
-#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f
+#define PARAM_MP_ROLL_P_DEFAULT 6.0f
+#define PARAM_MP_ROLLRATE_P_DEFAULT 0.1f
+#define PARAM_MP_ROLLRATE_I_DEFAULT 0.0f
+#define PARAM_MP_ROLLRATE_D_DEFAULT 0.002f
+#define PARAM_MP_PITCH_P_DEFAULT 6.0f
+#define PARAM_MP_PITCHRATE_P_DEFAULT 0.1f
+#define PARAM_MP_PITCHRATE_I_DEFAULT 0.0f
+#define PARAM_MP_PITCHRATE_D_DEFAULT 0.002f
+#define PARAM_MP_YAW_P_DEFAULT 2.0f
+#define PARAM_MP_YAWRATE_P_DEFAULT 0.3f
+#define PARAM_MP_YAWRATE_I_DEFAULT 0.0f
+#define PARAM_MP_YAWRATE_D_DEFAULT 0.0f
+#define PARAM_MP_YAW_FF_DEFAULT 0.5f
+#define PARAM_MP_YAWRATE_MAX_DEFAULT 120.0f
+#define PARAM_MP_ACRO_R_MAX_DEFAULT 35.0f
+#define PARAM_MP_ACRO_P_MAX_DEFAULT 35.0f
+#define PARAM_MP_ACRO_Y_MAX_DEFAULT 120.0f
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 14d860f29e..b6ad4fd547 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/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
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
index 72ee214292..b596226336 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+++ b/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_max = px4::ParameterFloat("MPC_THR_MAX", PARAM_MPC_THR_MAX_DEFAULT),
- .z_p = px4::ParameterFloat("MPC_Z_P", PARAM_MPC_Z_P_DEFAULT),
- .z_vel_p = px4::ParameterFloat("MPC_Z_VEL_P", PARAM_MPC_Z_VEL_P_DEFAULT),
- .z_vel_i = px4::ParameterFloat("MPC_Z_VEL_I", PARAM_MPC_Z_VEL_I_DEFAULT),
- .z_vel_d = px4::ParameterFloat("MPC_Z_VEL_D", PARAM_MPC_Z_VEL_D_DEFAULT),
- .z_vel_max = px4::ParameterFloat("MPC_Z_VEL_MAX", PARAM_MPC_Z_VEL_MAX_DEFAULT),
- .z_ff = px4::ParameterFloat("MPC_Z_FF", PARAM_MPC_Z_FF_DEFAULT),
- .xy_p = px4::ParameterFloat("MPC_XY_P", PARAM_MPC_XY_P_DEFAULT),
- .xy_vel_p = px4::ParameterFloat("MPC_XY_VEL_P", PARAM_MPC_XY_VEL_P_DEFAULT),
- .xy_vel_i = px4::ParameterFloat("MPC_XY_VEL_I", PARAM_MPC_XY_VEL_I_DEFAULT),
- .xy_vel_d = px4::ParameterFloat("MPC_XY_VEL_D", PARAM_MPC_XY_VEL_D_DEFAULT),
- .xy_vel_max = px4::ParameterFloat("MPC_XY_VEL_MAX", PARAM_MPC_XY_VEL_MAX_DEFAULT),
- .xy_ff = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT),
- .tilt_max_air = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT),
- .land_speed = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT),
- .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT),
- .man_roll_max = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT),
- .man_pitch_max = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT),
- .man_yaw_max = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT),
- .mc_att_yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT)
+ .thr_min = px4::ParameterFloat("MPP_THR_MIN", PARAM_MPP_THR_MIN_DEFAULT),
+ .thr_max = px4::ParameterFloat("MPP_THR_MAX", PARAM_MPP_THR_MAX_DEFAULT),
+ .z_p = px4::ParameterFloat("MPP_Z_P", PARAM_MPP_Z_P_DEFAULT),
+ .z_vel_p = px4::ParameterFloat("MPP_Z_VEL_P", PARAM_MPP_Z_VEL_P_DEFAULT),
+ .z_vel_i = px4::ParameterFloat("MPP_Z_VEL_I", PARAM_MPP_Z_VEL_I_DEFAULT),
+ .z_vel_d = px4::ParameterFloat("MPP_Z_VEL_D", PARAM_MPP_Z_VEL_D_DEFAULT),
+ .z_vel_max = px4::ParameterFloat("MPP_Z_VEL_MAX", PARAM_MPP_Z_VEL_MAX_DEFAULT),
+ .z_ff = px4::ParameterFloat("MPP_Z_FF", PARAM_MPP_Z_FF_DEFAULT),
+ .xy_p = px4::ParameterFloat("MPP_XY_P", PARAM_MPP_XY_P_DEFAULT),
+ .xy_vel_p = px4::ParameterFloat("MPP_XY_VEL_P", PARAM_MPP_XY_VEL_P_DEFAULT),
+ .xy_vel_i = px4::ParameterFloat("MPP_XY_VEL_I", PARAM_MPP_XY_VEL_I_DEFAULT),
+ .xy_vel_d = px4::ParameterFloat("MPP_XY_VEL_D", PARAM_MPP_XY_VEL_D_DEFAULT),
+ .xy_vel_max = px4::ParameterFloat("MPP_XY_VEL_MAX", PARAM_MPP_XY_VEL_MAX_DEFAULT),
+ .xy_ff = px4::ParameterFloat("MPP_XY_FF", PARAM_MPP_XY_FF_DEFAULT),
+ .tilt_max_air = px4::ParameterFloat("MPP_TILTMAX_AIR", PARAM_MPP_TILTMAX_AIR_DEFAULT),
+ .land_speed = px4::ParameterFloat("MPP_LAND_SPEED", PARAM_MPP_LAND_SPEED_DEFAULT),
+ .tilt_max_land = px4::ParameterFloat("MPP_TILTMAX_LND", PARAM_MPP_TILTMAX_LND_DEFAULT),
+ .man_roll_max = px4::ParameterFloat("MPP_MAN_R_MAX", PARAM_MPP_MAN_R_MAX_DEFAULT),
+ .man_pitch_max = px4::ParameterFloat("MPP_MAN_P_MAX", PARAM_MPP_MAN_P_MAX_DEFAULT),
+ .man_yaw_max = px4::ParameterFloat("MPP_MAN_Y_MAX", PARAM_MPP_MAN_Y_MAX_DEFAULT),
+ .mc_att_yaw_p = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT)
}),
_ref_alt(0.0f),
_ref_timestamp(0),
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
index 709085662a..1b3c5b7e93 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
+++ b/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);
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
index 8c8b707ae0..d9c9fb5957 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
+++ b/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_MPC_THR_MAX_DEFAULT 1.0f
-#define PARAM_MPC_Z_P_DEFAULT 1.0f
-#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f
-#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f
-#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f
-#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f
-#define PARAM_MPC_Z_FF_DEFAULT 0.5f
-#define PARAM_MPC_XY_P_DEFAULT 1.0f
-#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f
-#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f
-#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f
-#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f
-#define PARAM_MPC_XY_FF_DEFAULT 0.5f
-#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
-#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
-#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
-#define PARAM_MPC_MAN_R_MAX_DEFAULT 35.0f
-#define PARAM_MPC_MAN_P_MAX_DEFAULT 35.0f
-#define PARAM_MPC_MAN_Y_MAX_DEFAULT 120.0f
+#define PARAM_MPP_THR_MIN_DEFAULT 0.1f
+#define PARAM_MPP_THR_MAX_DEFAULT 1.0f
+#define PARAM_MPP_Z_P_DEFAULT 1.0f
+#define PARAM_MPP_Z_VEL_P_DEFAULT 0.1f
+#define PARAM_MPP_Z_VEL_I_DEFAULT 0.02f
+#define PARAM_MPP_Z_VEL_D_DEFAULT 0.0f
+#define PARAM_MPP_Z_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPP_Z_FF_DEFAULT 0.5f
+#define PARAM_MPP_XY_P_DEFAULT 1.0f
+#define PARAM_MPP_XY_VEL_P_DEFAULT 0.1f
+#define PARAM_MPP_XY_VEL_I_DEFAULT 0.02f
+#define PARAM_MPP_XY_VEL_D_DEFAULT 0.01f
+#define PARAM_MPP_XY_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPP_XY_FF_DEFAULT 0.5f
+#define PARAM_MPP_TILTMAX_AIR_DEFAULT 45.0f
+#define PARAM_MPP_TILTMAX_LND_DEFAULT 15.0f
+#define PARAM_MPP_LAND_SPEED_DEFAULT 1.0f
+#define PARAM_MPP_MAN_R_MAX_DEFAULT 35.0f
+#define PARAM_MPP_MAN_P_MAX_DEFAULT 35.0f
+#define PARAM_MPP_MAN_Y_MAX_DEFAULT 120.0f
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c1ed8cb7c1..b75b7fa221 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/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;
}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index 96b12f9e09..382e9e46d7 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/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
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 719db93a7a..8d04c0b966 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/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;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index c1e5720aec..34cc56c234 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/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();
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 5489e3924f..369e6807d1 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/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 */
- if (min_out < 0.0f) {
- float scale_in = thrust / (thrust - min_out);
+ float boost = 0.0f; // value added to demanded thrust (can also be negative)
+ float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch
- max_out = 0.0f;
-
- /* mix again with adjusted controls */
- for (unsigned i = 0; i < _rotor_count; i++) {
- 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;
+ if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
+ float max_thrust_diff = thrust * thrust_increase_factor - thrust;
+ if(max_thrust_diff >= -min_out) {
+ boost = -min_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;
- }
}
-
- /* 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(max_out > 0.0f) {
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;