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

Merged origin/master into batt_fixes

This commit is contained in:
Lorenz Meier 2014-01-07 15:34:13 +01:00
parent 2a2c8337e8 94b539dfdd
commit 9b711d6a72
152 changed files with 8832 additions and 2762 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ROMFS/px4fmu_common/init.d/15_tbs_discovery → ROMFS/px4fmu_common/init.d/10015_tbs_discovery
View File

@ -32,8 +32,6 @@ fi
#
param set MAV_TYPE 2
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
@ -74,8 +72,3 @@ pwm max -c 1234 -p 1900
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

10
ROMFS/px4fmu_common/init.d/16_3dr_iris → ROMFS/px4fmu_common/init.d/10016_3dr_iris
View File

@ -32,8 +32,6 @@ fi
#
param set MAV_TYPE 2
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
@ -67,15 +65,9 @@ pwm rate -c 1234 -r 400
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
pwm min -c 1234 -p 1050
#
# 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/100_mpx_easystar → ROMFS/px4fmu_common/init.d/2100_mpx_easystar
View File

@ -42,8 +42,6 @@ fi
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
@ -80,8 +78,3 @@ fi
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ]
then
exit
fi

7
ROMFS/px4fmu_common/init.d/101_hk_bixler → ROMFS/px4fmu_common/init.d/2101_hk_bixler
View File

@ -42,8 +42,6 @@ fi
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
@ -80,8 +78,3 @@ fi
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ]
then
exit
fi

21
ROMFS/px4fmu_common/init.d/31_io_phantom → ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
View File

@ -1,6 +1,6 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV"
echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
#
# Load default params for this platform
@ -28,7 +28,7 @@ then
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 17
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
@ -42,8 +42,6 @@ fi
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
@ -64,24 +62,21 @@ else
set EXIT_ON_END yes
fi
pwm disarmed -c 3 -p 1056
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ]
then
exit
fi

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

@ -20,8 +20,6 @@ fi
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
@ -58,8 +56,3 @@ fi
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ]
then
exit
fi

85
ROMFS/px4fmu_common/init.d/3031_io_phantom Normal file
View File

@ -0,0 +1,85 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_AIRSPD_MIN 11.4
param set FW_AIRSPD_TRIM 14
param set FW_AIRSPD_MAX 22
param set FW_L1_PERIOD 15
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 45
param set FW_P_LIM_MIN -45
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 15
param set FW_R_P 80
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.8
param set FW_THR_LND_MAX 0
param set FW_THR_MAX 1
param set FW_THR_MIN 0.5
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 2.0
param set FW_Y_ROLLFF 1.0
param set RC_SCALE_ROLL 0.6
param set RC_SCALE_PITCH 0.6
param set TRIM_PITCH 0.1
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 and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing

7
ROMFS/px4fmu_common/init.d/32_skywalker_x5 → ROMFS/px4fmu_common/init.d/3032_skywalker_x5
View File

@ -20,8 +20,6 @@ fi
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
@ -58,8 +56,3 @@ fi
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ]
then
exit
fi

84
ROMFS/px4fmu_common/init.d/3033_io_wingwing Normal file
View File

@ -0,0 +1,84 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
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 and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing

84
ROMFS/px4fmu_common/init.d/3034_io_fx79 Normal file
View File

@ -0,0 +1,84 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_AIRSPD_MAX 20
param set FW_AIRSPD_TRIM 12
param set FW_AIRSPD_MIN 15
param set FW_L1_PERIOD 12
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 80
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.75
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 1.1
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
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 and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix
else
echo "Using /etc/mixers/FMU_FX79.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing

0
ROMFS/px4fmu_common/init.d/08_ardrone → ROMFS/px4fmu_common/init.d/4008_ardrone
View File

5
ROMFS/px4fmu_common/init.d/09_ardrone_flow → ROMFS/px4fmu_common/init.d/4009_ardrone_flow
View File

@ -54,11 +54,6 @@ ardrone_interface start -d /dev/ttyS1
#
sh /etc/init.d/rc.sensors
#
# Start the commander.
#
commander start
#
# Start the attitude estimator
#

30
ROMFS/px4fmu_common/init.d/10_dji_f330 → ROMFS/px4fmu_common/init.d/4010_dji_f330
View File

@ -41,14 +41,6 @@ then
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
@ -69,29 +61,9 @@ else
set EXIT_ON_END yes
fi
#
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals (for DJI ESCs)
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
sh /etc/init.d/rc.mc_interface
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

9
ROMFS/px4fmu_common/init.d/11_dji_f450 → ROMFS/px4fmu_common/init.d/4011_dji_f450
View File

@ -32,8 +32,6 @@ fi
#
param set MAV_TYPE 2
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
@ -43,8 +41,6 @@ then
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
else
# Start MAVLink (on UART1 / ttyS0)
@ -76,8 +72,3 @@ pwm max -c 1234 -p 1800
# Start common multirotor apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

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

@ -28,11 +28,6 @@ usleep 5000
# Start and configure PX4IO interface
#
sh /etc/init.d/rc.io
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Start the sensors (depends on orb, px4io)

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

@ -34,8 +34,6 @@ fi
#
param set MAV_TYPE 2
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
@ -76,8 +74,3 @@ pwm max -c 1234 -p 1900
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

51
ROMFS/px4fmu_common/init.d/800_sdlogger Normal file
View File

@ -0,0 +1,51 @@
#!nsh
echo "[init] PX4FMU v1, v2 init to log only
#
# 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 save
fi
#
# 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
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
sh /etc/init.d/rc.sensors
gps start
attitude_estimator_ekf start
position_estimator_inav start
if [ -d /fs/microsd ]
then
if [ $BOARD == fmuv1 ]
then
sdlog2 start -r 50 -e -b 16
else
sdlog2 start -r 200 -e -b 16
fi
fi

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

@ -97,18 +97,11 @@ fi
#
if [ $MKBLCTRL_FRAME == x ]
then
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix
else
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix
fi
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
#
# Start common for all multirotors apps
#

14
ROMFS/px4fmu_common/init.d/12-13_hex → ROMFS/px4fmu_common/init.d/rc.hexa
View File

@ -52,7 +52,7 @@ param set MAV_TYPE 13
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
# Start and configure PX4IO interface
#
if px4io detect
then
@ -62,23 +62,19 @@ then
sh /etc/init.d/rc.io
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
# This is not possible on a hexa
tone_alarm error
fi
#
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix
mixer load /dev/pwm_output $MIXER
#
# Set PWM output frequency to 400 Hz
#
pwm rate -c 123456 -r 400
pwm rate -a -r 400
#
# Set disarmed, min and max PWM signals

49
ROMFS/px4fmu_common/init.d/rc.mc_interface Normal file
View File

@ -0,0 +1,49 @@
#!nsh
#
# Script to set PWM min / max limits and mixer
#
#
# Load mixer
#
if [ $FRAME_GEOMETRY == x ]
then
echo "Frame geometry X"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
else
if [ $FRAME_GEOMETRY == w ]
then
echo "Frame geometry W"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
else
echo "Frame geometry +"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
fi
fi
if [ $FRAME_COUNT == 4 ]
then
set OUTPUTS 1234
param set MAV_TYPE 2
else
if [ $FRAME_COUNT == 6 ]
then
set OUTPUTS 123456
param set MAV_TYPE 13
else
set OUTPUTS 12345678
fi
fi
#
# Set PWM output frequency
#
pwm rate -c $OUTPUTS -r $PWM_RATE
#
# Set disarmed, min and max PWM signals (for DJI ESCs)
#
pwm disarmed -c $OUTPUTS -p $PWM_DISARMED
pwm min -c $OUTPUTS -p $PWM_MIN
pwm max -c $OUTPUTS -p $PWM_MAX

94
ROMFS/px4fmu_common/init.d/rc.octo Normal file
View File

@ -0,0 +1,94 @@
#!nsh
echo "[init] Octorotor startup"
#
# 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 list: https://pixhawk.ethz.ch/mavlink/
# 14 = octorotor
#
param set MAV_TYPE 14
set EXIT_ON_END no
#
# Start and configure PX4IO interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
else
# This is not possible on an octo
tone_alarm error
fi
#
# Load mixer
#
mixer load /dev/pwm_output $MIXER
#
# Set PWM output frequency to 400 Hz
#
pwm rate -a -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 12345678 -p 900
pwm min -c 12345678 -p 1100
pwm max -c 12345678 -p 1900
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

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

@ -19,12 +19,18 @@ fi
if mpu6000 start
then
echo "using MPU6000"
set BOARD fmuv1
else
echo "using L3GD20 and LSM303D"
l3gd20 start
lsm303d start
fi
if l3gd20 start
then
echo "using L3GD20(H)"
fi
if lsm303d start
then
set BOARD fmuv2
else
set BOARD fmuv1
fi
# Start airspeed sensors

197
ROMFS/px4fmu_common/init.d/rcS Executable file → Normal file
View File

@ -8,6 +8,8 @@
#
set MODE autostart
set logfile /fs/microsd/bootlog.txt
#
# Try to mount the microSD card.
#
@ -147,86 +149,158 @@ then
nshterm /dev/ttyACM0 &
fi
#
# Upgrade PX4IO firmware
#
if px4io detect
then
echo "PX4IO running, not upgrading"
else
echo "Attempting to upgrade PX4IO"
if px4io update
then
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
if [ -f /etc/extras/px4io-v2_default.bin ]
then
set io_file /etc/extras/px4io-v2_default.bin
else
set io_file /etc/extras/px4io-v1_default.bin
fi
if px4io start
then
echo "PX4IO OK"
echo "PX4IO OK" >> $logfile
fi
if px4io checkcrc $io_file
then
echo "PX4IO CRC OK"
echo "PX4IO CRC OK" >> $logfile
else
echo "PX4IO CRC failure"
echo "PX4IO CRC failure" >> $logfile
tone_alarm MBABGP
if px4io forceupdate 14662 $io_file
then
usleep 500000
if px4io start
then
echo "PX4IO restart OK"
echo "PX4IO restart OK" >> $logfile
tone_alarm MSPAA
else
echo "PX4IO restart failed"
echo "PX4IO restart failed" >> $logfile
tone_alarm MNGGG
sleep 10
reboot
fi
else
echo "No PX4IO to upgrade here"
echo "PX4IO update failed"
echo "PX4IO update failed" >> $logfile
tone_alarm MNGGG
fi
fi
set EXIT_ON_END no
#
# Check if auto-setup from one of the standard scripts is wanted
# SYS_AUTOSTART = 0 means no autostart (default)
#
# AUTOSTART PARTITION:
# 0 .. 999 Reserved (historical)
# 1000 .. 1999 Simulation setups
# 2000 .. 2999 Standard planes
# 3000 .. 3999 Flying wing
# 4000 .. 4999 Quad X
# 5000 .. 5999 Quad +
# 6000 .. 6999 Hexa X
# 7000 .. 7999 Hexa +
# 8000 .. 8999 Octo X
# 9000 .. 9999 Octo +
# 10000 .. 10999 Wide arm / H frame
# 11000 .. 11999 Hexa Cox
# 12000 .. 12999 Octo Cox
if param compare SYS_AUTOSTART 8
if param compare SYS_AUTOSTART 4008 8
then
sh /etc/init.d/08_ardrone
sh /etc/init.d/4008_ardrone
set MODE custom
fi
if param compare SYS_AUTOSTART 9
if param compare SYS_AUTOSTART 4009 9
then
sh /etc/init.d/09_ardrone_flow
sh /etc/init.d/4009_ardrone_flow
set MODE custom
fi
if param compare SYS_AUTOSTART 10
if param compare SYS_AUTOSTART 4010 10
then
sh /etc/init.d/10_dji_f330
set FRAME_GEOMETRY x
set FRAME_COUNT 4
set PWM_MIN 1200
set PWM_MAX 1900
set PWM_DISARMED 900
sh /etc/init.d/4010_dji_f330
set MODE custom
fi
if param compare SYS_AUTOSTART 11
if param compare SYS_AUTOSTART 4011 11
then
sh /etc/init.d/11_dji_f450
sh /etc/init.d/4011_dji_f450
set MODE custom
fi
if param compare SYS_AUTOSTART 12
if param compare SYS_AUTOSTART 4012
then
sh /etc/init.d/666_fmu_q_x550
set MODE custom
fi
if param compare SYS_AUTOSTART 6012 12
then
set MIXER /etc/mixers/FMU_hex_x.mix
sh /etc/init.d/12-13_hex
sh /etc/init.d/rc.hexa
set MODE custom
fi
if param compare SYS_AUTOSTART 13
if param compare SYS_AUTOSTART 7013 13
then
set MIXER /etc/mixers/FMU_hex_+.mix
sh /etc/init.d/12-13_hex
sh /etc/init.d/rc.hexa
set MODE custom
fi
if param compare SYS_AUTOSTART 8001
then
set MIXER /etc/mixers/FMU_octo_x.mix
sh /etc/init.d/rc.octo
set MODE custom
fi
if param compare SYS_AUTOSTART 9001
then
set MIXER /etc/mixers/FMU_octo_+.mix
sh /etc/init.d/rc.octo
set MODE custom
fi
if param compare SYS_AUTOSTART 12001
then
set MIXER /etc/mixers/FMU_octo_cox.mix
sh /etc/init.d/rc.octo
set MODE custom
fi
if param compare SYS_AUTOSTART 15
if param compare SYS_AUTOSTART 10015 15
then
sh /etc/init.d/15_tbs_discovery
sh /etc/init.d/10015_tbs_discovery
set MODE custom
fi
if param compare SYS_AUTOSTART 16
if param compare SYS_AUTOSTART 10016 16
then
sh /etc/init.d/16_3dr_iris
sh /etc/init.d/10016_3dr_iris
set MODE custom
fi
# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame
if param compare SYS_AUTOSTART 17
if param compare SYS_AUTOSTART 4017 17
then
set MKBLCTRL_MODE no
set MKBLCTRL_FRAME x
@ -235,7 +309,7 @@ then
fi
# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame
if param compare SYS_AUTOSTART 18
if param compare SYS_AUTOSTART 5018 18
then
set MKBLCTRL_MODE no
set MKBLCTRL_FRAME +
@ -244,7 +318,7 @@ then
fi
# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing
if param compare SYS_AUTOSTART 19
if param compare SYS_AUTOSTART 4019 19
then
set MKBLCTRL_MODE yes
set MKBLCTRL_FRAME x
@ -253,7 +327,7 @@ then
fi
# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing
if param compare SYS_AUTOSTART 20
if param compare SYS_AUTOSTART 5020 20
then
set MKBLCTRL_MODE yes
set MKBLCTRL_FRAME +
@ -262,7 +336,7 @@ then
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 21
if param compare SYS_AUTOSTART 4021 21
then
set FRAME_GEOMETRY x
set ESC_MAKER afro
@ -271,46 +345,70 @@ then
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 22
if param compare SYS_AUTOSTART 10022 22
then
set FRAME_GEOMETRY w
sh /etc/init.d/rc.custom_io_esc
set MODE custom
fi
if param compare SYS_AUTOSTART 30
if param compare SYS_AUTOSTART 3030 30
then
sh /etc/init.d/30_io_camflyer
sh /etc/init.d/3030_io_camflyer
set MODE custom
fi
if param compare SYS_AUTOSTART 31
if param compare SYS_AUTOSTART 3031 31
then
sh /etc/init.d/31_io_phantom
sh /etc/init.d/3031_io_phantom
set MODE custom
fi
if param compare SYS_AUTOSTART 32
if param compare SYS_AUTOSTART 3032 32
then
sh /etc/init.d/32_skywalker_x5
sh /etc/init.d/3032_skywalker_x5
set MODE custom
fi
if param compare SYS_AUTOSTART 3033 33
then
sh /etc/init.d/3033_io_wingwing
set MODE custom
fi
if param compare SYS_AUTOSTART 3034 34
then
sh /etc/init.d/3034_io_fx79
set MODE custom
fi
if param compare SYS_AUTOSTART 40
then
sh /etc/init.d/40_io_segway
set MODE custom
fi
if param compare SYS_AUTOSTART 100
if param compare SYS_AUTOSTART 2100 100
then
sh /etc/init.d/100_mpx_easystar
sh /etc/init.d/2100_mpx_easystar
set MODE custom
fi
if param compare SYS_AUTOSTART 101
if param compare SYS_AUTOSTART 2101 101
then
sh /etc/init.d/101_hk_bixler
sh /etc/init.d/2101_hk_bixler
set MODE custom
fi
if param compare SYS_AUTOSTART 2102 102
then
sh /etc/init.d/2102_3dr_skywalker
set MODE custom
fi
if param compare SYS_AUTOSTART 800
then
sh /etc/init.d/800_sdlogger
set MODE custom
fi
@ -353,5 +451,10 @@ then
fi
if [ $EXIT_ON_END == yes ]
then
exit
fi
# End of autostart
fi

72
ROMFS/px4fmu_common/mixers/FMU_FX79.mix Executable file
View File

@ -0,0 +1,72 @@
FX-79 Delta-wing mixer for PX4FMU
=================================
Designed for FX-79.
TODO (sjwilks): Add mixers for flaps.
This file defines mixers suitable for controlling a delta wing aircraft using
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
assumed to be unused.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch) and 3 (thrust).
See the README for more information on the scaler format.
Elevon mixers
-------------
Three scalers total (output, roll, pitch).
On the assumption that the two elevon servos are physically reversed, the pitch
input is inverted between the two servos.
The scaling factor for roll inputs is adjusted to implement differential travel
for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 5000 8000 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 8000 5000 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
Output 2
--------
This mixer is empty.
Z:
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000

88
ROMFS/px4fmu_logging/init.d/rcS Normal file
View File

@ -0,0 +1,88 @@
#!nsh
#
# PX4FMU startup script for logging purposes
#
#
# Try to mount the microSD card.
#
echo "[init] looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "[init] card mounted at /fs/microsd"
# Start playing the startup tune
tone_alarm start
else
echo "[init] no microSD card found"
# Play SOS
tone_alarm error
fi
uorb start
#
# Start sensor drivers here.
#
ms5611 start
adc start
# mag might be external
if hmc5883 start
then
echo "using HMC5883"
fi
if mpu6000 start
then
echo "using MPU6000"
fi
if l3gd20 start
then
echo "using L3GD20(H)"
fi
if lsm303d start
then
set BOARD fmuv2
else
set BOARD fmuv1
fi
# Start airspeed sensors
if meas_airspeed start
then
echo "using MEAS airspeed sensor"
else
if ets_airspeed start
then
echo "using ETS airspeed sensor (bus 3)"
else
if ets_airspeed start -b 1
then
echo "Using ETS airspeed sensor (bus 1)"
fi
fi
fi
#
# Start the sensor collection task.
# IMPORTANT: this also loads param offsets
# ALWAYS start this task before the
# preflight_check.
#
if sensors start
then
echo "SENSORS STARTED"
fi
sdlog2 start -r 250 -e -b 16
if sercon
then
echo "[init] USB interface connected"
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi

BIN
ROMFS/px4fmu_logging/logging/conv.zip Normal file
View File

Binary file not shown.

8
ROMFS/px4fmu_test/init.d/rcS Executable file → Normal file
View File

@ -2,3 +2,11 @@
#
# PX4FMU startup script for test hackery.
#
if sercon
then
echo "[init] USB interface connected"
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi

19
Tools/px_uploader.py
View File

@ -370,14 +370,17 @@ class uploader(object):
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)
try:
# 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)
except:
return

146
makefiles/config_px4fmu-v1_backside.mk Normal file
View File

@ -0,0 +1,146 @@
#
# Makefile for the px4fmu_default configuration
#
#
# Use the configuration's ROMFS.
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
#
# Board support modules
#
MODULES += drivers/device
MODULES += drivers/stm32
MODULES += drivers/stm32/adc
MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/px4io
MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/ardrone_interface
MODULES += drivers/l3gd20
MODULES += drivers/bma180
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += modules/sensors
#
# System commands
#
MODULES += systemcmds/eeprom
MODULES += systemcmds/ramtron
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/i2c
MODULES += systemcmds/mixer
MODULES += systemcmds/param
MODULES += systemcmds/perf
MODULES += systemcmds/preflight_check
MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
#
# General system control
#
MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led
#
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/att_pos_estimator_ekf
#
# Vehicle Control
#
MODULES += modules/fixedwing_backside
#
# Logging
#
MODULES += modules/sdlog2
#
# Unit tests
#
#MODULES += modules/unit_test
#MODULES += modules/commander/commander_tests
#
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
#
# Libraries
#
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
#
# Demo apps
#
#MODULES += examples/math_demo
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/hello_sky
#MODULES += examples/px4_simple_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/daemon
#MODULES += examples/px4_daemon_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/debug_values
#MODULES += examples/px4_mavlink_debug
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
#MODULES += examples/fixedwing_control
# Hardware test
#MODULES += examples/hwtest
#
# Transitional support - add commands from the NuttX export archive.
#
# In general, these should move to modules over time.
#
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
# to make the table a bit more readable.
#
define _B
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
endef
# command priority stack entrypoint
BUILTIN_COMMANDS := \
$(call _B, sercon, , 2048, sercon_main ) \
$(call _B, serdis, , 2048, serdis_main ) \
$(call _B, sysinfo, , 2048, sysinfo_main )

14
makefiles/config_px4fmu-v1_default.mk
View File

@ -21,7 +21,7 @@ MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/ardrone_interface
MODULES += drivers/l3gd20
MODULES += drivers/bma180
#MODULES += drivers/bma180
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
@ -33,10 +33,10 @@ MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
#
@ -69,23 +69,23 @@ MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led
#
# Estimation modules (EKF / other filters)
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
#MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
MODULES += modules/multirotor_pos_control
MODULES += examples/flow_position_control
MODULES += examples/flow_speed_control
#MODULES += examples/flow_position_control
#MODULES += examples/flow_speed_control
#
# Logging

5
makefiles/config_px4fmu-v2_default.mk
View File

@ -21,6 +21,7 @@ MODULES += drivers/px4fmu
MODULES += drivers/px4io
MODULES += drivers/boards/px4fmu-v2
MODULES += drivers/rgbled
MODULES += drivers/mpu6000
MODULES += drivers/lsm303d
MODULES += drivers/l3gd20
MODULES += drivers/hmc5883
@ -35,6 +36,7 @@ MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
# Needs to be burned to the ground and re-written; for now,
@ -68,9 +70,10 @@ MODULES += modules/mavlink
MODULES += modules/mavlink_onboard
#
# Estimation modules (EKF / other filters)
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator

3
makefiles/toolchain_gnu-arm-eabi.mk
View File

@ -138,8 +138,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \
-Wold-style-declaration \
-Wmissing-parameter-type \
-Wmissing-prototypes \
-Wnested-externs \
-Wunsuffixed-float-constants
-Wnested-externs
# C++-specific warnings
#

23
mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
View File

File diff suppressed because one or more lines are too long

221
mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h Normal file
View File

@ -0,0 +1,221 @@
// MESSAGE RALLY_FETCH_POINT PACKING
#define MAVLINK_MSG_ID_RALLY_FETCH_POINT 176
typedef struct __mavlink_rally_fetch_point_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t idx; ///< point index (first point is 0)
} mavlink_rally_fetch_point_t;
#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN 3
#define MAVLINK_MSG_ID_176_LEN 3
#define MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC 234
#define MAVLINK_MSG_ID_176_CRC 234
#define MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT { \
"RALLY_FETCH_POINT", \
3, \
{ { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rally_fetch_point_t, target_system) }, \
{ "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rally_fetch_point_t, target_component) }, \
{ "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_rally_fetch_point_t, idx) }, \
} \
}
/**
* @brief Pack a rally_fetch_point message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @param idx point index (first point is 0)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_rally_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t target_system, uint8_t target_component, uint8_t idx)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
_mav_put_uint8_t(buf, 2, idx);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
#else
mavlink_rally_fetch_point_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
packet.idx = idx;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
#endif
}
/**
* @brief Pack a rally_fetch_point message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @param idx point index (first point is 0)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_rally_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t target_system,uint8_t target_component,uint8_t idx)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
_mav_put_uint8_t(buf, 2, idx);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
#else
mavlink_rally_fetch_point_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
packet.idx = idx;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_RALLY_FETCH_POINT;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
#endif
}
/**
* @brief Encode a rally_fetch_point struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param rally_fetch_point C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_rally_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point)
{
return mavlink_msg_rally_fetch_point_pack(system_id, component_id, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx);
}
/**
* @brief Encode a rally_fetch_point struct on a channel
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param rally_fetch_point C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_rally_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_fetch_point_t* rally_fetch_point)
{
return mavlink_msg_rally_fetch_point_pack_chan(system_id, component_id, chan, msg, rally_fetch_point->target_system, rally_fetch_point->target_component, rally_fetch_point->idx);
}
/**
* @brief Send a rally_fetch_point message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
* @param idx point index (first point is 0)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_rally_fetch_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
_mav_put_uint8_t(buf, 2, idx);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
#endif
#else
mavlink_rally_fetch_point_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
packet.idx = idx;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
#endif
#endif
}
#endif
// MESSAGE RALLY_FETCH_POINT UNPACKING
/**
* @brief Get field target_system from rally_fetch_point message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_rally_fetch_point_get_target_system(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 0);
}
/**
* @brief Get field target_component from rally_fetch_point message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_rally_fetch_point_get_target_component(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 1);
}
/**
* @brief Get field idx from rally_fetch_point message
*
* @return point index (first point is 0)
*/
static inline uint8_t mavlink_msg_rally_fetch_point_get_idx(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 2);
}
/**
* @brief Decode a rally_fetch_point message into a struct
*
* @param msg The message to decode
* @param rally_fetch_point C-struct to decode the message contents into
*/
static inline void mavlink_msg_rally_fetch_point_decode(const mavlink_message_t* msg, mavlink_rally_fetch_point_t* rally_fetch_point)
{
#if MAVLINK_NEED_BYTE_SWAP
rally_fetch_point->target_system = mavlink_msg_rally_fetch_point_get_target_system(msg);
rally_fetch_point->target_component = mavlink_msg_rally_fetch_point_get_target_component(msg);
rally_fetch_point->idx = mavlink_msg_rally_fetch_point_get_idx(msg);
#else
memcpy(rally_fetch_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
#endif
}

375
mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h Normal file
View File

@ -0,0 +1,375 @@
// MESSAGE RALLY_POINT PACKING
#define MAVLINK_MSG_ID_RALLY_POINT 175
typedef struct __mavlink_rally_point_t
{
int32_t lat; ///< Latitude of point in degrees * 1E7
int32_t lng; ///< Longitude of point in degrees * 1E7
int16_t alt; ///< Transit / loiter altitude in meters relative to home
int16_t break_alt; ///< Break altitude in meters relative to home
uint16_t land_dir; ///< Heading to aim for when landing. In centi-degrees.
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t idx; ///< point index (first point is 0)
uint8_t count; ///< total number of points (for sanity checking)
uint8_t flags; ///< See RALLY_FLAGS enum for definition of the bitmask.
} mavlink_rally_point_t;
#define MAVLINK_MSG_ID_RALLY_POINT_LEN 19
#define MAVLINK_MSG_ID_175_LEN 19
#define MAVLINK_MSG_ID_RALLY_POINT_CRC 138
#define MAVLINK_MSG_ID_175_CRC 138
#define MAVLINK_MESSAGE_INFO_RALLY_POINT { \
"RALLY_POINT", \
10, \
{ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_rally_point_t, lat) }, \
{ "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_rally_point_t, lng) }, \
{ "alt", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rally_point_t, alt) }, \
{ "break_alt", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rally_point_t, break_alt) }, \
{ "land_dir", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rally_point_t, land_dir) }, \
{ "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_rally_point_t, target_system) }, \
{ "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_rally_point_t, target_component) }, \
{ "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rally_point_t, idx) }, \
{ "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rally_point_t, count) }, \
{ "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_rally_point_t, flags) }, \
} \
}
/**
* @brief Pack a rally_point message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @param idx point index (first point is 0)
* @param count total number of points (for sanity checking)
* @param lat Latitude of point in degrees * 1E7
* @param lng Longitude of point in degrees * 1E7
* @param alt Transit / loiter altitude in meters relative to home
* @param break_alt Break altitude in meters relative to home
* @param land_dir Heading to aim for when landing. In centi-degrees.
* @param flags See RALLY_FLAGS enum for definition of the bitmask.
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_rally_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN];
_mav_put_int32_t(buf, 0, lat);
_mav_put_int32_t(buf, 4, lng);
_mav_put_int16_t(buf, 8, alt);
_mav_put_int16_t(buf, 10, break_alt);
_mav_put_uint16_t(buf, 12, land_dir);
_mav_put_uint8_t(buf, 14, target_system);
_mav_put_uint8_t(buf, 15, target_component);
_mav_put_uint8_t(buf, 16, idx);
_mav_put_uint8_t(buf, 17, count);
_mav_put_uint8_t(buf, 18, flags);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN);
#else
mavlink_rally_point_t packet;
packet.lat = lat;
packet.lng = lng;
packet.alt = alt;
packet.break_alt = break_alt;
packet.land_dir = land_dir;
packet.target_system = target_system;
packet.target_component = target_component;
packet.idx = idx;
packet.count = count;
packet.flags = flags;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_RALLY_POINT;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RALLY_POINT_LEN);
#endif
}
/**
* @brief Pack a rally_point message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @param idx point index (first point is 0)
* @param count total number of points (for sanity checking)
* @param lat Latitude of point in degrees * 1E7
* @param lng Longitude of point in degrees * 1E7
* @param alt Transit / loiter altitude in meters relative to home
* @param break_alt Break altitude in meters relative to home
* @param land_dir Heading to aim for when landing. In centi-degrees.
* @param flags See RALLY_FLAGS enum for definition of the bitmask.
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_rally_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t target_system,uint8_t target_component,uint8_t idx,uint8_t count,int32_t lat,int32_t lng,int16_t alt,int16_t break_alt,uint16_t land_dir,uint8_t flags)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN];
_mav_put_int32_t(buf, 0, lat);
_mav_put_int32_t(buf, 4, lng);
_mav_put_int16_t(buf, 8, alt);
_mav_put_int16_t(buf, 10, break_alt);
_mav_put_uint16_t(buf, 12, land_dir);
_mav_put_uint8_t(buf, 14, target_system);
_mav_put_uint8_t(buf, 15, target_component);
_mav_put_uint8_t(buf, 16, idx);
_mav_put_uint8_t(buf, 17, count);
_mav_put_uint8_t(buf, 18, flags);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RALLY_POINT_LEN);
#else
mavlink_rally_point_t packet;
packet.lat = lat;
packet.lng = lng;
packet.alt = alt;
packet.break_alt = break_alt;
packet.land_dir = land_dir;
packet.target_system = target_system;
packet.target_component = target_component;
packet.idx = idx;
packet.count = count;
packet.flags = flags;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RALLY_POINT_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_RALLY_POINT;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RALLY_POINT_LEN);
#endif
}
/**
* @brief Encode a rally_point struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param rally_point C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_rally_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point)
{
return mavlink_msg_rally_point_pack(system_id, component_id, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags);
}
/**
* @brief Encode a rally_point struct on a channel
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param rally_point C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_rally_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rally_point_t* rally_point)
{
return mavlink_msg_rally_point_pack_chan(system_id, component_id, chan, msg, rally_point->target_system, rally_point->target_component, rally_point->idx, rally_point->count, rally_point->lat, rally_point->lng, rally_point->alt, rally_point->break_alt, rally_point->land_dir, rally_point->flags);
}
/**
* @brief Send a rally_point message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
* @param idx point index (first point is 0)
* @param count total number of points (for sanity checking)
* @param lat Latitude of point in degrees * 1E7
* @param lng Longitude of point in degrees * 1E7
* @param alt Transit / loiter altitude in meters relative to home
* @param break_alt Break altitude in meters relative to home
* @param land_dir Heading to aim for when landing. In centi-degrees.
* @param flags See RALLY_FLAGS enum for definition of the bitmask.
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_rally_point_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_RALLY_POINT_LEN];
_mav_put_int32_t(buf, 0, lat);
_mav_put_int32_t(buf, 4, lng);
_mav_put_int16_t(buf, 8, alt);
_mav_put_int16_t(buf, 10, break_alt);
_mav_put_uint16_t(buf, 12, land_dir);
_mav_put_uint8_t(buf, 14, target_system);
_mav_put_uint8_t(buf, 15, target_component);
_mav_put_uint8_t(buf, 16, idx);
_mav_put_uint8_t(buf, 17, count);
_mav_put_uint8_t(buf, 18, flags);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN);
#endif
#else
mavlink_rally_point_t packet;
packet.lat = lat;
packet.lng = lng;
packet.alt = alt;
packet.break_alt = break_alt;
packet.land_dir = land_dir;
packet.target_system = target_system;
packet.target_component = target_component;
packet.idx = idx;
packet.count = count;
packet.flags = flags;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)&packet, MAVLINK_MSG_ID_RALLY_POINT_LEN);
#endif
#endif
}
#endif
// MESSAGE RALLY_POINT UNPACKING
/**
* @brief Get field target_system from rally_point message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_rally_point_get_target_system(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 14);
}
/**
* @brief Get field target_component from rally_point message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_rally_point_get_target_component(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 15);
}
/**
* @brief Get field idx from rally_point message
*
* @return point index (first point is 0)
*/
static inline uint8_t mavlink_msg_rally_point_get_idx(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 16);
}
/**
* @brief Get field count from rally_point message
*
* @return total number of points (for sanity checking)
*/
static inline uint8_t mavlink_msg_rally_point_get_count(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 17);
}
/**
* @brief Get field lat from rally_point message
*
* @return Latitude of point in degrees * 1E7
*/
static inline int32_t mavlink_msg_rally_point_get_lat(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 0);
}
/**
* @brief Get field lng from rally_point message
*
* @return Longitude of point in degrees * 1E7
*/
static inline int32_t mavlink_msg_rally_point_get_lng(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 4);
}
/**
* @brief Get field alt from rally_point message
*
* @return Transit / loiter altitude in meters relative to home
*/
static inline int16_t mavlink_msg_rally_point_get_alt(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 8);
}
/**
* @brief Get field break_alt from rally_point message
*
* @return Break altitude in meters relative to home
*/
static inline int16_t mavlink_msg_rally_point_get_break_alt(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 10);
}
/**
* @brief Get field land_dir from rally_point message
*
* @return Heading to aim for when landing. In centi-degrees.
*/
static inline uint16_t mavlink_msg_rally_point_get_land_dir(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 12);
}
/**
* @brief Get field flags from rally_point message
*
* @return See RALLY_FLAGS enum for definition of the bitmask.
*/
static inline uint8_t mavlink_msg_rally_point_get_flags(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 18);
}
/**
* @brief Decode a rally_point message into a struct
*
* @param msg The message to decode
* @param rally_point C-struct to decode the message contents into
*/
static inline void mavlink_msg_rally_point_decode(const mavlink_message_t* msg, mavlink_rally_point_t* rally_point)
{
#if MAVLINK_NEED_BYTE_SWAP
rally_point->lat = mavlink_msg_rally_point_get_lat(msg);
rally_point->lng = mavlink_msg_rally_point_get_lng(msg);
rally_point->alt = mavlink_msg_rally_point_get_alt(msg);
rally_point->break_alt = mavlink_msg_rally_point_get_break_alt(msg);
rally_point->land_dir = mavlink_msg_rally_point_get_land_dir(msg);
rally_point->target_system = mavlink_msg_rally_point_get_target_system(msg);
rally_point->target_component = mavlink_msg_rally_point_get_target_component(msg);
rally_point->idx = mavlink_msg_rally_point_get_idx(msg);
rally_point->count = mavlink_msg_rally_point_get_count(msg);
rally_point->flags = mavlink_msg_rally_point_get_flags(msg);
#else
memcpy(rally_point, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RALLY_POINT_LEN);
#endif
}

344
mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
View File

@ -31,17 +31,17 @@ static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id,
uint16_t i;
mavlink_sensor_offsets_t packet_in = {
17.0,
963497672,
963497880,
101.0,
129.0,
157.0,
185.0,
213.0,
241.0,
19107,
19211,
19315,
}963497672,
}963497880,
}101.0,
}129.0,
}157.0,
}185.0,
}213.0,
}241.0,
}19107,
}19211,
}19315,
};
mavlink_sensor_offsets_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -96,10 +96,10 @@ static void mavlink_test_set_mag_offsets(uint8_t system_id, uint8_t component_id
uint16_t i;
mavlink_set_mag_offsets_t packet_in = {
17235,
17339,
17443,
151,
218,
}17339,
}17443,
}151,
}218,
};
mavlink_set_mag_offsets_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -147,7 +147,7 @@ static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlin
uint16_t i;
mavlink_meminfo_t packet_in = {
17235,
17339,
}17339,
};
mavlink_meminfo_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -192,11 +192,11 @@ static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink
uint16_t i;
mavlink_ap_adc_t packet_in = {
17235,
17339,
17443,
17547,
17651,
17755,
}17339,
}17443,
}17547,
}17651,
}17755,
};
mavlink_ap_adc_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -245,16 +245,16 @@ static void mavlink_test_digicam_configure(uint8_t system_id, uint8_t component_
uint16_t i;
mavlink_digicam_configure_t packet_in = {
17.0,
17443,
151,
218,
29,
96,
163,
230,
41,
108,
175,
}17443,
}151,
}218,
}29,
}96,
}163,
}230,
}41,
}108,
}175,
};
mavlink_digicam_configure_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -308,15 +308,15 @@ static void mavlink_test_digicam_control(uint8_t system_id, uint8_t component_id
uint16_t i;
mavlink_digicam_control_t packet_in = {
17.0,
17,
84,
151,
218,
29,
96,
163,
230,
41,
}17,
}84,
}151,
}218,
}29,
}96,
}163,
}230,
}41,
};
mavlink_digicam_control_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -369,11 +369,11 @@ static void mavlink_test_mount_configure(uint8_t system_id, uint8_t component_id
uint16_t i;
mavlink_mount_configure_t packet_in = {
5,
72,
139,
206,
17,
84,
}72,
}139,
}206,
}17,
}84,
};
mavlink_mount_configure_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -422,11 +422,11 @@ static void mavlink_test_mount_control(uint8_t system_id, uint8_t component_id,
uint16_t i;
mavlink_mount_control_t packet_in = {
963497464,
963497672,
963497880,
41,
108,
175,
}963497672,
}963497880,
}41,
}108,
}175,
};
mavlink_mount_control_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -475,10 +475,10 @@ static void mavlink_test_mount_status(uint8_t system_id, uint8_t component_id, m
uint16_t i;
mavlink_mount_status_t packet_in = {
963497464,
963497672,
963497880,
41,
108,
}963497672,
}963497880,
}41,
}108,
};
mavlink_mount_status_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -526,11 +526,11 @@ static void mavlink_test_fence_point(uint8_t system_id, uint8_t component_id, ma
uint16_t i;
mavlink_fence_point_t packet_in = {
17.0,
45.0,
29,
96,
163,
230,
}45.0,
}29,
}96,
}163,
}230,
};
mavlink_fence_point_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -579,8 +579,8 @@ static void mavlink_test_fence_fetch_point(uint8_t system_id, uint8_t component_
uint16_t i;
mavlink_fence_fetch_point_t packet_in = {
5,
72,
139,
}72,
}139,
};
mavlink_fence_fetch_point_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -626,9 +626,9 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m
uint16_t i;
mavlink_fence_status_t packet_in = {
963497464,
17443,
151,
218,
}17443,
}151,
}218,
};
mavlink_fence_status_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -675,12 +675,12 @@ static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_m
uint16_t i;
mavlink_ahrs_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
185.0,
}45.0,
}73.0,
}101.0,
}129.0,
}157.0,
}185.0,
};
mavlink_ahrs_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -730,16 +730,16 @@ static void mavlink_test_simstate(uint8_t system_id, uint8_t component_id, mavli
uint16_t i;
mavlink_simstate_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
185.0,
213.0,
241.0,
963499336,
963499544,
}45.0,
}73.0,
}101.0,
}129.0,
}157.0,
}185.0,
}213.0,
}241.0,
}963499336,
}963499544,
};
mavlink_simstate_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -793,7 +793,7 @@ static void mavlink_test_hwstatus(uint8_t system_id, uint8_t component_id, mavli
uint16_t i;
mavlink_hwstatus_t packet_in = {
17235,
139,
}139,
};
mavlink_hwstatus_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -838,12 +838,12 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_
uint16_t i;
mavlink_radio_t packet_in = {
17235,
17339,
17,
84,
151,
218,
29,
}17339,
}17,
}84,
}151,
}218,
}29,
};
mavlink_radio_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -893,14 +893,14 @@ static void mavlink_test_limits_status(uint8_t system_id, uint8_t component_id,
uint16_t i;
mavlink_limits_status_t packet_in = {
963497464,
963497672,
963497880,
963498088,
18067,
187,
254,
65,
132,
}963497672,
}963497880,
}963498088,
}18067,
}187,
}254,
}65,
}132,
};
mavlink_limits_status_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -952,8 +952,8 @@ static void mavlink_test_wind(uint8_t system_id, uint8_t component_id, mavlink_m
uint16_t i;
mavlink_wind_t packet_in = {
17.0,
45.0,
73.0,
}45.0,
}73.0,
};
mavlink_wind_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -999,8 +999,8 @@ static void mavlink_test_data16(uint8_t system_id, uint8_t component_id, mavlink
uint16_t i;
mavlink_data16_t packet_in = {
5,
72,
{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154 },
}72,
}{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154 },
};
mavlink_data16_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1046,8 +1046,8 @@ static void mavlink_test_data32(uint8_t system_id, uint8_t component_id, mavlink
uint16_t i;
mavlink_data32_t packet_in = {
5,
72,
{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170 },
}72,
}{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170 },
};
mavlink_data32_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1093,8 +1093,8 @@ static void mavlink_test_data64(uint8_t system_id, uint8_t component_id, mavlink
uint16_t i;
mavlink_data64_t packet_in = {
5,
72,
{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 },
}72,
}{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 },
};
mavlink_data64_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1140,8 +1140,8 @@ static void mavlink_test_data96(uint8_t system_id, uint8_t component_id, mavlink
uint16_t i;
mavlink_data96_t packet_in = {
5,
72,
{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234 },
}72,
}{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234 },
};
mavlink_data96_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1187,7 +1187,7 @@ static void mavlink_test_rangefinder(uint8_t system_id, uint8_t component_id, ma
uint16_t i;
mavlink_rangefinder_t packet_in = {
17.0,
45.0,
}45.0,
};
mavlink_rangefinder_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1232,17 +1232,17 @@ static void mavlink_test_airspeed_autocal(uint8_t system_id, uint8_t component_i
uint16_t i;
mavlink_airspeed_autocal_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
185.0,
213.0,
241.0,
269.0,
297.0,
325.0,
}45.0,
}73.0,
}101.0,
}129.0,
}157.0,
}185.0,
}213.0,
}241.0,
}269.0,
}297.0,
}325.0,
};
mavlink_airspeed_autocal_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1290,6 +1290,114 @@ static void mavlink_test_airspeed_autocal(uint8_t system_id, uint8_t component_i
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_rally_point(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_rally_point_t packet_in = {
963497464,
}963497672,
}17651,
}17755,
}17859,
}175,
}242,
}53,
}120,
}187,
};
mavlink_rally_point_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.lat = packet_in.lat;
packet1.lng = packet_in.lng;
packet1.alt = packet_in.alt;
packet1.break_alt = packet_in.break_alt;
packet1.land_dir = packet_in.land_dir;
packet1.target_system = packet_in.target_system;
packet1.target_component = packet_in.target_component;
packet1.idx = packet_in.idx;
packet1.count = packet_in.count;
packet1.flags = packet_in.flags;
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_rally_point_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_rally_point_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_rally_point_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng , packet1.alt , packet1.break_alt , packet1.land_dir , packet1.flags );
mavlink_msg_rally_point_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_rally_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng , packet1.alt , packet1.break_alt , packet1.land_dir , packet1.flags );
mavlink_msg_rally_point_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_rally_point_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_rally_point_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.idx , packet1.count , packet1.lat , packet1.lng , packet1.alt , packet1.break_alt , packet1.land_dir , packet1.flags );
mavlink_msg_rally_point_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_rally_fetch_point(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_rally_fetch_point_t packet_in = {
5,
}72,
}139,
};
mavlink_rally_fetch_point_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.target_system = packet_in.target_system;
packet1.target_component = packet_in.target_component;
packet1.idx = packet_in.idx;
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_rally_fetch_point_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_rally_fetch_point_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_rally_fetch_point_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.idx );
mavlink_msg_rally_fetch_point_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_rally_fetch_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.idx );
mavlink_msg_rally_fetch_point_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_rally_fetch_point_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_rally_fetch_point_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.idx );
mavlink_msg_rally_fetch_point_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_test_sensor_offsets(system_id, component_id, last_msg);
@ -1316,6 +1424,8 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
mavlink_test_data96(system_id, component_id, last_msg);
mavlink_test_rangefinder(system_id, component_id, last_msg);
mavlink_test_airspeed_autocal(system_id, component_id, last_msg);
mavlink_test_rally_point(system_id, component_id, last_msg);
mavlink_test_rally_fetch_point(system_id, component_id, last_msg);
}
#ifdef __cplusplus

2
mavlink/include/mavlink/v1.0/ardupilotmega/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:25 2013"
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:56:32 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254

6
mavlink/include/mavlink/v1.0/autoquad/autoquad.h
View File

File diff suppressed because one or more lines are too long

40
mavlink/include/mavlink/v1.0/autoquad/testsuite.h
View File

@ -31,26 +31,26 @@ static void mavlink_test_aq_telemetry_f(uint8_t system_id, uint8_t component_id,
uint16_t i;
mavlink_aq_telemetry_f_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
185.0,
213.0,
241.0,
269.0,
297.0,
325.0,
353.0,
381.0,
409.0,
437.0,
465.0,
493.0,
521.0,
549.0,
21395,
}45.0,
}73.0,
}101.0,
}129.0,
}157.0,
}185.0,
}213.0,
}241.0,
}269.0,
}297.0,
}325.0,
}353.0,
}381.0,
}409.0,
}437.0,
}465.0,
}493.0,
}521.0,
}549.0,
}21395,
};
mavlink_aq_telemetry_f_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));

2
mavlink/include/mavlink/v1.0/autoquad/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:36 2013"
#define MAVLINK_BUILD_DATE "Mon Dec 16 09:03:20 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254

46
mavlink/include/mavlink/v1.0/common/common.h
View File

File diff suppressed because one or more lines are too long

154
mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h
View File

@ -4,6 +4,8 @@
typedef struct __mavlink_battery_status_t
{
int32_t current_consumed; ///< Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
int32_t energy_consumed; ///< Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
uint16_t voltage_cell_1; ///< Battery voltage of cell 1, in millivolts (1 = 1 millivolt)
uint16_t voltage_cell_2; ///< Battery voltage of cell 2, in millivolts (1 = 1 millivolt), -1: no cell
uint16_t voltage_cell_3; ///< Battery voltage of cell 3, in millivolts (1 = 1 millivolt), -1: no cell
@ -15,26 +17,28 @@ typedef struct __mavlink_battery_status_t
int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery
} mavlink_battery_status_t;
#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 16
#define MAVLINK_MSG_ID_147_LEN 16
#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 24
#define MAVLINK_MSG_ID_147_LEN 24
#define MAVLINK_MSG_ID_BATTERY_STATUS_CRC 42
#define MAVLINK_MSG_ID_147_CRC 42
#define MAVLINK_MSG_ID_BATTERY_STATUS_CRC 177
#define MAVLINK_MSG_ID_147_CRC 177
#define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \
"BATTERY_STATUS", \
9, \
{ { "voltage_cell_1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_battery_status_t, voltage_cell_1) }, \
{ "voltage_cell_2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_battery_status_t, voltage_cell_2) }, \
{ "voltage_cell_3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_battery_status_t, voltage_cell_3) }, \
{ "voltage_cell_4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_battery_status_t, voltage_cell_4) }, \
{ "voltage_cell_5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_battery_status_t, voltage_cell_5) }, \
{ "voltage_cell_6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_battery_status_t, voltage_cell_6) }, \
{ "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_battery_status_t, current_battery) }, \
{ "accu_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_battery_status_t, accu_id) }, \
{ "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 15, offsetof(mavlink_battery_status_t, battery_remaining) }, \
11, \
{ { "current_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_battery_status_t, current_consumed) }, \
{ "energy_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_battery_status_t, energy_consumed) }, \
{ "voltage_cell_1", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_battery_status_t, voltage_cell_1) }, \
{ "voltage_cell_2", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_battery_status_t, voltage_cell_2) }, \
{ "voltage_cell_3", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_battery_status_t, voltage_cell_3) }, \
{ "voltage_cell_4", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_battery_status_t, voltage_cell_4) }, \
{ "voltage_cell_5", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_battery_status_t, voltage_cell_5) }, \
{ "voltage_cell_6", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_battery_status_t, voltage_cell_6) }, \
{ "current_battery", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_battery_status_t, current_battery) }, \
{ "accu_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_battery_status_t, accu_id) }, \
{ "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 23, offsetof(mavlink_battery_status_t, battery_remaining) }, \
} \
}
@ -53,27 +57,33 @@ typedef struct __mavlink_battery_status_t
* @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell
* @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell
* @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
* @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
* @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
* @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int8_t battery_remaining)
uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN];
_mav_put_uint16_t(buf, 0, voltage_cell_1);
_mav_put_uint16_t(buf, 2, voltage_cell_2);
_mav_put_uint16_t(buf, 4, voltage_cell_3);
_mav_put_uint16_t(buf, 6, voltage_cell_4);
_mav_put_uint16_t(buf, 8, voltage_cell_5);
_mav_put_uint16_t(buf, 10, voltage_cell_6);
_mav_put_int16_t(buf, 12, current_battery);
_mav_put_uint8_t(buf, 14, accu_id);
_mav_put_int8_t(buf, 15, battery_remaining);
_mav_put_int32_t(buf, 0, current_consumed);
_mav_put_int32_t(buf, 4, energy_consumed);
_mav_put_uint16_t(buf, 8, voltage_cell_1);
_mav_put_uint16_t(buf, 10, voltage_cell_2);
_mav_put_uint16_t(buf, 12, voltage_cell_3);
_mav_put_uint16_t(buf, 14, voltage_cell_4);
_mav_put_uint16_t(buf, 16, voltage_cell_5);
_mav_put_uint16_t(buf, 18, voltage_cell_6);
_mav_put_int16_t(buf, 20, current_battery);
_mav_put_uint8_t(buf, 22, accu_id);
_mav_put_int8_t(buf, 23, battery_remaining);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
#else
mavlink_battery_status_t packet;
packet.current_consumed = current_consumed;
packet.energy_consumed = energy_consumed;
packet.voltage_cell_1 = voltage_cell_1;
packet.voltage_cell_2 = voltage_cell_2;
packet.voltage_cell_3 = voltage_cell_3;
@ -109,28 +119,34 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_
* @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell
* @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell
* @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
* @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
* @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
* @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t accu_id,uint16_t voltage_cell_1,uint16_t voltage_cell_2,uint16_t voltage_cell_3,uint16_t voltage_cell_4,uint16_t voltage_cell_5,uint16_t voltage_cell_6,int16_t current_battery,int8_t battery_remaining)
uint8_t accu_id,uint16_t voltage_cell_1,uint16_t voltage_cell_2,uint16_t voltage_cell_3,uint16_t voltage_cell_4,uint16_t voltage_cell_5,uint16_t voltage_cell_6,int16_t current_battery,int32_t current_consumed,int32_t energy_consumed,int8_t battery_remaining)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN];
_mav_put_uint16_t(buf, 0, voltage_cell_1);
_mav_put_uint16_t(buf, 2, voltage_cell_2);
_mav_put_uint16_t(buf, 4, voltage_cell_3);
_mav_put_uint16_t(buf, 6, voltage_cell_4);
_mav_put_uint16_t(buf, 8, voltage_cell_5);
_mav_put_uint16_t(buf, 10, voltage_cell_6);
_mav_put_int16_t(buf, 12, current_battery);
_mav_put_uint8_t(buf, 14, accu_id);
_mav_put_int8_t(buf, 15, battery_remaining);
_mav_put_int32_t(buf, 0, current_consumed);
_mav_put_int32_t(buf, 4, energy_consumed);
_mav_put_uint16_t(buf, 8, voltage_cell_1);
_mav_put_uint16_t(buf, 10, voltage_cell_2);
_mav_put_uint16_t(buf, 12, voltage_cell_3);
_mav_put_uint16_t(buf, 14, voltage_cell_4);
_mav_put_uint16_t(buf, 16, voltage_cell_5);
_mav_put_uint16_t(buf, 18, voltage_cell_6);
_mav_put_int16_t(buf, 20, current_battery);
_mav_put_uint8_t(buf, 22, accu_id);
_mav_put_int8_t(buf, 23, battery_remaining);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
#else
mavlink_battery_status_t packet;
packet.current_consumed = current_consumed;
packet.energy_consumed = energy_consumed;
packet.voltage_cell_1 = voltage_cell_1;
packet.voltage_cell_2 = voltage_cell_2;
packet.voltage_cell_3 = voltage_cell_3;
@ -162,7 +178,7 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u
*/
static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status)
{
return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->battery_remaining);
return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining);
}
/**
@ -176,7 +192,7 @@ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint
*/
static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status)
{
return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->battery_remaining);
return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining);
}
/**
@ -191,23 +207,27 @@ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id,
* @param voltage_cell_5 Battery voltage of cell 5, in millivolts (1 = 1 millivolt), -1: no cell
* @param voltage_cell_6 Battery voltage of cell 6, in millivolts (1 = 1 millivolt), -1: no cell
* @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
* @param current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
* @param energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
* @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int8_t battery_remaining)
static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN];
_mav_put_uint16_t(buf, 0, voltage_cell_1);
_mav_put_uint16_t(buf, 2, voltage_cell_2);
_mav_put_uint16_t(buf, 4, voltage_cell_3);
_mav_put_uint16_t(buf, 6, voltage_cell_4);
_mav_put_uint16_t(buf, 8, voltage_cell_5);
_mav_put_uint16_t(buf, 10, voltage_cell_6);
_mav_put_int16_t(buf, 12, current_battery);
_mav_put_uint8_t(buf, 14, accu_id);
_mav_put_int8_t(buf, 15, battery_remaining);
_mav_put_int32_t(buf, 0, current_consumed);
_mav_put_int32_t(buf, 4, energy_consumed);
_mav_put_uint16_t(buf, 8, voltage_cell_1);
_mav_put_uint16_t(buf, 10, voltage_cell_2);
_mav_put_uint16_t(buf, 12, voltage_cell_3);
_mav_put_uint16_t(buf, 14, voltage_cell_4);
_mav_put_uint16_t(buf, 16, voltage_cell_5);
_mav_put_uint16_t(buf, 18, voltage_cell_6);
_mav_put_int16_t(buf, 20, current_battery);
_mav_put_uint8_t(buf, 22, accu_id);
_mav_put_int8_t(buf, 23, battery_remaining);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC);
@ -216,6 +236,8 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8
#endif
#else
mavlink_battery_status_t packet;
packet.current_consumed = current_consumed;
packet.energy_consumed = energy_consumed;
packet.voltage_cell_1 = voltage_cell_1;
packet.voltage_cell_2 = voltage_cell_2;
packet.voltage_cell_3 = voltage_cell_3;
@ -246,7 +268,7 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8
*/
static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 14);
return _MAV_RETURN_uint8_t(msg, 22);
}
/**
@ -256,7 +278,7 @@ static inline uint8_t mavlink_msg_battery_status_get_accu_id(const mavlink_messa
*/
static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 0);
return _MAV_RETURN_uint16_t(msg, 8);
}
/**
@ -266,7 +288,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_1(const mavli
*/
static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 2);
return _MAV_RETURN_uint16_t(msg, 10);
}
/**
@ -276,7 +298,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_2(const mavli
*/
static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 4);
return _MAV_RETURN_uint16_t(msg, 12);
}
/**
@ -286,7 +308,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_3(const mavli
*/
static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 6);
return _MAV_RETURN_uint16_t(msg, 14);
}
/**
@ -296,7 +318,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_4(const mavli
*/
static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 8);
return _MAV_RETURN_uint16_t(msg, 16);
}
/**
@ -306,7 +328,7 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_5(const mavli
*/
static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 10);
return _MAV_RETURN_uint16_t(msg, 18);
}
/**
@ -316,7 +338,27 @@ static inline uint16_t mavlink_msg_battery_status_get_voltage_cell_6(const mavli
*/
static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 12);
return _MAV_RETURN_int16_t(msg, 20);
}
/**
* @brief Get field current_consumed from battery_status message
*
* @return Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
*/
static inline int32_t mavlink_msg_battery_status_get_current_consumed(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 0);
}
/**
* @brief Get field energy_consumed from battery_status message
*
* @return Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
*/
static inline int32_t mavlink_msg_battery_status_get_energy_consumed(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 4);
}
/**
@ -326,7 +368,7 @@ static inline int16_t mavlink_msg_battery_status_get_current_battery(const mavli
*/
static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavlink_message_t* msg)
{
return _MAV_RETURN_int8_t(msg, 15);
return _MAV_RETURN_int8_t(msg, 23);
}
/**
@ -338,6 +380,8 @@ static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavl
static inline void mavlink_msg_battery_status_decode(const mavlink_message_t* msg, mavlink_battery_status_t* battery_status)
{
#if MAVLINK_NEED_BYTE_SWAP
battery_status->current_consumed = mavlink_msg_battery_status_get_current_consumed(msg);
battery_status->energy_consumed = mavlink_msg_battery_status_get_energy_consumed(msg);
battery_status->voltage_cell_1 = mavlink_msg_battery_status_get_voltage_cell_1(msg);
battery_status->voltage_cell_2 = mavlink_msg_battery_status_get_voltage_cell_2(msg);
battery_status->voltage_cell_3 = mavlink_msg_battery_status_get_voltage_cell_3(msg);

10
mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h
View File

@ -9,7 +9,7 @@ typedef struct __mavlink_gps_raw_int_t
int32_t lon; ///< Longitude (WGS84), in degrees * 1E7
int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up)
uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
@ -53,7 +53,7 @@ typedef struct __mavlink_gps_raw_int_t
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (WGS84), in meters * 1000 (positive for up)
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
* @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
* @param satellites_visible Number of satellites visible. If unknown, set to 255
@ -112,7 +112,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (WGS84), in meters * 1000 (positive for up)
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
* @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
* @param satellites_visible Number of satellites visible. If unknown, set to 255
@ -197,7 +197,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, ui
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (WGS84), in meters * 1000 (positive for up)
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
* @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
* @param satellites_visible Number of satellites visible. If unknown, set to 255
@ -313,7 +313,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t*
/**
* @brief Get field epv from gps_raw_int message
*
* @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
*/
static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg)
{

10
mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h
View File

@ -9,7 +9,7 @@ typedef struct __mavlink_hil_gps_t
int32_t lon; ///< Longitude (WGS84), in degrees * 1E7
int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up)
uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535
int16_t vn; ///< GPS velocity in cm/s in NORTH direction in earth-fixed NED frame
int16_t ve; ///< GPS velocity in cm/s in EAST direction in earth-fixed NED frame
@ -59,7 +59,7 @@ typedef struct __mavlink_hil_gps_t
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (WGS84), in meters * 1000 (positive for up)
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
* @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
* @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
* @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535
* @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame
* @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame
@ -127,7 +127,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (WGS84), in meters * 1000 (positive for up)
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
* @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
* @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
* @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535
* @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame
* @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame
@ -221,7 +221,7 @@ static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (WGS84), in meters * 1000 (positive for up)
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
* @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
* @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
* @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535
* @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame
* @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame
@ -346,7 +346,7 @@ static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg)
/**
* @brief Get field epv from hil_gps message
*
* @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
* @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: 65535
*/
static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg)
{

237
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h Normal file
View File

@ -0,0 +1,237 @@
// MESSAGE LOG_DATA PACKING
#define MAVLINK_MSG_ID_LOG_DATA 120
typedef struct __mavlink_log_data_t
{
uint32_t ofs; ///< Offset into the log
uint16_t id; ///< Log id (from LOG_ENTRY reply)
uint8_t count; ///< Number of bytes (zero for end of log)
uint8_t data[90]; ///< log data
} mavlink_log_data_t;
#define MAVLINK_MSG_ID_LOG_DATA_LEN 97
#define MAVLINK_MSG_ID_120_LEN 97
#define MAVLINK_MSG_ID_LOG_DATA_CRC 134
#define MAVLINK_MSG_ID_120_CRC 134
#define MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN 90
#define MAVLINK_MESSAGE_INFO_LOG_DATA { \
"LOG_DATA", \
4, \
{ { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_data_t, ofs) }, \
{ "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_log_data_t, id) }, \
{ "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_log_data_t, count) }, \
{ "data", NULL, MAVLINK_TYPE_UINT8_T, 90, 7, offsetof(mavlink_log_data_t, data) }, \
} \
}
/**
* @brief Pack a log_data message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param id Log id (from LOG_ENTRY reply)
* @param ofs Offset into the log
* @param count Number of bytes (zero for end of log)
* @param data log data
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_DATA_LEN];
_mav_put_uint32_t(buf, 0, ofs);
_mav_put_uint16_t(buf, 4, id);
_mav_put_uint8_t(buf, 6, count);
_mav_put_uint8_t_array(buf, 7, data, 90);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN);
#else
mavlink_log_data_t packet;
packet.ofs = ofs;
packet.id = id;
packet.count = count;
mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_DATA;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_LEN);
#endif
}
/**
* @brief Pack a log_data message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param id Log id (from LOG_ENTRY reply)
* @param ofs Offset into the log
* @param count Number of bytes (zero for end of log)
* @param data log data
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint16_t id,uint32_t ofs,uint8_t count,const uint8_t *data)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_DATA_LEN];
_mav_put_uint32_t(buf, 0, ofs);
_mav_put_uint16_t(buf, 4, id);
_mav_put_uint8_t(buf, 6, count);
_mav_put_uint8_t_array(buf, 7, data, 90);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN);
#else
mavlink_log_data_t packet;
packet.ofs = ofs;
packet.id = id;
packet.count = count;
mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_DATA;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_DATA_LEN);
#endif
}
/**
* @brief Encode a log_data struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param log_data C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_data_t* log_data)
{
return mavlink_msg_log_data_pack(system_id, component_id, msg, log_data->id, log_data->ofs, log_data->count, log_data->data);
}
/**
* @brief Encode a log_data struct on a channel
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param log_data C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_data_t* log_data)
{
return mavlink_msg_log_data_pack_chan(system_id, component_id, chan, msg, log_data->id, log_data->ofs, log_data->count, log_data->data);
}
/**
* @brief Send a log_data message
* @param chan MAVLink channel to send the message
*
* @param id Log id (from LOG_ENTRY reply)
* @param ofs Offset into the log
* @param count Number of bytes (zero for end of log)
* @param data log data
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_log_data_send(mavlink_channel_t chan, uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_DATA_LEN];
_mav_put_uint32_t(buf, 0, ofs);
_mav_put_uint16_t(buf, 4, id);
_mav_put_uint8_t(buf, 6, count);
_mav_put_uint8_t_array(buf, 7, data, 90);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN);
#endif
#else
mavlink_log_data_t packet;
packet.ofs = ofs;
packet.id = id;
packet.count = count;
mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_LEN);
#endif
#endif
}
#endif
// MESSAGE LOG_DATA UNPACKING
/**
* @brief Get field id from log_data message
*
* @return Log id (from LOG_ENTRY reply)
*/
static inline uint16_t mavlink_msg_log_data_get_id(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 4);
}
/**
* @brief Get field ofs from log_data message
*
* @return Offset into the log
*/
static inline uint32_t mavlink_msg_log_data_get_ofs(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint32_t(msg, 0);
}
/**
* @brief Get field count from log_data message
*
* @return Number of bytes (zero for end of log)
*/
static inline uint8_t mavlink_msg_log_data_get_count(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 6);
}
/**
* @brief Get field data from log_data message
*
* @return log data
*/
static inline uint16_t mavlink_msg_log_data_get_data(const mavlink_message_t* msg, uint8_t *data)
{
return _MAV_RETURN_uint8_t_array(msg, data, 90, 7);
}
/**
* @brief Decode a log_data message into a struct
*
* @param msg The message to decode
* @param log_data C-struct to decode the message contents into
*/
static inline void mavlink_msg_log_data_decode(const mavlink_message_t* msg, mavlink_log_data_t* log_data)
{
#if MAVLINK_NEED_BYTE_SWAP
log_data->ofs = mavlink_msg_log_data_get_ofs(msg);
log_data->id = mavlink_msg_log_data_get_id(msg);
log_data->count = mavlink_msg_log_data_get_count(msg);
mavlink_msg_log_data_get_data(msg, log_data->data);
#else
memcpy(log_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_DATA_LEN);
#endif
}

265
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h Normal file
View File

@ -0,0 +1,265 @@
// MESSAGE LOG_ENTRY PACKING
#define MAVLINK_MSG_ID_LOG_ENTRY 118
typedef struct __mavlink_log_entry_t
{
uint32_t time_utc; ///< UTC timestamp of log in seconds since 1970, or 0 if not available
uint32_t size; ///< Size of the log (may be approximate) in bytes
uint16_t id; ///< Log id
uint16_t num_logs; ///< Total number of logs
uint16_t last_log_num; ///< High log number
} mavlink_log_entry_t;
#define MAVLINK_MSG_ID_LOG_ENTRY_LEN 14
#define MAVLINK_MSG_ID_118_LEN 14
#define MAVLINK_MSG_ID_LOG_ENTRY_CRC 56
#define MAVLINK_MSG_ID_118_CRC 56
#define MAVLINK_MESSAGE_INFO_LOG_ENTRY { \
"LOG_ENTRY", \
5, \
{ { "time_utc", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_entry_t, time_utc) }, \
{ "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_entry_t, size) }, \
{ "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_entry_t, id) }, \
{ "num_logs", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_log_entry_t, num_logs) }, \
{ "last_log_num", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_log_entry_t, last_log_num) }, \
} \
}
/**
* @brief Pack a log_entry message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param id Log id
* @param num_logs Total number of logs
* @param last_log_num High log number
* @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available
* @param size Size of the log (may be approximate) in bytes
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_entry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN];
_mav_put_uint32_t(buf, 0, time_utc);
_mav_put_uint32_t(buf, 4, size);
_mav_put_uint16_t(buf, 8, id);
_mav_put_uint16_t(buf, 10, num_logs);
_mav_put_uint16_t(buf, 12, last_log_num);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
#else
mavlink_log_entry_t packet;
packet.time_utc = time_utc;
packet.size = size;
packet.id = id;
packet.num_logs = num_logs;
packet.last_log_num = last_log_num;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
#endif
}
/**
* @brief Pack a log_entry message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param id Log id
* @param num_logs Total number of logs
* @param last_log_num High log number
* @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available
* @param size Size of the log (may be approximate) in bytes
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_entry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint16_t id,uint16_t num_logs,uint16_t last_log_num,uint32_t time_utc,uint32_t size)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN];
_mav_put_uint32_t(buf, 0, time_utc);
_mav_put_uint32_t(buf, 4, size);
_mav_put_uint16_t(buf, 8, id);
_mav_put_uint16_t(buf, 10, num_logs);
_mav_put_uint16_t(buf, 12, last_log_num);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
#else
mavlink_log_entry_t packet;
packet.time_utc = time_utc;
packet.size = size;
packet.id = id;
packet.num_logs = num_logs;
packet.last_log_num = last_log_num;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
#endif
}
/**
* @brief Encode a log_entry struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param log_entry C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_entry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry)
{
return mavlink_msg_log_entry_pack(system_id, component_id, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size);
}
/**
* @brief Encode a log_entry struct on a channel
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param log_entry C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_entry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry)
{
return mavlink_msg_log_entry_pack_chan(system_id, component_id, chan, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size);
}
/**
* @brief Send a log_entry message
* @param chan MAVLink channel to send the message
*
* @param id Log id
* @param num_logs Total number of logs
* @param last_log_num High log number
* @param time_utc UTC timestamp of log in seconds since 1970, or 0 if not available
* @param size Size of the log (may be approximate) in bytes
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_log_entry_send(mavlink_channel_t chan, uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN];
_mav_put_uint32_t(buf, 0, time_utc);
_mav_put_uint32_t(buf, 4, size);
_mav_put_uint16_t(buf, 8, id);
_mav_put_uint16_t(buf, 10, num_logs);
_mav_put_uint16_t(buf, 12, last_log_num);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
#endif
#else
mavlink_log_entry_t packet;
packet.time_utc = time_utc;
packet.size = size;
packet.id = id;
packet.num_logs = num_logs;
packet.last_log_num = last_log_num;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)&packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)&packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
#endif
#endif
}
#endif
// MESSAGE LOG_ENTRY UNPACKING
/**
* @brief Get field id from log_entry message
*
* @return Log id
*/
static inline uint16_t mavlink_msg_log_entry_get_id(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 8);
}
/**
* @brief Get field num_logs from log_entry message
*
* @return Total number of logs
*/
static inline uint16_t mavlink_msg_log_entry_get_num_logs(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 10);
}
/**
* @brief Get field last_log_num from log_entry message
*
* @return High log number
*/
static inline uint16_t mavlink_msg_log_entry_get_last_log_num(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 12);
}
/**
* @brief Get field time_utc from log_entry message
*
* @return UTC timestamp of log in seconds since 1970, or 0 if not available
*/
static inline uint32_t mavlink_msg_log_entry_get_time_utc(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint32_t(msg, 0);
}
/**
* @brief Get field size from log_entry message
*
* @return Size of the log (may be approximate) in bytes
*/
static inline uint32_t mavlink_msg_log_entry_get_size(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint32_t(msg, 4);
}
/**
* @brief Decode a log_entry message into a struct
*
* @param msg The message to decode
* @param log_entry C-struct to decode the message contents into
*/
static inline void mavlink_msg_log_entry_decode(const mavlink_message_t* msg, mavlink_log_entry_t* log_entry)
{
#if MAVLINK_NEED_BYTE_SWAP
log_entry->time_utc = mavlink_msg_log_entry_get_time_utc(msg);
log_entry->size = mavlink_msg_log_entry_get_size(msg);
log_entry->id = mavlink_msg_log_entry_get_id(msg);
log_entry->num_logs = mavlink_msg_log_entry_get_num_logs(msg);
log_entry->last_log_num = mavlink_msg_log_entry_get_last_log_num(msg);
#else
memcpy(log_entry, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ENTRY_LEN);
#endif
}

199
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h Normal file
View File

@ -0,0 +1,199 @@
// MESSAGE LOG_ERASE PACKING
#define MAVLINK_MSG_ID_LOG_ERASE 121
typedef struct __mavlink_log_erase_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
} mavlink_log_erase_t;
#define MAVLINK_MSG_ID_LOG_ERASE_LEN 2
#define MAVLINK_MSG_ID_121_LEN 2
#define MAVLINK_MSG_ID_LOG_ERASE_CRC 237
#define MAVLINK_MSG_ID_121_CRC 237
#define MAVLINK_MESSAGE_INFO_LOG_ERASE { \
"LOG_ERASE", \
2, \
{ { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_erase_t, target_system) }, \
{ "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_erase_t, target_component) }, \
} \
}
/**
* @brief Pack a log_erase message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_erase_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t target_system, uint8_t target_component)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN);
#else
mavlink_log_erase_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_ERASE;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_LEN);
#endif
}
/**
* @brief Pack a log_erase message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_erase_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t target_system,uint8_t target_component)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN);
#else
mavlink_log_erase_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_ERASE;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_ERASE_LEN);
#endif
}
/**
* @brief Encode a log_erase struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param log_erase C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_erase_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase)
{
return mavlink_msg_log_erase_pack(system_id, component_id, msg, log_erase->target_system, log_erase->target_component);
}
/**
* @brief Encode a log_erase struct on a channel
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param log_erase C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_erase_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase)
{
return mavlink_msg_log_erase_pack_chan(system_id, component_id, chan, msg, log_erase->target_system, log_erase->target_component);
}
/**
* @brief Send a log_erase message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN);
#endif
#else
mavlink_log_erase_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)&packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)&packet, MAVLINK_MSG_ID_LOG_ERASE_LEN);
#endif
#endif
}
#endif
// MESSAGE LOG_ERASE UNPACKING
/**
* @brief Get field target_system from log_erase message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_log_erase_get_target_system(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 0);
}
/**
* @brief Get field target_component from log_erase message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_log_erase_get_target_component(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 1);
}
/**
* @brief Decode a log_erase message into a struct
*
* @param msg The message to decode
* @param log_erase C-struct to decode the message contents into
*/
static inline void mavlink_msg_log_erase_decode(const mavlink_message_t* msg, mavlink_log_erase_t* log_erase)
{
#if MAVLINK_NEED_BYTE_SWAP
log_erase->target_system = mavlink_msg_log_erase_get_target_system(msg);
log_erase->target_component = mavlink_msg_log_erase_get_target_component(msg);
#else
memcpy(log_erase, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_ERASE_LEN);
#endif
}

265
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h Normal file
View File

@ -0,0 +1,265 @@
// MESSAGE LOG_REQUEST_DATA PACKING
#define MAVLINK_MSG_ID_LOG_REQUEST_DATA 119
typedef struct __mavlink_log_request_data_t
{
uint32_t ofs; ///< Offset into the log
uint32_t count; ///< Number of bytes
uint16_t id; ///< Log id (from LOG_ENTRY reply)
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
} mavlink_log_request_data_t;
#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN 12
#define MAVLINK_MSG_ID_119_LEN 12
#define MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC 116
#define MAVLINK_MSG_ID_119_CRC 116
#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA { \
"LOG_REQUEST_DATA", \
5, \
{ { "ofs", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_log_request_data_t, ofs) }, \
{ "count", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_log_request_data_t, count) }, \
{ "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_log_request_data_t, id) }, \
{ "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_log_request_data_t, target_system) }, \
{ "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_log_request_data_t, target_component) }, \
} \
}
/**
* @brief Pack a log_request_data message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @param id Log id (from LOG_ENTRY reply)
* @param ofs Offset into the log
* @param count Number of bytes
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_request_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN];
_mav_put_uint32_t(buf, 0, ofs);
_mav_put_uint32_t(buf, 4, count);
_mav_put_uint16_t(buf, 8, id);
_mav_put_uint8_t(buf, 10, target_system);
_mav_put_uint8_t(buf, 11, target_component);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
#else
mavlink_log_request_data_t packet;
packet.ofs = ofs;
packet.count = count;
packet.id = id;
packet.target_system = target_system;
packet.target_component = target_component;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
#endif
}
/**
* @brief Pack a log_request_data message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @param id Log id (from LOG_ENTRY reply)
* @param ofs Offset into the log
* @param count Number of bytes
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_request_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t target_system,uint8_t target_component,uint16_t id,uint32_t ofs,uint32_t count)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN];
_mav_put_uint32_t(buf, 0, ofs);
_mav_put_uint32_t(buf, 4, count);
_mav_put_uint16_t(buf, 8, id);
_mav_put_uint8_t(buf, 10, target_system);
_mav_put_uint8_t(buf, 11, target_component);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
#else
mavlink_log_request_data_t packet;
packet.ofs = ofs;
packet.count = count;
packet.id = id;
packet.target_system = target_system;
packet.target_component = target_component;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
#endif
}
/**
* @brief Encode a log_request_data struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param log_request_data C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_request_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data)
{
return mavlink_msg_log_request_data_pack(system_id, component_id, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count);
}
/**
* @brief Encode a log_request_data struct on a channel
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param log_request_data C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_request_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data)
{
return mavlink_msg_log_request_data_pack_chan(system_id, component_id, chan, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count);
}
/**
* @brief Send a log_request_data message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
* @param id Log id (from LOG_ENTRY reply)
* @param ofs Offset into the log
* @param count Number of bytes
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_log_request_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN];
_mav_put_uint32_t(buf, 0, ofs);
_mav_put_uint32_t(buf, 4, count);
_mav_put_uint16_t(buf, 8, id);
_mav_put_uint8_t(buf, 10, target_system);
_mav_put_uint8_t(buf, 11, target_component);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
#endif
#else
mavlink_log_request_data_t packet;
packet.ofs = ofs;
packet.count = count;
packet.id = id;
packet.target_system = target_system;
packet.target_component = target_component;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
#endif
#endif
}
#endif
// MESSAGE LOG_REQUEST_DATA UNPACKING
/**
* @brief Get field target_system from log_request_data message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_log_request_data_get_target_system(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 10);
}
/**
* @brief Get field target_component from log_request_data message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_log_request_data_get_target_component(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 11);
}
/**
* @brief Get field id from log_request_data message
*
* @return Log id (from LOG_ENTRY reply)
*/
static inline uint16_t mavlink_msg_log_request_data_get_id(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 8);
}
/**
* @brief Get field ofs from log_request_data message
*
* @return Offset into the log
*/
static inline uint32_t mavlink_msg_log_request_data_get_ofs(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint32_t(msg, 0);
}
/**
* @brief Get field count from log_request_data message
*
* @return Number of bytes
*/
static inline uint32_t mavlink_msg_log_request_data_get_count(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint32_t(msg, 4);
}
/**
* @brief Decode a log_request_data message into a struct
*
* @param msg The message to decode
* @param log_request_data C-struct to decode the message contents into
*/
static inline void mavlink_msg_log_request_data_decode(const mavlink_message_t* msg, mavlink_log_request_data_t* log_request_data)
{
#if MAVLINK_NEED_BYTE_SWAP
log_request_data->ofs = mavlink_msg_log_request_data_get_ofs(msg);
log_request_data->count = mavlink_msg_log_request_data_get_count(msg);
log_request_data->id = mavlink_msg_log_request_data_get_id(msg);
log_request_data->target_system = mavlink_msg_log_request_data_get_target_system(msg);
log_request_data->target_component = mavlink_msg_log_request_data_get_target_component(msg);
#else
memcpy(log_request_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
#endif
}

199
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h Normal file
View File

@ -0,0 +1,199 @@
// MESSAGE LOG_REQUEST_END PACKING
#define MAVLINK_MSG_ID_LOG_REQUEST_END 122
typedef struct __mavlink_log_request_end_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
} mavlink_log_request_end_t;
#define MAVLINK_MSG_ID_LOG_REQUEST_END_LEN 2
#define MAVLINK_MSG_ID_122_LEN 2
#define MAVLINK_MSG_ID_LOG_REQUEST_END_CRC 203
#define MAVLINK_MSG_ID_122_CRC 203
#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_END { \
"LOG_REQUEST_END", \
2, \
{ { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_log_request_end_t, target_system) }, \
{ "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_log_request_end_t, target_component) }, \
} \
}
/**
* @brief Pack a log_request_end message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_request_end_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t target_system, uint8_t target_component)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
#else
mavlink_log_request_end_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
#endif
}
/**
* @brief Pack a log_request_end message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_request_end_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t target_system,uint8_t target_component)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
#else
mavlink_log_request_end_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
#endif
}
/**
* @brief Encode a log_request_end struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param log_request_end C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_request_end_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end)
{
return mavlink_msg_log_request_end_pack(system_id, component_id, msg, log_request_end->target_system, log_request_end->target_component);
}
/**
* @brief Encode a log_request_end struct on a channel
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param log_request_end C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_request_end_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end)
{
return mavlink_msg_log_request_end_pack_chan(system_id, component_id, chan, msg, log_request_end->target_system, log_request_end->target_component);
}
/**
* @brief Send a log_request_end message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_log_request_end_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
#endif
#else
mavlink_log_request_end_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
#endif
#endif
}
#endif
// MESSAGE LOG_REQUEST_END UNPACKING
/**
* @brief Get field target_system from log_request_end message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_log_request_end_get_target_system(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 0);
}
/**
* @brief Get field target_component from log_request_end message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_log_request_end_get_target_component(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 1);
}
/**
* @brief Decode a log_request_end message into a struct
*
* @param msg The message to decode
* @param log_request_end C-struct to decode the message contents into
*/
static inline void mavlink_msg_log_request_end_decode(const mavlink_message_t* msg, mavlink_log_request_end_t* log_request_end)
{
#if MAVLINK_NEED_BYTE_SWAP
log_request_end->target_system = mavlink_msg_log_request_end_get_target_system(msg);
log_request_end->target_component = mavlink_msg_log_request_end_get_target_component(msg);
#else
memcpy(log_request_end, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
#endif
}

243
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h Normal file
View File

@ -0,0 +1,243 @@
// MESSAGE LOG_REQUEST_LIST PACKING
#define MAVLINK_MSG_ID_LOG_REQUEST_LIST 117
typedef struct __mavlink_log_request_list_t
{
uint16_t start; ///< First log id (0 for first available)
uint16_t end; ///< Last log id (0xffff for last available)
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
} mavlink_log_request_list_t;
#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN 6
#define MAVLINK_MSG_ID_117_LEN 6
#define MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC 128
#define MAVLINK_MSG_ID_117_CRC 128
#define MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST { \
"LOG_REQUEST_LIST", \
4, \
{ { "start", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_log_request_list_t, start) }, \
{ "end", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_log_request_list_t, end) }, \
{ "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_log_request_list_t, target_system) }, \
{ "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_log_request_list_t, target_component) }, \
} \
}
/**
* @brief Pack a log_request_list message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @param start First log id (0 for first available)
* @param end Last log id (0xffff for last available)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN];
_mav_put_uint16_t(buf, 0, start);
_mav_put_uint16_t(buf, 2, end);
_mav_put_uint8_t(buf, 4, target_system);
_mav_put_uint8_t(buf, 5, target_component);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
#else
mavlink_log_request_list_t packet;
packet.start = start;
packet.end = end;
packet.target_system = target_system;
packet.target_component = target_component;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
#endif
}
/**
* @brief Pack a log_request_list message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @param start First log id (0 for first available)
* @param end Last log id (0xffff for last available)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_log_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t target_system,uint8_t target_component,uint16_t start,uint16_t end)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN];
_mav_put_uint16_t(buf, 0, start);
_mav_put_uint16_t(buf, 2, end);
_mav_put_uint8_t(buf, 4, target_system);
_mav_put_uint8_t(buf, 5, target_component);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
#else
mavlink_log_request_list_t packet;
packet.start = start;
packet.end = end;
packet.target_system = target_system;
packet.target_component = target_component;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
#endif
}
/**
* @brief Encode a log_request_list struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param log_request_list C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list)
{
return mavlink_msg_log_request_list_pack(system_id, component_id, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end);
}
/**
* @brief Encode a log_request_list struct on a channel
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param log_request_list C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_log_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list)
{
return mavlink_msg_log_request_list_pack_chan(system_id, component_id, chan, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end);
}
/**
* @brief Send a log_request_list message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
* @param start First log id (0 for first available)
* @param end Last log id (0xffff for last available)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_log_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN];
_mav_put_uint16_t(buf, 0, start);
_mav_put_uint16_t(buf, 2, end);
_mav_put_uint8_t(buf, 4, target_system);
_mav_put_uint8_t(buf, 5, target_component);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
#endif
#else
mavlink_log_request_list_t packet;
packet.start = start;
packet.end = end;
packet.target_system = target_system;
packet.target_component = target_component;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
#endif
#endif
}
#endif
// MESSAGE LOG_REQUEST_LIST UNPACKING
/**
* @brief Get field target_system from log_request_list message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_log_request_list_get_target_system(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 4);
}
/**
* @brief Get field target_component from log_request_list message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_log_request_list_get_target_component(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 5);
}
/**
* @brief Get field start from log_request_list message
*
* @return First log id (0 for first available)
*/
static inline uint16_t mavlink_msg_log_request_list_get_start(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 0);
}
/**
* @brief Get field end from log_request_list message
*
* @return Last log id (0xffff for last available)
*/
static inline uint16_t mavlink_msg_log_request_list_get_end(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 2);
}
/**
* @brief Decode a log_request_list message into a struct
*
* @param msg The message to decode
* @param log_request_list C-struct to decode the message contents into
*/
static inline void mavlink_msg_log_request_list_decode(const mavlink_message_t* msg, mavlink_log_request_list_t* log_request_list)
{
#if MAVLINK_NEED_BYTE_SWAP
log_request_list->start = mavlink_msg_log_request_list_get_start(msg);
log_request_list->end = mavlink_msg_log_request_list_get_end(msg);
log_request_list->target_system = mavlink_msg_log_request_list_get_target_system(msg);
log_request_list->target_component = mavlink_msg_log_request_list_get_target_component(msg);
#else
memcpy(log_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
#endif
}

50
mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h
View File

@ -4,13 +4,13 @@
typedef struct __mavlink_mission_item_t
{
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float param1; ///< PARAM1, see MAV_CMD enum
float param2; ///< PARAM2, see MAV_CMD enum
float param3; ///< PARAM3, see MAV_CMD enum
float param4; ///< PARAM4, see MAV_CMD enum
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
float z; ///< PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
@ -62,13 +62,13 @@ typedef struct __mavlink_mission_item_t
* @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
* @param current false:0, true:1
* @param autocontinue autocontinue to next wp
* @param param1 PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
* @param param2 PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
* @param param3 PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
* @param param4 PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
* @param param1 PARAM1, see MAV_CMD enum
* @param param2 PARAM2, see MAV_CMD enum
* @param param3 PARAM3, see MAV_CMD enum
* @param param4 PARAM4, see MAV_CMD enum
* @param x PARAM5 / local: x position, global: latitude
* @param y PARAM6 / y position: global: longitude
* @param z PARAM7 / z position: global: altitude
* @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
@ -133,13 +133,13 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t
* @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
* @param current false:0, true:1
* @param autocontinue autocontinue to next wp
* @param param1 PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
* @param param2 PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
* @param param3 PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
* @param param4 PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
* @param param1 PARAM1, see MAV_CMD enum
* @param param2 PARAM2, see MAV_CMD enum
* @param param3 PARAM3, see MAV_CMD enum
* @param param4 PARAM4, see MAV_CMD enum
* @param x PARAM5 / local: x position, global: latitude
* @param y PARAM6 / y position: global: longitude
* @param z PARAM7 / z position: global: altitude
* @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
@ -230,13 +230,13 @@ static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, u
* @param command The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
* @param current false:0, true:1
* @param autocontinue autocontinue to next wp
* @param param1 PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
* @param param2 PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
* @param param3 PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
* @param param4 PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
* @param param1 PARAM1, see MAV_CMD enum
* @param param2 PARAM2, see MAV_CMD enum
* @param param3 PARAM3, see MAV_CMD enum
* @param param4 PARAM4, see MAV_CMD enum
* @param x PARAM5 / local: x position, global: latitude
* @param y PARAM6 / y position: global: longitude
* @param z PARAM7 / z position: global: altitude
* @param z PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
@ -367,7 +367,7 @@ static inline uint8_t mavlink_msg_mission_item_get_autocontinue(const mavlink_me
/**
* @brief Get field param1 from mission_item message
*
* @return PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
* @return PARAM1, see MAV_CMD enum
*/
static inline float mavlink_msg_mission_item_get_param1(const mavlink_message_t* msg)
{
@ -377,7 +377,7 @@ static inline float mavlink_msg_mission_item_get_param1(const mavlink_message_t*
/**
* @brief Get field param2 from mission_item message
*
* @return PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
* @return PARAM2, see MAV_CMD enum
*/
static inline float mavlink_msg_mission_item_get_param2(const mavlink_message_t* msg)
{
@ -387,7 +387,7 @@ static inline float mavlink_msg_mission_item_get_param2(const mavlink_message_t*
/**
* @brief Get field param3 from mission_item message
*
* @return PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
* @return PARAM3, see MAV_CMD enum
*/
static inline float mavlink_msg_mission_item_get_param3(const mavlink_message_t* msg)
{
@ -397,7 +397,7 @@ static inline float mavlink_msg_mission_item_get_param3(const mavlink_message_t*
/**
* @brief Get field param4 from mission_item message
*
* @return PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
* @return PARAM4, see MAV_CMD enum
*/
static inline float mavlink_msg_mission_item_get_param4(const mavlink_message_t* msg)
{
@ -427,7 +427,7 @@ static inline float mavlink_msg_mission_item_get_y(const mavlink_message_t* msg)
/**
* @brief Get field z from mission_item message
*
* @return PARAM7 / z position: global: altitude
* @return PARAM7 / z position: global: altitude (relative or absolute, depending on frame.
*/
static inline float mavlink_msg_mission_item_get_z(const mavlink_message_t* msg)
{

375
mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h Normal file
View File

@ -0,0 +1,375 @@
// MESSAGE SCALED_IMU2 PACKING
#define MAVLINK_MSG_ID_SCALED_IMU2 116
typedef struct __mavlink_scaled_imu2_t
{
uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
int16_t xacc; ///< X acceleration (mg)
int16_t yacc; ///< Y acceleration (mg)
int16_t zacc; ///< Z acceleration (mg)
int16_t xgyro; ///< Angular speed around X axis (millirad /sec)
int16_t ygyro; ///< Angular speed around Y axis (millirad /sec)
int16_t zgyro; ///< Angular speed around Z axis (millirad /sec)
int16_t xmag; ///< X Magnetic field (milli tesla)
int16_t ymag; ///< Y Magnetic field (milli tesla)
int16_t zmag; ///< Z Magnetic field (milli tesla)
} mavlink_scaled_imu2_t;
#define MAVLINK_MSG_ID_SCALED_IMU2_LEN 22
#define MAVLINK_MSG_ID_116_LEN 22
#define MAVLINK_MSG_ID_SCALED_IMU2_CRC 76
#define MAVLINK_MSG_ID_116_CRC 76
#define MAVLINK_MESSAGE_INFO_SCALED_IMU2 { \
"SCALED_IMU2", \
10, \
{ { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu2_t, time_boot_ms) }, \
{ "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu2_t, xacc) }, \
{ "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu2_t, yacc) }, \
{ "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu2_t, zacc) }, \
{ "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu2_t, xgyro) }, \
{ "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu2_t, ygyro) }, \
{ "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu2_t, zgyro) }, \
{ "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu2_t, xmag) }, \
{ "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu2_t, ymag) }, \
{ "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu2_t, zmag) }, \
} \
}
/**
* @brief Pack a scaled_imu2 message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param time_boot_ms Timestamp (milliseconds since system boot)
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
* @param xgyro Angular speed around X axis (millirad /sec)
* @param ygyro Angular speed around Y axis (millirad /sec)
* @param zgyro Angular speed around Z axis (millirad /sec)
* @param xmag X Magnetic field (milli tesla)
* @param ymag Y Magnetic field (milli tesla)
* @param zmag Z Magnetic field (milli tesla)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN];
_mav_put_uint32_t(buf, 0, time_boot_ms);
_mav_put_int16_t(buf, 4, xacc);
_mav_put_int16_t(buf, 6, yacc);
_mav_put_int16_t(buf, 8, zacc);
_mav_put_int16_t(buf, 10, xgyro);
_mav_put_int16_t(buf, 12, ygyro);
_mav_put_int16_t(buf, 14, zgyro);
_mav_put_int16_t(buf, 16, xmag);
_mav_put_int16_t(buf, 18, ymag);
_mav_put_int16_t(buf, 20, zmag);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
#else
mavlink_scaled_imu2_t packet;
packet.time_boot_ms = time_boot_ms;
packet.xacc = xacc;
packet.yacc = yacc;
packet.zacc = zacc;
packet.xgyro = xgyro;
packet.ygyro = ygyro;
packet.zgyro = zgyro;
packet.xmag = xmag;
packet.ymag = ymag;
packet.zmag = zmag;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
#endif
}
/**
* @brief Pack a scaled_imu2 message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param time_boot_ms Timestamp (milliseconds since system boot)
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
* @param xgyro Angular speed around X axis (millirad /sec)
* @param ygyro Angular speed around Y axis (millirad /sec)
* @param zgyro Angular speed around Z axis (millirad /sec)
* @param xmag X Magnetic field (milli tesla)
* @param ymag Y Magnetic field (milli tesla)
* @param zmag Z Magnetic field (milli tesla)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN];
_mav_put_uint32_t(buf, 0, time_boot_ms);
_mav_put_int16_t(buf, 4, xacc);
_mav_put_int16_t(buf, 6, yacc);
_mav_put_int16_t(buf, 8, zacc);
_mav_put_int16_t(buf, 10, xgyro);
_mav_put_int16_t(buf, 12, ygyro);
_mav_put_int16_t(buf, 14, zgyro);
_mav_put_int16_t(buf, 16, xmag);
_mav_put_int16_t(buf, 18, ymag);
_mav_put_int16_t(buf, 20, zmag);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
#else
mavlink_scaled_imu2_t packet;
packet.time_boot_ms = time_boot_ms;
packet.xacc = xacc;
packet.yacc = yacc;
packet.zacc = zacc;
packet.xgyro = xgyro;
packet.ygyro = ygyro;
packet.zgyro = zgyro;
packet.xmag = xmag;
packet.ymag = ymag;
packet.zmag = zmag;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
#endif
}
/**
* @brief Encode a scaled_imu2 struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param scaled_imu2 C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_scaled_imu2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2)
{
return mavlink_msg_scaled_imu2_pack(system_id, component_id, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag);
}
/**
* @brief Encode a scaled_imu2 struct on a channel
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param scaled_imu2 C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2)
{
return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag);
}
/**
* @brief Send a scaled_imu2 message
* @param chan MAVLink channel to send the message
*
* @param time_boot_ms Timestamp (milliseconds since system boot)
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
* @param xgyro Angular speed around X axis (millirad /sec)
* @param ygyro Angular speed around Y axis (millirad /sec)
* @param zgyro Angular speed around Z axis (millirad /sec)
* @param xmag X Magnetic field (milli tesla)
* @param ymag Y Magnetic field (milli tesla)
* @param zmag Z Magnetic field (milli tesla)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN];
_mav_put_uint32_t(buf, 0, time_boot_ms);
_mav_put_int16_t(buf, 4, xacc);
_mav_put_int16_t(buf, 6, yacc);
_mav_put_int16_t(buf, 8, zacc);
_mav_put_int16_t(buf, 10, xgyro);
_mav_put_int16_t(buf, 12, ygyro);
_mav_put_int16_t(buf, 14, zgyro);
_mav_put_int16_t(buf, 16, xmag);
_mav_put_int16_t(buf, 18, ymag);
_mav_put_int16_t(buf, 20, zmag);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
#endif
#else
mavlink_scaled_imu2_t packet;
packet.time_boot_ms = time_boot_ms;
packet.xacc = xacc;
packet.yacc = yacc;
packet.zacc = zacc;
packet.xgyro = xgyro;
packet.ygyro = ygyro;
packet.zgyro = zgyro;
packet.xmag = xmag;
packet.ymag = ymag;
packet.zmag = zmag;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
#endif
#endif
}
#endif
// MESSAGE SCALED_IMU2 UNPACKING
/**
* @brief Get field time_boot_ms from scaled_imu2 message
*
* @return Timestamp (milliseconds since system boot)
*/
static inline uint32_t mavlink_msg_scaled_imu2_get_time_boot_ms(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint32_t(msg, 0);
}
/**
* @brief Get field xacc from scaled_imu2 message
*
* @return X acceleration (mg)
*/
static inline int16_t mavlink_msg_scaled_imu2_get_xacc(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 4);
}
/**
* @brief Get field yacc from scaled_imu2 message
*
* @return Y acceleration (mg)
*/
static inline int16_t mavlink_msg_scaled_imu2_get_yacc(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 6);
}
/**
* @brief Get field zacc from scaled_imu2 message
*
* @return Z acceleration (mg)
*/
static inline int16_t mavlink_msg_scaled_imu2_get_zacc(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 8);
}
/**
* @brief Get field xgyro from scaled_imu2 message
*
* @return Angular speed around X axis (millirad /sec)
*/
static inline int16_t mavlink_msg_scaled_imu2_get_xgyro(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 10);
}
/**
* @brief Get field ygyro from scaled_imu2 message
*
* @return Angular speed around Y axis (millirad /sec)
*/
static inline int16_t mavlink_msg_scaled_imu2_get_ygyro(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 12);
}
/**
* @brief Get field zgyro from scaled_imu2 message
*
* @return Angular speed around Z axis (millirad /sec)
*/
static inline int16_t mavlink_msg_scaled_imu2_get_zgyro(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 14);
}
/**
* @brief Get field xmag from scaled_imu2 message
*
* @return X Magnetic field (milli tesla)
*/
static inline int16_t mavlink_msg_scaled_imu2_get_xmag(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 16);
}
/**
* @brief Get field ymag from scaled_imu2 message
*
* @return Y Magnetic field (milli tesla)
*/
static inline int16_t mavlink_msg_scaled_imu2_get_ymag(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 18);
}
/**
* @brief Get field zmag from scaled_imu2 message
*
* @return Z Magnetic field (milli tesla)
*/
static inline int16_t mavlink_msg_scaled_imu2_get_zmag(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 20);
}
/**
* @brief Decode a scaled_imu2 message into a struct
*
* @param msg The message to decode
* @param scaled_imu2 C-struct to decode the message contents into
*/
static inline void mavlink_msg_scaled_imu2_decode(const mavlink_message_t* msg, mavlink_scaled_imu2_t* scaled_imu2)
{
#if MAVLINK_NEED_BYTE_SWAP
scaled_imu2->time_boot_ms = mavlink_msg_scaled_imu2_get_time_boot_ms(msg);
scaled_imu2->xacc = mavlink_msg_scaled_imu2_get_xacc(msg);
scaled_imu2->yacc = mavlink_msg_scaled_imu2_get_yacc(msg);
scaled_imu2->zacc = mavlink_msg_scaled_imu2_get_zacc(msg);
scaled_imu2->xgyro = mavlink_msg_scaled_imu2_get_xgyro(msg);
scaled_imu2->ygyro = mavlink_msg_scaled_imu2_get_ygyro(msg);
scaled_imu2->zgyro = mavlink_msg_scaled_imu2_get_zgyro(msg);
scaled_imu2->xmag = mavlink_msg_scaled_imu2_get_xmag(msg);
scaled_imu2->ymag = mavlink_msg_scaled_imu2_get_ymag(msg);
scaled_imu2->zmag = mavlink_msg_scaled_imu2_get_zmag(msg);
#else
memcpy(scaled_imu2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SCALED_IMU2_LEN);
#endif
}

30
mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h
View File

@ -4,9 +4,9 @@
typedef struct __mavlink_sys_status_t
{
uint32_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
uint32_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
uint32_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
uint32_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
uint32_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
uint32_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
uint16_t voltage_battery; ///< Battery voltage, in millivolts (1 = 1 millivolt)
int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
@ -53,9 +53,9 @@ typedef struct __mavlink_sys_status_t
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
* @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
* @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
* @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
* @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
* @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
@ -121,9 +121,9 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message will be sent over
* @param msg The MAVLink message to compress the data into
* @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
* @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
* @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
* @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
* @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
* @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
@ -215,9 +215,9 @@ static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uin
* @brief Send a sys_status message
* @param chan MAVLink channel to send the message
*
* @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
* @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
* @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
* @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
* @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
* @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
@ -286,7 +286,7 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t
/**
* @brief Get field onboard_control_sensors_present from sys_status message
*
* @return Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @return Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
*/
static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_present(const mavlink_message_t* msg)
{
@ -296,7 +296,7 @@ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_presen
/**
* @brief Get field onboard_control_sensors_enabled from sys_status message
*
* @return Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @return Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
*/
static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled(const mavlink_message_t* msg)
{
@ -306,7 +306,7 @@ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enable
/**
* @brief Get field onboard_control_sensors_health from sys_status message
*
* @return Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
* @return Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
*/
static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health(const mavlink_message_t* msg)
{

1380
mavlink/include/mavlink/v1.0/common/testsuite.h
View File

File diff suppressed because it is too large Load Diff

2
mavlink/include/mavlink/v1.0/common/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Tue Sep 10 23:50:05 2013"
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:59:18 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254

6
mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
View File

File diff suppressed because one or more lines are too long

282
mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h
View File

@ -31,7 +31,7 @@ static void mavlink_test_flexifunction_set(uint8_t system_id, uint8_t component_
uint16_t i;
mavlink_flexifunction_set_t packet_in = {
5,
72,
}72,
};
mavlink_flexifunction_set_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -76,9 +76,9 @@ static void mavlink_test_flexifunction_read_req(uint8_t system_id, uint8_t compo
uint16_t i;
mavlink_flexifunction_read_req_t packet_in = {
17235,
17339,
17,
84,
}17339,
}17,
}84,
};
mavlink_flexifunction_read_req_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -125,12 +125,12 @@ static void mavlink_test_flexifunction_buffer_function(uint8_t system_id, uint8_
uint16_t i;
mavlink_flexifunction_buffer_function_t packet_in = {
17235,
17339,
17443,
17547,
29,
96,
{ 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210 },
}17339,
}17443,
}17547,
}29,
}96,
}{ 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210 },
};
mavlink_flexifunction_buffer_function_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -180,9 +180,9 @@ static void mavlink_test_flexifunction_buffer_function_ack(uint8_t system_id, ui
uint16_t i;
mavlink_flexifunction_buffer_function_ack_t packet_in = {
17235,
17339,
17,
84,
}17339,
}17,
}84,
};
mavlink_flexifunction_buffer_function_ack_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -229,11 +229,11 @@ static void mavlink_test_flexifunction_directory(uint8_t system_id, uint8_t comp
uint16_t i;
mavlink_flexifunction_directory_t packet_in = {
5,
72,
139,
206,
17,
{ 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131 },
}72,
}139,
}206,
}17,
}{ 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131 },
};
mavlink_flexifunction_directory_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -282,11 +282,11 @@ static void mavlink_test_flexifunction_directory_ack(uint8_t system_id, uint8_t
uint16_t i;
mavlink_flexifunction_directory_ack_t packet_in = {
17235,
139,
206,
17,
84,
151,
}139,
}206,
}17,
}84,
}151,
};
mavlink_flexifunction_directory_ack_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -335,8 +335,8 @@ static void mavlink_test_flexifunction_command(uint8_t system_id, uint8_t compon
uint16_t i;
mavlink_flexifunction_command_t packet_in = {
5,
72,
139,
}72,
}139,
};
mavlink_flexifunction_command_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -382,7 +382,7 @@ static void mavlink_test_flexifunction_command_ack(uint8_t system_id, uint8_t co
uint16_t i;
mavlink_flexifunction_command_ack_t packet_in = {
17235,
17339,
}17339,
};
mavlink_flexifunction_command_ack_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -427,33 +427,33 @@ static void mavlink_test_serial_udb_extra_f2_a(uint8_t system_id, uint8_t compon
uint16_t i;
mavlink_serial_udb_extra_f2_a_t packet_in = {
963497464,
963497672,
963497880,
963498088,
18067,
18171,
18275,
18379,
18483,
18587,
18691,
18795,
18899,
19003,
19107,
19211,
19315,
19419,
19523,
19627,
19731,
19835,
19939,
20043,
20147,
20251,
20355,
63,
}963497672,
}963497880,
}963498088,
}18067,
}18171,
}18275,
}18379,
}18483,
}18587,
}18691,
}18795,
}18899,
}19003,
}19107,
}19211,
}19315,
}19419,
}19523,
}19627,
}19731,
}19835,
}19939,
}20043,
}20147,
}20251,
}20355,
}63,
};
mavlink_serial_udb_extra_f2_a_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -524,38 +524,38 @@ static void mavlink_test_serial_udb_extra_f2_b(uint8_t system_id, uint8_t compon
uint16_t i;
mavlink_serial_udb_extra_f2_b_t packet_in = {
963497464,
963497672,
17651,
17755,
17859,
17963,
18067,
18171,
18275,
18379,
18483,
18587,
18691,
18795,
18899,
19003,
19107,
19211,
19315,
19419,
19523,
19627,
19731,
19835,
19939,
20043,
20147,
20251,
20355,
20459,
20563,
20667,
20771,
}963497672,
}17651,
}17755,
}17859,
}17963,
}18067,
}18171,
}18275,
}18379,
}18483,
}18587,
}18691,
}18795,
}18899,
}19003,
}19107,
}19211,
}19315,
}19419,
}19523,
}19627,
}19731,
}19835,
}19939,
}20043,
}20147,
}20251,
}20355,
}20459,
}20563,
}20667,
}20771,
};
mavlink_serial_udb_extra_f2_b_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -631,15 +631,15 @@ static void mavlink_test_serial_udb_extra_f4(uint8_t system_id, uint8_t componen
uint16_t i;
mavlink_serial_udb_extra_f4_t packet_in = {
5,
72,
139,
206,
17,
84,
151,
218,
29,
96,
}72,
}139,
}206,
}17,
}84,
}151,
}218,
}29,
}96,
};
mavlink_serial_udb_extra_f4_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -692,11 +692,11 @@ static void mavlink_test_serial_udb_extra_f5(uint8_t system_id, uint8_t componen
uint16_t i;
mavlink_serial_udb_extra_f5_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
}45.0,
}73.0,
}101.0,
}129.0,
}157.0,
};
mavlink_serial_udb_extra_f5_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -745,10 +745,10 @@ static void mavlink_test_serial_udb_extra_f6(uint8_t system_id, uint8_t componen
uint16_t i;
mavlink_serial_udb_extra_f6_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
}45.0,
}73.0,
}101.0,
}129.0,
};
mavlink_serial_udb_extra_f6_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -796,11 +796,11 @@ static void mavlink_test_serial_udb_extra_f7(uint8_t system_id, uint8_t componen
uint16_t i;
mavlink_serial_udb_extra_f7_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
}45.0,
}73.0,
}101.0,
}129.0,
}157.0,
};
mavlink_serial_udb_extra_f7_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -849,12 +849,12 @@ static void mavlink_test_serial_udb_extra_f8(uint8_t system_id, uint8_t componen
uint16_t i;
mavlink_serial_udb_extra_f8_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
185.0,
}45.0,
}73.0,
}101.0,
}129.0,
}157.0,
}185.0,
};
mavlink_serial_udb_extra_f8_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -904,9 +904,9 @@ static void mavlink_test_serial_udb_extra_f13(uint8_t system_id, uint8_t compone
uint16_t i;
mavlink_serial_udb_extra_f13_t packet_in = {
963497464,
963497672,
963497880,
17859,
}963497672,
}963497880,
}17859,
};
mavlink_serial_udb_extra_f13_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -953,16 +953,16 @@ static void mavlink_test_serial_udb_extra_f14(uint8_t system_id, uint8_t compone
uint16_t i;
mavlink_serial_udb_extra_f14_t packet_in = {
963497464,
17443,
17547,
17651,
163,
230,
41,
108,
175,
242,
53,
}17443,
}17547,
}17651,
}163,
}230,
}41,
}108,
}175,
}242,
}53,
};
mavlink_serial_udb_extra_f14_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1016,7 +1016,7 @@ static void mavlink_test_serial_udb_extra_f15(uint8_t system_id, uint8_t compone
uint16_t i;
mavlink_serial_udb_extra_f15_t packet_in = {
{ 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44 },
{ 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144 },
}{ 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144 },
};
mavlink_serial_udb_extra_f15_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1061,7 +1061,7 @@ static void mavlink_test_serial_udb_extra_f16(uint8_t system_id, uint8_t compone
uint16_t i;
mavlink_serial_udb_extra_f16_t packet_in = {
{ 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44 },
{ 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194 },
}{ 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194 },
};
mavlink_serial_udb_extra_f16_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1106,12 +1106,12 @@ static void mavlink_test_altitudes(uint8_t system_id, uint8_t component_id, mavl
uint16_t i;
mavlink_altitudes_t packet_in = {
963497464,
963497672,
963497880,
963498088,
963498296,
963498504,
963498712,
}963497672,
}963497880,
}963498088,
}963498296,
}963498504,
}963498712,
};
mavlink_altitudes_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1161,12 +1161,12 @@ static void mavlink_test_airspeeds(uint8_t system_id, uint8_t component_id, mavl
uint16_t i;
mavlink_airspeeds_t packet_in = {
963497464,
17443,
17547,
17651,
17755,
17859,
17963,
}17443,
}17547,
}17651,
}17755,
}17859,
}17963,
};
mavlink_airspeeds_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));

2
mavlink/include/mavlink/v1.0/matrixpilot/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:51 2013"
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:57:49 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254

6
mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
View File

File diff suppressed because one or more lines are too long

228
mavlink/include/mavlink/v1.0/pixhawk/testsuite.h
View File

@ -31,11 +31,11 @@ static void mavlink_test_set_cam_shutter(uint8_t system_id, uint8_t component_id
uint16_t i;
mavlink_set_cam_shutter_t packet_in = {
17.0,
17443,
17547,
29,
96,
163,
}17443,
}17547,
}29,
}96,
}163,
};
mavlink_set_cam_shutter_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -84,17 +84,17 @@ static void mavlink_test_image_triggered(uint8_t system_id, uint8_t component_id
uint16_t i;
mavlink_image_triggered_t packet_in = {
93372036854775807ULL,
963497880,
101.0,
129.0,
157.0,
185.0,
213.0,
241.0,
269.0,
297.0,
325.0,
353.0,
}963497880,
}101.0,
}129.0,
}157.0,
}185.0,
}213.0,
}241.0,
}269.0,
}297.0,
}325.0,
}353.0,
};
mavlink_image_triggered_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -192,28 +192,28 @@ static void mavlink_test_image_available(uint8_t system_id, uint8_t component_id
uint16_t i;
mavlink_image_available_t packet_in = {
93372036854775807ULL,
93372036854776311ULL,
93372036854776815ULL,
963498712,
963498920,
963499128,
963499336,
297.0,
325.0,
353.0,
381.0,
409.0,
437.0,
465.0,
493.0,
521.0,
549.0,
577.0,
21603,
21707,
21811,
147,
214,
}93372036854776311ULL,
}93372036854776815ULL,
}963498712,
}963498920,
}963499128,
}963499336,
}297.0,
}325.0,
}353.0,
}381.0,
}409.0,
}437.0,
}465.0,
}493.0,
}521.0,
}549.0,
}577.0,
}21603,
}21707,
}21811,
}147,
}214,
};
mavlink_image_available_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -279,11 +279,11 @@ static void mavlink_test_set_position_control_offset(uint8_t system_id, uint8_t
uint16_t i;
mavlink_set_position_control_offset_t packet_in = {
17.0,
45.0,
73.0,
101.0,
53,
120,
}45.0,
}73.0,
}101.0,
}53,
}120,
};
mavlink_set_position_control_offset_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -332,10 +332,10 @@ static void mavlink_test_position_control_setpoint(uint8_t system_id, uint8_t co
uint16_t i;
mavlink_position_control_setpoint_t packet_in = {
17.0,
45.0,
73.0,
101.0,
18067,
}45.0,
}73.0,
}101.0,
}18067,
};
mavlink_position_control_setpoint_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -383,12 +383,12 @@ static void mavlink_test_marker(uint8_t system_id, uint8_t component_id, mavlink
uint16_t i;
mavlink_marker_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
18483,
}45.0,
}73.0,
}101.0,
}129.0,
}157.0,
}18483,
};
mavlink_marker_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -438,12 +438,12 @@ static void mavlink_test_raw_aux(uint8_t system_id, uint8_t component_id, mavlin
uint16_t i;
mavlink_raw_aux_t packet_in = {
963497464,
17443,
17547,
17651,
17755,
17859,
17963,
}17443,
}17547,
}17651,
}17755,
}17859,
}17963,
};
mavlink_raw_aux_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -493,7 +493,7 @@ static void mavlink_test_watchdog_heartbeat(uint8_t system_id, uint8_t component
uint16_t i;
mavlink_watchdog_heartbeat_t packet_in = {
17235,
17339,
}17339,
};
mavlink_watchdog_heartbeat_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -538,10 +538,10 @@ static void mavlink_test_watchdog_process_info(uint8_t system_id, uint8_t compon
uint16_t i;
mavlink_watchdog_process_info_t packet_in = {
963497464,
17443,
17547,
"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABC",
"EFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRST",
}17443,
}17547,
}"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABC",
}"EFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRST",
};
mavlink_watchdog_process_info_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -589,11 +589,11 @@ static void mavlink_test_watchdog_process_status(uint8_t system_id, uint8_t comp
uint16_t i;
mavlink_watchdog_process_status_t packet_in = {
963497464,
17443,
17547,
17651,
163,
230,
}17443,
}17547,
}17651,
}163,
}230,
};
mavlink_watchdog_process_status_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -642,9 +642,9 @@ static void mavlink_test_watchdog_command(uint8_t system_id, uint8_t component_i
uint16_t i;
mavlink_watchdog_command_t packet_in = {
17235,
17339,
17,
84,
}17339,
}17,
}84,
};
mavlink_watchdog_command_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -691,9 +691,9 @@ static void mavlink_test_pattern_detected(uint8_t system_id, uint8_t component_i
uint16_t i;
mavlink_pattern_detected_t packet_in = {
17.0,
17,
"FGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZ",
128,
}17,
}"FGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZ",
}128,
};
mavlink_pattern_detected_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -740,13 +740,13 @@ static void mavlink_test_point_of_interest(uint8_t system_id, uint8_t component_
uint16_t i;
mavlink_point_of_interest_t packet_in = {
17.0,
45.0,
73.0,
17859,
175,
242,
53,
"RSTUVWXYZABCDEFGHIJKLMNOP",
}45.0,
}73.0,
}17859,
}175,
}242,
}53,
}"RSTUVWXYZABCDEFGHIJKLMNOP",
};
mavlink_point_of_interest_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -797,16 +797,16 @@ static void mavlink_test_point_of_interest_connection(uint8_t system_id, uint8_t
uint16_t i;
mavlink_point_of_interest_connection_t packet_in = {
17.0,
45.0,
73.0,
101.0,
129.0,
157.0,
18483,
211,
22,
89,
"DEFGHIJKLMNOPQRSTUVWXYZAB",
}45.0,
}73.0,
}101.0,
}129.0,
}157.0,
}18483,
}211,
}22,
}89,
}"DEFGHIJKLMNOPQRSTUVWXYZAB",
};
mavlink_point_of_interest_connection_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -860,12 +860,12 @@ static void mavlink_test_data_transmission_handshake(uint8_t system_id, uint8_t
uint16_t i;
mavlink_data_transmission_handshake_t packet_in = {
963497464,
17443,
17547,
29,
96,
163,
230,
}17443,
}17547,
}29,
}96,
}163,
}230,
};
mavlink_data_transmission_handshake_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -915,7 +915,7 @@ static void mavlink_test_encapsulated_data(uint8_t system_id, uint8_t component_
uint16_t i;
mavlink_encapsulated_data_t packet_in = {
17235,
{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135 },
}{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135 },
};
mavlink_encapsulated_data_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -960,13 +960,13 @@ static void mavlink_test_brief_feature(uint8_t system_id, uint8_t component_id,
uint16_t i;
mavlink_brief_feature_t packet_in = {
17.0,
45.0,
73.0,
101.0,
18067,
18171,
65,
{ 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163 },
}45.0,
}73.0,
}101.0,
}18067,
}18171,
}65,
}{ 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163 },
};
mavlink_brief_feature_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@ -1017,14 +1017,14 @@ static void mavlink_test_attitude_control(uint8_t system_id, uint8_t component_i
uint16_t i;
mavlink_attitude_control_t packet_in = {
17.0,
45.0,
73.0,
101.0,
53,
120,
187,
254,
65,
}45.0,
}73.0,
}101.0,
}53,
}120,
}187,
}254,
}65,
};
mavlink_attitude_control_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));

2
mavlink/include/mavlink/v1.0/pixhawk/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:13 2013"
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:58:02 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255

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

@ -566,7 +566,7 @@ CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96
CONFIG_CDCACM_RXBUFSIZE=512
CONFIG_CDCACM_TXBUFSIZE=512
CONFIG_CDCACM_TXBUFSIZE=2048
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0010
CONFIG_CDCACM_VENDORSTR="3D Robotics"

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

@ -295,19 +295,20 @@ CONFIG_STM32_USART=y
# U[S]ART Configuration
#
# CONFIG_USART1_RS485 is not set
# CONFIG_USART1_RXDMA is not set
CONFIG_USART1_RXDMA=y
# CONFIG_USART2_RS485 is not set
CONFIG_USART2_RXDMA=y
# CONFIG_USART3_RS485 is not set
# CONFIG_USART3_RXDMA is not set
CONFIG_USART3_RXDMA=y
# CONFIG_UART4_RS485 is not set
# CONFIG_UART4_RXDMA is not set
CONFIG_UART4_RXDMA=y
# CONFIG_UART5_RXDMA is not set
# CONFIG_USART6_RS485 is not set
# CONFIG_USART6_RXDMA is not set
CONFIG_USART6_RXDMA=y
# CONFIG_UART7_RS485 is not set
# CONFIG_UART7_RXDMA is not set
# CONFIG_UART8_RS485 is not set
# CONFIG_UART8_RXDMA is not set
CONFIG_UART8_RXDMA=y
CONFIG_SERIAL_DISABLE_REORDERING=y
CONFIG_STM32_USART_SINGLEWIRE=y
@ -581,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y
#
# UART4 Configuration
#
CONFIG_UART4_RXBUFSIZE=128
CONFIG_UART4_TXBUFSIZE=128
CONFIG_UART4_RXBUFSIZE=512
CONFIG_UART4_TXBUFSIZE=512
CONFIG_UART4_BAUD=57600
CONFIG_UART4_BITS=8
CONFIG_UART4_PARITY=0
@ -593,8 +594,8 @@ CONFIG_UART4_2STOP=0
#
# USART6 Configuration
#
CONFIG_USART6_RXBUFSIZE=256
CONFIG_USART6_TXBUFSIZE=256
CONFIG_USART6_RXBUFSIZE=512
CONFIG_USART6_TXBUFSIZE=512
CONFIG_USART6_BAUD=57600
CONFIG_USART6_BITS=8
CONFIG_USART6_PARITY=0
@ -661,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96
CONFIG_CDCACM_RXBUFSIZE=512
CONFIG_CDCACM_TXBUFSIZE=512
CONFIG_CDCACM_TXBUFSIZE=2048
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0011
CONFIG_CDCACM_VENDORSTR="3D Robotics"

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

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

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

@ -46,7 +46,6 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <systemlib/err.h>
#include "ardrone_motor_control.h"
@ -384,9 +383,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
static bool initialized = false;
/* publish effective outputs */
static struct actuator_controls_effective_s actuator_controls_effective;
static orb_advert_t actuator_controls_effective_pub;
/* linearly scale the control inputs from 0 to startpoint_full_control */
if (motor_thrust < startpoint_full_control) {
@ -430,25 +426,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
}
/* publish effective outputs */
actuator_controls_effective.control_effective[0] = roll_control;
actuator_controls_effective.control_effective[1] = pitch_control;
/* yaw output after limiting */
actuator_controls_effective.control_effective[2] = yaw_control;
/* possible motor thrust limiting */
actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
if (!initialized) {
/* advertise and publish */
actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
initialized = true;
} else {
/* already initialized, just publishing */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
}
/* set the motor values */
/* scale up from 0..1 to 10..500) */

6
src/drivers/boards/px4fmu-v1/board_config.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -34,7 +34,7 @@
/**
* @file board_config.h
*
* PX4FMU internal definitions
* PX4FMUv1 internal definitions
*/
#pragma once
@ -180,7 +180,7 @@ __BEGIN_DECLS
#define HRT_TIMER 1 /* use timer1 for the HRT */
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */
#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
/****************************************************************************************************
* Public Types

34
src/drivers/boards/px4fmu-v2/board_config.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 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
@ -34,7 +34,7 @@
/**
* @file board_config.h
*
* PX4FMU internal definitions
* PX4FMUv2 internal definitions
*/
#pragma once
@ -79,17 +79,37 @@ __BEGIN_DECLS
#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
/* Data ready pins off */
#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
/* SPI1 off */
#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6)
#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7)
/* SPI1 chip selects off */
#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
/* SPI chip selects */
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
#define PX4_SPIDEV_GYRO 1
#define PX4_SPIDEV_ACCEL_MAG 2
#define PX4_SPIDEV_BARO 3
#define PX4_SPIDEV_MPU 4
/* I2C busses */
#define PX4_I2C_BUS_EXPANSION 1
@ -157,7 +177,7 @@ __BEGIN_DECLS
*
* PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED)
*/
#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
/* High-resolution timer */
#define HRT_TIMER 8 /* use timer8 for the HRT */

7
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
View File

@ -215,9 +215,9 @@ __EXPORT int nsh_archinitialize(void)
stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */
stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */
stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */
stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */
stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */
// stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */
// stm32_configgpio(GPIO_ADC1_IN11); /* unused */
// stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */
stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */
stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */
stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
@ -279,6 +279,7 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\n");

18
src/drivers/boards/px4fmu-v2/px4fmu_spi.c
View File

@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void)
stm32_configgpio(GPIO_SPI_CS_GYRO);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
stm32_configgpio(GPIO_SPI_CS_BARO);
stm32_configgpio(GPIO_SPI_CS_MPU);
/* De-activate all peripherals,
* required for some peripheral
@ -81,6 +82,12 @@ __EXPORT void weak_function stm32_spiinitialize(void)
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
stm32_configgpio(GPIO_EXTI_MAG_DRDY);
stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
stm32_configgpio(GPIO_EXTI_MPU_DRDY);
#endif
#ifdef CONFIG_STM32_SPI2
@ -99,6 +106,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_ACCEL_MAG:
@ -106,6 +114,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_BARO:
@ -113,6 +122,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_MPU:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
break;
default:

4
src/drivers/boards/px4io-v1/board_config.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -92,4 +92,4 @@
#define HRT_TIMER 1 /* use timer1 for the HRT */
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
#define GPIO_PPM_IN GPIO_TIM1_CH1IN
#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)

6
src/drivers/boards/px4io-v2/board_config.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 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
@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file px4iov2_internal.h
* @file board_config.h
*
* PX4IOV2 internal definitions
*/
@ -122,7 +122,7 @@
#define HRT_TIMER 1 /* use timer1 for the HRT */
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */
#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */
#define GPIO_PPM_IN GPIO_TIM1_CH1IN
#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9)
/* LED definitions ******************************************************************/
/* PX4 has two LEDs that we will encode as: */

36
src/drivers/device/cdev.cpp
View File

@ -108,6 +108,42 @@ CDev::~CDev()
unregister_driver(_devname);
}
int
CDev::register_class_devname(const char *class_devname)
{
if (class_devname == nullptr) {
return -EINVAL;
}
int class_instance = 0;
int ret = -ENOSPC;
while (class_instance < 4) {
if (class_instance == 0) {
ret = register_driver(class_devname, &fops, 0666, (void *)this);
if (ret == OK) break;
} else {
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
ret = register_driver(name, &fops, 0666, (void *)this);
if (ret == OK) break;
}
class_instance++;
}
if (class_instance == 4)
return ret;
return class_instance;
}
int
CDev::unregister_class_devname(const char *class_devname, unsigned class_instance)
{
if (class_instance > 0) {
char name[32];
snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
return unregister_driver(name);
}
return unregister_driver(class_devname);
}
int
CDev::init()
{

22
src/drivers/device/device.h
View File

@ -396,6 +396,25 @@ protected:
*/
virtual int close_last(struct file *filp);
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
* @param class_devname Device class name
* @return class_instamce Class instance created, or -errno on failure
*/
virtual int register_class_devname(const char *class_devname);
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
* @param class_devname Device class name
* @param class_instance Device class instance from register_class_devname()
* @return OK on success, -errno otherwise
*/
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
private:
static const unsigned _max_pollwaiters = 8;
@ -488,4 +507,7 @@ private:
} // namespace device
// class instance for primary driver of each class
#define CLASS_DEVICE_PRIMARY 0
#endif /* _DEVICE_DEVICE_H */

6
src/drivers/device/spi.cpp
View File

@ -181,4 +181,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
return OK;
}
void
SPI::set_frequency(uint32_t frequency)
{
_frequency = frequency;
}
} // namespace device

11
src/drivers/device/spi.h
View File

@ -101,6 +101,17 @@ protected:
*/
int transfer(uint8_t *send, uint8_t *recv, unsigned len);
/**
* Set the SPI bus frequency
* This is used to change frequency on the fly. Some sensors
* (such as the MPU6000) need a lower frequency for setup
* registers and can handle higher frequency for sensor
* value registers
*
* @param frequency Frequency to set (Hz)
*/
void set_frequency(uint32_t frequency);
/**
* Locking modes supported by the driver.
*/

4
src/drivers/drv_gpio.h
View File

@ -46,7 +46,7 @@
/*
* PX4FMU GPIO numbers.
*
* For shared pins, alternate function 1 selects the non-GPIO mode
* For shared pins, alternate function 1 selects the non-GPIO mode
* (USART2, CAN2, etc.)
*/
# define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */
@ -144,4 +144,6 @@
/** read all the GPIOs and return their values in *(uint32_t *)arg */
#define GPIO_GET GPIOC(12)
#define GPIO_SENSOR_RAIL_RESET GPIOC(13)
#endif /* _DRV_GPIO_H */

14
src/drivers/drv_hrt.h
View File

@ -141,6 +141,20 @@ __EXPORT extern bool hrt_called(struct hrt_call *entry);
*/
__EXPORT extern void hrt_cancel(struct hrt_call *entry);
/*
* initialise a hrt_call structure
*/
__EXPORT extern void hrt_call_init(struct hrt_call *entry);
/*
* delay a hrt_call_every() periodic call by the given number of
* microseconds. This should be called from within the callout to
* cause the callout to be re-scheduled for a later time. The periodic
* callouts will then continue from that new base time at the
* previously specified period.
*/
__EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
/*
* Initialise the HRT.
*/

5
src/drivers/drv_rc_input.h
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
@ -89,6 +89,9 @@ struct rc_input_values {
/** number of channels actually being seen */
uint32_t channel_count;
/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
int32_t rssi;
/** Input source */
enum RC_INPUT_SOURCE input_source;

289
src/drivers/frsky_telemetry/frsky_data.c Normal file
View File

@ -0,0 +1,289 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Stefan Rado <px4@sradonia.net>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file frsky_data.c
* @author Stefan Rado <px4@sradonia.net>
*
* FrSky telemetry implementation.
*
*/
#include "frsky_data.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
/* FrSky sensor hub data IDs */
#define FRSKY_ID_GPS_ALT_BP 0x01
#define FRSKY_ID_TEMP1 0x02
#define FRSKY_ID_RPM 0x03
#define FRSKY_ID_FUEL 0x04
#define FRSKY_ID_TEMP2 0x05
#define FRSKY_ID_VOLTS 0x06
#define FRSKY_ID_GPS_ALT_AP 0x09
#define FRSKY_ID_BARO_ALT_BP 0x10
#define FRSKY_ID_GPS_SPEED_BP 0x11
#define FRSKY_ID_GPS_LONG_BP 0x12
#define FRSKY_ID_GPS_LAT_BP 0x13
#define FRSKY_ID_GPS_COURS_BP 0x14
#define FRSKY_ID_GPS_DAY_MONTH 0x15
#define FRSKY_ID_GPS_YEAR 0x16
#define FRSKY_ID_GPS_HOUR_MIN 0x17
#define FRSKY_ID_GPS_SEC 0x18
#define FRSKY_ID_GPS_SPEED_AP 0x19
#define FRSKY_ID_GPS_LONG_AP 0x1A
#define FRSKY_ID_GPS_LAT_AP 0x1B
#define FRSKY_ID_GPS_COURS_AP 0x1C
#define FRSKY_ID_BARO_ALT_AP 0x21
#define FRSKY_ID_GPS_LONG_EW 0x22
#define FRSKY_ID_GPS_LAT_NS 0x23
#define FRSKY_ID_ACCEL_X 0x24
#define FRSKY_ID_ACCEL_Y 0x25
#define FRSKY_ID_ACCEL_Z 0x26
#define FRSKY_ID_CURRENT 0x28
#define FRSKY_ID_VARIO 0x30
#define FRSKY_ID_VFAS 0x39
#define FRSKY_ID_VOLTS_BP 0x3A
#define FRSKY_ID_VOLTS_AP 0x3B
#define frac(f) (f - (int)f)
static int battery_sub = -1;
static int sensor_sub = -1;
static int global_position_sub = -1;
static int vehicle_status_sub = -1;
/**
* Initializes the uORB subscriptions.
*/
void frsky_init()
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}
/**
* Sends a 0x5E start/stop byte.
*/
static void frsky_send_startstop(int uart)
{
static const uint8_t c = 0x5E;
write(uart, &c, sizeof(c));
}
/**
* Sends one byte, performing byte-stuffing if necessary.
*/
static void frsky_send_byte(int uart, uint8_t value)
{
const uint8_t x5E[] = { 0x5D, 0x3E };
const uint8_t x5D[] = { 0x5D, 0x3D };
switch (value) {
case 0x5E:
write(uart, x5E, sizeof(x5E));
break;
case 0x5D:
write(uart, x5D, sizeof(x5D));
break;
default:
write(uart, &value, sizeof(value));
break;
}
}
/**
* Sends one data id/value pair.
*/
static void frsky_send_data(int uart, uint8_t id, int16_t data)
{
/* Cast data to unsigned, because signed shift might behave incorrectly */
uint16_t udata = data;
frsky_send_startstop(uart);
frsky_send_byte(uart, id);
frsky_send_byte(uart, udata); /* LSB */
frsky_send_byte(uart, udata >> 8); /* MSB */
}
/**
* Sends frame 1 (every 200ms):
* acceleration values, barometer altitude, temperature, battery voltage & current
*/
void frsky_send_frame1(int uart)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/* get a local copy of the battery data */
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* send formatted frame */
frsky_send_data(uart, FRSKY_ID_ACCEL_X,
roundf(raw.accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
roundf(raw.accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
roundf(raw.accelerometer_m_s2[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
raw.baro_alt_meter);
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
roundf(frac(raw.baro_alt_meter) * 100.0f));
frsky_send_data(uart, FRSKY_ID_TEMP1,
roundf(raw.baro_temp_celcius));
frsky_send_data(uart, FRSKY_ID_VFAS,
roundf(battery.voltage_v * 10.0f));
frsky_send_data(uart, FRSKY_ID_CURRENT,
(battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f));
frsky_send_startstop(uart);
}
/**
* Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
*/
static float frsky_format_gps(float dec)
{
float dms_deg = (int) dec;
float dec_deg = dec - dms_deg;
float dms_min = (int) (dec_deg * 60);
float dec_min = (dec_deg * 60) - dms_min;
float dms_sec = dec_min * 60;
return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
}
/**
* Sends frame 2 (every 1000ms):
* GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level
*/
void frsky_send_frame2(int uart)
{
/* get a local copy of the global position data */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* get a local copy of the vehicle status data */
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status));
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
/* send formatted frame */
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
if (global_pos.valid) {
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f);
lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f);
lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy)
* 25.0f / 46.0f;
alt = global_pos.alt;
sec = tm_gps->tm_sec;
}
frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course);
frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew);
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed);
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f);
frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
frsky_send_data(uart, FRSKY_ID_FUEL,
roundf(vehicle_status.battery_remaining * 100.0f));
frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
frsky_send_startstop(uart);
}
/**
* Sends frame 3 (every 5000ms):
* GPS date & time
*/
void frsky_send_frame3(int uart)
{
/* get a local copy of the battery data */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* send formatted frame */
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);
uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);
frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year);
frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min);
frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec);
frsky_send_startstop(uart);
}

51
src/drivers/frsky_telemetry/frsky_data.h Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Stefan Rado <px4@sradonia.net>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file frsky_data.h
* @author Stefan Rado <px4@sradonia.net>
*
* FrSky telemetry implementation.
*
*/
#ifndef _FRSKY_DATA_H
#define _FRSKY_DATA_H
// Public functions
void frsky_init(void);
void frsky_send_frame1(int uart);
void frsky_send_frame2(int uart);
void frsky_send_frame3(int uart);
#endif /* _FRSKY_TELEMETRY_H */

266
src/drivers/frsky_telemetry/frsky_telemetry.c Normal file
View File

@ -0,0 +1,266 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Stefan Rado <px4@sradonia.net>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file frsky_telemetry.c
* @author Stefan Rado <px4@sradonia.net>
*
* FrSky telemetry implementation.
*
* This daemon emulates an FrSky sensor hub by periodically sending data
* packets to an attached FrSky receiver.
*
*/
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <termios.h>
#include "frsky_data.h"
/* thread state */
static volatile bool thread_should_exit = false;
static volatile bool thread_running = false;
static int frsky_task;
/* functions */
static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original);
static void usage(void);
static int frsky_telemetry_thread_main(int argc, char *argv[]);
__EXPORT int frsky_telemetry_main(int argc, char *argv[]);
/**
* Opens the UART device and sets all required serial parameters.
*/
static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original)
{
/* Open UART */
const int uart = open(uart_name, O_WRONLY | O_NOCTTY);
if (uart < 0) {
err(1, "Error opening port: %s", uart_name);
}
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
/* Fill the struct for the new configuration */
struct termios uart_config;
tcgetattr(uart, &uart_config);
/* Disable output post-processing */
uart_config.c_oflag &= ~OPOST;
/* Set baud rate */
static const speed_t speed = B9600;
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
return uart;
}
/**
* Print command usage information
*/
static void usage()
{
fprintf(stderr,
"usage: frsky_telemetry start [-d <devicename>]\n"
" frsky_telemetry stop\n"
" frsky_telemetry status\n");
exit(1);
}
/**
* The daemon thread.
*/
static int frsky_telemetry_thread_main(int argc, char *argv[])
{
/* Default values for arguments */
char *device_name = "/dev/ttyS1"; /* USART2 */
/* Work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "d:")) != EOF) {
switch (ch) {
case 'd':
device_name = optarg;
break;
default:
usage();
break;
}
}
/* Print welcome text */
warnx("FrSky telemetry interface starting...");
/* Open UART */
struct termios uart_config_original;
const int uart = frsky_open_uart(device_name, &uart_config_original);
if (uart < 0)
err(1, "could not open %s", device_name);
/* Subscribe to topics */
frsky_init();
thread_running = true;
/* Main thread loop */
unsigned int iteration = 0;
while (!thread_should_exit) {
/* Sleep 200 ms */
usleep(200000);
/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
frsky_send_frame1(uart);
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
if (iteration % 5 == 0)
{
frsky_send_frame2(uart);
}
/* Send frame 3 (every 5000ms): date, time */
if (iteration % 25 == 0)
{
frsky_send_frame3(uart);
iteration = 0;
}
iteration++;
}
/* Reset the UART flags to original state */
tcsetattr(uart, TCSANOW, &uart_config_original);
close(uart);
thread_running = false;
return 0;
}
/**
* The main command function.
* Processes command line arguments and starts the daemon.
*/
int frsky_telemetry_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("missing command");
usage();
}
if (!strcmp(argv[1], "start")) {
/* this is not an error */
if (thread_running)
errx(0, "frsky_telemetry already running");
thread_should_exit = false;
frsky_task = task_spawn_cmd("frsky_telemetry",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
frsky_telemetry_thread_main,
(const char **)argv);
while (!thread_running) {
usleep(200);
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
/* this is not an error */
if (!thread_running)
errx(0, "frsky_telemetry already stopped");
thread_should_exit = true;
while (thread_running) {
usleep(200000);
warnx(".");
}
warnx("terminated.");
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
usage();
/* not getting here */
return 0;
}

41
src/drivers/frsky_telemetry/module.mk Normal file
View File

@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 2013-2014 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
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# FrSky telemetry application.
#
MODULE_COMMAND = frsky_telemetry
SRCS = frsky_data.c \
frsky_telemetry.c

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

@ -77,6 +77,7 @@
*/
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
#define HMC5883L_DEVICE_PATH "/dev/hmc5883"
/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
@ -154,6 +155,7 @@ private:
float _range_scale;
float _range_ga;
bool _collect_phase;
int _class_instance;
orb_advert_t _mag_topic;
@ -315,12 +317,13 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
HMC5883::HMC5883(int bus) :
I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
_measure_ticks(0),
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_mag_topic(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@ -351,6 +354,9 @@ HMC5883::~HMC5883()
if (_reports != nullptr)
delete _reports;
if (_class_instance != -1)
unregister_class_devname(MAG_DEVICE_PATH, _class_instance);
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
@ -374,13 +380,17 @@ HMC5883::init()
/* reset the device configuration */
reset();
/* get a publish handle on the mag topic */
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
_class_instance = register_class_devname(MAG_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the mag topic if we are
* the primary mag */
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
}
ret = OK;
/* sensor is ok, but not calibrated */
@ -875,8 +885,10 @@ HMC5883::collect()
}
#endif
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
}
/* post a report to the ring */
if (_reports->force(&new_report)) {
@ -1256,7 +1268,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(MAG_DEVICE_PATH, O_RDONLY);
fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
@ -1288,10 +1300,10 @@ test()
ssize_t sz;
int ret;
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
@ -1388,10 +1400,10 @@ int calibrate()
{
int ret;
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
warnx("failed to enable sensor calibration mode");
@ -1413,7 +1425,7 @@ int calibrate()
void
reset()
{
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");

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

@ -66,6 +66,8 @@
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#define L3GD20_DEVICE_PATH "/dev/l3gd20"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@ -92,9 +94,17 @@ static const int ERROR = -1;
#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
/* keep lowpass low to avoid noise issues */
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
#define ADDR_CTRL_REG2 0x21
#define ADDR_CTRL_REG3 0x22
@ -191,6 +201,7 @@ private:
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
int _class_instance;
unsigned _current_rate;
unsigned _orientation;
@ -198,6 +209,8 @@ private:
unsigned _read;
perf_counter_t _sample_perf;
perf_counter_t _reschedules;
perf_counter_t _errors;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
@ -218,6 +231,11 @@ private:
*/
void reset();
/**
* disable I2C on the chip
*/
void disable_i2c();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@ -298,16 +316,19 @@ private:
};
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
_call_interval(0),
_reports(nullptr),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_270_DEG),
_read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
@ -333,8 +354,13 @@ L3GD20::~L3GD20()
if (_reports != nullptr)
delete _reports;
if (_class_instance != -1)
unregister_class_devname(GYRO_DEVICE_PATH, _class_instance);
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_reschedules);
perf_free(_errors);
}
int
@ -352,10 +378,13 @@ L3GD20::init()
if (_reports == nullptr)
goto out;
/* advertise sensor topic */
struct gyro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic */
struct gyro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
}
reset();
@ -574,6 +603,7 @@ L3GD20::read_reg(unsigned reg)
uint8_t cmd[2];
cmd[0] = reg | DIR_READ;
cmd[1] = 0;
transfer(cmd, cmd, sizeof(cmd));
@ -653,16 +683,15 @@ L3GD20::set_samplerate(unsigned frequency)
} else if (frequency <= 200) {
_current_rate = 190;
bits |= RATE_190HZ_LP_25HZ;
bits |= RATE_190HZ_LP_50HZ;
} else if (frequency <= 400) {
_current_rate = 380;
bits |= RATE_380HZ_LP_20HZ;
bits |= RATE_380HZ_LP_50HZ;
} else if (frequency <= 800) {
_current_rate = 760;
bits |= RATE_760HZ_LP_30HZ;
bits |= RATE_760HZ_LP_50HZ;
} else {
return -EINVAL;
}
@ -699,13 +728,31 @@ L3GD20::stop()
hrt_cancel(&_call);
}
void
L3GD20::disable_i2c(void)
{
uint8_t retries = 10;
while (retries--) {
// add retries
uint8_t a = read_reg(0x05);
write_reg(0x05, (0x20 | a));
if (read_reg(0x05) == (a | 0x20)) {
return;
}
}
debug("FAILED TO DISABLE I2C");
}
void
L3GD20::reset()
{
// ensure the chip doesn't interpret any other bus traffic as I2C
disable_i2c();
/* set default configuration */
write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
write_reg(ADDR_CTRL_REG4, REG4_BDU);
write_reg(ADDR_CTRL_REG5, 0);
@ -716,7 +763,7 @@ L3GD20::reset()
* callback fast enough to not miss data. */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
set_samplerate(L3GD20_DEFAULT_RATE);
set_samplerate(0); // 760Hz
set_range(L3GD20_DEFAULT_RANGE_DPS);
set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
@ -732,9 +779,26 @@ L3GD20::measure_trampoline(void *arg)
dev->measure();
}
#ifdef GPIO_EXTI_GYRO_DRDY
# define L3GD20_USE_DRDY 1
#else
# define L3GD20_USE_DRDY 0
#endif
void
L3GD20::measure()
{
#if L3GD20_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
return;
}
#endif
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
@ -753,9 +817,20 @@ L3GD20::measure()
perf_begin(_sample_perf);
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
#if L3GD20_USE_DRDY
if ((raw_report.status & 0xF) != 0xF) {
/*
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
return;
}
#endif
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
@ -833,6 +908,8 @@ L3GD20::print_info()
{
printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf);
perf_print_counter(_reschedules);
perf_print_counter(_errors);
_reports->print_info("report queue");
}
@ -883,7 +960,7 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
if (g_dev == nullptr)
goto fail;
@ -892,7 +969,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(GYRO_DEVICE_PATH, O_RDONLY);
fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
@ -900,6 +977,8 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
exit(0);
fail:
@ -924,10 +1003,10 @@ test()
ssize_t sz;
/* get the driver */
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
err(1, "%s open failed", L3GD20_DEVICE_PATH);
/* reset to manual polling */
if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@ -948,6 +1027,8 @@ test()
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
close(fd_gyro);
/* XXX add poll-rate tests here too */
reset();
@ -960,7 +1041,7 @@ test()
void
reset()
{
int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
int fd = open(L3GD20_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -971,6 +1052,8 @@ reset()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "accel pollrate reset failed");
close(fd);
exit(0);
}

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

@ -39,6 +39,7 @@
#include <nuttx/config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
@ -63,6 +64,7 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_tone_alarm.h>
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
@ -78,23 +80,29 @@ static const int ERROR = -1;
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel"
#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag"
/* register addresses: A: accel, M: mag, T: temp */
#define ADDR_WHO_AM_I 0x0F
#define WHO_I_AM 0x49
#define WHO_I_AM 0x49
#define ADDR_OUT_L_T 0x05
#define ADDR_OUT_H_T 0x06
#define ADDR_STATUS_M 0x07
#define ADDR_OUT_X_L_M 0x08
#define ADDR_OUT_X_H_M 0x09
#define ADDR_OUT_Y_L_M 0x0A
#define ADDR_OUT_Y_H_M 0x0B
#define ADDR_OUT_Z_L_M 0x0C
#define ADDR_OUT_Z_H_M 0x0D
#define ADDR_OUT_TEMP_L 0x05
#define ADDR_OUT_TEMP_H 0x06
#define ADDR_STATUS_M 0x07
#define ADDR_OUT_X_L_M 0x08
#define ADDR_OUT_X_H_M 0x09
#define ADDR_OUT_Y_L_M 0x0A
#define ADDR_OUT_Y_H_M 0x0B
#define ADDR_OUT_Z_L_M 0x0C
#define ADDR_OUT_Z_H_M 0x0D
#define ADDR_INT_CTRL_M 0x12
#define ADDR_INT_SRC_M 0x13
#define ADDR_REFERENCE_X 0x1c
#define ADDR_REFERENCE_Y 0x1d
#define ADDR_REFERENCE_Z 0x1e
#define ADDR_OUT_TEMP_A 0x26
#define ADDR_STATUS_A 0x27
#define ADDR_OUT_X_L_A 0x28
#define ADDR_OUT_X_H_A 0x29
@ -112,6 +120,26 @@ static const int ERROR = -1;
#define ADDR_CTRL_REG6 0x25
#define ADDR_CTRL_REG7 0x26
#define ADDR_FIFO_CTRL 0x2e
#define ADDR_FIFO_SRC 0x2f
#define ADDR_IG_CFG1 0x30
#define ADDR_IG_SRC1 0x31
#define ADDR_IG_THS1 0x32
#define ADDR_IG_DUR1 0x33
#define ADDR_IG_CFG2 0x34
#define ADDR_IG_SRC2 0x35
#define ADDR_IG_THS2 0x36
#define ADDR_IG_DUR2 0x37
#define ADDR_CLICK_CFG 0x38
#define ADDR_CLICK_SRC 0x39
#define ADDR_CLICK_THS 0x3a
#define ADDR_TIME_LIMIT 0x3b
#define ADDR_TIME_LATENCY 0x3c
#define ADDR_TIME_WINDOW 0x3d
#define ADDR_ACT_THS 0x3e
#define ADDR_ACT_DUR 0x3f
#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4))
#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4))
#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4))
@ -201,6 +229,21 @@ public:
*/
void print_info();
/**
* dump register values
*/
void print_registers();
/**
* toggle logging
*/
void toggle_logging();
/**
* check for extreme accel values
*/
void check_extremes(const accel_report *arb);
protected:
virtual int probe();
@ -234,7 +277,7 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_advert_t _mag_topic;
int _class_instance;
unsigned _accel_read;
unsigned _mag_read;
@ -243,6 +286,8 @@ private:
perf_counter_t _mag_sample_perf;
perf_counter_t _reg7_resets;
perf_counter_t _reg1_resets;
perf_counter_t _extreme_values;
perf_counter_t _accel_reschedules;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@ -253,6 +298,15 @@ private:
uint8_t _reg7_expected;
uint8_t _reg1_expected;
// accel logging
int _accel_log_fd;
bool _accel_logging_enabled;
uint64_t _last_extreme_us;
uint64_t _last_log_us;
uint64_t _last_log_sync_us;
uint64_t _last_log_reg_us;
uint64_t _last_log_alarm_us;
/**
* Start automatic measurement.
*/
@ -270,6 +324,11 @@ private:
*/
void reset();
/**
* disable I2C on the chip
*/
void disable_i2c();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@ -408,6 +467,8 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class LSM303D;
@ -415,6 +476,9 @@ protected:
private:
LSM303D *_parent;
orb_advert_t _mag_topic;
int _mag_class_instance;
void measure();
void measure_trampoline(void *arg);
@ -422,7 +486,7 @@ private:
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
_mag(new LSM303D_mag(this)),
_call_accel_interval(0),
_call_mag_interval(0),
@ -436,18 +500,26 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(-1),
_mag_topic(-1),
_class_instance(-1),
_accel_read(0),
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
_extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")),
_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_reg1_expected(0),
_reg7_expected(0)
_reg7_expected(0),
_accel_log_fd(-1),
_accel_logging_enabled(false),
_last_log_us(0),
_last_log_sync_us(0),
_last_log_reg_us(0),
_last_log_alarm_us(0)
{
// enable debug() calls
_debug_enabled = true;
@ -479,11 +551,17 @@ LSM303D::~LSM303D()
if (_mag_reports != nullptr)
delete _mag_reports;
if (_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
delete _mag;
/* delete the perf counter */
perf_free(_accel_sample_perf);
perf_free(_mag_sample_perf);
perf_free(_reg1_resets);
perf_free(_reg7_resets);
perf_free(_extreme_values);
}
int
@ -505,10 +583,6 @@ LSM303D::init()
goto out;
/* advertise accel topic */
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
_mag_reports = new RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
@ -516,26 +590,45 @@ LSM303D::init()
reset();
/* advertise mag topic */
struct mag_report zero_mag_report;
memset(&zero_mag_report, 0, sizeof(zero_mag_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
/* do CDev init for the mag device node, keep it optional */
mag_ret = _mag->init();
if (mag_ret != OK) {
_mag_topic = -1;
/* do CDev init for the mag device node */
ret = _mag->init();
if (ret != OK) {
warnx("MAG init failed");
goto out;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
// we are the primary accel device, so advertise to
// the ORB
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
}
ret = OK;
out:
return ret;
}
void
LSM303D::disable_i2c(void)
{
uint8_t a = read_reg(0x02);
write_reg(0x02, (0x10 | a));
a = read_reg(0x02);
write_reg(0x02, (0xF7 & a));
a = read_reg(0x15);
write_reg(0x15, (0x80 | a));
a = read_reg(0x02);
write_reg(0x02, (0xE7 & a));
}
void
LSM303D::reset()
{
// ensure the chip doesn't interpret any other bus traffic as I2C
disable_i2c();
/* enable accel*/
_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
write_reg(ADDR_CTRL_REG1, _reg1_expected);
@ -544,10 +637,17 @@ LSM303D::reset()
_reg7_expected = REG7_CONT_MODE_M;
write_reg(ADDR_CTRL_REG7, _reg7_expected);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
// we setup the anti-alias on-chip filter as 50Hz. We believe
// this operates in the analog domain, and is critical for
// anti-aliasing. The 2 pole software filter is designed to
// operate in conjunction with this on-chip filter
accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
@ -572,6 +672,122 @@ LSM303D::probe()
return -EIO;
}
#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log"
/**
check for extreme accelerometer values and log to a file on the SD card
*/
void
LSM303D::check_extremes(const accel_report *arb)
{
const float extreme_threshold = 30;
static bool boot_ok = false;
bool is_extreme = (fabsf(arb->x) > extreme_threshold &&
fabsf(arb->y) > extreme_threshold &&
fabsf(arb->z) > extreme_threshold);
if (is_extreme) {
perf_count(_extreme_values);
// force accel logging on if we see extreme values
_accel_logging_enabled = true;
} else {
boot_ok = true;
}
if (! _accel_logging_enabled) {
// logging has been disabled by user, close
if (_accel_log_fd != -1) {
::close(_accel_log_fd);
_accel_log_fd = -1;
}
return;
}
if (_accel_log_fd == -1) {
// keep last 10 logs
::unlink(ACCEL_LOGFILE ".9");
for (uint8_t i=8; i>0; i--) {
uint8_t len = strlen(ACCEL_LOGFILE)+3;
char log1[len], log2[len];
snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
::rename(log1, log2);
}
::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");
// open the new logfile
_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
if (_accel_log_fd == -1) {
return;
}
}
uint64_t now = hrt_absolute_time();
// log accels at 1Hz
if (_last_log_us == 0 ||
now - _last_log_us > 1000*1000) {
_last_log_us = now;
::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
(unsigned long long)arb->timestamp,
arb->x, arb->y, arb->z,
(int)arb->x_raw,
(int)arb->y_raw,
(int)arb->z_raw,
(unsigned)boot_ok);
}
const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1,
ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6,
ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M,
ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A,
ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL,
ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2,
ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC,
ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW,
ADDR_ACT_THS, ADDR_ACT_DUR,
ADDR_OUT_X_L_M, ADDR_OUT_X_H_M,
ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
uint8_t regval[sizeof(reglist)];
for (uint8_t i=0; i<sizeof(reglist); i++) {
regval[i] = read_reg(reglist[i]);
}
// log registers at 10Hz when we have extreme values, or 0.5 Hz without
if (_last_log_reg_us == 0 ||
(is_extreme && (now - _last_log_reg_us > 250*1000)) ||
(now - _last_log_reg_us > 10*1000*1000)) {
_last_log_reg_us = now;
::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
for (uint8_t i=0; i<sizeof(reglist); i++) {
::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
}
::dprintf(_accel_log_fd, "\n");
}
// fsync at 0.1Hz
if (now - _last_log_sync_us > 10*1000*1000) {
_last_log_sync_us = now;
::fsync(_accel_log_fd);
}
// play alarm every 10s if we have had an extreme value
if (perf_event_count(_extreme_values) != 0 &&
(now - _last_log_alarm_us > 10*1000*1000)) {
_last_log_alarm_us = now;
int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
if (tfd != -1) {
uint8_t tone = 3;
if (!is_extreme) {
tone = 3;
} else if (boot_ok) {
tone = 4;
} else {
tone = 5;
}
::ioctl(tfd, TONE_SET_ALARM, tone);
::close(tfd);
}
}
}
ssize_t
LSM303D::read(struct file *filp, char *buffer, size_t buflen)
{
@ -590,6 +806,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
*/
while (count--) {
if (_accel_reports->get(arb)) {
check_extremes(arb);
ret += sizeof(*arb);
arb++;
}
@ -952,6 +1169,7 @@ LSM303D::read_reg(unsigned reg)
uint8_t cmd[2];
cmd[0] = reg | DIR_READ;
cmd[1] = 0;
transfer(cmd, cmd, sizeof(cmd));
@ -1224,6 +1442,14 @@ LSM303D::mag_measure_trampoline(void *arg)
void
LSM303D::measure()
{
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
return;
}
if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
perf_count(_reg1_resets);
reset();
@ -1248,6 +1474,7 @@ LSM303D::measure()
perf_begin(_accel_sample_perf);
/* fetch data from the sensor */
memset(&raw_accel_report, 0, sizeof(raw_accel_report));
raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
@ -1290,8 +1517,10 @@ LSM303D::measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
if (_accel_topic != -1) {
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_accel_read++;
@ -1325,6 +1554,7 @@ LSM303D::mag_measure()
perf_begin(_mag_sample_perf);
/* fetch data from the sensor */
memset(&raw_mag_report, 0, sizeof(raw_mag_report));
raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
@ -1361,8 +1591,10 @@ LSM303D::mag_measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
if (_mag->_mag_topic != -1) {
/* publish for subscribers */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
@ -1380,14 +1612,111 @@ LSM303D::print_info()
_mag_reports->print_info("mag reports");
}
void
LSM303D::print_registers()
{
const struct {
uint8_t reg;
const char *name;
} regmap[] = {
{ ADDR_WHO_AM_I, "WHO_AM_I" },
{ 0x02, "I2C_CONTROL1" },
{ 0x15, "I2C_CONTROL2" },
{ ADDR_STATUS_A, "STATUS_A" },
{ ADDR_STATUS_M, "STATUS_M" },
{ ADDR_CTRL_REG0, "CTRL_REG0" },
{ ADDR_CTRL_REG1, "CTRL_REG1" },
{ ADDR_CTRL_REG2, "CTRL_REG2" },
{ ADDR_CTRL_REG3, "CTRL_REG3" },
{ ADDR_CTRL_REG4, "CTRL_REG4" },
{ ADDR_CTRL_REG5, "CTRL_REG5" },
{ ADDR_CTRL_REG6, "CTRL_REG6" },
{ ADDR_CTRL_REG7, "CTRL_REG7" },
{ ADDR_OUT_TEMP_L, "TEMP_L" },
{ ADDR_OUT_TEMP_H, "TEMP_H" },
{ ADDR_INT_CTRL_M, "INT_CTRL_M" },
{ ADDR_INT_SRC_M, "INT_SRC_M" },
{ ADDR_REFERENCE_X, "REFERENCE_X" },
{ ADDR_REFERENCE_Y, "REFERENCE_Y" },
{ ADDR_REFERENCE_Z, "REFERENCE_Z" },
{ ADDR_OUT_X_L_A, "ACCEL_XL" },
{ ADDR_OUT_X_H_A, "ACCEL_XH" },
{ ADDR_OUT_Y_L_A, "ACCEL_YL" },
{ ADDR_OUT_Y_H_A, "ACCEL_YH" },
{ ADDR_OUT_Z_L_A, "ACCEL_ZL" },
{ ADDR_OUT_Z_H_A, "ACCEL_ZH" },
{ ADDR_FIFO_CTRL, "FIFO_CTRL" },
{ ADDR_FIFO_SRC, "FIFO_SRC" },
{ ADDR_IG_CFG1, "IG_CFG1" },
{ ADDR_IG_SRC1, "IG_SRC1" },
{ ADDR_IG_THS1, "IG_THS1" },
{ ADDR_IG_DUR1, "IG_DUR1" },
{ ADDR_IG_CFG2, "IG_CFG2" },
{ ADDR_IG_SRC2, "IG_SRC2" },
{ ADDR_IG_THS2, "IG_THS2" },
{ ADDR_IG_DUR2, "IG_DUR2" },
{ ADDR_CLICK_CFG, "CLICK_CFG" },
{ ADDR_CLICK_SRC, "CLICK_SRC" },
{ ADDR_CLICK_THS, "CLICK_THS" },
{ ADDR_TIME_LIMIT, "TIME_LIMIT" },
{ ADDR_TIME_LATENCY,"TIME_LATENCY" },
{ ADDR_TIME_WINDOW, "TIME_WINDOW" },
{ ADDR_ACT_THS, "ACT_THS" },
{ ADDR_ACT_DUR, "ACT_DUR" }
};
for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) {
printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name);
}
printf("_reg1_expected=0x%02x\n", _reg1_expected);
printf("_reg7_expected=0x%02x\n", _reg7_expected);
}
void
LSM303D::toggle_logging()
{
if (! _accel_logging_enabled) {
_accel_logging_enabled = true;
printf("Started logging to %s\n", ACCEL_LOGFILE);
} else {
_accel_logging_enabled = false;
printf("Stopped logging\n");
}
}
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", MAG_DEVICE_PATH),
_parent(parent)
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
_parent(parent),
_mag_topic(-1),
_mag_class_instance(-1)
{
}
LSM303D_mag::~LSM303D_mag()
{
if (_mag_class_instance != -1)
unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance);
}
int
LSM303D_mag::init()
{
int ret;
ret = CDev::init();
if (ret != OK)
goto out;
_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
if (_mag_class_instance == CLASS_DEVICE_PRIMARY) {
// we are the primary mag device, so advertise to
// the ORB
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
}
out:
return ret;
}
void
@ -1432,6 +1761,8 @@ void start();
void test();
void reset();
void info();
void regdump();
void logging();
/**
* Start the driver.
@ -1445,7 +1776,7 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
if (g_dev == nullptr) {
warnx("failed instantiating LSM303D obj");
@ -1456,7 +1787,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
goto fail;
@ -1464,7 +1795,7 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
/* don't fail if open cannot be opened */
if (0 <= fd_mag) {
@ -1473,6 +1804,8 @@ start()
}
}
close(fd);
close(fd_mag);
exit(0);
fail:
@ -1499,10 +1832,10 @@ test()
int ret;
/* get the driver */
fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd_accel < 0)
err(1, "%s open failed", ACCEL_DEVICE_PATH);
err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL);
/* do a simple demand read */
sz = read(fd_accel, &accel_report, sizeof(accel_report));
@ -1528,10 +1861,10 @@ test()
struct mag_report m_report;
/* get the driver */
fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
if (fd_mag < 0)
err(1, "%s open failed", MAG_DEVICE_PATH);
err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG);
/* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
@ -1554,6 +1887,9 @@ test()
/* XXX add poll-rate tests here too */
close(fd_accel);
close(fd_mag);
reset();
errx(0, "PASS");
}
@ -1564,7 +1900,7 @@ test()
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -1575,7 +1911,9 @@ reset()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "accel pollrate reset failed");
fd = open(MAG_DEVICE_PATH, O_RDONLY);
close(fd);
fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
if (fd < 0) {
warnx("mag could not be opened, external mag might be used");
@ -1585,6 +1923,8 @@ reset()
err(1, "mag pollrate reset failed");
}
close(fd);
exit(0);
}
@ -1603,6 +1943,35 @@ info()
exit(0);
}
/**
* dump registers from device
*/
void
regdump()
{
if (g_dev == nullptr)
errx(1, "driver not running\n");
printf("regdump @ %p\n", g_dev);
g_dev->print_registers();
exit(0);
}
/**
* toggle logging
*/
void
logging()
{
if (g_dev == nullptr)
errx(1, "driver not running\n");
g_dev->toggle_logging();
exit(0);
}
} // namespace
@ -1634,5 +2003,17 @@ lsm303d_main(int argc, char *argv[])
if (!strcmp(argv[1], "info"))
lsm303d::info();
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
/*
* dump device registers
*/
if (!strcmp(argv[1], "regdump"))
lsm303d::regdump();
/*
* dump device registers
*/
if (!strcmp(argv[1], "logging"))
lsm303d::logging();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
}

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

@ -198,7 +198,9 @@ MEASAirspeed::collect()
// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset);
float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
if (diff_press_pa < 0.0f)
diff_press_pa = 0.0f;
struct differential_pressure_s report;

198
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Author: Marco Bauer <marco@wtns.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -35,7 +36,8 @@
* @file mkblctrl.cpp
*
* Driver/configurator for the Mikrokopter BL-Ctrl.
* Marco Bauer
*
* @author Marco Bauer <marco@wtns.de>
*
*/
@ -73,7 +75,6 @@
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h>
@ -89,8 +90,8 @@
#define BLCTRL_MIN_VALUE -0.920F
#define MOTOR_STATE_PRESENT_MASK 0x80
#define MOTOR_STATE_ERROR_MASK 0x7F
#define MOTOR_SPINUP_COUNTER 2500
#define ESC_UORB_PUBLISH_DELAY 200
#define MOTOR_SPINUP_COUNTER 30
#define ESC_UORB_PUBLISH_DELAY 500000
class MK : public device::I2C
{
@ -112,7 +113,7 @@ public:
FRAME_X,
};
MK(int bus);
MK(int bus, const char *_device_path);
~MK();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
@ -126,7 +127,7 @@ public:
int set_overrideSecurityChecks(bool overrideSecurityChecks);
int set_px4mode(int px4mode);
int set_frametype(int frametype);
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput);
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
private:
static const unsigned _max_actuators = MAX_MOTORS;
@ -141,9 +142,9 @@ private:
unsigned int _motor;
int _px4mode;
int _frametype;
char _device[20]; ///< device
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
orb_advert_t _t_esc_status;
unsigned int _num_outputs;
@ -244,7 +245,7 @@ MK *g_mk;
} // namespace
MK::MK(int bus) :
MK::MK(int bus, const char *_device_path) :
I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
_mode(MODE_NONE),
_update_rate(50),
@ -252,7 +253,6 @@ MK::MK(int bus) :
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_t_esc_status(0),
_num_outputs(0),
_motortest(false),
@ -265,6 +265,10 @@ MK::MK(int bus) :
_armed(false),
_mixers(nullptr)
{
strncpy(_device, _device_path, sizeof(_device));
/* enforce null termination */
_device[sizeof(_device) - 1] = '\0';
_debug_enabled = true;
}
@ -291,7 +295,7 @@ MK::~MK()
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
unregister_driver(_device);
g_mk = nullptr;
}
@ -313,13 +317,15 @@ MK::init(unsigned motors)
usleep(500000);
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (sizeof(_device) > 0) {
ret = register_driver(_device, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
if (ret == OK) {
log("creating alternate output device");
_primary_pwm_device = true;
}
}
/* reset GPIOs */
gpio_reset();
@ -525,13 +531,6 @@ MK::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
/* advertise the blctrl status */
esc_status_s esc;
memset(&esc, 0, sizeof(esc));
@ -595,9 +594,6 @@ MK::task_main()
outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
outputs.timestamp = hrt_absolute_time();
// XXX output actual limited values
memcpy(&controls_effective, &_controls, sizeof(controls_effective));
/* iterate actuators */
for (unsigned int i = 0; i < _num_outputs; i++) {
@ -633,10 +629,7 @@ MK::task_main()
}
/* output to BLCtrl's */
if (_motortest == true) {
mk_servo_test(i);
} else {
if (_motortest != true) {
//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
// 11 Bit
Motor[i].SetPoint_PX4 = outputs.output[i];
@ -668,7 +661,7 @@ MK::task_main()
* Only update esc topic every half second.
*/
if (hrt_absolute_time() - esc.timestamp > 500000) {
if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) {
esc.counter++;
esc.timestamp = hrt_absolute_time();
esc.esc_count = (uint8_t) _num_outputs;
@ -692,16 +685,22 @@ MK::task_main()
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
esc.esc[i].esc_errorcount = (uint16_t) 0;
// if motortest is requested - do it...
if (_motortest == true) {
mk_servo_test(i);
}
}
orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
}
}
//::close(_t_esc_status);
::close(_t_esc_status);
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_actuator_armed);
@ -727,8 +726,12 @@ MK::mk_servo_arm(bool status)
unsigned int
MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
{
if(initI2C) {
I2C::init();
}
_retries = 50;
uint8_t foundMotorCount = 0;
@ -952,6 +955,7 @@ MK::mk_servo_test(unsigned int chan)
if (_motor >= _num_outputs) {
_motor = -1;
_motortest = false;
fprintf(stderr, "[mkblctrl] Motortest finished...\n");
}
}
@ -1367,7 +1371,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
/* count used motors */
do {
if (g_mk->mk_check_for_blctrl(8, false) != 0) {
if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
shouldStop = 4;
} else {
@ -1377,7 +1381,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
sleep(1);
} while (shouldStop < 3);
g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
/* (re)set the PWM output mode */
g_mk->set_mode(servo_mode);
@ -1390,13 +1394,13 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
}
int
mk_start(unsigned bus, unsigned motors)
mk_start(unsigned bus, unsigned motors, char *device_path)
{
int ret = OK;
if (g_mk == nullptr) {
g_mk = new MK(bus);
g_mk = new MK(bus, device_path);
if (g_mk == nullptr) {
ret = -ENOMEM;
@ -1415,6 +1419,52 @@ mk_start(unsigned bus, unsigned motors)
}
int
mk_check_for_i2c_esc_bus(char *device_path, int motors)
{
int ret;
if (g_mk == nullptr) {
g_mk = new MK(3, device_path);
if (g_mk == nullptr) {
return -1;
} else {
ret = g_mk->mk_check_for_blctrl(8, false, true);
delete g_mk;
g_mk = nullptr;
if (ret > 0) {
return 3;
}
}
g_mk = new MK(1, device_path);
if (g_mk == nullptr) {
return -1;
} else {
ret = g_mk->mk_check_for_blctrl(8, false, true);
delete g_mk;
g_mk = nullptr;
if (ret > 0) {
return 1;
}
}
}
return -1;
}
} // namespace
extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
@ -1425,13 +1475,14 @@ mkblctrl_main(int argc, char *argv[])
PortMode port_mode = PORT_FULL_PWM;
int pwm_update_rate_in_hz = UPDATE_RATE;
int motorcount = 8;
int bus = 1;
int bus = -1;
int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default
bool motortest = false;
bool overrideSecurityChecks = false;
bool showHelp = false;
bool newMode = false;
char *devicepath = "";
/*
* optional parameters
@ -1491,36 +1542,69 @@ mkblctrl_main(int argc, char *argv[])
newMode = true;
}
/* look for the optional device parameter */
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (argc > i + 1) {
devicepath = argv[i + 1];
newMode = true;
} else {
errx(1, "missing the devicename (-d)");
return 1;
}
}
}
if (showHelp) {
fprintf(stderr, "mkblctrl: help:\n");
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n");
fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n");
fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n");
fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n");
fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n");
fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
fprintf(stderr, "\n");
fprintf(stderr, "Motortest:\n");
fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
fprintf(stderr, "mkblctrl -t\n");
fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
exit(1);
}
if (g_mk == nullptr) {
if (mk_start(bus, motorcount) != OK) {
errx(1, "failed to start the MK-BLCtrl driver");
if (!motortest) {
if (g_mk == nullptr) {
if (bus == -1) {
bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
}
} else {
newMode = true;
}
}
if (bus != -1) {
if (mk_start(bus, motorcount, devicepath) != OK) {
errx(1, "failed to start the MK-BLCtrl driver");
}
} else {
errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)");
}
/* parameter set ? */
if (newMode) {
/* switch parameter */
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
}
/* parameter set ? */
if (newMode) {
/* switch parameter */
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
}
exit(0);
} else {
errx(1, "MK-BLCtrl driver already running");
}
/* test, etc. here g*/
} else {
if (g_mk == nullptr) {
errx(1, "MK-BLCtrl driver not running. You have to start it first.");
exit(1);
} else {
g_mk->set_motor_test(motortest);
exit(0);
}
}
}

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

@ -75,6 +75,9 @@
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel"
#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro"
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
@ -161,6 +164,14 @@
#define MPU6000_ONE_G 9.80665f
/*
the MPU6000 can only handle high SPI bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
SPI speed
*/
#define MPU6000_LOW_BUS_SPEED 1000*1000
#define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
class MPU6000_gyro;
class MPU6000 : public device::SPI
@ -200,17 +211,19 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _accel_class_instance;
RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
unsigned _reads;
unsigned _sample_rate;
perf_counter_t _accel_reads;
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@ -338,12 +351,17 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class MPU6000;
void parent_poll_notify();
private:
MPU6000 *_parent;
orb_advert_t _gyro_topic;
int _gyro_class_instance;
};
@ -351,7 +369,7 @@ private:
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
MPU6000::MPU6000(int bus, spi_dev_e device) :
SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000),
SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_gyro(new MPU6000_gyro(this)),
_product(0),
_call_interval(0),
@ -359,13 +377,15 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_reads(0),
_sample_rate(1000),
_accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")),
_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@ -409,8 +429,14 @@ MPU6000::~MPU6000()
if (_gyro_reports != nullptr)
delete _gyro_reports;
if (_accel_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_accel_reads);
perf_free(_gyro_reads);
perf_free(_bad_transfers);
}
int
@ -455,24 +481,23 @@ MPU6000::init()
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
gyro_ret = _gyro->init();
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("gyro init failed");
return ret;
}
/* fetch an initial set of measurements for advertisement */
measure();
if (gyro_ret != OK) {
_gyro_topic = -1;
} else {
gyro_report gr;
_gyro_reports->get(&gr);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
/* advertise accel topic */
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise accel topic */
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
}
out:
return ret;
@ -480,17 +505,26 @@ out:
void MPU6000::reset()
{
// if the mpu6000 is initialised after the l3gd20 and lsm303d
// then if we don't do an irqsave/irqrestore here the mpu6000
// frequenctly comes up in a bad state where all transfers
// come as zero
irqstate_t state;
state = irqsave();
// Chip reset
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
// Wake up device and select GyroZ clock (better performance)
// Wake up device and select GyroZ clock. Note that the
// MPU6000 starts up in sleep mode, and it can take some time
// for it to come out of sleep
write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Disable I2C bus (recommended on datasheet)
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
up_udelay(1000);
// SAMPLE RATE
@ -652,6 +686,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
if (_accel_reports->empty())
return -EAGAIN;
perf_count(_accel_reads);
/* copy reports out of our buffer to the caller */
accel_report *arp = reinterpret_cast<accel_report *>(buffer);
int transferred = 0;
@ -669,12 +705,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
int
MPU6000::self_test()
{
if (_reads == 0) {
if (perf_event_count(_sample_perf) == 0) {
measure();
}
/* return 0 on success, 1 else */
return (_reads > 0) ? 0 : 1;
return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
}
int
@ -746,6 +782,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
if (_gyro_reports->empty())
return -EAGAIN;
perf_count(_gyro_reads);
/* copy reports out of our buffer to the caller */
gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
int transferred = 0;
@ -987,9 +1025,10 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
uint8_t
MPU6000::read_reg(unsigned reg)
{
uint8_t cmd[2];
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
cmd[0] = reg | DIR_READ;
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
transfer(cmd, cmd, sizeof(cmd));
@ -999,9 +1038,10 @@ MPU6000::read_reg(unsigned reg)
uint16_t
MPU6000::read_reg16(unsigned reg)
{
uint8_t cmd[3];
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
cmd[0] = reg | DIR_READ;
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
transfer(cmd, cmd, sizeof(cmd));
@ -1016,6 +1056,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value)
cmd[0] = reg | DIR_WRITE;
cmd[1] = value;
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
transfer(cmd, nullptr, sizeof(cmd));
}
@ -1139,12 +1182,13 @@ MPU6000::measure()
* Fetch the full set of measurements from the MPU6000 in one pass.
*/
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
// sensor transfer at high clock speed
set_frequency(MPU6000_HIGH_BUS_SPEED);
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
return;
/* count measurement */
_reads++;
/*
* Convert from big to little endian
*/
@ -1159,6 +1203,20 @@ MPU6000::measure()
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
return;
}
/*
* Swap axes and negate y
*/
@ -1249,10 +1307,11 @@ MPU6000::measure()
poll_notify(POLLIN);
_gyro->parent_poll_notify();
/* and publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
if (_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
if (_accel_topic != -1) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (_gyro->_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
@ -1263,19 +1322,48 @@ void
MPU6000::print_info()
{
perf_print_counter(_sample_perf);
printf("reads: %u\n", _reads);
perf_print_counter(_accel_reads);
perf_print_counter(_gyro_reads);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
}
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
_parent(parent)
CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
_parent(parent),
_gyro_class_instance(-1)
{
}
MPU6000_gyro::~MPU6000_gyro()
{
if (_gyro_class_instance != -1)
unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance);
}
int
MPU6000_gyro::init()
{
int ret;
// do base class init
ret = CDev::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("gyro init failed");
return ret;
}
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
gyro_report gr;
memset(&gr, 0, sizeof(gr));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
out:
return ret;
}
void
@ -1331,7 +1419,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
goto fail;
@ -1339,6 +1427,8 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
exit(0);
fail:
@ -1363,17 +1453,17 @@ test()
ssize_t sz;
/* get the driver */
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
ACCEL_DEVICE_PATH);
MPU_DEVICE_PATH_ACCEL);
/* get the driver */
int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@ -1431,7 +1521,7 @@ test()
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -1442,6 +1532,8 @@ reset()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
close(fd);
exit(0);
}

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

@ -124,6 +124,8 @@ protected:
int32_t _TEMP;
int64_t _OFF;
int64_t _SENS;
float _P;
float _T;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in kPa */
@ -420,8 +422,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
/*
* Since we are initialized, we do not need to do anything, since the
* PROM is correctly read and the part does not need to be configured.
*/
return OK;
case BAROIOCSMSLPRESSURE:
@ -620,6 +625,8 @@ MS5611::collect()
/* pressure calculation, result in Pa */
int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
_P = P * 0.01f;
_T = _TEMP * 0.01f;
/* generate a new report */
report.temperature = _TEMP / 100.0f;
@ -692,6 +699,8 @@ MS5611::print_info()
printf("TEMP: %d\n", _TEMP);
printf("SENS: %lld\n", _SENS);
printf("OFF: %lld\n", _OFF);
printf("P: %.3f\n", _P);
printf("T: %.3f\n", _T);
printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f));
printf("factory_setup %u\n", _prom.factory_setup);

24
src/drivers/ms5611/ms5611_spi.cpp
View File

@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf)
}
MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000),
SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
_prom(prom_buf)
{
}
@ -134,7 +134,6 @@ int
MS5611_SPI::init()
{
int ret;
irqstate_t flags;
ret = SPI::init();
if (ret != OK) {
@ -167,10 +166,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count)
uint8_t b[4];
uint32_t w;
} *cvt = (_cvt *)data;
uint8_t buf[4];
uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 };
/* read the most recent measurement */
buf[0] = 0 | DIR_WRITE;
int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
if (ret == OK) {
@ -238,21 +236,31 @@ MS5611_SPI::_read_prom()
usleep(3000);
/* read and convert PROM words */
bool all_zero = true;
for (int i = 0; i < 8; i++) {
uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
_prom.c[i] = _reg16(cmd);
if (_prom.c[i] != 0)
all_zero = false;
//debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
}
/* calculate CRC and return success/failure accordingly */
return ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
if (ret != OK) {
debug("crc failed");
}
if (all_zero) {
debug("prom all zero");
ret = -EIO;
}
return ret;
}
uint16_t
MS5611_SPI::_reg16(unsigned reg)
{
uint8_t cmd[3];
cmd[0] = reg | DIR_READ;
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
_transfer(cmd, cmd, sizeof(cmd));

500
src/drivers/px4fmu/fmu.cpp
View File

@ -69,7 +69,6 @@
#include <drivers/drv_rc_input.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
@ -123,7 +122,6 @@ private:
int _t_actuators;
int _t_actuator_armed;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
bool _primary_pwm_device;
@ -164,6 +162,7 @@ private:
static const unsigned _ngpio;
void gpio_reset(void);
void sensor_reset(int ms);
void gpio_set_function(uint32_t gpios, int function);
void gpio_write(uint32_t gpios, int function);
uint32_t gpio_read(void);
@ -219,17 +218,16 @@ PX4FMU::PX4FMU() :
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
_pwm_on(false),
_mixers(nullptr),
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
_failsafe_pwm( {0}),
_disarmed_pwm( {0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
@ -293,11 +291,11 @@ PX4FMU::init()
/* start the IO interface task */
_task = task_spawn_cmd("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
@ -326,7 +324,7 @@ PX4FMU::set_mode(Mode mode)
switch (mode) {
case MODE_2PWM: // v1 multi-port with flow control lines as PWM
debug("MODE_2PWM");
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
@ -340,7 +338,7 @@ PX4FMU::set_mode(Mode mode)
case MODE_4PWM: // v1 multi-port as 4 PWM outs
debug("MODE_4PWM");
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
@ -354,7 +352,7 @@ PX4FMU::set_mode(Mode mode)
case MODE_6PWM: // v2 PWMs as 6 PWM outs
debug("MODE_6PWM");
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
@ -396,6 +394,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
// get the channel mask for this rate group
uint32_t mask = up_pwm_servo_get_rate_group(group);
if (mask == 0)
continue;
@ -409,6 +408,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
// not a legal map, bail
return -EINVAL;
}
} else {
// set it - errors here are unexpected
if (alt != 0) {
@ -416,6 +416,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
warn("rate group set alt failed");
return -EINVAL;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
warn("rate group set default failed");
@ -425,6 +426,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
}
}
}
_pwm_alt_rate_channels = rate_map;
_pwm_default_rate = default_rate;
_pwm_alt_rate = alt_rate;
@ -466,13 +468,6 @@ PX4FMU::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
@ -503,6 +498,7 @@ PX4FMU::task_main()
* We always mix at max rate; some channels may update slower.
*/
unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
@ -511,6 +507,7 @@ PX4FMU::task_main()
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
}
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
@ -532,6 +529,7 @@ PX4FMU::task_main()
log("poll error %d", errno);
usleep(1000000);
continue;
} else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe");
@ -542,7 +540,7 @@ PX4FMU::task_main()
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
@ -553,12 +551,15 @@ PX4FMU::task_main()
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
default:
num_outputs = 0;
break;
@ -572,9 +573,9 @@ PX4FMU::task_main()
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i >= outputs.noutputs ||
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
@ -588,12 +589,6 @@ PX4FMU::task_main()
pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output actual limited values */
for (unsigned i = 0; i < num_outputs; i++) {
controls_effective.control_effective[i] = (float)pwm_limited[i];
}
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
@ -613,11 +608,13 @@ PX4FMU::task_main()
/* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown;
if (_armed != set_armed)
_armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (aa.armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
@ -626,31 +623,36 @@ PX4FMU::task_main()
}
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i=0; i<rc_in.channel_count; i++) {
for (uint8_t i = 0; i < rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i];
}
rc_in.timestamp = ppm_last_valid_decode;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
} else {
} else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
#endif
}
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_actuator_armed);
/* make sure servos are off */
@ -753,142 +755,176 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break;
}
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i];
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN;
} else {
_disarmed_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_disarmed_pwm[i] > 0)
_num_disarmed_set++;
}
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN;
} else {
_disarmed_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_disarmed_pwm[i] > 0)
_num_disarmed_set++;
}
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _disarmed_pwm[i];
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _disarmed_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
} else {
_min_pwm[i] = pwm->values[i];
}
}
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
} else {
_min_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _min_pwm[i];
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _min_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX;
} else {
_max_pwm[i] = pwm->values[i];
}
}
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX;
} else {
_max_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _max_pwm[i];
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _max_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
@ -910,6 +946,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET(0):
if (arg <= 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
ret = -EINVAL;
}
@ -946,18 +983,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) {
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
case MODE_4PWM:
*(unsigned *)arg = 4;
break;
case MODE_2PWM:
*(unsigned *)arg = 2;
break;
default:
ret = -EINVAL;
break;
@ -1015,6 +1055,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
@ -1049,9 +1090,80 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
for (uint8_t i = 0; i < count; i++) {
up_pwm_servo_set(i, values[i]);
}
return count * 2;
}
void
PX4FMU::sensor_reset(int ms)
{
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
if (ms < 1) {
ms = 1;
}
/* disable SPI bus */
stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
stm32_configgpio(GPIO_SPI1_SCK_OFF);
stm32_configgpio(GPIO_SPI1_MISO_OFF);
stm32_configgpio(GPIO_SPI1_MOSI_OFF);
stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
stm32_configgpio(GPIO_GYRO_DRDY_OFF);
stm32_configgpio(GPIO_MAG_DRDY_OFF);
stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
/* set the sensor rail off */
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
/* wait for the sensor rail to reach GND */
usleep(ms * 1000);
warnx("reset done, %d ms", ms);
/* re-enable power */
/* switch the sensor rail back on */
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
/* wait a bit before starting SPI, different times didn't influence results */
usleep(100);
/* reconfigure the SPI pins */
#ifdef CONFIG_STM32_SPI1
stm32_configgpio(GPIO_SPI_CS_GYRO);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
stm32_configgpio(GPIO_SPI_CS_BARO);
stm32_configgpio(GPIO_SPI_CS_MPU);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
#endif
#endif
}
void
PX4FMU::gpio_reset(void)
{
@ -1062,6 +1174,7 @@ PX4FMU::gpio_reset(void)
for (unsigned i = 0; i < _ngpio; i++) {
if (_gpio_tab[i].input != 0) {
stm32_configgpio(_gpio_tab[i].input);
} else if (_gpio_tab[i].output != 0) {
stm32_configgpio(_gpio_tab[i].output);
}
@ -1078,6 +1191,7 @@ void
PX4FMU::gpio_set_function(uint32_t gpios, int function)
{
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/*
* GPIOs 0 and 1 must have the same direction as they are buffered
* by a shared 2-port driver. Any attempt to set either sets both.
@ -1089,6 +1203,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
if (GPIO_SET_OUTPUT == function)
stm32_gpiowrite(GPIO_GPIO_DIR, 1);
}
#endif
/* configure selected GPIOs as required */
@ -1113,9 +1228,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
}
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/* flip buffer to input mode if required */
if ((GPIO_SET_INPUT == function) && (gpios & 3))
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
#endif
}
@ -1154,6 +1271,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
gpio_reset();
break;
case GPIO_SENSOR_RAIL_RESET:
sensor_reset(arg);
break;
case GPIO_SET_OUTPUT:
case GPIO_SET_INPUT:
case GPIO_SET_ALT_1:
@ -1227,8 +1348,9 @@ fmu_new_mode(PortMode new_mode)
#endif
break;
/* mixed modes supported on v1 board only */
/* mixed modes supported on v1 board only */
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
case PORT_FULL_SERIAL:
/* set all multi-GPIOs to serial mode */
gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
@ -1251,6 +1373,7 @@ fmu_new_mode(PortMode new_mode)
servo_mode = PX4FMU::MODE_2PWM;
break;
#endif
default:
return -1;
}
@ -1304,15 +1427,31 @@ fmu_stop(void)
return ret;
}
void
sensor_reset(int ms)
{
int fd;
int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0)
errx(1, "open fail");
if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
err(1, "servo arm failed");
}
void
test(void)
{
int fd;
unsigned servo_count = 0;
unsigned servo_count = 0;
unsigned pwm_value = 1000;
int direction = 1;
int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0)
@ -1320,9 +1459,9 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
err(1, "Unable to get servo count\n");
}
if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
err(1, "Unable to get servo count\n");
}
warnx("Testing %u servos", (unsigned)servo_count);
@ -1335,32 +1474,38 @@ test(void)
for (;;) {
/* sweep all servos between 1000..2000 */
servo_position_t servos[servo_count];
for (unsigned i = 0; i < servo_count; i++)
servos[i] = pwm_value;
if (direction == 1) {
// use ioctl interface for one direction
for (unsigned i=0; i < servo_count; i++) {
if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
err(1, "servo %u set failed", i);
}
}
} else {
// and use write interface for the other direction
ret = write(fd, servos, sizeof(servos));
if (ret != (int)sizeof(servos))
err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
}
if (direction == 1) {
// use ioctl interface for one direction
for (unsigned i = 0; i < servo_count; i++) {
if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
err(1, "servo %u set failed", i);
}
}
} else {
// and use write interface for the other direction
ret = write(fd, servos, sizeof(servos));
if (ret != (int)sizeof(servos))
err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
}
if (direction > 0) {
if (pwm_value < 2000) {
pwm_value++;
} else {
direction = -1;
}
} else {
if (pwm_value > 1000) {
pwm_value--;
} else {
direction = 1;
}
@ -1372,6 +1517,7 @@ test(void)
if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
err(1, "error reading PWM servo %d", i);
if (value != servos[i])
errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
}
@ -1379,12 +1525,14 @@ test(void)
/* Check if user wants to quit */
char c;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
break;
break;
}
}
}
@ -1457,6 +1605,7 @@ fmu_main(int argc, char *argv[])
new_mode = PORT_FULL_PWM;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
} else if (!strcmp(verb, "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
@ -1489,11 +1638,24 @@ fmu_main(int argc, char *argv[])
if (!strcmp(verb, "fake"))
fake(argc - 1, argv + 1);
if (!strcmp(verb, "sensor_reset")) {
if (argc > 2) {
int reset_time = strtol(argv[2], 0, 0);
sensor_reset(reset_time);
} else {
sensor_reset(0);
warnx("resettet default time");
}
exit(0);
}
fprintf(stderr, "FMU: unrecognised command, try:\n");
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
fprintf(stderr, " mode_gpio, mode_pwm, test\n");
fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
#endif
exit(1);
}

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

@ -54,6 +54,7 @@
#include <unistd.h>
#include <fcntl.h>
#include <math.h>
#include <crc32.h>
#include <arch/board/board.h>
@ -72,7 +73,6 @@
#include <systemlib/param/param.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
@ -95,6 +95,8 @@ extern device::Device *PX4IO_serial_interface() weak_function;
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2)
#define PX4IO_CHECK_CRC _IOC(0xff00, 3)
#define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz
#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz
@ -226,30 +228,36 @@ private:
device::Device *_interface;
// XXX
unsigned _hardware; ///< Hardware revision
unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO
unsigned _max_controls; ///<Maximum # of controls supported by PX4IO
unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO
unsigned _max_relays; ///<Maximum relays supported by PX4IO
unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO
unsigned _hardware; ///< Hardware revision
unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO
unsigned _max_controls; ///< Maximum # of controls supported by PX4IO
unsigned _max_rc_input; ///< Maximum receiver channels supported by PX4IO
unsigned _max_relays; ///< Maximum relays supported by PX4IO
unsigned _max_transfer; ///< Maximum number of I2C transfers supported by PX4IO
unsigned _update_interval; ///< Subscription interval limiting send rate
bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels
volatile int _task; ///<worker task id
volatile bool _task_should_exit; ///<worker terminate flag
volatile int _task; ///< worker task id
volatile bool _task_should_exit; ///< worker terminate flag
int _mavlink_fd; ///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
int _thread_mavlink_fd; ///<mavlink file descriptor for thread.
int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
int _thread_mavlink_fd; ///< mavlink file descriptor for thread.
perf_counter_t _perf_update; ///<local performance counter
perf_counter_t _perf_update; ///<local performance counter for status updates
perf_counter_t _perf_write; ///<local performance counter for PWM control writes
perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes
/* cached IO state */
uint16_t _status; ///<Various IO status flags
uint16_t _alarms; ///<Various IO alarms
uint16_t _status; ///< Various IO status flags
uint16_t _alarms; ///< Various IO alarms
/* subscribed topics */
int _t_actuators; ///< actuator controls topic
int _t_actuator_controls_0; ///< actuator controls group 0 topic
int _t_actuator_controls_1; ///< actuator controls group 1 topic
int _t_actuator_controls_2; ///< actuator controls group 2 topic
int _t_actuator_controls_3; ///< actuator controls group 3 topic
int _t_actuator_armed; ///< system armed control topic
int _t_vehicle_control_mode;///< vehicle control mode topic
int _t_param; ///< parameter update topic
@ -257,24 +265,22 @@ private:
/* advertised topics */
orb_advert_t _to_input_rc; ///< rc inputs from io
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
orb_advert_t _to_outputs; ///< mixed servo outputs topic
orb_advert_t _to_battery; ///< battery status / voltage
orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///<mixed outputs
actuator_controls_effective_s _controls_effective; ///<effective controls
bool _primary_pwm_device; ///<true if we are the default PWM output
bool _primary_pwm_device; ///< true if we are the default PWM output
float _battery_amp_per_volt; ///<current sensor amps/volt
float _battery_amp_bias; ///<current sensor bias
float _battery_mamphour_total;///<amp hours consumed so far
uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp
float _battery_amp_per_volt; ///< current sensor amps/volt
float _battery_amp_bias; ///< current sensor bias
float _battery_mamphour_total;///< amp hours consumed so far
uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power
#endif
/**
@ -288,9 +294,14 @@ private:
void task_main();
/**
* Send controls to IO
* Send controls for one group to IO
*/
int io_set_control_state();
int io_set_control_state(unsigned group);
/**
* Send all controls to IO
*/
int io_set_control_groups();
/**
* Update IO's arming-related state
@ -327,11 +338,6 @@ private:
*/
int io_publish_raw_rc();
/**
* Fetch and publish the mixed control values.
*/
int io_publish_mixed_controls();
/**
* Fetch and publish the PWM servo outputs.
*/
@ -459,20 +465,25 @@ PX4IO::PX4IO(device::Device *interface) :
_max_transfer(16), /* sensible default */
_update_interval(0),
_rc_handling_disabled(false),
_rc_chan_count(0),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
_thread_mavlink_fd(-1),
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
_perf_update(perf_alloc(PC_ELAPSED, "io update")),
_perf_write(perf_alloc(PC_ELAPSED, "io write")),
_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
_status(0),
_alarms(0),
_t_actuators(-1),
_t_actuator_controls_0(-1),
_t_actuator_controls_1(-1),
_t_actuator_controls_2(-1),
_t_actuator_controls_3(-1),
_t_actuator_armed(-1),
_t_vehicle_control_mode(-1),
_t_param(-1),
_t_vehicle_command(-1),
_to_input_rc(0),
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_to_servorail(0),
@ -769,15 +780,20 @@ PX4IO::task_main()
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
orb_set_interval(_t_actuators, 20); /* default to 50Hz */
_t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0));
orb_set_interval(_t_actuator_controls_0, 20); /* default to 50Hz */
_t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1));
orb_set_interval(_t_actuator_controls_1, 33); /* default to 30Hz */
_t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2));
orb_set_interval(_t_actuator_controls_2, 33); /* default to 30Hz */
_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
orb_set_interval(_t_actuator_controls_3, 33); /* default to 30Hz */
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
_t_param = orb_subscribe(ORB_ID(parameter_update));
_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
if ((_t_actuators < 0) ||
if ((_t_actuator_controls_0 < 0) ||
(_t_actuator_armed < 0) ||
(_t_vehicle_control_mode < 0) ||
(_t_param < 0) ||
@ -788,7 +804,7 @@ PX4IO::task_main()
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _t_actuators;
fds[0].fd = _t_actuator_controls_0;
fds[0].events = POLLIN;
log("ready");
@ -807,7 +823,11 @@ PX4IO::task_main()
if (_update_interval > 100)
_update_interval = 100;
orb_set_interval(_t_actuators, _update_interval);
orb_set_interval(_t_actuator_controls_0, _update_interval);
/*
* NOT changing the rate of groups 1-3 here, because only attitude
* really needs to run fast.
*/
_update_interval = 0;
}
@ -827,7 +847,10 @@ PX4IO::task_main()
/* if we have new control data from the ORB, handle it */
if (fds[0].revents & POLLIN) {
io_set_control_state();
/* we're not nice to the lower-priority control groups and only check them
when the primary group updated (which is now). */
(void)io_set_control_groups();
}
if (now >= poll_last + IO_POLL_INTERVAL) {
@ -840,8 +863,7 @@ PX4IO::task_main()
/* get raw R/C input from IO */
io_publish_raw_rc();
/* fetch mixed servo controls and PWM outputs from IO */
io_publish_mixed_controls();
/* fetch PWM outputs from IO */
io_publish_pwm_outputs();
}
@ -871,7 +893,7 @@ PX4IO::task_main()
orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
// Check for a DSM pairing command
if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) {
if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
dsm_bind_ioctl((int)cmd.param2);
}
}
@ -901,7 +923,23 @@ PX4IO::task_main()
/* re-upload RC input config as it may have changed */
io_set_rc_config();
/* re-set the battery scaling */
int32_t voltage_scaling_val;
param_t voltage_scaling_param;
/* set battery voltage scaling */
param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val);
/* send scaling voltage to IO */
uint16_t scaling = voltage_scaling_val;
int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
if (pret != OK) {
log("voltage scaling upload failed");
}
}
}
perf_end(_perf_update);
@ -922,20 +960,74 @@ out:
}
int
PX4IO::io_set_control_state()
PX4IO::io_set_control_groups()
{
int ret = io_set_control_state(0);
/* send auxiliary control groups */
(void)io_set_control_state(1);
(void)io_set_control_state(2);
(void)io_set_control_state(3);
return ret;
}
int
PX4IO::io_set_control_state(unsigned group)
{
actuator_controls_s controls; ///< actuator outputs
uint16_t regs[_max_actuators];
/* get controls */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &controls);
bool changed;
switch (group) {
case 0:
{
orb_check(_t_actuator_controls_0, &changed);
if (changed) {
orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
}
}
break;
case 1:
{
orb_check(_t_actuator_controls_1, &changed);
if (changed) {
orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls);
}
}
break;
case 2:
{
orb_check(_t_actuator_controls_2, &changed);
if (changed) {
orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls);
}
}
break;
case 3:
{
orb_check(_t_actuator_controls_3, &changed);
if (changed) {
orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls);
}
}
break;
}
if (!changed)
return -1;
for (unsigned i = 0; i < _max_controls; i++)
regs[i] = FLOAT_TO_REG(controls.control[i]);
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
}
@ -1003,8 +1095,10 @@ PX4IO::io_set_rc_config()
* assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical
* controls.
*/
/* fill the mapping with an error condition triggering value */
for (unsigned i = 0; i < _max_rc_input; i++)
input_map[i] = -1;
input_map[i] = UINT8_MAX;
/*
* NOTE: The indices for mapped channels are 1-based
@ -1031,11 +1125,10 @@ PX4IO::io_set_rc_config()
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 3;
ichan = 4;
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
for (unsigned i = 0; i < _max_rc_input; i++)
if (input_map[i] == -1)
input_map[i] = ichan++;
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan - 1] = 4;
/*
* Iterate all possible RC inputs.
@ -1309,6 +1402,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*/
channel_count = regs[0];
if (channel_count != _rc_chan_count)
perf_count(_perf_chan_count);
_rc_chan_count = channel_count;
if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
@ -1363,50 +1461,6 @@ PX4IO::io_publish_raw_rc()
return OK;
}
int
PX4IO::io_publish_mixed_controls()
{
/* if no FMU comms(!) just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
return OK;
/* if not taking raw PPM from us, must be mixing */
if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM)
return OK;
/* data we are going to fetch */
actuator_controls_effective_s controls_effective;
controls_effective.timestamp = hrt_absolute_time();
/* get actuator controls from IO */
uint16_t act[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
if (ret != OK)
return ret;
/* convert from register format to float */
for (unsigned i = 0; i < _max_actuators; i++)
controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
/* laxily advertise on first publication */
if (_to_actuators_effective == 0) {
_to_actuators_effective =
orb_advertise((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
&controls_effective);
} else {
orb_publish((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
_to_actuators_effective, &controls_effective);
}
return OK;
}
int
PX4IO::io_publish_pwm_outputs()
{
@ -1659,11 +1713,13 @@ void
PX4IO::print_status()
{
/* basic configuration */
printf("protocol %u hardware %u bootloader %u buffer %uB\n",
printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC+1));
printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
@ -1741,6 +1797,16 @@ PX4IO::print_status()
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
printf("\n");
if (raw_inputs > 0) {
int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
printf("RC data (PPM frame len) %u us\n", frame_len);
if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
printf("WARNING WARNING WARNING! This RC receiver does not allow safe frame detection.\n");
}
}
uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID);
printf("mapped R/C inputs 0x%04x", mapped_inputs);
@ -2146,6 +2212,29 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
break;
case PX4IO_REBOOT_BOOTLOADER:
if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
return -EINVAL;
/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
// we don't expect a reply from this operation
ret = OK;
break;
case PX4IO_CHECK_CRC: {
/* check IO firmware CRC against passed value */
uint32_t io_crc = 0;
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
if (ret != OK)
return ret;
if (io_crc != arg) {
debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg);
return -EINVAL;
}
break;
}
case PX4IO_INAIR_RESTART_ENABLE:
/* set/clear the 'in-air restart' bit */
@ -2176,7 +2265,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
count = _max_actuators;
if (count > 0) {
perf_begin(_perf_write);
int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
perf_end(_perf_write);
if (ret != OK)
return ret;
@ -2255,7 +2347,7 @@ void
start(int argc, char *argv[])
{
if (g_dev != nullptr)
errx(1, "already loaded");
errx(0, "already loaded");
/* allocate the interface */
device::Device *interface = get_interface();
@ -2530,7 +2622,7 @@ px4io_main(int argc, char *argv[])
}
PX4IO_Uploader *up;
const char *fn[3];
const char *fn[4];
/* work out what we're uploading... */
if (argc > 2) {
@ -2588,6 +2680,39 @@ px4io_main(int argc, char *argv[])
if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
}
if (!strcmp(argv[1], "forceupdate")) {
/*
force update of the IO firmware without requiring
the user to hold the safety switch down
*/
if (argc <= 3) {
warnx("usage: px4io forceupdate MAGIC filename");
exit(1);
}
if (g_dev == nullptr) {
warnx("px4io is not started, still attempting upgrade");
} else {
uint16_t arg = atol(argv[2]);
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
if (ret != OK) {
printf("reboot failed - %d\n", ret);
exit(1);
}
// tear down the px4io instance
delete g_dev;
}
// upload the specified firmware
const char *fn[2];
fn[0] = argv[3];
fn[1] = nullptr;
PX4IO_Uploader *up = new PX4IO_Uploader;
up->upload(&fn[0]);
delete up;
exit(0);
}
/* commands below here require a started driver */
if (g_dev == nullptr)
@ -2673,6 +2798,49 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "checkcrc")) {
/*
check IO CRC against CRC of a file
*/
if (argc <= 2) {
printf("usage: px4io checkcrc filename\n");
exit(1);
}
if (g_dev == nullptr) {
printf("px4io is not started\n");
exit(1);
}
int fd = open(argv[2], O_RDONLY);
if (fd == -1) {
printf("open of %s failed - %d\n", argv[2], errno);
exit(1);
}
const uint32_t app_size_max = 0xf000;
uint32_t fw_crc = 0;
uint32_t nbytes = 0;
while (true) {
uint8_t buf[16];
int n = read(fd, buf, sizeof(buf));
if (n <= 0) break;
fw_crc = crc32part(buf, n, fw_crc);
nbytes += n;
}
close(fd);
while (nbytes < app_size_max) {
uint8_t b = 0xff;
fw_crc = crc32part(&b, 1, fw_crc);
nbytes++;
}
int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
if (ret != OK) {
printf("check CRC failed - %d\n", ret);
exit(1);
}
printf("CRCs match\n");
exit(0);
}
if (!strcmp(argv[1], "rx_dsm") ||
!strcmp(argv[1], "rx_dsm_10bit") ||
!strcmp(argv[1], "rx_dsm_11bit") ||
@ -2690,5 +2858,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv);
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
}

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

@ -274,7 +274,10 @@ PX4IO_Uploader::drain()
int ret;
do {
ret = recv(c, 1000);
// the small recv timeout here is to allow for fast
// drain when rebooting the io board for a forced
// update of the fw without using the safety switch
ret = recv(c, 40);
#ifdef UDEBUG
if (ret == OK) {

162
src/drivers/stm32/drv_hrt.c
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
@ -168,7 +168,7 @@
# error HRT_TIMER_CLOCK must be greater than 1MHz
#endif
/*
/**
* Minimum/maximum deadlines.
*
* These are suitable for use with a 16-bit timer/counter clocked
@ -276,12 +276,16 @@ static void hrt_call_invoke(void);
* Specific registers and bits used by PPM sub-functions
*/
#ifdef HRT_PPM_CHANNEL
/*
/*
* If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
*
* Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
*/
# ifdef CONFIG_ARCH_CORTEXM3
# undef GTIM_CCER_CC1NP
# undef GTIM_CCER_CC2NP
# undef GTIM_CCER_CC3NP
# undef GTIM_CCER_CC4NP
# define GTIM_CCER_CC1NP 0
# define GTIM_CCER_CC2NP 0
# define GTIM_CCER_CC3NP 0
@ -332,14 +336,21 @@ static void hrt_call_invoke(void);
/*
* PPM decoder tuning parameters
*/
# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */
# define PPM_MIN_START 2500 /* shortest valid start gap */
# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */
# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */
# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */
/* decoded PPM buffer */
#define PPM_MAX_CHANNELS 12
#define PPM_MIN_CHANNELS 5
#define PPM_MAX_CHANNELS 20
/** Number of same-sized frames required to 'lock' */
#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
__EXPORT uint16_t ppm_frame_length = 0;
__EXPORT unsigned ppm_decoded_channels = 0;
__EXPORT uint64_t ppm_last_valid_decode = 0;
@ -353,11 +364,12 @@ unsigned ppm_pulse_next;
static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
/* PPM decoder state machine */
/** PPM decoder state machine */
struct {
uint16_t last_edge; /* last capture time */
uint16_t last_mark; /* last significant edge */
unsigned next_channel;
uint16_t last_edge; /**< last capture time */
uint16_t last_mark; /**< last significant edge */
uint16_t frame_start; /**< the frame width */
unsigned next_channel; /**< next channel index */
enum {
UNSYNCH = 0,
ARM,
@ -379,7 +391,7 @@ static void hrt_ppm_decode(uint32_t status);
# define CCER_PPM 0
#endif /* HRT_PPM_CHANNEL */
/*
/**
* Initialise the timer we are going to use.
*
* We expect that we'll own one of the reduced-function STM32 general
@ -425,7 +437,7 @@ hrt_tim_init(void)
}
#ifdef HRT_PPM_CHANNEL
/*
/**
* Handle the PPM decoder state machine.
*/
static void
@ -440,9 +452,8 @@ hrt_ppm_decode(uint32_t status)
if (status & SR_OVF_PPM)
goto error;
/* how long since the last edge? */
/* how long since the last edge? - this handles counter wrapping implicitely. */
width = count - ppm.last_edge;
ppm.last_edge = count;
ppm_edge_history[ppm_edge_next++] = width;
@ -455,14 +466,39 @@ hrt_ppm_decode(uint32_t status)
*/
if (width >= PPM_MIN_START) {
/* export the last set of samples if we got something sensible */
if (ppm.next_channel > 4) {
for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
ppm_buffer[i] = ppm_temp_buffer[i];
/*
* If the number of channels changes unexpectedly, we don't want
* to just immediately jump on the new count as it may be a result
* of noise or dropped edges. Instead, take a few frames to settle.
*/
if (ppm.next_channel != ppm_decoded_channels) {
static unsigned new_channel_count;
static unsigned new_channel_holdoff;
ppm_decoded_channels = i;
ppm_last_valid_decode = hrt_absolute_time();
if (new_channel_count != ppm.next_channel) {
/* start the lock counter for the new channel count */
new_channel_count = ppm.next_channel;
new_channel_holdoff = PPM_CHANNEL_LOCK;
} else if (new_channel_holdoff > 0) {
/* this frame matched the last one, decrement the lock counter */
new_channel_holdoff--;
} else {
/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
ppm_decoded_channels = new_channel_count;
new_channel_count = 0;
}
} else {
/* frame channel count matches expected, let's use it */
if (ppm.next_channel > PPM_MIN_CHANNELS) {
for (i = 0; i < ppm.next_channel; i++)
ppm_buffer[i] = ppm_temp_buffer[i];
ppm_last_valid_decode = hrt_absolute_time();
}
}
/* reset for the next frame */
@ -471,29 +507,39 @@ hrt_ppm_decode(uint32_t status)
/* next edge is the reference for the first channel */
ppm.phase = ARM;
ppm.last_edge = count;
return;
}
switch (ppm.phase) {
case UNSYNCH:
/* we are waiting for a start pulse - nothing useful to do here */
return;
break;
case ARM:
/* we expect a pulse giving us the first mark */
if (width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too long */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* record the mark timing, expect an inactive edge */
ppm.last_mark = count;
ppm.phase = INACTIVE;
return;
ppm.last_mark = ppm.last_edge;
/* frame length is everything including the start gap */
ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
ppm.frame_start = ppm.last_edge;
ppm.phase = ACTIVE;
break;
case INACTIVE:
/* we expect a short pulse */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* this edge is not interesting, but now we are ready for the next mark */
ppm.phase = ACTIVE;
return;
break;
case ACTIVE:
/* determine the interval from the last mark */
@ -514,10 +560,13 @@ hrt_ppm_decode(uint32_t status)
ppm_temp_buffer[ppm.next_channel++] = interval;
ppm.phase = INACTIVE;
return;
break;
}
ppm.last_edge = count;
return;
/* the state machine is corrupted; reset it */
error:
@ -528,7 +577,7 @@ error:
}
#endif /* HRT_PPM_CHANNEL */
/*
/**
* Handle the compare interupt by calling the callout dispatcher
* and then re-scheduling the next deadline.
*/
@ -557,6 +606,7 @@ hrt_tim_isr(int irq, void *context)
hrt_ppm_decode(status);
}
#endif
/* was this a timer tick? */
@ -575,7 +625,7 @@ hrt_tim_isr(int irq, void *context)
return OK;
}
/*
/**
* Fetch a never-wrapping absolute time value in microseconds from
* some arbitrary epoch shortly after system start.
*/
@ -622,7 +672,7 @@ hrt_absolute_time(void)
return abstime;
}
/*
/**
* Convert a timespec to absolute time
*/
hrt_abstime
@ -636,7 +686,7 @@ ts_to_abstime(struct timespec *ts)
return result;
}
/*
/**
* Convert absolute time to a timespec.
*/
void
@ -647,7 +697,7 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
ts->tv_nsec = abstime * 1000;
}
/*
/**
* Compare a time value with the current time.
*/
hrt_abstime
@ -662,7 +712,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then)
return delta;
}
/*
/**
* Store the absolute time in an interrupt-safe fashion
*/
hrt_abstime
@ -677,7 +727,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now)
return ts;
}
/*
/**
* Initalise the high-resolution timing module.
*/
void
@ -692,7 +742,7 @@ hrt_init(void)
#endif
}
/*
/**
* Call callout(arg) after interval has elapsed.
*/
void
@ -705,7 +755,7 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v
arg);
}
/*
/**
* Call callout(arg) at calltime.
*/
void
@ -714,7 +764,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v
hrt_call_internal(entry, calltime, 0, callout, arg);
}
/*
/**
* Call callout(arg) every period.
*/
void
@ -733,6 +783,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
irqstate_t flags = irqsave();
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
@ -746,7 +803,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
irqrestore(flags);
}
/*
/**
* If this returns true, the call has been invoked and removed from the callout list.
*
* Always returns false for repeating callouts.
@ -757,7 +814,7 @@ hrt_called(struct hrt_call *entry)
return (entry->deadline == 0);
}
/*
/**
* Remove the entry from the callout list.
*/
void
@ -839,13 +896,19 @@ hrt_call_invoke(void)
/* if the callout has a non-zero period, it has to be re-entered */
if (call->period != 0) {
call->deadline = deadline + call->period;
// re-check call->deadline to allow for
// callouts to re-schedule themselves
// using hrt_call_delay()
if (call->deadline <= now) {
call->deadline = deadline + call->period;
}
hrt_call_enter(call);
}
}
}
/*
/**
* Reschedule the next timer interrupt.
*
* This routine must be called with interrupts disabled.
@ -906,5 +969,16 @@ hrt_latency_update(void)
latency_counters[index]++;
}
void
hrt_call_init(struct hrt_call *entry)
{
memset(entry, 0, sizeof(*entry));
}
void
hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
{
entry->deadline = hrt_absolute_time() + delay;
}
#endif /* HRT_TIMER */

10
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) < 0.01f) {
if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
_throttle_dem = constrain(_throttle_dem,
@ -404,10 +404,18 @@ void TECS::_update_pitch(void)
// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
if (_climbOutDem)
{
temp += _PITCHminf * gainInv;
}
_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integ7_state) / gainInv;

8
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
View File

@ -46,6 +46,10 @@ namespace math
void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
if (_cutoff_freq <= 0.0f) {
// no filtering
return;
}
float fr = sample_freq/_cutoff_freq;
float ohm = tanf(M_PI_F/fr);
float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
float LowPassFilter2p::apply(float sample)
{
if (_cutoff_freq <= 0.0f) {
// no filtering
return sample;
}
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {

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

@ -41,6 +41,7 @@
#include "KalmanNav.hpp"
#include <systemlib/err.h>
#include <geo/geo.h>
// constants
// Titterton pg. 52
@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// attitude representations
C_nb(),
q(),
_accel_sub(-1),
_gyro_sub(-1),
_mag_sub(-1),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
lat0(0), lon0(0), alt0(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
lon = 0.0f;
alt = 0.0f;
// gyro, accel and mag subscriptions
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
struct accel_report accel;
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel);
struct mag_report mag;
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag);
// initialize rotation quaternion with a single raw sensor measurement
q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z);
_sensors.update();
q = init(
_sensors.accelerometer_m_s2[0],
_sensors.accelerometer_m_s2[1],
_sensors.accelerometer_m_s2[2],
_sensors.magnetometer_ga[0],
_sensors.magnetometer_ga[1],
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = Dcm(q);
@ -252,11 +248,21 @@ void KalmanNav::update()
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// set reference position for
// local position
lat0 = lat;
lon0 = lon;
alt0 = alt;
// XXX map_projection has internal global
// states that multiple things could change,
// should make map_projection take reference
// lat/lon and not have init
map_projection_init(lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS\n");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
lat, lon, alt);
lat, lon, double(alt));
}
// prediction step
@ -311,7 +317,7 @@ void KalmanNav::updatePublications()
{
using namespace math;
// position publication
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.valid = true;
@ -324,6 +330,31 @@ void KalmanNav::updatePublications()
_pos.vz = vD;
_pos.yaw = psi;
// local position publication
float x;
float y;
bool landed = alt < (alt0 + 0.1); // XXX improve?
map_projection_project(lat, lon, &x, &y);
_localPos.timestamp = _pubTimeStamp;
_localPos.xy_valid = true;
_localPos.z_valid = true;
_localPos.v_xy_valid = true;
_localPos.v_z_valid = true;
_localPos.x = x;
_localPos.y = y;
_localPos.z = alt0 - alt;
_localPos.vx = vN;
_localPos.vy = vE;
_localPos.vz = vD;
_localPos.yaw = psi;
_localPos.xy_global = true;
_localPos.z_global = true;
_localPos.ref_timestamp = _pubTimeStamp;
_localPos.ref_lat = getLatDegE7();
_localPos.ref_lon = getLonDegE7();
_localPos.ref_alt = 0;
_localPos.landed = landed;
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
@ -347,8 +378,10 @@ void KalmanNav::updatePublications()
// selectively update publications,
// do NOT call superblock do-all method
if (_positionInitialized)
if (_positionInitialized) {
_pos.update();
_localPos.update();
}
if (_attitudeInitialized)
_att.update();

11
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
View File

@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
@ -142,12 +143,8 @@ protected:
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
int _accel_sub; /**< Accelerometer subscription */
int _gyro_sub; /**< Gyroscope subscription */
int _mag_sub; /**< Magnetometer subscription */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
@ -164,8 +161,10 @@ protected:
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon; /**< lat, lon radians */
float alt; /**< altitude, meters */
// parameters
float alt; /**< altitude, meters */
double lat0, lon0; /**< reference latitude and longitude */
float alt0; /**< refeerence altitude (ground height) */
control::BlockParamFloat _vGyro; /**< gyro process noise */
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
control::BlockParamFloat _rMag; /**< magnetometer measurement noise */

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

@ -52,11 +52,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
/* gyro measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f);
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
/* accel measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
/* mag measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/* offset estimation - UNUSED */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
@ -72,10 +74,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->q3 = param_find("EKF_ATT_V3_Q3");
h->q4 = param_find("EKF_ATT_V3_Q4");
h->r0 = param_find("EKF_ATT_V3_R0");
h->r1 = param_find("EKF_ATT_V3_R1");
h->r2 = param_find("EKF_ATT_V3_R2");
h->r3 = param_find("EKF_ATT_V3_R3");
h->r0 = param_find("EKF_ATT_V4_R0");
h->r1 = param_find("EKF_ATT_V4_R1");
h->r2 = param_find("EKF_ATT_V4_R2");
h->r3 = param_find("EKF_ATT_V4_R3");
h->roll_off = param_find("ATT_ROLL_OFF3");
h->pitch_off = param_find("ATT_PITCH_OFF3");

3
src/modules/attitude_estimator_so3/README Normal file
View File

@ -0,0 +1,3 @@
Synopsis
nsh> attitude_estimator_so3_comp start

Some files were not shown because too many files have changed in this diff Show More