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

Merge branch 'master' of https://github.com/PX4/Firmware into bottledrop

This commit is contained in:
Juchli D 2013-11-15 10:34:12 +01:00
parent adee479782 2116966b1e
commit 4b8c3c38cd
39 changed files with 307 additions and 336 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
View File

@ -55,11 +55,6 @@ hil mode_pwm
# #
param set MAV_TYPE 1 param set MAV_TYPE 1
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #

5
ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
View File

@ -60,11 +60,6 @@ hil mode_pwm
# #
param set MAV_TYPE 2 param set MAV_TYPE 2
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #

5
ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
View File

@ -52,11 +52,6 @@ hil mode_pwm
# #
param set MAV_TYPE 1 param set MAV_TYPE 1
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #

5
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
View File

@ -60,11 +60,6 @@ hil mode_pwm
# #
param set MAV_TYPE 2 param set MAV_TYPE 2
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #

5
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
View File

@ -55,11 +55,6 @@ hil mode_pwm
# #
param set MAV_TYPE 1 param set MAV_TYPE 1
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #

35
ROMFS/px4fmu_common/init.d/100_mpx_easystar
View File

@ -52,8 +52,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@ -61,46 +59,27 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi fi
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
# #
# Load mixer and start controllers (depends on px4io) # Load mixer and start controllers (depends on px4io)
# #
if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ]
then then
echo "Using FMU_RET mixer from sd card" echo "Using /fs/microsd/etc/mixers/FMU_RET.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix
else else
echo "Using standard FMU_RET mixer" echo "Using /etc/mixers/FMU_RET.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
fi fi
fw_att_control start
fw_pos_control_l1 start #
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ] if [ $EXIT_ON_END == yes ]
then then

40
ROMFS/px4fmu_common/init.d/101_hk_bixler
View File

@ -52,8 +52,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@ -61,39 +59,27 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi fi
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
# #
# Load mixer and start controllers (depends on px4io) # Load mixer and start controllers (depends on px4io)
# #
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
fw_att_control start then
fw_pos_control_l1 start echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ] if [ $EXIT_ON_END == yes ]
then then

5
ROMFS/px4fmu_common/init.d/10_dji_f330
View File

@ -59,10 +59,7 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
else else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
@ -83,7 +80,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
pwm rate -c 1234 -r 400 pwm rate -c 1234 -r 400
# #
# Set disarmed, min and max PWM signals # Set disarmed, min and max PWM signals (for DJI ESCs)
# #
pwm disarmed -c 1234 -p 900 pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200 pwm min -c 1234 -p 1200

2
ROMFS/px4fmu_common/init.d/11_dji_f450
View File

@ -73,7 +73,7 @@ pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800 pwm max -c 1234 -p 1800
# #
# Start common for all multirotors apps # Start common multirotor apps
# #
sh /etc/init.d/rc.multirotor sh /etc/init.d/rc.multirotor

15
ROMFS/px4fmu_common/init.d/12-13_hex
View File

@ -61,10 +61,6 @@ then
usleep 5000 usleep 5000
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900 900 900
px4io min 1200 1200 1200 1200 1200 1200
px4io max 1900 1900 1900 1900 1900 1900
else else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
@ -77,12 +73,19 @@ fi
# #
# Load mixer # Load mixer
# #
mixer load /dev/pwm_output $MIXER mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix
# #
# Set PWM output frequency to 400 Hz # Set PWM output frequency to 400 Hz
# #
pwm -u 400 -m 0xff pwm rate -c 123456 -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 123456 -p 900
pwm min -c 123456 -p 1100
pwm max -c 123456 -p 1900
# #
# Start common for all multirotors apps # Start common for all multirotors apps

10
ROMFS/px4fmu_common/init.d/15_tbs_discovery
View File

@ -31,7 +31,7 @@ fi
# MAV_TYPE 2 = quadrotor # MAV_TYPE 2 = quadrotor
# #
param set MAV_TYPE 2 param set MAV_TYPE 2
set EXIT_ON_END no set EXIT_ON_END no
# #
@ -43,8 +43,6 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
else else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
@ -66,11 +64,11 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
pwm rate -c 1234 -r 400 pwm rate -c 1234 -r 400
# #
# Set disarmed, min and max PWM signals (for DJI ESCs) # Set disarmed, min and max PWM signals
# #
pwm disarmed -c 1234 -p 900 pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200 pwm min -c 1234 -p 1100
pwm max -c 1234 -p 1800 pwm max -c 1234 -p 1900
# #
# Start common for all multirotors apps # Start common for all multirotors apps

7
ROMFS/px4fmu_common/init.d/16_3dr_iris
View File

@ -31,7 +31,7 @@ fi
# MAV_TYPE 2 = quadrotor # MAV_TYPE 2 = quadrotor
# #
param set MAV_TYPE 2 param set MAV_TYPE 2
set EXIT_ON_END no set EXIT_ON_END no
# #
@ -43,8 +43,6 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
else else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
@ -69,8 +67,7 @@ pwm rate -c 1234 -r 400
# Set disarmed, min and max PWM signals # Set disarmed, min and max PWM signals
# #
pwm disarmed -c 1234 -p 900 pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200 pwm min -c 1234 -p 1050
pwm max -c 1234 -p 1800
# #
# Start common for all multirotors apps # Start common for all multirotors apps

42
ROMFS/px4fmu_common/init.d/30_io_camflyer
View File

@ -30,8 +30,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@ -39,41 +37,29 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi fi
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
# #
# Load mixer and start controllers (depends on px4io) # Load mixer and start controllers (depends on px4io)
# #
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
fw_att_control start then
fw_pos_control_l1 start echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ] if [ $EXIT_ON_END == yes ]
then then
exit exit
fi fi

42
ROMFS/px4fmu_common/init.d/31_io_phantom
View File

@ -52,8 +52,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@ -61,41 +59,29 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi fi
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
# #
# Load mixer and start controllers (depends on px4io) # Load mixer and start controllers (depends on px4io)
# #
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
fw_att_control start then
fw_pos_control_l1 start echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ] if [ $EXIT_ON_END == yes ]
then then
exit exit
fi fi

31
ROMFS/px4fmu_common/init.d/32_skywalker_x5
View File

@ -30,8 +30,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@ -39,32 +37,10 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi fi
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
# #
# Load mixer and start controllers (depends on px4io) # Load mixer and start controllers (depends on px4io)
@ -77,8 +53,11 @@ else
echo "Using /etc/mixers/FMU_Q.mix" echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi fi
fw_att_control start
fw_pos_control_l1 start #
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ] if [ $EXIT_ON_END == yes ]
then then

43
ROMFS/px4fmu_common/init.d/666_fmu_q_x550
View File

@ -1,6 +1,6 @@
#!nsh #!nsh
echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO"
# #
# Load default params for this platform # Load default params for this platform
@ -33,17 +33,27 @@ fi
# MAV_TYPE 2 = quadrotor # MAV_TYPE 2 = quadrotor
# #
param set MAV_TYPE 2 param set MAV_TYPE 2
# set EXIT_ON_END no
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
# #
# Start PWM output # Start and configure PX4IO or FMU interface
# #
fmu mode_pwm if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
# #
# Load mixer # Load mixer
@ -55,10 +65,19 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
# #
pwm rate -c 1234 -r 400 pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1100
pwm max -c 1234 -p 1900
# #
# Start common for all multirotors apps # Start common for all multirotors apps
# #
sh /etc/init.d/rc.multirotor sh /etc/init.d/rc.multirotor
# Exit, because /dev/ttyS0 is needed for MAVLink if [ $EXIT_ON_END == yes ]
exit then
exit
fi

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

@ -84,10 +84,10 @@ then
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
else else
fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
usleep 5000 usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi fi
@ -102,13 +102,6 @@ else
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
fi fi
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
# #
# Start common for all multirotors apps # Start common for all multirotors apps
# #

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

@ -60,9 +60,7 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start -d /dev/ttyS1 -b 57600 mavlink start -d /dev/ttyS1 -b 57600
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
else else
fmu mode_pwm fmu mode_pwm

34
ROMFS/px4fmu_common/init.d/rc.fixedwing Normal file
View File

@ -0,0 +1,34 @@
#!nsh
#
# Standard everything needed for fixedwing except mixer, actuator output and mavlink
#
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
#
# Start attitude controller
#
fw_att_control start
#
# Start the position controller
#
fw_pos_control_l1 start

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

@ -1,6 +1,6 @@
#!nsh #!nsh
# #
# Standard everything needed for multirotors except mixer, output and mavlink # Standard everything needed for multirotors except mixer, actuator output and mavlink
# #
# #

5
ROMFS/px4fmu_common/init.d/rcS
View File

@ -105,6 +105,11 @@ then
blinkm systemstate blinkm systemstate
fi fi
fi fi
#
# Start the Commander (needs to be this early for in-air-restarts)
#
commander start
if param compare SYS_AUTOSTART 1000 if param compare SYS_AUTOSTART 1000
then then

2
src/drivers/airspeed/airspeed.h
View File

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

17
src/drivers/boards/px4fmu-v1/px4fmu_init.c
View File

@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void)
* If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
* Keep the SPI2 init optional and conditionally initialize the ADC pins * Keep the SPI2 init optional and conditionally initialize the ADC pins
*/ */
spi2 = up_spiinitialize(2);
if (!spi2) { #ifdef CONFIG_STM32_SPI2
message("[boot] Enabling IN12/13 instead of SPI2\n"); spi2 = up_spiinitialize(2);
/* no SPI2, use pins for ADC */
stm32_configgpio(GPIO_ADC1_IN12);
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
} else {
/* Default SPI2 to 1MHz and de-assert the known chip selects. */ /* Default SPI2 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi2, 10000000); SPI_SETFREQUENCY(spi2, 10000000);
SPI_SETBITS(spi2, 8); SPI_SETBITS(spi2, 8);
@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
} #else
spi2 = NULL;
message("[boot] Enabling IN12/13 instead of SPI2\n");
/* no SPI2, use pins for ADC */
stm32_configgpio(GPIO_ADC1_IN12);
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
#endif
/* Get the SPI port for the microSD slot */ /* Get the SPI port for the microSD slot */

2
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -180,7 +180,7 @@ ETSAirspeed::collect()
differential_pressure_s report; differential_pressure_s report;
report.timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors); report.error_count = perf_event_count(_comms_errors);
report.differential_pressure_pa = diff_pres_pa; report.differential_pressure_pa = (float)diff_pres_pa;
report.voltage = 0; report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa; report.max_differential_pressure_pa = _max_differential_pressure_pa;

10
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -36,6 +36,7 @@
* @author Lorenz Meier * @author Lorenz Meier
* @author Sarthak Kaingade * @author Sarthak Kaingade
* @author Simon Wilks * @author Simon Wilks
* @author Thomas Gubler
* *
* Driver for the MEAS Spec series connected via I2C. * Driver for the MEAS Spec series connected via I2C.
* *
@ -76,6 +77,7 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_airspeed.h> #include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@ -184,7 +186,7 @@ MEASAirspeed::collect()
//diff_pres_pa -= _diff_pres_offset; //diff_pres_pa -= _diff_pres_offset;
int16_t dp_raw = 0, dT_raw = 0; int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1]; dp_raw = (val[0] << 8) + val[1];
dp_raw = 0x3FFF & dp_raw; dp_raw = 0x3FFF & dp_raw; //mask the used bits
dT_raw = (val[2] << 8) + val[3]; dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5; dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50; float temperature = ((200 * dT_raw) / 2047) - 50;
@ -193,7 +195,11 @@ MEASAirspeed::collect()
// Calculate differential pressure. As its centered around 8000 // Calculate differential pressure. As its centered around 8000
// and can go positive or negative, enforce absolute value // and can go positive or negative, enforce absolute value
uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); // uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset);
struct differential_pressure_s report; struct differential_pressure_s report;
// Track maximum differential pressure measured (so we can work out top speed). // Track maximum differential pressure measured (so we can work out top speed).

5
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -62,8 +62,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
/* get the usual dt estimate */ /* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run); uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time(); _last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
float dt = dt_micros / 1000000;
/* lock integral for long intervals */ /* lock integral for long intervals */
if (dt_micros > 500000) if (dt_micros > 500000)
@ -115,7 +114,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
_rate_error = _rate_setpoint - pitch_rate; _rate_error = _rate_setpoint - pitch_rate;
float ilimit_scaled = 0.0f; float ilimit_scaled = _integrator_max * scaler;
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {

7
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -64,8 +64,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
/* get the usual dt estimate */ /* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run); uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time(); _last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; /* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc; float k_i_rate = _k_i * _tc;
@ -90,7 +93,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
_rate_error = _rate_setpoint - roll_rate; _rate_error = _rate_setpoint - roll_rate;
float ilimit_scaled = 0.0f; float ilimit_scaled = _integrator_max * scaler;
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {

9
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
View File

@ -34,7 +34,7 @@
/** /**
* @file KalmanNav.cpp * @file KalmanNav.cpp
* *
* kalman filter navigation code * Kalman filter navigation code
*/ */
#include <poll.h> #include <poll.h>
@ -228,10 +228,7 @@ void KalmanNav::update()
updateSubscriptions(); updateSubscriptions();
// initialize attitude when sensors online // initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate && if (!_attitudeInitialized && sensorsUpdate) {
_sensors.accelerometer_counter > 10 &&
_sensors.gyro_counter > 10 &&
_sensors.magnetometer_counter > 10) {
if (correctAtt() == ret_ok) _attitudeInitCounter++; if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) { if (_attitudeInitCounter > 100) {
@ -643,7 +640,7 @@ int KalmanNav::correctAtt()
if (beta > _faultAtt.get()) { if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta); warnx("fault in attitude: beta = %8.4f", (double)beta);
warnx("y:\n"); y.print(); warnx("y:"); y.print();
} }
// update quaternions from euler // update quaternions from euler

44
src/modules/commander/airspeed_calibration.cpp
View File

@ -37,12 +37,15 @@
*/ */
#include "airspeed_calibration.h" #include "airspeed_calibration.h"
#include "calibration_messages.h"
#include "commander_helper.h" #include "commander_helper.h"
#include <stdio.h> #include <stdio.h>
#include <fcntl.h>
#include <poll.h> #include <poll.h>
#include <math.h> #include <math.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_airspeed.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h> #include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
@ -55,10 +58,13 @@
#endif #endif
static const int ERROR = -1; static const int ERROR = -1;
static const char *sensor_name = "dpress";
int do_airspeed_calibration(int mavlink_fd) int do_airspeed_calibration(int mavlink_fd)
{ {
/* give directions */ /* give directions */
mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
const int calibration_count = 2500; const int calibration_count = 2500;
@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd)
int calibration_counter = 0; int calibration_counter = 0;
float diff_pres_offset = 0.0f; float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
0.0f,
1.0f,
};
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
}
close(fd);
}
if (!paramreset_successful) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
while (calibration_counter < calibration_count) { while (calibration_counter < calibration_count) {
/* wait blocking for new data */ /* wait blocking for new data */
@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd)
diff_pres_offset += diff_pres.differential_pressure_pa; diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++; calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
} else if (poll_ret == 0) { } else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */ /* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }
@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (isfinite(diff_pres_offset)) { if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!"); mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }
@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) { if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed"); warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration"); mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }
mavlink_log_info(mavlink_fd, "airspeed calibration done"); mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral();
close(diff_pres_sub); close(diff_pres_sub);
return OK; return OK;
} else { } else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }

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

@ -116,6 +116,8 @@ extern struct system_load_s system_load;
#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define MAVLINK_OPEN_INTERVAL 50000
#define STICK_ON_OFF_LIMIT 0.75f #define STICK_ON_OFF_LIMIT 0.75f
#define STICK_THRUST_RANGE 1.0f #define STICK_THRUST_RANGE 1.0f
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
/* try again later */
usleep(20000);
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
}
}
/* Main state machine */ /* Main state machine */
/* make sure we are in preflight state */ /* make sure we are in preflight state */
memset(&status, 0, sizeof(status)); memset(&status, 0, sizeof(status));
@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[])
while (!thread_should_exit) { while (!thread_should_exit) {
if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
/* try to open the mavlink log device every once in a while */
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
/* update parameters */ /* update parameters */
orb_check(param_changed_sub, &updated); orb_check(param_changed_sub, &updated);

67
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.p_i = param_find("FW_P_I"); _parameter_handles.p_i = param_find("FW_P_I");
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
_parameter_handles.p_integrator_max = param_find("FW_P_integrator_max"); _parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
_parameter_handles.r_p = param_find("FW_R_P"); _parameter_handles.r_p = param_find("FW_R_P");
_parameter_handles.r_d = param_find("FW_R_D"); _parameter_handles.r_d = param_find("FW_R_D");
_parameter_handles.r_i = param_find("FW_R_I"); _parameter_handles.r_i = param_find("FW_R_I");
_parameter_handles.r_integrator_max = param_find("FW_R_integrator_max"); _parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
_parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX");
_parameter_handles.y_p = param_find("FW_Y_P"); _parameter_handles.y_p = param_find("FW_Y_P");
_parameter_handles.y_i = param_find("FW_Y_I"); _parameter_handles.y_i = param_find("FW_Y_I");
_parameter_handles.y_d = param_find("FW_Y_D"); _parameter_handles.y_d = param_find("FW_Y_D");
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
_parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max"); _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main()
} }
} }
float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, if (isfinite(roll_sp) && isfinite(pitch_sp)) {
airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
_parameters.airspeed_min, _parameters.airspeed_max, airspeed); lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
/* throttle passed through */ float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", /* throttle passed through */
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
// _actuators.control[2], _actuators.control[3]);
/* // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
* Lazily publish the rate setpoint (for analysis, the actuators are published below) // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
* only once available // _actuators.control[2], _actuators.control[3]);
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = 0.0f; // XXX not yet implemented
rates_sp.timestamp = hrt_absolute_time(); /*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = 0.0f; // XXX not yet implemented
if (_rate_sp_pub > 0) { rates_sp.timestamp = hrt_absolute_time();
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else { if (_rate_sp_pub > 0) {
/* advertise and publish */ /* publish the attitude setpoint */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else {
/* advertise and publish */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
}
} }
} else { } else {

1
src/modules/mavlink/missionlib.c
View File

@ -64,6 +64,7 @@
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
#include "geo/geo.h"
#include "waypoints.h" #include "waypoints.h"
#include "orb_topics.h" #include "orb_topics.h"
#include "missionlib.h" #include "missionlib.h"

28
src/modules/mavlink/orb_listener.c
View File

@ -75,6 +75,7 @@ struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_effective_0; struct actuator_controls_effective_s actuators_effective_0;
struct actuator_controls_s actuators_0; struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
struct airspeed_s airspeed;
struct mavlink_subscriptions mavlink_subs; struct mavlink_subscriptions mavlink_subs;
@ -92,6 +93,8 @@ static unsigned int gps_counter;
*/ */
static uint64_t last_sensor_timestamp; static uint64_t last_sensor_timestamp;
static hrt_abstime last_sent_vfr = 0;
static void *uorb_receive_thread(void *arg); static void *uorb_receive_thread(void *arg);
struct listener { struct listener {
@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l)
/* copy attitude data into local buffer */ /* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
if (gcs_link) if (gcs_link) {
/* send sensor values */ /* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0, mavlink_msg_attitude_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000, last_sensor_timestamp / 1000,
@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l)
att.pitchspeed, att.pitchspeed,
att.yawspeed); att.yawspeed);
/* limit VFR message rate to 10Hz */
hrt_abstime t = hrt_absolute_time();
if (t >= last_sent_vfr + 100000) {
last_sent_vfr = t;
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
}
}
attitude_counter++; attitude_counter++;
} }
@ -681,17 +695,7 @@ l_home(const struct listener *l)
void void
l_airspeed(const struct listener *l) l_airspeed(const struct listener *l)
{ {
struct airspeed_s airspeed;
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
float alt = global_pos.relative_alt;
float climb = -global_pos.vz;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
} }
void void
@ -849,7 +853,7 @@ uorb_receive_start(void)
mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
/* --- AIRSPEED / VFR / HUD --- */ /* --- AIRSPEED --- */
mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */

52
src/modules/multirotor_att_control/multirotor_rate_control.c
View File

@ -59,31 +59,23 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */ PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */
struct mc_rate_control_params { struct mc_rate_control_params {
float yawrate_p; float yawrate_p;
float yawrate_d; float yawrate_d;
float yawrate_i; float yawrate_i;
//float yawrate_awu;
//float yawrate_lim;
float attrate_p; float attrate_p;
float attrate_d; float attrate_d;
float attrate_i; float attrate_i;
//float attrate_awu;
//float attrate_lim;
float rate_lim; float rate_lim;
}; };
@ -93,14 +85,10 @@ struct mc_rate_control_param_handles {
param_t yawrate_p; param_t yawrate_p;
param_t yawrate_i; param_t yawrate_i;
param_t yawrate_d; param_t yawrate_d;
//param_t yawrate_awu;
//param_t yawrate_lim;
param_t attrate_p; param_t attrate_p;
param_t attrate_i; param_t attrate_i;
param_t attrate_d; param_t attrate_d;
//param_t attrate_awu;
//param_t attrate_lim;
}; };
/** /**
@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
h->yawrate_p = param_find("MC_YAWRATE_P"); h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I"); h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D"); h->yawrate_d = param_find("MC_YAWRATE_D");
//h->yawrate_awu = param_find("MC_YAWRATE_AWU");
//h->yawrate_lim = param_find("MC_YAWRATE_LIM");
h->attrate_p = param_find("MC_ATTRATE_P"); h->attrate_p = param_find("MC_ATTRATE_P");
h->attrate_i = param_find("MC_ATTRATE_I"); h->attrate_i = param_find("MC_ATTRATE_I");
h->attrate_d = param_find("MC_ATTRATE_D"); h->attrate_d = param_find("MC_ATTRATE_D");
//h->attrate_awu = param_find("MC_ATTRATE_AWU");
//h->attrate_lim = param_find("MC_ATTRATE_LIM");
return OK; return OK;
} }
@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d)); param_get(h->yawrate_d, &(p->yawrate_d));
//param_get(h->yawrate_awu, &(p->yawrate_awu));
//param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->attrate_p, &(p->attrate_p)); param_get(h->attrate_p, &(p->attrate_p));
param_get(h->attrate_i, &(p->attrate_i)); param_get(h->attrate_i, &(p->attrate_i));
param_get(h->attrate_d, &(p->attrate_d)); param_get(h->attrate_d, &(p->attrate_d));
//param_get(h->attrate_awu, &(p->attrate_awu));
//param_get(h->attrate_lim, &(p->attrate_lim));
return OK; return OK;
} }
@ -168,6 +148,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static PID_t pitch_rate_controller; static PID_t pitch_rate_controller;
static PID_t roll_rate_controller; static PID_t roll_rate_controller;
static PID_t yaw_rate_controller;
static struct mc_rate_control_params p; static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h; static struct mc_rate_control_param_handles h;
@ -182,7 +163,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
} }
/* load new parameters with lower rate */ /* load new parameters with lower rate */
@ -191,35 +172,24 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
parameters_update(&h, &p); parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
} }
/* reset integrals if needed */ /* reset integrals if needed */
if (reset_integral) { if (reset_integral) {
pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller); pid_reset_integral(&roll_rate_controller);
// TODO pid_reset_integral(&yaw_rate_controller); pid_reset_integral(&yaw_rate_controller);
} }
/* control pitch (forward) output */ /* run pitch, roll and yaw controllers */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
rates[1], 0.0f, deltaT); float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
rates[0], 0.0f, deltaT);
/* control yaw rate */ //XXX use library here
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
/* increase resilience to faulty control inputs */
if (!isfinite(yaw_rate_control)) {
yaw_rate_control = 0.0f;
warnx("rej. NaN ctrl yaw");
}
actuators->control[0] = roll_control; actuators->control[0] = roll_control;
actuators->control[1] = pitch_control; actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control; actuators->control[2] = yaw_control;
actuators->control[3] = rate_sp->thrust; actuators->control[3] = rate_sp->thrust;
motor_skip_counter++; motor_skip_counter++;

5
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else { } else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
} }
att_sp.yaw_body = global_pos_sp.yaw; /* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
att_sp.yaw_body = global_pos_sp.yaw;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);

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

@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced)
if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
warn("WARNING: failed to set scale / offsets for airspeed sensor"); warn("WARNING: failed to set scale / offsets for airspeed sensor");
close(fd);
} }
#if 0 #if 0

2
src/modules/systemlib/airspeed.c
View File

@ -59,7 +59,7 @@
float calc_indicated_airspeed(float differential_pressure) float calc_indicated_airspeed(float differential_pressure)
{ {
if (differential_pressure > 0) { if (differential_pressure > 0.0f) {
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else { } else {
return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);

4
src/modules/uORB/topics/differential_pressure.h
View File

@ -54,8 +54,8 @@
struct differential_pressure_s { struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count; uint64_t error_count;
uint16_t differential_pressure_pa; /**< Differential pressure reading */ float differential_pressure_pa; /**< Differential pressure reading */
uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor */ float temperature; /**< Temperature provided by sensor */

19
src/systemcmds/pwm/pwm.c
View File

@ -395,6 +395,17 @@ pwm_main(int argc, char *argv[])
if (pwm_value == 0) if (pwm_value == 0)
usage("no PWM value provided"); usage("no PWM value provided");
/* get current servo values */
struct pwm_output_values last_spos;
for (unsigned i = 0; i < servo_count; i++) {
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
if (ret != OK)
err(1, "PWM_SERVO_GET(%d)", i);
}
/* perform PWM output */ /* perform PWM output */
/* Open console directly to grab CTRL-C signal */ /* Open console directly to grab CTRL-C signal */
@ -420,6 +431,14 @@ pwm_main(int argc, char *argv[])
read(0, &c, 1); read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') { if (c == 0x03 || c == 0x63 || c == 'q') {
/* reset output to the last value */
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
ret = ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
}
}
warnx("User abort\n"); warnx("User abort\n");
exit(0); exit(0);
} }