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
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.004
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 2.5
param set MC_YAWRATE_P 0.25

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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.
#
ms5611 start
adc start
if ms5611 start
then
fi
if adc start
then
fi
if ver hwcmp PX4FMU_V2
then

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

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

16
launch/ardrone.launch
View File

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

16
launch/iris.launch
View File

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

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

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

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

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

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

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

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

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

57
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
#endif
/*
we set the timer interrupt to run a bit faster than the desired
sample rate and then throw away duplicates using the data ready bit.
This time reduction is enough to cope with worst case timing jitter
due to other timers
*/
#define L3GD20_TIMER_REDUCTION 200
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI
@ -236,9 +244,9 @@ private:
unsigned _read;
perf_counter_t _sample_perf;
perf_counter_t _reschedules;
perf_counter_t _errors;
perf_counter_t _bad_registers;
perf_counter_t _duplicates;
uint8_t _register_wait;
@ -410,9 +418,9 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
_read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
_bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "l3gd20_duplicates")),
_register_wait(0),
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
@ -449,9 +457,9 @@ L3GD20::~L3GD20()
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_reschedules);
perf_free(_errors);
perf_free(_bad_registers);
perf_free(_duplicates);
}
int
@ -608,7 +616,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
_call_interval = ticks;
_call.period = _call_interval - L3GD20_TIMER_REDUCTION;
/* adjust filters */
float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
@ -834,7 +844,10 @@ L3GD20::start()
_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
hrt_call_every(&_call,
1000,
_call_interval - L3GD20_TIMER_REDUCTION,
(hrt_callout)&L3GD20::measure_trampoline, this);
}
void
@ -899,12 +912,6 @@ L3GD20::measure_trampoline(void *arg)
dev->measure();
}
#ifdef GPIO_EXTI_GYRO_DRDY
# define L3GD20_USE_DRDY 1
#else
# define L3GD20_USE_DRDY 0
#endif
void
L3GD20::check_registers(void)
{
@ -954,33 +961,17 @@ L3GD20::measure()
check_registers();
#if L3GD20_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
perf_end(_sample_perf);
return;
}
#endif
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
#if L3GD20_USE_DRDY
if (_bus == PX4_SPI_BUS_SENSORS && (raw_report.status & 0xF) != 0xF) {
/*
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
return;
if (!(raw_report.status & STATUS_ZYXDA)) {
perf_end(_sample_perf);
perf_count(_duplicates);
return;
}
#endif
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
@ -1071,9 +1062,9 @@ L3GD20::print_info()
{
printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf);
perf_print_counter(_reschedules);
perf_print_counter(_errors);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);
_reports->print_info("report queue");
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i=0; 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 REG_STATUS_A_NEW_ZYXADA 0x08
#define INT_CTRL_M 0x12
#define INT_SRC_M 0x13
@ -217,6 +218,14 @@ static const int ERROR = -1;
#define EXTERNAL_BUS 0
#endif
/*
we set the timer interrupt to run a bit faster than the desired
sample rate and then throw away duplicates using the data ready bit.
This time reduction is enough to cope with worst case timing jitter
due to other timers
*/
#define LSM303D_TIMER_REDUCTION 200
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
@ -289,9 +298,9 @@ private:
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
perf_counter_t _accel_reschedules;
perf_counter_t _bad_registers;
perf_counter_t _bad_values;
perf_counter_t _accel_duplicates;
uint8_t _register_wait;
@ -561,9 +570,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
_bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")),
_bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")),
_accel_duplicates(perf_alloc(PC_COUNT, "lsm303d_accel_duplicates")),
_register_wait(0),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@ -622,7 +631,7 @@ LSM303D::~LSM303D()
perf_free(_mag_sample_perf);
perf_free(_bad_registers);
perf_free(_bad_values);
perf_free(_accel_reschedules);
perf_free(_accel_duplicates);
}
int
@ -874,7 +883,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_accel_call.period = _call_accel_interval = ticks;
_call_accel_interval = ticks;
_accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION;
/* if we need to start the poll state machine, do it */
if (want_start)
@ -1388,7 +1399,10 @@ LSM303D::start()
_mag_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
hrt_call_every(&_accel_call,
1000,
_call_accel_interval - LSM303D_TIMER_REDUCTION,
(hrt_callout)&LSM303D::measure_trampoline, this);
hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
}
@ -1466,20 +1480,6 @@ LSM303D::measure()
check_registers();
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value.
// Note that DRDY is not available when the lsm303d is
// connected on the external bus
#ifdef GPIO_EXTI_ACCEL_DRDY
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
perf_end(_accel_sample_perf);
return;
}
#endif
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again.
@ -1493,6 +1493,12 @@ LSM303D::measure()
raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
if (!(raw_accel_report.status & REG_STATUS_A_NEW_ZYXADA)) {
perf_end(_accel_sample_perf);
perf_count(_accel_duplicates);
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
@ -1681,7 +1687,7 @@ LSM303D::print_info()
perf_print_counter(_mag_sample_perf);
perf_print_counter(_bad_registers);
perf_print_counter(_bad_values);
perf_print_counter(_accel_reschedules);
perf_print_counter(_accel_duplicates);
_accel_reports->print_info("accel reports");
_mag_reports->print_info("mag reports");
::printf("checked_next: %u\n", _checked_next);

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

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

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

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

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;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (minspeed + maxspeed);
airspeed_result = 0.5f * (minspeed + maxspeed);
} else if (airspeed < minspeed) {
airspeed = minspeed;
airspeed_result = minspeed;
}
return airspeed_result;
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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
* @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

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

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

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

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

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