dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
Mark Charlebois 2015-05-16 15:04:38 -07:00
parent 6a1c28fbc1 26954ece2c
commit 36f5d47ed9
45 changed files with 714 additions and 400 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/10016_3dr_iris
View File

@ -12,11 +12,11 @@ then
# TODO tune roll/pitch separately # TODO tune roll/pitch separately
param set MC_ROLL_P 7.0 param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.13 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_ROLLRATE_D 0.004
param set MC_PITCH_P 7.0 param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.13 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_PITCHRATE_D 0.004
param set MC_YAW_P 2.5 param set MC_YAW_P 2.5
param set MC_YAWRATE_P 0.25 param set MC_YAWRATE_P 0.25

4
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
View File

@ -12,11 +12,11 @@ then
# TODO tune roll/pitch separately # TODO tune roll/pitch separately
param set MC_ROLL_P 7.0 param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.13 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_ROLLRATE_D 0.004
param set MC_PITCH_P 7.0 param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.13 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_PITCHRATE_D 0.004
param set MC_YAW_P 0.5 param set MC_YAW_P 0.5
param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_P 0.2

20
ROMFS/px4fmu_common/init.d/13002_firefly6
View File

@ -7,6 +7,26 @@
sh /etc/init.d/rc.vtol_defaults 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 MIXER firefly6
set PWM_OUT 12345678 set PWM_OUT 12345678

4
ROMFS/px4fmu_common/init.d/4010_dji_f330
View File

@ -11,11 +11,11 @@ if [ $AUTOCNF == yes ]
then then
param set MC_ROLL_P 7.0 param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1 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_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0 param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1 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_PITCHRATE_D 0.003
param set MC_YAW_P 2.8 param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_P 0.2

4
ROMFS/px4fmu_common/init.d/4011_dji_f450
View File

@ -12,11 +12,11 @@ then
# TODO REVIEW # TODO REVIEW
param set MC_ROLL_P 7.0 param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1 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_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0 param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1 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_PITCHRATE_D 0.003
param set MC_YAW_P 2.8 param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_P 0.2

4
ROMFS/px4fmu_common/init.d/4012_quad_x_can
View File

@ -12,11 +12,11 @@ then
# TODO REVIEW # TODO REVIEW
param set MC_ROLL_P 7.0 param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1 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_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0 param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1 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_PITCHRATE_D 0.003
param set MC_YAW_P 2.8 param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_P 0.2

6
ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
View File

@ -13,15 +13,15 @@ if [ $AUTOCNF == yes ]
then then
param set MC_ROLL_P 7.0 param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1 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_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0 param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1 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_PITCHRATE_D 0.003
param set MC_YAW_P 2.8 param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2 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 param set MC_YAWRATE_D 0.0
fi fi

2
ROMFS/px4fmu_common/init.d/rc.logging
View File

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

9
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -3,8 +3,13 @@
# Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers. # 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 if ver hwcmp PX4FMU_V2
then then

4
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -3,9 +3,9 @@
# USB MAVLink start # 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 # 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 MISSION_ITEM -r 50
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10

16
launch/ardrone.launch
View File

@ -6,14 +6,14 @@
</include> </include>
<group ns="$(arg ns)"> <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" /> <param name="vehicle_model" type="string" value="ardrone" />
</group> </group>

16
launch/iris.launch
View File

@ -7,14 +7,14 @@
<group ns="$(arg ns)"> <group ns="$(arg ns)">
<param name="mixer" type="string" value="i" /> <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" /> <param name="vehicle_model" type="string" value="iris" />
</group> </group>

18
nuttx-configs/px4-stm32f4discovery/include/board.h
View File

@ -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 * Included Files
@ -201,6 +201,18 @@
#define GPIO_USART2_RX GPIO_USART2_RX_1 #define GPIO_USART2_RX GPIO_USART2_RX_1
#define GPIO_USART2_TX GPIO_USART2_TX_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: */ /* SPI - There is a MEMS device on SPI1 using these pins: */
#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 #define GPIO_SPI1_MISO GPIO_SPI1_MISO_1
@ -270,4 +282,4 @@ EXTERN void stm32_setleds(uint8_t ledset);
#endif #endif
#endif /* __ASSEMBLY__ */ #endif /* __ASSEMBLY__ */
#endif /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */ #endif /* __CONFIG_DISCOVERY_INCLUDE_BOARD_H */

35
nuttx-configs/px4-stm32f4discovery/nsh/defconfig
View File

@ -91,6 +91,8 @@ CONFIG_ARCH_HAVE_MPU=y
# ARMV7M Configuration Options # ARMV7M Configuration Options
# #
# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set # 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_TOOLCHAIN_GNU_EABI=y
CONFIG_ARMV7M_STACKCHECK=y CONFIG_ARMV7M_STACKCHECK=y
CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_TERMIOS=y
@ -128,6 +130,7 @@ CONFIG_SERIAL_TERMIOS=y
# CONFIG_ARCH_CHIP_STM32F100VC is not set # CONFIG_ARCH_CHIP_STM32F100VC is not set
# CONFIG_ARCH_CHIP_STM32F100VD is not set # CONFIG_ARCH_CHIP_STM32F100VD is not set
# CONFIG_ARCH_CHIP_STM32F100VE 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_STM32F103C4 is not set
# CONFIG_ARCH_CHIP_STM32F103C8 is not set # CONFIG_ARCH_CHIP_STM32F103C8 is not set
# CONFIG_ARCH_CHIP_STM32F103RET6 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_STM32F20XX is not set
# CONFIG_STM32_STM32F30XX is not set # CONFIG_STM32_STM32F30XX is not set
CONFIG_STM32_STM32F40XX=y CONFIG_STM32_STM32F40XX=y
# CONFIG_STM32_STM32F427 is not set
# CONFIG_STM32_DFU is not set # CONFIG_STM32_DFU is not set
# #
@ -207,9 +209,6 @@ CONFIG_STM32_PWR=y
CONFIG_STM32_SPI1=y CONFIG_STM32_SPI1=y
# CONFIG_STM32_SPI2 is not set # CONFIG_STM32_SPI2 is not set
# CONFIG_STM32_SPI3 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_SYSCFG=y
CONFIG_STM32_TIM1=y CONFIG_STM32_TIM1=y
# CONFIG_STM32_TIM2 is not set # CONFIG_STM32_TIM2 is not set
@ -230,9 +229,7 @@ CONFIG_STM32_USART2=y
# CONFIG_STM32_USART3 is not set # CONFIG_STM32_USART3 is not set
# CONFIG_STM32_UART4 is not set # CONFIG_STM32_UART4 is not set
# CONFIG_STM32_UART5 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_IWDG is not set
# CONFIG_STM32_WWDG is not set # CONFIG_STM32_WWDG is not set
CONFIG_STM32_ADC=y CONFIG_STM32_ADC=y
@ -268,6 +265,8 @@ CONFIG_STM32_USART=y
# #
# CONFIG_USART2_RS485 is not set # CONFIG_USART2_RS485 is not set
CONFIG_USART2_RXDMA=y CONFIG_USART2_RXDMA=y
# CONFIG_USART6_RS485 is not set
CONFIG_USART6_RXDMA=y
CONFIG_SERIAL_DISABLE_REORDERING=y CONFIG_SERIAL_DISABLE_REORDERING=y
CONFIG_STM32_USART_SINGLEWIRE=y CONFIG_STM32_USART_SINGLEWIRE=y
@ -336,8 +335,10 @@ CONFIG_BOOT_RUNFROMFLASH=y
# #
# Board Selection # 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 # Common Board Options
@ -431,7 +432,7 @@ CONFIG_SPI=y
# CONFIG_SPI_OWNBUS is not set # CONFIG_SPI_OWNBUS is not set
CONFIG_SPI_EXCHANGE=y CONFIG_SPI_EXCHANGE=y
# CONFIG_SPI_CMDDATA is not set # CONFIG_SPI_CMDDATA is not set
# CONFIG_RTC is not set # CONFIG_RTC= is not set
CONFIG_WATCHDOG=y CONFIG_WATCHDOG=y
# CONFIG_ANALOG is not set # CONFIG_ANALOG is not set
# CONFIG_AUDIO_DEVICES is not set # CONFIG_AUDIO_DEVICES is not set
@ -449,10 +450,12 @@ CONFIG_SERIAL=y
CONFIG_SERIAL_REMOVABLE=y CONFIG_SERIAL_REMOVABLE=y
# CONFIG_16550_UART is not set # CONFIG_16550_UART is not set
CONFIG_ARCH_HAVE_USART2=y CONFIG_ARCH_HAVE_USART2=y
CONFIG_ARCH_HAVE_USART6=y
CONFIG_MCU_SERIAL=y CONFIG_MCU_SERIAL=y
CONFIG_STANDARD_SERIAL=y CONFIG_STANDARD_SERIAL=y
CONFIG_SERIAL_NPOLLWAITERS=2 CONFIG_SERIAL_NPOLLWAITERS=2
CONFIG_USART2_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=y
# CONFIG_USART6_SERIAL_CONSOLE is not set
# CONFIG_NO_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_2STOP=0
# CONFIG_USART2_IFLOWCONTROL is not set # CONFIG_USART2_IFLOWCONTROL is not set
# CONFIG_USART2_OFLOWCONTROL 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_IFLOWCONTROL is not set
# CONFIG_SERIAL_OFLOWCONTROL is not set # CONFIG_SERIAL_OFLOWCONTROL is not set
CONFIG_USBDEV=y CONFIG_USBDEV=y

4
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -663,8 +663,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512
CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96 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_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_PRODUCTID=0x0011
CONFIG_CDCACM_VENDORSTR="3D Robotics" CONFIG_CDCACM_VENDORSTR="3D Robotics"

5
src/drivers/boards/px4-stm32f4discovery/board_config.h
View File

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

55
src/drivers/l3gd20/l3gd20.cpp
View File

@ -189,6 +189,14 @@ static const int ERROR = -1;
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
#endif #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[]); } extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI class L3GD20 : public device::SPI
@ -236,9 +244,9 @@ private:
unsigned _read; unsigned _read;
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _reschedules;
perf_counter_t _errors; perf_counter_t _errors;
perf_counter_t _bad_registers; perf_counter_t _bad_registers;
perf_counter_t _duplicates;
uint8_t _register_wait; 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), _orientation(SENSOR_BOARD_ROTATION_DEFAULT),
_read(0), _read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
_bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")), _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "l3gd20_duplicates")),
_register_wait(0), _register_wait(0),
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(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 */ /* delete the perf counter */
perf_free(_sample_perf); perf_free(_sample_perf);
perf_free(_reschedules);
perf_free(_errors); perf_free(_errors);
perf_free(_bad_registers); perf_free(_bad_registers);
perf_free(_duplicates);
} }
int int
@ -608,7 +616,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */ /* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */ /* 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 */ /* adjust filters */
float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
@ -834,7 +844,10 @@ L3GD20::start()
_reports->flush(); _reports->flush();
/* start polling at the specified rate */ /* 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 void
@ -899,12 +912,6 @@ L3GD20::measure_trampoline(void *arg)
dev->measure(); dev->measure();
} }
#ifdef GPIO_EXTI_GYRO_DRDY
# define L3GD20_USE_DRDY 1
#else
# define L3GD20_USE_DRDY 0
#endif
void void
L3GD20::check_registers(void) L3GD20::check_registers(void)
{ {
@ -954,33 +961,17 @@ L3GD20::measure()
check_registers(); 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 */ /* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report)); memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); 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
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
return; return;
} }
#endif
/* /*
* 1) Scale raw value to SI units using scaling from datasheet. * 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units) * 2) Subtract static offset (in SI units)
@ -1071,9 +1062,9 @@ L3GD20::print_info()
{ {
printf("gyro reads: %u\n", _read); printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_reschedules);
perf_print_counter(_errors); perf_print_counter(_errors);
perf_print_counter(_bad_registers); perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);
_reports->print_info("report queue"); _reports->print_info("report queue");
::printf("checked_next: %u\n", _checked_next); ::printf("checked_next: %u\n", _checked_next);
for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) { for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {

46
src/drivers/lsm303d/lsm303d.cpp
View File

@ -196,6 +196,7 @@ static const int ERROR = -1;
#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) #define REG7_CONT_MODE_M ((0<<1) | (0<<0))
#define REG_STATUS_A_NEW_ZYXADA 0x08
#define INT_CTRL_M 0x12 #define INT_CTRL_M 0x12
#define INT_SRC_M 0x13 #define INT_SRC_M 0x13
@ -217,6 +218,14 @@ static const int ERROR = -1;
#define EXTERNAL_BUS 0 #define EXTERNAL_BUS 0
#endif #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[]); } extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
@ -289,9 +298,9 @@ private:
perf_counter_t _accel_sample_perf; perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf; perf_counter_t _mag_sample_perf;
perf_counter_t _accel_reschedules;
perf_counter_t _bad_registers; perf_counter_t _bad_registers;
perf_counter_t _bad_values; perf_counter_t _bad_values;
perf_counter_t _accel_duplicates;
uint8_t _register_wait; 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), _mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_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_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")),
_bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")), _bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")),
_accel_duplicates(perf_alloc(PC_COUNT, "lsm303d_accel_duplicates")),
_register_wait(0), _register_wait(0),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@ -622,7 +631,7 @@ LSM303D::~LSM303D()
perf_free(_mag_sample_perf); perf_free(_mag_sample_perf);
perf_free(_bad_registers); perf_free(_bad_registers);
perf_free(_bad_values); perf_free(_bad_values);
perf_free(_accel_reschedules); perf_free(_accel_duplicates);
} }
int int
@ -874,7 +883,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */ /* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */ /* 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 we need to start the poll state machine, do it */
if (want_start) if (want_start)
@ -1388,7 +1399,10 @@ LSM303D::start()
_mag_reports->flush(); _mag_reports->flush();
/* start polling at the specified rate */ /* 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); hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
} }
@ -1466,20 +1480,6 @@ LSM303D::measure()
check_registers(); 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) { if (_register_wait != 0) {
// we are waiting for some good transfers before using // we are waiting for some good transfers before using
// the sensor again. // the sensor again.
@ -1493,6 +1493,12 @@ LSM303D::measure()
raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; 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)); 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. * 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units) * 2) Subtract static offset (in SI units)
@ -1681,7 +1687,7 @@ LSM303D::print_info()
perf_print_counter(_mag_sample_perf); perf_print_counter(_mag_sample_perf);
perf_print_counter(_bad_registers); perf_print_counter(_bad_registers);
perf_print_counter(_bad_values); perf_print_counter(_bad_values);
perf_print_counter(_accel_reschedules); perf_print_counter(_accel_duplicates);
_accel_reports->print_info("accel reports"); _accel_reports->print_info("accel reports");
_mag_reports->print_info("mag reports"); _mag_reports->print_info("mag reports");
::printf("checked_next: %u\n", _checked_next); ::printf("checked_next: %u\n", _checked_next);

58
src/drivers/mpu6000/mpu6000.cpp
View File

@ -189,6 +189,14 @@
#define MPU6000_LOW_BUS_SPEED 1000*1000 #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 */ #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_gyro;
class MPU6000 : public device::SPI class MPU6000 : public device::SPI
@ -257,6 +265,7 @@ private:
perf_counter_t _bad_registers; perf_counter_t _bad_registers;
perf_counter_t _good_transfers; perf_counter_t _good_transfers;
perf_counter_t _reset_retries; perf_counter_t _reset_retries;
perf_counter_t _duplicates;
perf_counter_t _system_latency_perf; perf_counter_t _system_latency_perf;
perf_counter_t _controller_latency_perf; perf_counter_t _controller_latency_perf;
@ -287,6 +296,10 @@ private:
// last temperature reading for print_info() // last temperature reading for print_info()
float _last_temperature; float _last_temperature;
// keep last accel reading for duplicate detection
uint16_t _last_accel[3];
bool _got_duplicate;
/** /**
* Start automatic measurement. * 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")), _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")),
_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
_reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _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")), _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_register_wait(0), _register_wait(0),
@ -522,7 +536,9 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_rotation(rotation), _rotation(rotation),
_checked_next(0), _checked_next(0),
_in_factory_test(false), _in_factory_test(false),
_last_temperature(0) _last_temperature(0),
_last_accel{},
_got_duplicate(false)
{ {
// disable debug() calls // disable debug() calls
_debug_enabled = false; _debug_enabled = false;
@ -576,6 +592,8 @@ MPU6000::~MPU6000()
perf_free(_bad_transfers); perf_free(_bad_transfers);
perf_free(_bad_registers); perf_free(_bad_registers);
perf_free(_good_transfers); perf_free(_good_transfers);
perf_free(_reset_retries);
perf_free(_duplicates);
} }
int int
@ -1198,7 +1216,15 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */ /* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */ /* 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 we need to start the poll state machine, do it */
if (want_start) if (want_start)
@ -1476,7 +1502,10 @@ MPU6000::start()
_gyro_reports->flush(); _gyro_reports->flush();
/* start polling at the specified rate */ /* 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 void
@ -1578,14 +1607,32 @@ MPU6000::measure()
*/ */
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
check_registers();
// sensor transfer at high clock speed // sensor transfer at high clock speed
set_frequency(MPU6000_HIGH_BUS_SPEED); set_frequency(MPU6000_HIGH_BUS_SPEED);
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
return; 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 * Convert from big to little endian
*/ */
@ -1766,6 +1813,7 @@ MPU6000::print_info()
perf_print_counter(_bad_registers); perf_print_counter(_bad_registers);
perf_print_counter(_good_transfers); perf_print_counter(_good_transfers);
perf_print_counter(_reset_retries); perf_print_counter(_reset_retries);
perf_print_counter(_duplicates);
_accel_reports->print_info("accel queue"); _accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue"); _gyro_reports->print_info("gyro queue");
::printf("checked_next: %u\n", _checked_next); ::printf("checked_next: %u\n", _checked_next);

5
src/drivers/px4io/px4io.cpp
View File

@ -909,6 +909,9 @@ PX4IO::task_main()
goto out; goto out;
} }
/* Fetch initial flight termination circuit breaker state */
_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
/* poll descriptor */ /* poll descriptor */
pollfd fds[1]; pollfd fds[1];
fds[0].fd = _t_actuator_controls_0; 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); _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan);

4
src/lib/ecl/attitude_fw/ecl_controller.cpp
View File

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

13
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -397,16 +397,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
hrt_abstime vel_t = 0; hrt_abstime vel_t = 0;
bool vel_valid = false; 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; vel_valid = true;
if (global_pos_updated) { if (global_pos_updated) {
vel_t = global_pos.timestamp; 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) { 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)); 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 */ /* update mag declination rotation matrix */

14
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
View File

@ -108,11 +108,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
*/ */
PARAM_DEFINE_INT32(EKF_ATT_ENABLED, 1); 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) * 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->r1 = param_find("EKF_ATT_V4_R1");
h->r2 = param_find("EKF_ATT_V4_R2"); 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[0] = param_find("ATT_J11");
h->moment_inertia_J[1] = param_find("ATT_J22"); h->moment_inertia_J[1] = param_find("ATT_J22");
h->moment_inertia_J[2] = param_find("ATT_J33"); 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->r1, &(p->r[1]));
param_get(h->r2, &(p->r[2])); 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++) { for (int i = 0; i < 3; i++) {
param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
} }

68
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
View File

@ -41,10 +41,70 @@
#include <systemlib/param/param.h> #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); 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); 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_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);

10
src/modules/commander/commander.cpp
View File

@ -2164,10 +2164,12 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
if (set_normal_color) { if (set_normal_color) {
/* set 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); 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 { } else {
if (status_local->condition_global_position_valid) { if (status_local->condition_global_position_valid) {
rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_color(RGBLED_COLOR_GREEN);
@ -2801,7 +2803,7 @@ void *commander_low_prio_loop(void *arg)
need_param_autosave_timeout = 0; 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); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else { } else {

57
src/modules/commander/gyro_calibration.cpp
View File

@ -77,7 +77,7 @@ typedef struct {
struct gyro_report gyro_report_0; struct gyro_report gyro_report_0;
} gyro_worker_data_t; } 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); gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data);
unsigned calibration_counter[max_gyros] = { 0 }; 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_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 */ /* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) { 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 do_gyro_calibration(int mavlink_fd)
{ {
int res = OK; 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); mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
@ -200,31 +201,25 @@ int do_gyro_calibration(int mavlink_fd)
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
} }
// Calibrate right-side up
bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
int cancel_sub = calibrate_cancel_subscribe(); 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
calibrate_cancel_unsubscribe(cancel_sub);
for (unsigned s = 0; s < max_gyros; s++) { unsigned try_count = 0;
px4_close(worker_data.gyro_sensor_sub[s]); unsigned max_tries = 20;
} res = ERROR;
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) { if (cal_return == calibrate_return_cancelled) {
// Cancel message already sent, we are done here // Cancel message already sent, we are done here
return ERROR; res = ERROR;
break;
} else if (cal_return == calibrate_return_error) { } else if (cal_return == calibrate_return_error) {
res = ERROR; res = ERROR;
}
if (res == OK) { } else {
/* check offsets */ /* check offsets */
float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset; 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 ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
@ -233,15 +228,33 @@ int do_gyro_calibration(int mavlink_fd)
/* maximum allowable calibration error in radians */ /* maximum allowable calibration error in radians */
const float maxoff = 0.0055f; const float maxoff = 0.0055f;
if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) || if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
!PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) || !isfinite(worker_data.gyro_scale[0].y_offset) ||
!PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) || !isfinite(worker_data.gyro_scale[0].z_offset) ||
fabsf(xdiff) > maxoff || fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff || fabsf(ydiff) > maxoff ||
fabsf(zdiff) > 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"); mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
res = ERROR; res = ERROR;
} }
calibrate_cancel_unsubscribe(cancel_sub);
for (unsigned s = 0; s < max_gyros; s++) {
px4_close(worker_data.gyro_sensor_sub[s]);
} }
if (res == OK) { if (res == OK) {

67
src/modules/commander/mag_calibration.cpp
View File

@ -53,6 +53,7 @@
#include <fcntl.h> #include <fcntl.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h> #include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <mavlink/mavlink_log.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] 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); 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; uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
unsigned poll_errcount = 0; unsigned poll_errcount = 0;

41
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -304,20 +304,29 @@ 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); DIR *dp = opendir(dirPath);
if (dp == nullptr) { 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 #ifdef MAVLINK_FTP_DEBUG
warnx("FTP: list %s offset %d", dirPath, payload->offset); warnx("FTP: list %s offset %d", dirPath, payload->offset);
#endif #endif
ErrorCode errorCode = kErrNone;
struct dirent entry, *result = nullptr; struct dirent entry, *result = nullptr;
unsigned offset = 0;
// move to the requested offset // move to the requested offset
seekdir(dp, payload->offset); seekdir(dp, payload->offset);
@ -325,9 +334,16 @@ MavlinkFTP::_workList(PayloadHeader* payload)
for (;;) { for (;;) {
// read the directory entry // read the directory entry
if (readdir_r(dp, &entry, &result)) { 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? // no more entries?
@ -365,7 +381,8 @@ MavlinkFTP::_workList(PayloadHeader* payload)
#else #else
case DT_DIR: case DT_DIR:
#endif #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 // Don't bother sending these back
direntType = kDirentSkip; direntType = kDirentSkip;
} else { } else {
@ -499,6 +516,7 @@ MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id)
// Setup for streaming sends // Setup for streaming sends
_session_info.stream_download = true; _session_info.stream_download = true;
_session_info.stream_offset = payload->offset; _session_info.stream_offset = payload->offset;
_session_info.stream_chunk_transmitted = 0;
_session_info.stream_seq_number = payload->seq_number + 1; _session_info.stream_seq_number = payload->seq_number + 1;
_session_info.stream_target_system_id = target_system_id; _session_info.stream_target_system_id = target_system_id;
@ -885,6 +903,7 @@ void MavlinkFTP::send(const hrt_abstime t)
} else { } else {
payload->size = bytes_read; payload->size = bytes_read;
_session_info.stream_offset += 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 #ifndef MAVLINK_FTP_UNIT_TEST
if (max_bytes_to_send < (get_size()*2)) { if (max_bytes_to_send < (get_size()*2)) {
more_data = false; more_data = false;
/* perform transfers in 35K chunks - this is determined empirical */
if (_session_info.stream_chunk_transmitted > 35000) {
payload->burst_complete = true; payload->burst_complete = true;
_session_info.stream_download = false; _session_info.stream_download = false;
_session_info.stream_chunk_transmitted = 0;
}
} else { } else {
#endif #endif
more_data = true; more_data = true;

3
src/modules/mavlink/mavlink_ftp.h
View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -165,6 +165,7 @@ private:
uint32_t stream_offset; uint32_t stream_offset;
uint16_t stream_seq_number; uint16_t stream_seq_number;
uint8_t stream_target_system_id; uint8_t stream_target_system_id;
unsigned stream_chunk_transmitted;
}; };
struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session

14
src/modules/mavlink/mavlink_main.cpp
View File

@ -93,7 +93,7 @@
static const int ERROR = -1; static const int ERROR = -1;
#define DEFAULT_DEVICE_NAME "/dev/ttyS1" #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 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. #define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it.
@ -262,7 +262,9 @@ Mavlink::~Mavlink()
} while (_task_running); } while (_task_running);
} }
if (_mavlink_instances) {
LL_DELETE(_mavlink_instances, this); LL_DELETE(_mavlink_instances, this);
}
} }
void void
@ -1025,7 +1027,7 @@ void
Mavlink::adjust_stream_rates(const float multiplier) Mavlink::adjust_stream_rates(const float multiplier)
{ {
/* do not allow to push us to zero */ /* do not allow to push us to zero */
if (multiplier < 0.01f) { if (multiplier < 0.0005f) {
return; return;
} }
@ -1036,9 +1038,9 @@ Mavlink::adjust_stream_rates(const float multiplier)
unsigned interval = stream->get_interval(); unsigned interval = stream->get_interval();
interval /= multiplier; interval /= multiplier;
/* allow max ~600 Hz */ /* allow max ~2000 Hz */
if (interval < 1600) { if (interval < 1600) {
interval = 1600; interval = 500;
} }
/* set new interval */ /* set new interval */
@ -1421,7 +1423,7 @@ Mavlink::task_main(int argc, char *argv[])
/* MAVLINK_FTP stream */ /* MAVLINK_FTP stream */
_mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this); _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); LL_APPEND(_streams, _mavlink_ftp);
/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on /* 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 */ /* 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 */ /* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this); LL_APPEND(_mavlink_instances, this);

4
src/modules/mavlink/mavlink_parameters.cpp
View File

@ -181,8 +181,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
void void
MavlinkParametersManager::send(const hrt_abstime t) 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 */ /* skip if no space is available */
if (_mavlink->get_free_tx_buf() < get_size()) { if (_mavlink->get_free_tx_buf() < get_size()) {

50
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -131,6 +131,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_hil_local_alt0(0.0f), _hil_local_alt0(0.0f),
_hil_local_proj_ref{}, _hil_local_proj_ref{},
_offboard_control_mode{}, _offboard_control_mode{},
_att_sp{},
_rates_sp{}, _rates_sp{},
_time_offset_avg_alpha(0.6), _time_offset_avg_alpha(0.6),
_time_offset(0) _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 * 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 * 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 */ /* 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 { } 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; _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 */ /* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(_offboard_control_mode.ignore_attitude)) { 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) { 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 { } 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 ///XXX add support for ignoring individual axes
if (!(_offboard_control_mode.ignore_bodyrate)) { if (!(_offboard_control_mode.ignore_bodyrate)) {
_rates_sp.timestamp = hrt_absolute_time(); _rates_sp.timestamp = hrt_absolute_time();
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.roll = set_attitude_target.body_roll_rate;
_rates_sp.pitch = set_attitude_target.body_pitch_rate; _rates_sp.pitch = set_attitude_target.body_pitch_rate;
_rates_sp.yaw = set_attitude_target.body_yaw_rate; _rates_sp.yaw = set_attitude_target.body_yaw_rate;
}
if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
_rates_sp.thrust = set_attitude_target.thrust; _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)); memset(&hil_global_pos, 0, sizeof(hil_global_pos));
hil_global_pos.timestamp = timestamp; 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.alt = hil_state.alt / 1000.0f;
hil_global_pos.vel_n = hil_state.vx / 100.0f; hil_global_pos.vel_n = hil_state.vx / 100.0f;
hil_global_pos.vel_e = hil_state.vy / 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; param.sched_priority = SCHED_PRIORITY_MAX - 80;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param); (void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 1800); pthread_attr_setstacksize(&receiveloop_attr, 2100);
pthread_t thread; pthread_t thread;
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);

1
src/modules/mavlink/mavlink_receiver.h
View File

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

34
src/modules/mc_att_control_multiplatform/mc_att_control.cpp
View File

@ -75,23 +75,23 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
/* parameters */ /* parameters */
_params_handles({ _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 */ /* performance counters */

34
src/modules/mc_att_control_multiplatform/mc_att_control_params.c
View File

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

36
src/modules/mc_att_control_multiplatform/mc_att_control_params.h
View File

@ -33,7 +33,7 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file mc_att_control_params.h * @file MP_att_control_params.h
* Parameters for multicopter attitude controller. * Parameters for multicopter attitude controller.
* *
* @author Tobias Naegeli <naegelit@student.ethz.ch> * @author Tobias Naegeli <naegelit@student.ethz.ch>
@ -43,20 +43,20 @@
*/ */
#pragma once #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

19
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1105,7 +1105,6 @@ MulticopterPositionControl::task_main()
/* derivative of velocity error, not includes setpoint acceleration */ /* 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; 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 */ /* 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; 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)); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true; _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 */ /* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = R.to_euler(); math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0); _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)); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true; _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.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f; _att_sp.pitch_body = 0.0f;
} }
@ -1388,12 +1397,20 @@ MulticopterPositionControl::task_main()
math::Matrix<3,3> R_sp; math::Matrix<3,3> R_sp;
R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); 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)); 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(); _att_sp.timestamp = hrt_absolute_time();
} }
else { else {
reset_yaw_sp = true; reset_yaw_sp = true;
} }
/* update previous velocity for velocity controller D part */
_vel_prev = _vel;
/* publish attitude setpoint /* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled, * 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 * in this case the attitude setpoint is published by the mavlink app

42
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
View File

@ -69,27 +69,27 @@ MulticopterPositionControl::MulticopterPositionControl() :
/* parameters */ /* parameters */
_params_handles({ _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_alt(0.0f),
_ref_timestamp(0), _ref_timestamp(0),

40
src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
View File

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

40
src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
View File

@ -41,24 +41,24 @@
#pragma once #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

10
src/modules/navigator/rtl.cpp
View File

@ -103,6 +103,7 @@ RTL::on_activation()
_mission_item.altitude_is_relative = false; _mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt; _mission_item.altitude = _navigator->get_global_position()->alt;
} }
} }
set_rtl_item(); set_rtl_item();
@ -146,7 +147,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true; _mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD; _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)); (int)(climb_alt - _navigator->get_home_position()->alt));
break; break;
} }
@ -177,7 +179,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true; _mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD; _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)); (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break; break;
} }
@ -197,7 +200,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = false; _mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD; _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)); (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break; break;
} }

2
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f);
* @max 10.0 * @max 10.0
* @group Position Estimator INAV * @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 * Z axis weight for sonar

4
src/modules/sdlog2/sdlog2.c
View File

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

1
src/modules/sensors/sensors.cpp
View File

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

127
src/modules/systemlib/mixer/mixer_multirotor.cpp
View File

@ -200,18 +200,31 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
unsigned unsigned
MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) 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); 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 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 yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
float thrust = constrain(get_control(0, 3), 0.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 min_out = 0.0f;
float max_out = 0.0f; float max_out = 0.0f;
// clean register for saturation status flags
if (status_reg != NULL) { if (status_reg != NULL) {
(*status_reg) = 0; (*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 */ /* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) { 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; 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 */ /* calculate min and max output values */
if (out < min_out) { if (out < min_out) {
min_out = out; min_out = out;
@ -240,51 +245,89 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
outputs[i] = out; 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; }
else {
/* update max output value */ boost = max_thrust_diff;
if (out > max_out) { roll_pitch_scale = (thrust + boost)/(thrust - min_out);
max_out = out; }
} }
else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
outputs[i] = out; 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) { if(status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; (*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) { if(status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; (*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++) { 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; return _rotor_count;