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

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

This commit is contained in:
Lorenz Meier 2013-09-05 22:18:00 +02:00
parent 7fa2b9c91a 373a74adb9
commit d74bea57ac
87 changed files with 3144 additions and 1074 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -33,3 +33,5 @@ mavlink/include/mavlink/v0.9/
/Documentation/doxy.log
/Documentation/html/
/Documentation/doxygen*objdb*tmp
.tags
.tags_sorted_by_file

1962
Documentation/control_flow.graffle
View File

File diff suppressed because it is too large Load Diff

64
ROMFS/px4fmu_common/init.d/01_fmu_quad_x
View File

@ -1,64 +0,0 @@
#!nsh
echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set MC_ATTRATE_P 0.14
param set MC_ATTRATE_I 0
param set MC_ATTRATE_D 0.006
param set MC_ATT_P 5.5
param set MC_ATT_I 0
param set MC_ATT_D 0
param set MC_YAWPOS_D 0
param set MC_YAWPOS_I 0
param set MC_YAWPOS_P 0.6
param set MC_YAWRATE_D 0
param set MC_YAWRATE_I 0
param set MC_YAWRATE_P 0.08
param set RC_SCALE_PITCH 1
param set RC_SCALE_ROLL 1
param set RC_SCALE_YAW 3
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Start PWM output
#
fmu mode_pwm
#
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
# Exit, because /dev/ttyS0 is needed for MAVLink
exit

47
ROMFS/px4fmu_common/init.d/02_io_quad_x
View File

@ -1,47 +0,0 @@
#!nsh
echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start MAVLink
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start and configure PX4IO interface
#
sh /etc/init.d/rc.io
#
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor

2
ROMFS/px4fmu_common/init.d/08_ardrone
View File

@ -11,7 +11,7 @@ then
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
param save
fi
#

2
ROMFS/px4fmu_common/init.d/09_ardrone_flow
View File

@ -11,7 +11,7 @@ then
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
param save
fi
#

68
ROMFS/px4fmu_common/init.d/100_mpx_easystar Normal file
View File

@ -0,0 +1,68 @@
#!nsh
echo "[init] Multiplex Easystar"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start MAVLink (depends on orb)
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start and configure PX4IO interface
#
sh /etc/init.d/rc.io
#
# Set actuator limit to 100 Hz update (50 Hz PWM)
px4io limit 100
#
# Start the commander
#
commander start
#
# Start the sensors
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
fw_att_control start
fw_pos_control_l1 start

94
ROMFS/px4fmu_common/init.d/10_dji_f330 Normal file
View File

@ -0,0 +1,94 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on DJI F330"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 7.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.7
param set MPC_THR_MIN 0.3
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
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
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

67
ROMFS/px4fmu_common/init.d/10_io_f330
View File

@ -1,67 +0,0 @@
#!nsh
echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 7.0
param set MC_RCLOSS_THR 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start MAVLink (depends on orb)
#
mavlink start
usleep 5000
#
# Start and configure PX4IO interface
#
sh /etc/init.d/rc.io
#
# Set PWM values for DJI ESCs
#
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
#
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor

78
ROMFS/px4fmu_common/init.d/11_dji_f450 Normal file
View File

@ -0,0 +1,78 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on DJI F450"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 7.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
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
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

67
ROMFS/px4fmu_common/init.d/15_io_tbs
View File

@ -1,67 +0,0 @@
#!nsh
echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.006
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.17
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 5.0
param set MC_RCLOSS_THR 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.15
param set MC_YAWPOS_P 0.5
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_P 0.2
param save /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start MAVLink (depends on orb)
#
mavlink start
usleep 5000
#
# Start and configure PX4IO interface
#
sh /etc/init.d/rc.io
#
# Set PWM values for DJI ESCs
#
px4io idle 900 900 900 900
px4io min 1180 1180 1180 1180
px4io max 1800 1800 1800 1800
#
# Load the mixer for a quad with wide arms
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor

78
ROMFS/px4fmu_common/init.d/15_tbs_discovery Normal file
View File

@ -0,0 +1,78 @@
#!nsh
echo "[init] Team Blacksheep Discovery Quad"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.006
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.17
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 5.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.15
param set MC_YAWPOS_P 0.5
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_P 0.2
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
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 the mixer for a quad with wide arms
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

78
ROMFS/px4fmu_common/init.d/16_3dr_iris Normal file
View File

@ -0,0 +1,78 @@
#!nsh
echo "[init] 3DR Iris Quad"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.006
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.17
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 5.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.15
param set MC_YAWPOS_P 0.5
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_P 0.2
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
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 the mixer for a quad with wide arms
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

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

@ -11,7 +11,7 @@ then
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
param save
fi
#
@ -61,12 +61,13 @@ sh /etc/init.d/rc.logging
gps start
#
# Start the attitude estimator (depends on orb)
# Start the attitude and position estimator
#
kalman_demo start
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fw_att_control start
fw_pos_control_l1 start

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

@ -11,7 +11,7 @@ then
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
param save
fi
#
@ -56,12 +56,13 @@ sh /etc/init.d/rc.logging
gps start
#
# Start the attitude estimator
# Start the attitude and position estimator
#
kalman_demo start
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fw_att_control start
fw_pos_control_l1 start

2
ROMFS/px4fmu_common/init.d/40_io_segway
View File

@ -9,7 +9,7 @@ then
# TODO
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
param save
fi
#

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

@ -8,24 +8,24 @@ echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs"
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set MC_ATTRATE_P 0.14
param set MC_ATTRATE_I 0
param set MC_ATTRATE_D 0.006
param set MC_ATT_P 5.5
param set MC_ATT_I 0
param set MC_ATT_D 0
param set MC_YAWPOS_D 0
param set MC_YAWPOS_I 0
param set MC_YAWPOS_P 0.6
param set MC_YAWRATE_D 0
param set MC_YAWRATE_I 0
param set MC_YAWRATE_P 0.08
param set RC_SCALE_PITCH 1
param set RC_SCALE_ROLL 1
param set RC_SCALE_YAW 3
param set MC_ATTRATE_P 0.14
param set MC_ATTRATE_I 0
param set MC_ATTRATE_D 0.006
param set MC_ATT_P 5.5
param set MC_ATT_I 0
param set MC_ATT_D 0
param set MC_YAWPOS_D 0
param set MC_YAWPOS_I 0
param set MC_YAWPOS_P 0.6
param set MC_YAWRATE_D 0
param set MC_YAWRATE_I 0
param set MC_YAWRATE_P 0.08
param set RC_SCALE_PITCH 1
param set RC_SCALE_ROLL 1
param set RC_SCALE_YAW 3
param set SYS_AUTOCONFIG 0
param save /fs/microsd/params
param save
fi
#

66
ROMFS/px4fmu_common/init.d/rc.boarddetect
View File

@ -1,66 +0,0 @@
#!nsh
#
# If we are still in flight mode, work out what airframe
# configuration we have and start up accordingly.
#
if [ $MODE != autostart ]
then
echo "[init] automatic startup cancelled by user script"
else
echo "[init] detecting attached hardware..."
#
# Assume that we are PX4FMU in standalone mode
#
set BOARD PX4FMU
#
# Are we attached to a PX4IOAR (AR.Drone carrier board)?
#
if boardinfo test name PX4IOAR
then
set BOARD PX4IOAR
if [ -f /etc/init.d/rc.PX4IOAR ]
then
echo "[init] reading /etc/init.d/rc.PX4IOAR"
usleep 500
sh /etc/init.d/rc.PX4IOAR
fi
else
echo "[init] PX4IOAR not detected"
fi
#
# Are we attached to a PX4IO?
#
if boardinfo test name PX4IO
then
set BOARD PX4IO
if [ -f /etc/init.d/rc.PX4IO ]
then
echo "[init] reading /etc/init.d/rc.PX4IO"
usleep 500
sh /etc/init.d/rc.PX4IO
fi
else
echo "[init] PX4IO not detected"
fi
#
# Looks like we are stand-alone
#
if [ $BOARD == PX4FMU ]
then
echo "[init] no expansion board detected"
if [ -f /etc/init.d/rc.standalone ]
then
echo "[init] reading /etc/init.d/rc.standalone"
sh /etc/init.d/rc.standalone
fi
fi
#
# We may not reach here if the airframe-specific script exits the shell.
#
echo "[init] startup done."
fi

10
ROMFS/px4fmu_common/init.d/rc.hil
View File

@ -13,16 +13,6 @@ mavlink start -b 230400 -d /dev/ttyS1
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor

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

@ -19,5 +19,5 @@ then
fi
else
# SOS
tone_alarm 6
tone_alarm error
fi

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

@ -7,14 +7,6 @@
# Start sensor drivers here.
#
#
# Check for UORB
#
if uorb start
then
echo "uORB started"
fi
ms5611 start
adc start

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

@ -8,6 +8,14 @@ echo "Starting MAVLink on this USB console"
# Stop tone alarm
tone_alarm stop
#
# Check for UORB
#
if uorb start
then
echo "uORB started"
fi
# Tell MAVLink that this link is "fast"
if mavlink stop
then

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

@ -20,7 +20,7 @@ then
else
echo "[init] no microSD card found"
# Play SOS
tone_alarm 2
tone_alarm error
fi
#
@ -63,9 +63,6 @@ then
if sercon
then
echo "USB connected"
else
# second attempt
sercon &
fi
#
@ -110,32 +107,29 @@ then
#
# Upgrade PX4IO firmware
#
if px4io update
if px4io detect
then
if [ -d /fs/microsd ]
echo "PX4IO running, not upgrading"
else
echo "Attempting to upgrade PX4IO"
if px4io update
then
echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log
fi
if [ -d /fs/microsd ]
then
echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log
fi
# Allow IO to safely kick back to app
usleep 200000
# Allow IO to safely kick back to app
usleep 200000
else
echo "No PX4IO to upgrade here"
fi
fi
#
# Check if auto-setup from one of the standard scripts is wanted
# SYS_AUTOSTART = 0 means no autostart (default)
#
if param compare SYS_AUTOSTART 1
then
sh /etc/init.d/01_fmu_quad_x
set MODE custom
fi
if param compare SYS_AUTOSTART 2
then
sh /etc/init.d/02_io_quad_x
set MODE custom
fi
if param compare SYS_AUTOSTART 8
then
@ -151,13 +145,25 @@ then
if param compare SYS_AUTOSTART 10
then
sh /etc/init.d/10_io_f330
sh /etc/init.d/10_dji_f330
set MODE custom
fi
if param compare SYS_AUTOSTART 11
then
sh /etc/init.d/11_dji_f450
set MODE custom
fi
if param compare SYS_AUTOSTART 15
then
sh /etc/init.d/15_io_tbs
sh /etc/init.d/15_tbs_discovery
set MODE custom
fi
if param compare SYS_AUTOSTART 16
then
sh /etc/init.d/16_3dr_iris
set MODE custom
fi
@ -190,13 +196,13 @@ then
commander start
# Start px4io if present
if px4io start
if px4io detect
then
echo "PX4IO driver started"
px4io start
else
if fmu mode_serial
then
echo "FMU driver started"
echo "FMU driver (no PWM) started"
fi
fi
@ -210,5 +216,6 @@ then
gps start
fi
# End of autostart
fi

8
Tools/px_mkfw.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
# Copyright (C) 2012, 2013 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
@ -60,7 +60,7 @@ def mkdesc():
proto['description'] = ""
proto['git_identity'] = ""
proto['build_time'] = 0
proto['image'] = base64.b64encode(bytearray())
proto['image'] = bytes()
proto['image_size'] = 0
return proto
@ -99,12 +99,12 @@ if args.description != None:
if args.git_identity != None:
cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"])
p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout
desc['git_identity'] = p.read().strip()
desc['git_identity'] = str(p.read().strip())
p.close()
if args.image != None:
f = open(args.image, "rb")
bytes = f.read()
desc['image_size'] = len(bytes)
desc['image'] = base64.b64encode(zlib.compress(bytes,9))
desc['image'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8')
print(json.dumps(desc, indent=4))

101
Tools/px_uploader.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
# Copyright (C) 2012, 2013 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
@ -102,7 +102,7 @@ class firmware(object):
0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d])
crcpad = bytearray('\xff\xff\xff\xff')
crcpad = bytearray(b'\xff\xff\xff\xff')
def __init__(self, path):
@ -137,34 +137,40 @@ class uploader(object):
'''Uploads a firmware file to the PX FMU bootloader'''
# protocol bytes
INSYNC = chr(0x12)
EOC = chr(0x20)
INSYNC = b'\x12'
EOC = b'\x20'
# reply bytes
OK = chr(0x10)
FAILED = chr(0x11)
INVALID = chr(0x13) # rev3+
OK = b'\x10'
FAILED = b'\x11'
INVALID = b'\x13' # rev3+
# command bytes
NOP = chr(0x00) # guaranteed to be discarded by the bootloader
GET_SYNC = chr(0x21)
GET_DEVICE = chr(0x22)
CHIP_ERASE = chr(0x23)
CHIP_VERIFY = chr(0x24) # rev2 only
PROG_MULTI = chr(0x27)
READ_MULTI = chr(0x28) # rev2 only
GET_CRC = chr(0x29) # rev3+
REBOOT = chr(0x30)
NOP = b'\x00' # guaranteed to be discarded by the bootloader
GET_SYNC = b'\x21'
GET_DEVICE = b'\x22'
CHIP_ERASE = b'\x23'
CHIP_VERIFY = b'\x24' # rev2 only
PROG_MULTI = b'\x27'
READ_MULTI = b'\x28' # rev2 only
GET_CRC = b'\x29' # rev3+
REBOOT = b'\x30'
INFO_BL_REV = chr(1) # bootloader protocol revision
INFO_BL_REV = b'\x01' # bootloader protocol revision
BL_REV_MIN = 2 # minimum supported bootloader protocol
BL_REV_MAX = 3 # maximum supported bootloader protocol
INFO_BOARD_ID = chr(2) # board type
INFO_BOARD_REV = chr(3) # board revision
INFO_FLASH_SIZE = chr(4) # max firmware size in bytes
BL_REV_MAX = 4 # maximum supported bootloader protocol
INFO_BOARD_ID = b'\x02' # board type
INFO_BOARD_REV = b'\x03' # board revision
INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes
PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4
READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64
NSH_INIT = bytearray(b'\x0d\x0d\x0d')
NSH_REBOOT_BL = b"reboot -b\n"
NSH_REBOOT = b"reboot\n"
MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0')
MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac')
def __init__(self, portname, baudrate):
# open the port, keep the default timeout short so we can poll quickly
@ -176,7 +182,7 @@ class uploader(object):
def __send(self, c):
# print("send " + binascii.hexlify(c))
self.port.write(str(c))
self.port.write(c)
def __recv(self, count=1):
c = self.port.read(count)
@ -192,9 +198,9 @@ class uploader(object):
def __getSync(self):
self.port.flush()
c = self.__recv()
if c is not self.INSYNC:
raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c))
c = bytes(self.__recv())
if c != self.INSYNC:
raise RuntimeError("unexpected %s instead of INSYNC" % c)
c = self.__recv()
if c == self.INVALID:
raise RuntimeError("bootloader reports INVALID OPERATION")
@ -249,17 +255,29 @@ class uploader(object):
# send a PROG_MULTI command to write a collection of bytes
def __program_multi(self, data):
self.__send(uploader.PROG_MULTI
+ chr(len(data)))
if runningPython3 == True:
length = len(data).to_bytes(1, byteorder='big')
else:
length = chr(len(data))
self.__send(uploader.PROG_MULTI)
self.__send(length)
self.__send(data)
self.__send(uploader.EOC)
self.__getSync()
# verify multiple bytes in flash
def __verify_multi(self, data):
self.__send(uploader.READ_MULTI
+ chr(len(data))
+ uploader.EOC)
if runningPython3 == True:
length = len(data).to_bytes(1, byteorder='big')
else:
length = chr(len(data))
self.__send(uploader.READ_MULTI)
self.__send(length)
self.__send(uploader.EOC)
self.port.flush()
programmed = self.__recv(len(data))
if programmed != data:
@ -350,7 +368,24 @@ class uploader(object):
print("done, rebooting.")
self.__reboot()
self.port.close()
def send_reboot(self):
# try reboot via NSH first
self.__send(uploader.NSH_INIT)
self.__send(uploader.NSH_REBOOT_BL)
self.__send(uploader.NSH_INIT)
self.__send(uploader.NSH_REBOOT)
# then try MAVLINK command
self.__send(uploader.MAVLINK_REBOOT_ID1)
self.__send(uploader.MAVLINK_REBOOT_ID0)
# Detect python version
if sys.version_info[0] < 3:
runningPython3 = False
else:
runningPython3 = True
# Parse commandline arguments
parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.")
@ -397,7 +432,11 @@ while True:
print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
except:
# most probably a timeout talking to the port, no bootloader
# most probably a timeout talking to the port, no bootloader, try to reboot the board
print("attempting reboot on %s..." % port)
up.send_reboot()
# wait for the reboot, without we might run into Serial I/O Error 5
time.sleep(1.5)
continue
try:

4
makefiles/config_px4fmu-v1_default.mk
View File

@ -94,8 +94,8 @@ MODULES += modules/sdlog2
#
# Unit tests
#
MODULES += modules/unit_test
MODULES += modules/commander/commander_tests
#MODULES += modules/unit_test
#MODULES += modules/commander/commander_tests
#
# Library modules

4
makefiles/config_px4fmu-v2_default.mk
View File

@ -89,8 +89,8 @@ MODULES += modules/sdlog2
#
# Unit tests
#
MODULES += modules/unit_test
MODULES += modules/commander/commander_tests
#MODULES += modules/unit_test
#MODULES += modules/commander/commander_tests
#
# Library modules

1
misc/tones/charge.txt Normal file
View File

@ -0,0 +1 @@
MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4

1
misc/tones/cucuracha.txt Normal file
View File

@ -0,0 +1 @@
MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8

1
misc/tones/daisy.txt Normal file
View File

@ -0,0 +1 @@
MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8

1
misc/tones/dixie.txt Normal file
View File

@ -0,0 +1 @@
MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16

1
misc/tones/tell.txt Normal file
View File

File diff suppressed because one or more lines are too long

1
misc/tones/yankee.txt Normal file
View File

@ -0,0 +1 @@
MNT150L8O2GGABGBADGGABL4GL8F+

9
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -509,8 +509,8 @@ CONFIG_USART2_OFLOWCONTROL=y
#
# UART5 Configuration
#
CONFIG_UART5_RXBUFSIZE=32
CONFIG_UART5_TXBUFSIZE=32
CONFIG_UART5_RXBUFSIZE=512
CONFIG_UART5_TXBUFSIZE=512
CONFIG_UART5_BAUD=57600
CONFIG_UART5_BITS=8
CONFIG_UART5_PARITY=0
@ -538,9 +538,10 @@ CONFIG_USBDEV=y
#
# CONFIG_USBDEV_ISOCHRONOUS is not set
# CONFIG_USBDEV_DUALSPEED is not set
CONFIG_USBDEV_SELFPOWERED=y
# CONFIG_USBDEV_BUSPOWERED is not set
# CONFIG_USBDEV_SELFPOWERED is not set
CONFIG_USBDEV_BUSPOWERED=y
CONFIG_USBDEV_MAXPOWER=500
# CONFIG_USBDEV_REMOTEWAKEUP is not set
# CONFIG_USBDEV_DMA is not set
# CONFIG_USBDEV_TRACE is not set

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

@ -608,8 +608,8 @@ CONFIG_USART6_2STOP=0
#
# UART7 Configuration
#
CONFIG_UART7_RXBUFSIZE=256
CONFIG_UART7_TXBUFSIZE=256
CONFIG_UART7_RXBUFSIZE=512
CONFIG_UART7_TXBUFSIZE=512
CONFIG_UART7_BAUD=57600
CONFIG_UART7_BITS=8
CONFIG_UART7_PARITY=0
@ -620,8 +620,8 @@ CONFIG_UART7_2STOP=0
#
# UART8 Configuration
#
CONFIG_UART8_RXBUFSIZE=256
CONFIG_UART8_TXBUFSIZE=256
CONFIG_UART8_RXBUFSIZE=512
CONFIG_UART8_TXBUFSIZE=512
CONFIG_UART8_BAUD=57600
CONFIG_UART8_BITS=8
CONFIG_UART8_PARITY=0

7
src/drivers/airspeed/airspeed.cpp
View File

@ -106,6 +106,11 @@ Airspeed::~Airspeed()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
@ -152,7 +157,7 @@ Airspeed::probe()
*/
_retries = 4;
int ret = measure();
_retries = 0;
_retries = 2;
return ret;
}

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

@ -165,5 +165,5 @@ protected:
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)

84
src/drivers/ardrone_interface/ardrone_motor_control.c
View File

@ -41,6 +41,7 @@
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <math.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
@ -369,11 +370,9 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
float yaw_control = actuators->control[2];
float motor_thrust = actuators->control[3];
//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
const float min_thrust = 0.02f; /**< 2% minimum thrust */
const float max_thrust = 1.0f; /**< 100% max thrust */
const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
const float scaling = 510.0f; /**< 100% thrust equals a value of 510 which works, 512 leads to cutoff */
const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */
const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */
@ -382,71 +381,56 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
float motor_calc[4] = {0};
float output_band = 0.0f;
float band_factor = 0.75f;
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
float yaw_factor = 1.0f;
static bool initialized = false;
/* publish effective outputs */
static struct actuator_controls_effective_s actuator_controls_effective;
static orb_advert_t actuator_controls_effective_pub;
if (motor_thrust <= min_thrust) {
motor_thrust = min_thrust;
output_band = 0.0f;
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
output_band = band_factor * (motor_thrust - min_thrust);
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * startpoint_full_control;
} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * (max_thrust - motor_thrust);
/* linearly scale the control inputs from 0 to startpoint_full_control */
if (motor_thrust < startpoint_full_control) {
output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1
} else {
output_band = 1.0f;
}
roll_control *= output_band;
pitch_control *= output_band;
yaw_control *= output_band;
//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
// if we are not in the output band
if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
&& motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
&& motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
&& motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
/* if one motor is saturated, reduce throttle */
float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust;
if (saturation > 0.0f) {
/* reduce the motor thrust according to the saturation */
motor_thrust = motor_thrust - saturation;
yaw_factor = 0.5f;
yaw_control *= yaw_factor;
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
}
for (int i = 0; i < 4; i++) {
//check for limits
if (motor_calc[i] < motor_thrust - output_band) {
motor_calc[i] = motor_thrust - output_band;
}
if (motor_calc[i] > motor_thrust + output_band) {
motor_calc[i] = motor_thrust + output_band;
}
}
/* publish effective outputs */
actuator_controls_effective.control_effective[0] = roll_control;
@ -467,25 +451,29 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
/* set the motor values */
/* scale up from 0..1 to 10..512) */
/* scale up from 0..1 to 10..500) */
motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
/* Keep motors spinning while armed and prevent overflows */
/* scale up from 0..1 to 10..500) */
motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas));
motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas));
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
/* Failsafe logic for min values - should never be necessary */
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas;
motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : min_gas;
motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : min_gas;
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : min_gas;
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
/* Failsafe logic for max values - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= max_gas) ? motor_pwm[0] : max_gas;
motor_pwm[1] = (motor_pwm[1] <= max_gas) ? motor_pwm[1] : max_gas;
motor_pwm[2] = (motor_pwm[2] <= max_gas) ? motor_pwm[2] : max_gas;
motor_pwm[3] = (motor_pwm[3] <= max_gas) ? motor_pwm[3] : max_gas;
/* send motors via UART */
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);

4
src/drivers/blinkm/blinkm.cpp
View File

@ -541,7 +541,7 @@ BlinkM::led()
printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
@ -553,7 +553,7 @@ BlinkM::led()
led_color_8 = LED_YELLOW;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
/* LED Pattern for battery critical alerting */
led_color_1 = LED_RED;
led_color_2 = LED_RED;

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

@ -234,7 +234,7 @@ private:
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
BMA180::BMA180(int bus, spi_dev_e device) :

2
src/drivers/drv_rgbled.h
View File

@ -78,6 +78,7 @@
/** set pattern */
#define RGBLED_SET_PATTERN _RGBLEDIOC(7)
/*
structure passed to RGBLED_SET_RGB ioctl()
Note that the driver scales the brightness to 0 to 255, regardless
@ -115,6 +116,7 @@ typedef enum {
RGBLED_MODE_BLINK_SLOW,
RGBLED_MODE_BLINK_NORMAL,
RGBLED_MODE_BLINK_FAST,
RGBLED_MODE_BREATHE,
RGBLED_MODE_PATTERN
} rgbled_mode_t;

20
src/drivers/drv_tone_alarm.h
View File

@ -125,4 +125,24 @@ enum tone_pitch {
TONE_NOTE_MAX
};
enum {
TONE_STOP_TUNE = 0,
TONE_STARTUP_TUNE,
TONE_ERROR_TUNE,
TONE_NOTIFY_POSITIVE_TUNE,
TONE_NOTIFY_NEUTRAL_TUNE,
TONE_NOTIFY_NEGATIVE_TUNE,
/* Do not include these unused tunes
TONE_CHARGE_TUNE,
TONE_DIXIE_TUNE,
TONE_CUCURACHA_TUNE,
TONE_YANKEE_TUNE,
TONE_DAISY_TUNE,
TONE_WILLIAM_TELL_TUNE, */
TONE_ARMING_WARNING_TUNE,
TONE_BATTERY_WARNING_SLOW_TUNE,
TONE_BATTERY_WARNING_FAST_TUNE,
TONE_NUMBER_OF_TUNES
};
#endif /* DRV_TONE_ALARM_H_ */

24
src/drivers/gps/ubx.cpp
View File

@ -42,13 +42,14 @@
*
* @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
*/
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <math.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <poll.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
@ -452,7 +453,16 @@ UBX::handle_message()
timeinfo.tm_min = packet->min;
timeinfo.tm_sec = packet->sec;
time_t epoch = mktime(&timeinfo);
#ifndef CONFIG_RTC
//Since we lack a hardware RTC, set the system time clock based on GPS UTC
//TODO generalize this by moving into gps.cpp?
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = packet->time_nanoseconds;
clock_settime(CLOCK_REALTIME,&ts);
#endif
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
_gps_position->timestamp_time = hrt_absolute_time();

18
src/drivers/hmc5883/hmc5883.cpp
View File

@ -311,7 +311,7 @@ private:
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
/*
* Driver 'main' command.
@ -359,6 +359,11 @@ HMC5883::~HMC5883()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
@ -957,11 +962,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
warnx("sampling 500 samples for scaling offset");
/* set the queue depth to 10 */
if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
warn("failed to set queue depth");
ret = 1;
goto out;
}
/* don't do this for now, it can lead to a crash in start() respectively work_queue() */
// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
// warn("failed to set queue depth");
// ret = 1;
// goto out;
// }
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {

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

@ -300,7 +300,7 @@ private:
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :

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

@ -421,7 +421,7 @@ private:
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :

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

@ -184,7 +184,7 @@ private:
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
/*
* Driver 'main' command.

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

@ -162,6 +162,8 @@ MEASAirspeed::collect()
if (ret < 0) {
log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
@ -169,9 +171,14 @@ MEASAirspeed::collect()
if (status == 2) {
log("err: stale data");
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
} else if (status == 3) {
log("err: fault");
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);

8
src/drivers/ms5611/ms5611.cpp
View File

@ -77,7 +77,7 @@ static const int ERROR = -1;
#endif
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
/* helper macro for arithmetic - returns the square of the argument */
#define POW2(_x) ((_x) * (_x))
@ -225,6 +225,12 @@ MS5611::~MS5611()
if (_reports != nullptr)
delete[] _reports;
// free perf counters
perf_free(_sample_perf);
perf_free(_measure_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
delete _interface;
}

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

@ -119,10 +119,17 @@ public:
/**
* Initialize the PX4IO class.
*
* Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
* Retrieve relevant initial system parameters. Initialize PX4IO registers.
*/
virtual int init();
/**
* Detect if a PX4IO is connected.
*
* Only validate if there is a PX4IO to talk to.
*/
virtual int detect();
/**
* IO Control handler.
*
@ -475,6 +482,35 @@ PX4IO::~PX4IO()
g_dev = nullptr;
}
int
PX4IO::detect()
{
int ret;
ASSERT(_task == -1);
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* get some parameters */
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
if (protocol != PX4IO_PROTOCOL_VERSION) {
if (protocol == _io_reg_get_error) {
log("IO not installed");
} else {
log("IO version error");
mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
}
return -1;
}
log("IO found");
return 0;
}
int
PX4IO::init()
{
@ -860,7 +896,6 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
int
PX4IO::set_min_values(const uint16_t *vals, unsigned len)
{
uint16_t regs[_max_actuators];
if (len > _max_actuators)
/* fail with error */
@ -873,7 +908,6 @@ PX4IO::set_min_values(const uint16_t *vals, unsigned len)
int
PX4IO::set_max_values(const uint16_t *vals, unsigned len)
{
uint16_t regs[_max_actuators];
if (len > _max_actuators)
/* fail with error */
@ -886,7 +920,6 @@ PX4IO::set_max_values(const uint16_t *vals, unsigned len)
int
PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
{
uint16_t regs[_max_actuators];
if (len > _max_actuators)
/* fail with error */
@ -1340,7 +1373,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
}
int ret = _interface->write((page << 8) | offset, (void *)values, num_values);
if (ret != num_values) {
if (ret != (int)num_values) {
debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret);
return -1;
}
@ -1363,7 +1396,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
}
int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
if (ret != num_values) {
if (ret != (int)num_values) {
debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
return -1;
}
@ -1584,7 +1617,7 @@ PX4IO::print_status()
}
printf("failsafe");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
printf("\nidle values");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
@ -1894,6 +1927,7 @@ start(int argc, char *argv[])
if (OK != g_dev->init()) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
}
@ -1920,6 +1954,34 @@ start(int argc, char *argv[])
exit(0);
}
void
detect(int argc, char *argv[])
{
if (g_dev != nullptr)
errx(0, "already loaded");
/* allocate the interface */
device::Device *interface = get_interface();
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr)
errx(1, "driver alloc failed");
int ret = g_dev->detect();
delete g_dev;
g_dev = nullptr;
if (ret) {
/* nonzero, error */
exit(1);
} else {
exit(0);
}
}
void
bind(int argc, char *argv[])
{
@ -2079,6 +2141,9 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "start"))
start(argc - 1, argv + 1);
if (!strcmp(argv[1], "detect"))
detect(argc - 1, argv + 1);
if (!strcmp(argv[1], "update")) {
if (g_dev != nullptr) {
@ -2173,7 +2238,7 @@ px4io_main(int argc, char *argv[])
/* set values for first 8 channels, fill unassigned channels with 1500. */
uint16_t failsafe[8];
for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
/* set channel to commandline argument or to 900 for non-provided channels */
if (argc > i + 2) {
@ -2205,7 +2270,7 @@ px4io_main(int argc, char *argv[])
/* set values for first 8 channels, fill unassigned channels with 900. */
uint16_t min[8];
for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++)
for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++)
{
/* set channel to commanline argument or to 900 for non-provided channels */
if (argc > i + 2) {
@ -2240,7 +2305,7 @@ px4io_main(int argc, char *argv[])
/* set values for first 8 channels, fill unassigned channels with 2100. */
uint16_t max[8];
for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++)
for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++)
{
/* set channel to commanline argument or to 2100 for non-provided channels */
if (argc > i + 2) {
@ -2275,7 +2340,7 @@ px4io_main(int argc, char *argv[])
/* set values for first 8 channels, fill unassigned channels with 0. */
uint16_t idle[8];
for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++)
for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++)
{
/* set channel to commanline argument or to 0 for non-provided channels */
if (argc > i + 2) {

15
src/drivers/px4io/px4io_uploader.cpp
View File

@ -110,6 +110,10 @@ PX4IO_Uploader::upload(const char *filenames[])
return -errno;
}
/* save initial uart configuration to reset after the update */
struct termios t_original;
tcgetattr(_io_fd, &t_original);
/* adjust line speed to match bootloader */
struct termios t;
tcgetattr(_io_fd, &t);
@ -122,6 +126,7 @@ PX4IO_Uploader::upload(const char *filenames[])
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -EIO;
@ -130,6 +135,7 @@ PX4IO_Uploader::upload(const char *filenames[])
struct stat st;
if (stat(filename, &st) != 0) {
log("Failed to stat %s - %d\n", filename, (int)errno);
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -errno;
@ -137,6 +143,7 @@ PX4IO_Uploader::upload(const char *filenames[])
fw_size = st.st_size;
if (_fw_fd == -1) {
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -ENOENT;
@ -151,6 +158,7 @@ PX4IO_Uploader::upload(const char *filenames[])
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -EIO;
@ -164,6 +172,7 @@ PX4IO_Uploader::upload(const char *filenames[])
log("found bootloader revision: %d", bl_rev);
} else {
log("found unsupported bootloader revision %d, exiting", bl_rev);
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return OK;
@ -199,6 +208,9 @@ PX4IO_Uploader::upload(const char *filenames[])
if (ret != OK) {
log("reboot failed");
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return ret;
}
@ -208,6 +220,9 @@ PX4IO_Uploader::upload(const char *filenames[])
break;
}
/* reset uart to previous/default baudrate */
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_fw_fd);
close(_io_fd);
_io_fd = -1;

4
src/drivers/px4io/uploader.h
View File

@ -69,7 +69,7 @@ private:
PROTO_REBOOT = 0x30,
INFO_BL_REV = 1, /**< bootloader protocol revision */
BL_REV = 3, /**< supported bootloader protocol */
BL_REV = 4, /**< supported bootloader protocol */
INFO_BOARD_ID = 2, /**< board type */
INFO_BOARD_REV = 3, /**< board revision */
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
@ -91,7 +91,7 @@ private:
void drain();
int send(uint8_t c);
int send(uint8_t *p, unsigned count);
int get_sync(unsigned timeout = 100);
int get_sync(unsigned timeout = 1000);
int sync();
int get_info(int param, uint32_t &val);
int erase();

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2012, 2013 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
@ -96,6 +96,11 @@ private:
rgbled_mode_t _mode;
rgbled_pattern_t _pattern;
float _brightness;
uint8_t _r;
uint8_t _g;
uint8_t _b;
bool _should_run;
bool _running;
int _led_interval;
@ -104,6 +109,7 @@ private:
void set_color(rgbled_color_t ledcolor);
void set_mode(rgbled_mode_t mode);
void set_pattern(rgbled_pattern_t *pattern);
void set_brightness(float brightness);
static void led_trampoline(void *arg);
void led();
@ -128,6 +134,10 @@ RGBLED::RGBLED(int bus, int rgbled) :
_color(RGBLED_COLOR_OFF),
_mode(RGBLED_MODE_OFF),
_running(false),
_brightness(1.0f),
_r(0),
_g(0),
_b(0),
_led_interval(0),
_counter(0)
{
@ -191,7 +201,6 @@ int
RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = ENOTTY;
switch (cmd) {
case RGBLED_SET_RGB:
/* set the specified RGB values */
@ -246,6 +255,15 @@ RGBLED::led()
else
set_on(false);
break;
case RGBLED_MODE_BREATHE:
if (_counter >= 30)
_counter = 0;
if (_counter <= 15) {
set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f));
} else {
set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f));
}
break;
case RGBLED_MODE_PATTERN:
/* don't run out of the pattern array and stop if the next frame is 0 */
if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
@ -294,7 +312,7 @@ RGBLED::set_color(rgbled_color_t color) {
case RGBLED_COLOR_AMBER: // amber
set_rgb(255,20,0);
break;
case RGBLED_COLOR_DIM_RED: // red
case RGBLED_COLOR_DIM_RED: // red
set_rgb(90,0,0);
break;
case RGBLED_COLOR_DIM_YELLOW: // yellow
@ -306,7 +324,7 @@ RGBLED::set_color(rgbled_color_t color) {
case RGBLED_COLOR_DIM_GREEN: // green
set_rgb(0,90,0);
break;
case RGBLED_COLOR_DIM_BLUE: // blue
case RGBLED_COLOR_DIM_BLUE: // blue
set_rgb(0,0,90);
break;
case RGBLED_COLOR_DIM_WHITE: // white
@ -347,6 +365,12 @@ RGBLED::set_mode(rgbled_mode_t mode)
_should_run = true;
_led_interval = 100;
break;
case RGBLED_MODE_BREATHE:
_should_run = true;
set_on(true);
_counter = 0;
_led_interval = 1000/15;
break;
case RGBLED_MODE_PATTERN:
_should_run = true;
set_on(true);
@ -377,6 +401,13 @@ RGBLED::set_pattern(rgbled_pattern_t *pattern)
set_mode(RGBLED_MODE_PATTERN);
}
void
RGBLED::set_brightness(float brightness) {
_brightness = brightness;
set_rgb(_r, _g, _b);
}
int
RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
{
@ -413,7 +444,12 @@ RGBLED::set_on(bool on)
int
RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
{
const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)};
/* save the RGB values in case we want to change the brightness later */
_r = r;
_g = g;
_b = b;
const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)};
return transfer(msg, sizeof(msg), nullptr, 0);
}

248
src/drivers/stm32/tone_alarm/tone_alarm.cpp
View File

@ -230,11 +230,14 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
inline const char *name(int tune) {
return _tune_names[tune];
}
private:
static const unsigned _tune_max = 1024; // be reasonable about user tunes
static const char * const _default_tunes[];
static const unsigned _default_ntunes;
static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes
const char * _default_tunes[TONE_NUMBER_OF_TUNES];
const char * _tune_names[TONE_NUMBER_OF_TUNES];
static const uint8_t _note_tab[];
unsigned _default_tune_number; // number of currently playing default tune (0 for none)
@ -304,163 +307,6 @@ private:
};
// predefined tune array
const char * const ToneAlarm::_default_tunes[] = {
"MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc", // startup tune
"MBT200a8a8a8PaaaP", // ERROR tone
"MFT200e8a8a", // NotifyPositive tone
"MFT200e8e", // NotifyNeutral tone
"MFT200e8c8e8c8e8c8", // NotifyNegative tone
"MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge!
"MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16", // dixie
"MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8", // cucuracha
"MNT150L8O2GGABGBADGGABL4GL8F+", // yankee
"MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy
"T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell
"O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16"
"O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16"
"O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8"
"P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8"
"O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16"
"O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4"
"O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8"
"O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16"
"O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8"
"O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16"
"O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8"
"O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8"
"O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16"
"O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
"O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8"
"O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16"
"O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16"
"O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16"
"O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16"
"O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8"
"O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8"
"O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16"
"O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64"
"O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16"
"O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16"
"O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16"
"O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16"
"O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16"
"O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
"O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16"
"O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16"
"O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16"
"O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16"
"O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16"
"O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16"
"O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16"
"O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16"
"O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16"
"O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16"
"O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16"
"O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16"
"O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16"
"O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16"
"O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16"
"O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16"
"O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16"
"O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16"
"O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16"
"O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16"
"O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16"
"O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16"
"O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16"
"O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16"
"O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
"O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16"
"O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4."
"O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16"
"O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16"
"O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16"
"O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16"
"O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8"
"O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16"
"O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8"
"O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
"O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8"
"O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8"
"O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16"
"O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16"
"O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8"
"O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8"
"O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16"
"O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16"
"O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16"
"O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16"
"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
"O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16"
"O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16"
"O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16"
"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
"O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16"
"O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16"
"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
"P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16"
"O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8"
"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16"
"O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16"
"O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16"
"O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16"
"O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16"
"P16O4E16P16D+16P16C+16P16O3B16P16A+16P16"
"O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16"
"O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16"
"O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16"
"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
"O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16"
"O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16"
"O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
"P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16"
"O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16"
"O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16"
"O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16"
"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8"
"O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16"
"O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16"
"O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8"
"O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16"
"O4E16P16D+16P16C+16P16O3B16P16A+16P16A16"
"P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16"
"O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8"
"O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8"
"O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16"
"O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16"
"O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16"
"O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16"
"O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16"
"O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8"
"O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16"
"O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16"
"O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8"
"O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8"
"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
"O2E2P64",
"MNT75L1O2G", //arming warning
"MBNT100a8", //battery warning slow
"MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" //battery warning fast // XXX why is there a break before a repetition
};
const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
// semitone offsets from C for the characters 'A'-'G'
const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7};
@ -479,6 +325,23 @@ ToneAlarm::ToneAlarm() :
{
// enable debug() calls
//_debug_enabled = true;
_default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune
_default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP"; // ERROR tone
_default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone
_default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone
_default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone
_default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning
_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow
_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
_tune_names[TONE_NOTIFY_POSITIVE_TUNE] = "positive"; // Notify Positive tone
_tune_names[TONE_NOTIFY_NEUTRAL_TUNE] = "neutral"; // Notify Neutral tone
_tune_names[TONE_NOTIFY_NEGATIVE_TUNE] = "negative"; // Notify Negative tone
_tune_names[TONE_ARMING_WARNING_TUNE] = "arming"; // arming warning
_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow
_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast
}
ToneAlarm::~ToneAlarm()
@ -873,17 +736,19 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
case TONE_SET_ALARM:
debug("TONE_SET_ALARM %u", arg);
if (arg <= _default_ntunes) {
if (arg == 0) {
if (arg < TONE_NUMBER_OF_TUNES) {
if (arg == TONE_STOP_TUNE) {
// stop the tune
_tune = nullptr;
_next = nullptr;
_repeat = false;
_default_tune_number = 0;
} else {
/* always interrupt alarms, unless they are repeating and already playing */
if (!(_repeat && _default_tune_number == arg)) {
/* play the selected tune */
_default_tune_number = arg;
start_tune(_default_tunes[arg - 1]);
start_tune(_default_tunes[arg]);
}
}
} else {
@ -970,7 +835,7 @@ play_tune(unsigned tune)
}
int
play_string(const char *str)
play_string(const char *str, bool free_buffer)
{
int fd, ret;
@ -982,6 +847,9 @@ play_string(const char *str)
ret = write(fd, str, strlen(str) + 1);
close(fd);
if (free_buffer)
free((void *)str);
if (ret < 0)
err(1, "play tune");
exit(0);
@ -1007,22 +875,50 @@ tone_alarm_main(int argc, char *argv[])
}
}
if ((argc > 1) && !strcmp(argv[1], "start"))
play_tune(1);
if (argc > 1) {
const char *argv1 = argv[1];
if ((argc > 1) && !strcmp(argv[1], "stop"))
play_tune(0);
if (!strcmp(argv1, "start"))
play_tune(TONE_STARTUP_TUNE);
if ((tune = strtol(argv[1], nullptr, 10)) != 0)
play_tune(tune);
if (!strcmp(argv1, "stop"))
play_tune(TONE_STOP_TUNE);
/* if it looks like a PLAY string... */
if (strlen(argv[1]) > 2) {
const char *str = argv[1];
if (str[0] == 'M') {
play_string(str);
if ((tune = strtol(argv1, nullptr, 10)) != 0)
play_tune(tune);
/* It might be a tune name */
for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++)
if (!strcmp(g_dev->name(tune), argv1))
play_tune(tune);
/* If it is a file name then load and play it as a string */
if (*argv1 == '/') {
FILE *fd = fopen(argv1, "r");
int sz;
char *buffer;
if (fd == nullptr)
errx(1, "couldn't open '%s'", argv1);
fseek(fd, 0, SEEK_END);
sz = ftell(fd);
fseek(fd, 0, SEEK_SET);
buffer = (char *)malloc(sz + 1);
if (buffer == nullptr)
errx(1, "not enough memory memory");
fread(buffer, sz, 1, fd);
/* terminate the string */
buffer[sz] = 0;
play_string(buffer, true);
}
/* if it looks like a PLAY string... */
if (strlen(argv1) > 2) {
if (*argv1 == 'M') {
play_string(argv1, false);
}
}
}
errx(1, "unrecognised command, try 'start', 'stop' or an alarm number");
errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'");
}

3
src/include/mavlink/mavlink_log.h
View File

@ -107,7 +107,6 @@ struct mavlink_logmessage {
struct mavlink_logbuffer {
unsigned int start;
// unsigned int end;
unsigned int size;
int count;
struct mavlink_logmessage *elems;
@ -115,6 +114,8 @@ struct mavlink_logbuffer {
void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb);
int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb);
int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb);

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

@ -308,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
if (!initialized) {
// XXX disabling init for now
initialized = true;
gyro_offsets[0] += raw.gyro_rad_s[0];
gyro_offsets[1] += raw.gyro_rad_s[1];
gyro_offsets[2] += raw.gyro_rad_s[2];
offset_count++;
// gyro_offsets[0] += raw.gyro_rad_s[0];
// gyro_offsets[1] += raw.gyro_rad_s[1];
// gyro_offsets[2] += raw.gyro_rad_s[2];
// offset_count++;
if (hrt_absolute_time() - start_time > 3000000LL) {
initialized = true;
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
}
// if (hrt_absolute_time() - start_time > 3000000LL) {
// initialized = true;
// gyro_offsets[0] /= offset_count;
// gyro_offsets[1] /= offset_count;
// gyro_offsets[2] /= offset_count;
// }
} else {

4
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
View File

@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
// Convert q->R.
// Convert q->R, This R converts inertial frame to body frame.
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13
@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
memcpy(&att.R, Rot_matrix, sizeof(float)*9);
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {

4
src/modules/commander/accelerometer_calibration.cpp
View File

@ -241,8 +241,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
break;
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0)
if (orient < 0) {
close(sensor_combined_sub);
return ERROR;
}
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);

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

@ -85,6 +85,7 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
close(diff_pres_sub);
return ERROR;
}
}
@ -95,6 +96,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
close(diff_pres_sub);
return ERROR;
}
@ -104,18 +106,17 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
close(diff_pres_sub);
return ERROR;
}
//char buf[50];
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "airspeed calibration done");
close(diff_pres_sub);
return OK;
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
close(diff_pres_sub);
return ERROR;
}
}

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

@ -50,6 +50,7 @@
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <systemlib/err.h>
#include <debug.h>
#include <sys/prctl.h>
#include <sys/stat.h>
@ -153,6 +154,7 @@ static uint64_t last_print_mode_reject_time = 0;
/* if connected via USB */
static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
/* tasks waiting for low prio thread */
typedef enum {
@ -318,6 +320,8 @@ void print_status()
break;
}
close(state_sub);
warnx("arming: %s", armed_str);
}
@ -492,9 +496,10 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_sys_type = param_find("MAV_TYPE");
param_t _param_system_id = param_find("MAV_SYS_ID");
param_t _param_component_id = param_find("MAV_COMP_ID");
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
/* welcome user */
warnx("[commander] starting");
warnx("starting");
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@ -592,9 +597,10 @@ int commander_thread_main(int argc, char *argv[])
pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, 2048);
pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
@ -689,6 +695,8 @@ int commander_thread_main(int argc, char *argv[])
struct subsystem_info_s info;
memset(&info, 0, sizeof(info));
toggle_status_leds(&status, &armed, true);
/* now initialized */
commander_initialized = true;
thread_running = true;
@ -731,8 +739,11 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_component_id, &(status.component_id));
status_changed = true;
/* Re-check RC calibration */
/* re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check());
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
}
}
@ -890,7 +901,7 @@ int commander_thread_main(int argc, char *argv[])
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
}
@ -902,7 +913,7 @@ int commander_thread_main(int argc, char *argv[])
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
if (armed.armed) {
@ -1160,12 +1171,12 @@ int commander_thread_main(int argc, char *argv[])
if (tune_arm() == OK)
arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
/* play tune on battery warning */
if (tune_low_bat() == OK)
battery_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
/* play tune on battery critical */
if (tune_critical_bat() == OK)
battery_tune_played = true;
@ -1251,73 +1262,43 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
#endif
if (changed) {
int i;
rgbled_pattern_t pattern;
memset(&pattern, 0, sizeof(pattern));
/* XXX TODO blink fast when armed and serious error occurs */
if (armed->armed) {
/* armed, solid */
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
}
pattern.duration[0] = 1000;
rgbled_set_mode(RGBLED_MODE_ON);
} else if (armed->ready_to_arm) {
for (i = 0; i < 3; i++) {
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
}
pattern.duration[i * 2] = 200;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 800;
}
if (status->condition_global_position_valid) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i * 2] = 1000;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 800;
} else {
for (i = 3; i < 6; i++) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i * 2] = 100;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 100;
}
pattern.color[6 * 2] = RGBLED_COLOR_OFF;
pattern.duration[6 * 2] = 700;
}
rgbled_set_mode(RGBLED_MODE_BREATHE);
} else {
for (i = 0; i < 3; i++) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
pattern.duration[i * 2] = 200;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 200;
}
/* not ready to arm, blink at 10Hz */
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
}
}
rgbled_set_pattern(&pattern);
if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
switch (status->battery_warning) {
case VEHICLE_BATTERY_WARNING_LOW:
rgbled_set_color(RGBLED_COLOR_YELLOW);
break;
case VEHICLE_BATTERY_WARNING_CRITICAL:
rgbled_set_color(RGBLED_COLOR_AMBER);
break;
default:
break;
}
} else {
switch (status->main_state) {
case MAIN_STATE_MANUAL:
rgbled_set_color(RGBLED_COLOR_WHITE);
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_EASY:
rgbled_set_color(RGBLED_COLOR_GREEN);
break;
case MAIN_STATE_AUTO:
rgbled_set_color(RGBLED_COLOR_BLUE);
break;
default:
break;
}
}
/* give system warnings on error LED, XXX maybe add memory usage warning too */
@ -1480,54 +1461,52 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
if (status->main_state == MAIN_STATE_AUTO) {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
// TODO AUTO_LAND handling
if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't switch to other states until takeoff not completed */
if (local_pos->z > -5.0f || status->condition_landed) {
res = TRANSITION_NOT_CHANGED;
if (local_pos->z > -takeoff_alt || status->condition_landed) {
return TRANSITION_NOT_CHANGED;
}
}
if (res != TRANSITION_NOT_CHANGED) {
/* check again, state can be changed */
if (status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
// TRANSITION_DENIED is not possible here
if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
/* possibly on ground, switch to TAKEOFF if needed */
if (local_pos->z > -takeoff_alt || status->condition_landed) {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
return res;
}
}
/* switch to AUTO mode */
if (status->rc_signal_found_once && !status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} else {
/* not landed */
if (status->rc_signal_found_once && !status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} else {
if (status->mission_switch == MISSION_SWITCH_MISSION) {
/* MISSION */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* LOITER */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
if (status->mission_switch == MISSION_SWITCH_MISSION) {
/* MISSION */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* switch to MISSION in air when no RC control */
if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
res = TRANSITION_NOT_CHANGED;
} else {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
}
/* LOITER */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
}
} else {
/* switch to MISSION when no RC control and first time in some AUTO mode */
if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
res = TRANSITION_NOT_CHANGED;
} else {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
}
}
} else {
/* disarmed, always switch to AUTO_READY */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
@ -1678,13 +1657,13 @@ void *commander_low_prio_loop(void *arg)
if (((int)(cmd.param1)) == 1) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
usleep(1000000);
usleep(100000);
/* reboot */
systemreset(false);
} else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
usleep(1000000);
usleep(100000);
/* reboot to bootloader */
systemreset(true);
@ -1727,6 +1706,7 @@ void *commander_low_prio_loop(void *arg)
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
calib_ret = do_rc_calibration(mavlink_fd);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
@ -1752,22 +1732,36 @@ void *commander_low_prio_loop(void *arg)
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
if (((int)(cmd.param1)) == 0) {
if (0 == param_load_default()) {
int ret = param_load_default();
if (ret == OK) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
ret = -ret;
if (ret < 1000)
mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
} else if (((int)(cmd.param1)) == 1) {
if (0 == param_save_default()) {
int ret = param_save_default();
if (ret == OK) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
ret = -ret;
if (ret < 1000)
mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
}
@ -1789,5 +1783,7 @@ void *commander_low_prio_loop(void *arg)
}
close(cmd_sub);
return 0;
}

16
src/modules/commander/commander_helper.cpp
View File

@ -99,44 +99,44 @@ void buzzer_deinit()
void tune_error()
{
ioctl(buzzer, TONE_SET_ALARM, 2);
ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
}
void tune_positive()
{
ioctl(buzzer, TONE_SET_ALARM, 3);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
}
void tune_neutral()
{
ioctl(buzzer, TONE_SET_ALARM, 4);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
}
void tune_negative()
{
ioctl(buzzer, TONE_SET_ALARM, 5);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
}
int tune_arm()
{
return ioctl(buzzer, TONE_SET_ALARM, 12);
return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE);
}
int tune_low_bat()
{
return ioctl(buzzer, TONE_SET_ALARM, 13);
return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE);
}
int tune_critical_bat()
{
return ioctl(buzzer, TONE_SET_ALARM, 14);
return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
}
void tune_stop()
{
ioctl(buzzer, TONE_SET_ALARM, 0);
ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
}
static int leds;

3
src/modules/commander/commander_params.c
View File

@ -45,7 +45,8 @@
#include <nuttx/config.h>
#include <systemlib/param/param.h>
PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f);
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);

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

@ -60,12 +60,12 @@ int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
const int calibration_count = 5000;
const unsigned calibration_count = 5000;
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s raw;
int calibration_counter = 0;
unsigned calibration_counter = 0;
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
/* set offsets to zero */
@ -101,6 +101,8 @@ int do_gyro_calibration(int mavlink_fd)
gyro_offset[1] += raw.gyro_rad_s[1];
gyro_offset[2] += raw.gyro_rad_s[2];
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
} else {
poll_errcount++;
@ -108,6 +110,7 @@ int do_gyro_calibration(int mavlink_fd)
if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
close(sub_sensor_combined);
return ERROR;
}
}
@ -147,24 +150,23 @@ int do_gyro_calibration(int mavlink_fd)
if (save_ret != 0) {
warnx("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, "gyro store failed");
close(sub_sensor_combined);
return ERROR;
}
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_neutral();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
close(sub_sensor_combined);
return ERROR;
}
/*** --- SCALING --- ***/
mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x");
mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip.");
mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
unsigned rotations_count = 30;
@ -187,8 +189,8 @@ int do_gyro_calibration(int mavlink_fd)
/* abort this loop if not rotated more than 180 degrees within 5 seconds */
if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
mavlink_log_info(mavlink_fd, "gyro calibration done");
mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
close(sub_sensor_combined);
return OK;
}
@ -281,9 +283,11 @@ int do_gyro_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, "gyro calibration done");
/* third beep by cal end routine */
close(sub_sensor_combined);
return OK;
} else {
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
close(sub_sensor_combined);
return ERROR;
}
}

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

@ -142,7 +142,6 @@ int do_mag_calibration(int mavlink_fd)
axis_index++;
mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter);
tune_neutral();
axis_deadline += calibration_interval / 3;
@ -152,14 +151,6 @@ int do_mag_calibration(int mavlink_fd)
break;
}
// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
// if ((axis_left / 1000) == 0 && axis_left > 0) {
// char buf[50];
// sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
// mavlink_log_info(mavlink_fd, buf);
// }
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
@ -169,30 +160,10 @@ int do_mag_calibration(int mavlink_fd)
y[calibration_counter] = mag.y;
z[calibration_counter] = mag.z;
/* get min/max values */
// if (mag.x < mag_min[0]) {
// mag_min[0] = mag.x;
// }
// else if (mag.x > mag_max[0]) {
// mag_max[0] = mag.x;
// }
// if (raw.magnetometer_ga[1] < mag_min[1]) {
// mag_min[1] = raw.magnetometer_ga[1];
// }
// else if (raw.magnetometer_ga[1] > mag_max[1]) {
// mag_max[1] = raw.magnetometer_ga[1];
// }
// if (raw.magnetometer_ga[2] < mag_min[2]) {
// mag_min[2] = raw.magnetometer_ga[2];
// }
// else if (raw.magnetometer_ga[2] > mag_max[2]) {
// mag_max[2] = raw.magnetometer_ga[2];
// }
calibration_counter++;
if (calibration_counter % (calibration_maxcount / 20) == 0)
mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount);
} else {
poll_errcount++;
@ -200,6 +171,10 @@ int do_mag_calibration(int mavlink_fd)
if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
close(sub_mag);
free(x);
free(y);
free(z);
return ERROR;
}
@ -211,7 +186,9 @@ int do_mag_calibration(int mavlink_fd)
float sphere_z;
float sphere_radius;
mavlink_log_info(mavlink_fd, "mag cal progress <70> percent");
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
mavlink_log_info(mavlink_fd, "mag cal progress <80> percent");
free(x);
free(y);
@ -269,6 +246,7 @@ int do_mag_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
close(sub_mag);
return ERROR;
}
@ -288,11 +266,13 @@ int do_mag_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
close(sub_mag);
return OK;
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
close(sub_mag);
return ERROR;
}
}

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

@ -40,6 +40,7 @@
#include "commander_helper.h"
#include <poll.h>
#include <unistd.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <mavlink/mavlink_log.h>
@ -56,14 +57,16 @@ int do_rc_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "trim calibration starting");
/* XXX fix this */
// if (current_status.rc_signal) {
// mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
// return;
// }
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp;
bool changed;
orb_check(sub_man, &changed);
if (!changed) {
mavlink_log_critical(mavlink_fd, "no manual control, aborting");
return ERROR;
}
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
@ -80,9 +83,11 @@ int do_rc_calibration(int mavlink_fd)
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
close(sub_man);
return ERROR;
}
mavlink_log_info(mavlink_fd, "trim calibration done");
close(sub_man);
return OK;
}

23
src/modules/commander/state_machine_helper.cpp
View File

@ -348,20 +348,15 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t
break;
case NAVIGATION_STATE_AUTO_TAKEOFF:
/* only transitions from AUTO_READY */
if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
}
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_LOITER:

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

@ -516,7 +516,7 @@ void mavlink_update_system(void)
int mavlink_thread_main(int argc, char *argv[])
{
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&lb, 5);
mavlink_logbuffer_init(&lb, 10);
int ch;
char *device_name = "/dev/ttyS1";
@ -738,6 +738,9 @@ int mavlink_thread_main(int argc, char *argv[])
/* Reset the UART flags to original state */
tcsetattr(uart, TCSANOW, &uart_config_original);
/* destroy log buffer */
//mavlink_logbuffer_destroy(&lb);
thread_running = false;
exit(0);

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

@ -747,6 +747,7 @@ receive_start(int uart)
fcntl(uart, F_SETFL, flags | O_NONBLOCK);
struct sched_param param;
(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);

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

@ -227,7 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
const float takeoff_alt_default = 10.0f;
float ref_alt = 0.0f;
hrt_abstime ref_alt_t = 0;
uint64_t local_ref_timestamp = 0;
@ -244,6 +244,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
parameters_init(&params_h);
parameters_update(&params_h, &params);
for (int i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
@ -252,36 +253,35 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
int paramcheck_counter = 0;
while (!thread_should_exit) {
/* check parameters at 1 Hz */
if (++paramcheck_counter >= 50) {
paramcheck_counter = 0;
bool param_updated;
orb_check(param_sub, &param_updated);
if (param_updated) {
parameters_update(&params_h, &params);
bool param_updated;
orb_check(param_sub, &param_updated);
for (int i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
/* use integral_limit_out = tilt_max / 2 */
float i_limit;
if (param_updated) {
/* clear updated flag */
struct parameter_update_s ps;
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* update params */
parameters_update(&params_h, &params);
if (params.xy_vel_i == 0.0) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0;
for (int i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
/* use integral_limit_out = tilt_max / 2 */
float i_limit;
} else {
i_limit = 1.0f; // not used really
}
if (params.xy_vel_i == 0.0f) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
} else {
i_limit = 1.0f; // not used really
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
}
bool updated;
@ -351,7 +351,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (reset_sp_z) {
reset_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z);
mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
}
/* move altitude setpoint with throttle stick */
@ -377,7 +377,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp.y = local_pos.y;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
/* move position setpoint with roll/pitch stick */
@ -424,12 +424,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (reset_auto_pos) {
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = -takeoff_alt_default;
local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
local_pos_sp.yaw = att.yaw;
local_pos_sp_valid = true;
att_sp.yaw_body = att.yaw;
reset_auto_pos = false;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z);
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
@ -444,7 +444,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home);
mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
}
if (global_pos_sp_reproject) {
@ -468,7 +468,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp.yaw = global_pos_sp.yaw;
att_sp.yaw_body = global_pos_sp.yaw;
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, 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);
} else {
if (!local_pos_sp_valid) {
@ -481,7 +481,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp_valid = true;
}
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
}
@ -505,7 +505,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* set altitude setpoint to 5m under ground,
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
local_pos_sp.z = 5.0f;
mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z);
mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
}
reset_sp_z = true;
@ -515,7 +515,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (reset_sp_z) {
reset_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z);
mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
}
}
@ -527,7 +527,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp.yaw = att.yaw;
local_pos_sp_valid = true;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y);
mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
}
}
@ -588,7 +588,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
}
thrust_pid_set_integral(&z_vel_pid, -i);
mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i);
mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);

4
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
View File

@ -59,6 +59,8 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
int parameters_init(struct multirotor_position_control_param_handles *h)
{
h->takeoff_alt = param_find("NAV_TAKEOFF_ALT");
h->takeoff_gap = param_find("NAV_TAKEOFF_GAP");
h->thr_min = param_find("MPC_THR_MIN");
h->thr_max = param_find("MPC_THR_MAX");
h->z_p = param_find("MPC_Z_P");
@ -84,6 +86,8 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
{
param_get(h->takeoff_alt, &(p->takeoff_alt));
param_get(h->takeoff_gap, &(p->takeoff_gap));
param_get(h->thr_min, &(p->thr_min));
param_get(h->thr_max, &(p->thr_max));
param_get(h->z_p, &(p->z_p));

4
src/modules/multirotor_pos_control/multirotor_pos_control_params.h
View File

@ -41,6 +41,8 @@
#include <systemlib/param/param.h>
struct multirotor_position_control_params {
float takeoff_alt;
float takeoff_gap;
float thr_min;
float thr_max;
float z_p;
@ -63,6 +65,8 @@ struct multirotor_position_control_params {
};
struct multirotor_position_control_param_handles {
param_t takeoff_alt;
param_t takeoff_gap;
param_t thr_min;
param_t thr_max;
param_t z_p;

45
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -53,7 +53,7 @@
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@ -181,7 +181,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
uint32_t baro_counter = 0;
/* declare and safely initialize all structs */
struct actuator_controls_effective_s actuator;
struct actuator_controls_s actuator;
memset(&actuator, 0, sizeof(actuator));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
@ -200,7 +200,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* subscribe */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@ -337,7 +337,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* actuator */
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
}
/* armed */
@ -429,26 +429,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fds[6].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) {
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
/* initialize reference position if needed */
if (!ref_xy_inited) {
if (ref_xy_init_start == 0) {
ref_xy_init_start = t;
/* require EPH < 10m */
if (gps.eph_m < 10.0f) {
if (ref_xy_init_start == 0) {
ref_xy_init_start = t;
} else if (t > ref_xy_init_start + ref_xy_init_delay) {
ref_xy_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
} else if (t > ref_xy_init_start + ref_xy_init_delay) {
ref_xy_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
local_pos.ref_lat = gps.lat;
local_pos.ref_lon = gps.lon;
local_pos.ref_timestamp = t;
local_pos.ref_lat = gps.lat;
local_pos.ref_lon = gps.lon;
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(lat, lon);
warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon);
mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
/* initialize projection */
map_projection_init(lat, lon);
warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon);
mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
}
} else {
ref_xy_init_start = 0;
}
}
@ -541,7 +546,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
alt_disp = alt_disp * alt_disp;
float land_disp2 = params.land_disp * params.land_disp;
/* get actual thrust output */
float thrust = armed.armed ? actuator.control_effective[3] : 0.0f;
float thrust = armed.armed ? actuator.control[3] : 0.0f;
if (landed) {
if (alt_disp > land_disp2 && thrust > params.land_thr) {

7
src/modules/position_estimator_mc/position_estimator_mc_main.c
View File

@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[])
local_pos_est.vz = x_z_aposteriori_k[1];
local_pos_est.timestamp = hrt_absolute_time();
if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
if(local_pos_est_pub > 0)
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
else
local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
//char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]);
//mavlink_log_info(mavlink_fd, buf);
}
}
} /* end of poll return value check */

4
src/modules/px4iofirmware/mixer.cpp
View File

@ -358,8 +358,8 @@ static unsigned mixer_text_length = 0;
void
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while outputs armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
/* do not allow a mixer change while safety off */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
return;
}

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

@ -890,13 +890,14 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) {
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state;
log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
LOGBUFFER_WRITE_AND_COUNT(STAT);
}

3
src/modules/sdlog2/sdlog2_messages.h
View File

@ -155,6 +155,7 @@ struct log_STAT_s {
float battery_current;
float battery_remaining;
uint8_t battery_warning;
uint8_t landed;
};
/* --- RC - RC INPUT CHANNELS --- */
@ -263,7 +264,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"),
LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),

12
src/modules/sensors/sensor_params.c
View File

@ -68,7 +68,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
@ -157,6 +158,13 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC15_MIN, 1000);
PARAM_DEFINE_FLOAT(RC15_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
@ -164,6 +172,8 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
#else
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
/* PX4IOAR: 0.00838095238 */
/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
#endif

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

@ -301,6 +301,7 @@ private:
float accel_offset[3];
float accel_scale[3];
float diff_pres_offset_pa;
float diff_pres_analog_enabled;
int board_rotation;
int external_mag_rotation;
@ -348,6 +349,7 @@ private:
param_t mag_offset[3];
param_t mag_scale[3];
param_t diff_pres_offset_pa;
param_t diff_pres_analog_enabled;
param_t rc_map_roll;
param_t rc_map_pitch;
@ -617,6 +619,7 @@ Sensors::Sensors() :
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
@ -657,7 +660,9 @@ int
Sensors::parameters_update()
{
bool rc_valid = true;
float tmpScaleFactor = 0.0f;
float tmpRevFactor = 0.0f;
/* rc values */
for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
@ -667,18 +672,21 @@ Sensors::parameters_update()
param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
/* handle blowup in the scaling factor calculation */
if (!isfinite(_parameters.scaling_factor[i]) ||
_parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f ||
_parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) {
if (!isfinite(tmpScaleFactor) ||
(tmpRevFactor < 0.000001f) ||
(tmpRevFactor > 0.2f) ) {
warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
/* scaling factors do not make sense, lock them down */
_parameters.scaling_factor[i] = 0;
_parameters.scaling_factor[i] = 0.0f;
rc_valid = false;
}
else {
_parameters.scaling_factor[i] = tmpScaleFactor;
}
}
/* handle wrong values */
@ -784,6 +792,7 @@ Sensors::parameters_update()
/* Airspeed offset */
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@ -1266,9 +1275,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/**
* The voltage divider pulls the signal down, only act on
* a valid voltage from a connected sensor
* a valid voltage from a connected sensor. Also assume a non-
* zero offset from the sensor if its connected.
*/
if (voltage > 0.4f) {
if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) {
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor

10
src/modules/systemlib/mavlink_log.c
View File

@ -51,7 +51,15 @@ __EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
lb->size = size;
lb->start = 0;
lb->count = 0;
lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
lb->elems = calloc(lb->size, sizeof(struct mavlink_logmessage));
}
__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb)
{
lb->size = 0;
lb->start = 0;
lb->count = 0;
free(lb->elems);
}
__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)

15
src/modules/systemlib/param/param.c
View File

@ -505,24 +505,31 @@ param_get_default_file(void)
int
param_save_default(void)
{
int result;
/* delete the file in case it exists */
unlink(param_get_default_file());
struct stat buffer;
if (stat(param_get_default_file(), &buffer) == 0) {
result = unlink(param_get_default_file());
if (result != OK)
warnx("unlinking file %s failed.", param_get_default_file());
}
/* create the file */
int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0) {
warn("opening '%s' for writing failed", param_get_default_file());
return -1;
return fd;
}
int result = param_export(fd, false);
result = param_export(fd, false);
close(fd);
if (result != 0) {
warn("error exporting parameters to '%s'", param_get_default_file());
unlink(param_get_default_file());
return -2;
return result;
}
return 0;

16
src/modules/systemlib/pid/pid.c
View File

@ -51,6 +51,8 @@
#include "pid.h"
#include <math.h>
#define SIGMA 0.000001f
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
float limit, uint8_t mode, float dt_min)
{
@ -168,8 +170,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
// Calculate the error integral and check for saturation
i = pid->integral + (error * dt);
if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
fabsf(i) > pid->intmax) {
if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
fabsf(i) > pid->intmax) {
i = pid->integral; // If saturated then do not update integral value
pid->saturated = 1;
@ -186,11 +188,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (isfinite(output)) {
if (output > pid->limit) {
output = pid->limit;
if (pid->limit > SIGMA) {
if (output > pid->limit) {
output = pid->limit;
} else if (output < -pid->limit) {
output = -pid->limit;
} else if (output < -pid->limit) {
output = -pid->limit;
}
}
pid->last_output = output;

11
src/modules/systemlib/rc_check.c
View File

@ -66,7 +66,7 @@ int rc_calibration_check(void) {
// count++;
// }
int channel_fail_count = 0;
for (int i = 0; i < RC_CHANNELS_MAX; i++) {
/* should the channel be enabled? */
@ -112,13 +112,13 @@ int rc_calibration_check(void) {
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max);
/* give system time to flush error message in case there are more */
usleep(100000);
}
@ -142,7 +142,12 @@ int rc_calibration_check(void) {
/* sanity checks pass, enable channel */
if (count) {
mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
usleep(100000);
}
channel_fail_count += count;
}
return channel_fail_count;
}

6
src/modules/uORB/topics/vehicle_attitude_setpoint.h
View File

@ -64,6 +64,12 @@ struct vehicle_attitude_setpoint_s
float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
bool R_valid; /**< Set to true if rotation matrix is valid */
//! For quaternion-based attitude control
float q_d[4]; /** Desired quaternion for quaternion control */
bool q_d_valid; /**< Set to true if quaternion vector is valid */
float q_e[4]; /** Attitude error in quaternion */
bool q_e_valid; /**< Set to true if quaternion error vector is valid */
float thrust; /**< Thrust in Newton the power system should generate */
};

6
src/modules/uORB/topics/vehicle_status.h
View File

@ -153,9 +153,9 @@ enum VEHICLE_TYPE {
};
enum VEHICLE_BATTERY_WARNING {
VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */
VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */
VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */
VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */
};
/**

13
src/systemcmds/preflight_check/preflight_check.c
View File

@ -142,6 +142,10 @@ int preflight_check_main(int argc, char *argv[])
bool rc_ok = (OK == rc_calibration_check());
/* warn */
if (!rc_ok)
warnx("rc calibration test failed");
/* require RC ok to keep system_ok */
system_ok &= rc_ok;
@ -156,6 +160,9 @@ system_eval:
} else {
fflush(stdout);
warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM");
fflush(stderr);
int buzzer = open("/dev/tone_alarm", O_WRONLY);
int leds = open(LED_DEVICE_PATH, 0);
@ -176,15 +183,15 @@ system_eval:
led_toggle(leds, LED_AMBER);
if (i % 10 == 0) {
ioctl(buzzer, TONE_SET_ALARM, 4);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
} else if (i % 5 == 0) {
ioctl(buzzer, TONE_SET_ALARM, 2);
ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
}
usleep(100000);
}
/* stop alarm */
ioctl(buzzer, TONE_SET_ALARM, 0);
ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
/* switch on leds */
led_on(leds, LED_BLUE);

4
src/systemcmds/tests/test_hrt.c
View File

@ -137,7 +137,7 @@ int test_tone(int argc, char *argv[])
tone = atoi(argv[1]);
if (tone == 0) {
result = ioctl(fd, TONE_SET_ALARM, 0);
result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);
if (result < 0) {
printf("failed clearing alarms\n");
@ -148,7 +148,7 @@ int test_tone(int argc, char *argv[])
}
} else {
result = ioctl(fd, TONE_SET_ALARM, 0);
result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);
if (result < 0) {
printf("failed clearing alarms\n");

30
src/systemcmds/tests/tests_file.c
View File

@ -76,9 +76,39 @@ test_file(int argc, char *argv[])
close(fd);
unlink("/fs/microsd/testfile");
warnx("512KiB in %llu microseconds", end - start);
perf_print_counter(wperf);
perf_free(wperf);
warnx("running unlink test");
/* ensure that common commands do not run against file count limits */
for (unsigned i = 0; i < 64; i++) {
warnx("unlink iteration #%u", i);
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
if (fd < 0)
errx(1, "failed opening test file before unlink()");
int ret = write(fd, buf, sizeof(buf));
if (ret < 0)
errx(1, "failed writing test file before unlink()");
close(fd);
ret = unlink("/fs/microsd/testfile");
if (ret != OK)
errx(1, "failed unlinking test file");
fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
if (fd < 0)
errx(1, "failed opening test file after unlink()");
ret = write(fd, buf, sizeof(buf));
if (ret < 0)
errx(1, "failed writing test file after unlink()");
close(fd);
}
return 0;
}