fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-15 01:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into frsky_telemetry

Browse Source
This commit is contained in:
Stefan Rado
2013-12-28 21:25:45 +01:00
parent 5f18ce506d 31f808d872
commit 810f33c3f3
120 changed files with 7708 additions and 2456 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/102_3dr_skywalker
+89
View File
@@ -0,0 +1,89 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
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 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
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_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
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
set EXIT_ON_END no
#
# 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
pwm disarmed -c 3 -p 1056
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
then
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_AETR.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
if [ $EXIT_ON_END == yes ]
then
exit
fi
ROMFS/px4fmu_common/init.d/12-13_hex
+1 -1
View File
@@ -78,7 +78,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix
#
# 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
ROMFS/px4fmu_common/init.d/31_io_phantom
+19 -14
View File
@@ -8,30 +8,35 @@ echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV"
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 50
param set FW_P_LIM_MIN -50
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 1.1
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 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
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
param set FW_THR_MIN 0.5
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 RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.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
ROMFS/px4fmu_common/init.d/33_io_wingwing
+91
View File
@@ -0,0 +1,91 @@
#!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
set EXIT_ON_END no
#
# 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
if [ $EXIT_ON_END == yes ]
then
exit
fi
ROMFS/px4fmu_common/init.d/34_io_fx79
+91
View File
@@ -0,0 +1,91 @@
#!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
set EXIT_ON_END no
#
# 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
if [ $EXIT_ON_END == yes ]
then
exit
fi
ROMFS/px4fmu_common/init.d/800_sdlogger
+60
View File
@@ -0,0 +1,60 @@
#!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
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
commander start
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
if [ $EXIT_ON_END == yes ]
then
exit
fi
ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
+2 -2
View File
@@ -97,9 +97,9 @@ 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
#
ROMFS/px4fmu_common/init.d/rc.sensors
+11 -5
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
ROMFS/px4fmu_common/init.d/rcS
Executable → Regular
+63 -14
View File
@@ -8,6 +8,8 @@
#
set MODE autostart
set logfile /fs/microsd/bootlog.txt
#
# Try to mount the microSD card.
#
@@ -147,26 +149,49 @@ 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
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 200000
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
sh /etc/init.d/rc.error
fi
else
echo "No PX4IO to upgrade here"
echo "PX4IO update failed"
echo "PX4IO update failed" >> $logfile
tone_alarm MNGGG
fi
fi
@@ -296,6 +321,18 @@ then
set MODE custom
fi
if param compare SYS_AUTOSTART 33
then
sh /etc/init.d/33_io_wingwing
set MODE custom
fi
if param compare SYS_AUTOSTART 34
then
sh /etc/init.d/34_io_fx79
set MODE custom
fi
if param compare SYS_AUTOSTART 40
then
sh /etc/init.d/40_io_segway
@@ -313,6 +350,18 @@ then
sh /etc/init.d/101_hk_bixler
set MODE custom
fi
if param compare SYS_AUTOSTART 102
then
sh /etc/init.d/102_3dr_skywalker
set MODE custom
fi
if param compare SYS_AUTOSTART 800
then
sh /etc/init.d/800_sdlogger
set MODE custom
fi
# Start any custom extensions that might be missing
if [ -f /fs/microsd/etc/rc.local ]
ROMFS/px4fmu_common/mixers/FMU_FX79.mix
Executable
+72
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
ROMFS/px4fmu_logging/init.d/rcS
+88
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
ROMFS/px4fmu_logging/logging/conv.zip
BIN
View File
Binary file not shown.
ROMFS/px4fmu_test/init.d/rcS
Executable → Regular
+8
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
Tools/px_uploader.py
+11 -8
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
makefiles/config_px4fmu-v1_backside.mk
+2 -1
View File
@@ -69,12 +69,13 @@ 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/att_pos_estimator_ekf
#MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
MODULES += modules/attitude_estimator_so3
#
# Vehicle Control
makefiles/config_px4fmu-v1_default.mk
+2 -1
View File
@@ -70,9 +70,10 @@ 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
makefiles/config_px4fmu-v2_default.mk
+3 -1
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
@@ -69,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
makefiles/config_px4fmu-v2_logging.mk
+157
View File
@@ -0,0 +1,157 @@
#
# Makefile for the px4fmu_default configuration
#
#
# Use the configuration's ROMFS, copy the px4iov2 firmware into
# the ROMFS if it's available
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_logging
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_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/px4fmu
MODULES += drivers/px4io
MODULES += drivers/boards/px4fmu-v2
MODULES += drivers/rgbled
MODULES += drivers/mpu6000
MODULES += drivers/lsm303d
MODULES += drivers/l3gd20
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += modules/sensors
# Needs to be burned to the ground and re-written; for now,
# just don't build it.
#MODULES += drivers/mkblctrl
#
# System commands
#
MODULES += systemcmds/ramtron
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
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
#
# 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
#
# 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
#
# 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 )
mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
+19 -4
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h
+221
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
}
mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h
+375
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
}
mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
+227 -117
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
mavlink/include/mavlink/v1.0/ardupilotmega/version.h
+1 -1
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
mavlink/include/mavlink/v1.0/autoquad/autoquad.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/autoquad/testsuite.h
+20 -20
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));
mavlink/include/mavlink/v1.0/autoquad/version.h
+1 -1
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
mavlink/include/mavlink/v1.0/common/common.h
+42 -4
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h
+99 -55
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);
mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h
+5 -5
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)
{
mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h
+5 -5
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)
{
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h
+237
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
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h
+265
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
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h
+199
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
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h
+265
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
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h
+199
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
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h
+243
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
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h
+25 -25
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)
{
mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h
+375
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
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h
+15 -15
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)
{
mavlink/include/mavlink/v1.0/common/testsuite.h
+871 -509
View File
File diff suppressed because it is too large Load Diff
mavlink/include/mavlink/v1.0/common/version.h
+1 -1
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
mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/matrixpilot/testsuite.h
+141 -141
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));
mavlink/include/mavlink/v1.0/matrixpilot/version.h
+1 -1
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
mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/pixhawk/testsuite.h
+114 -114
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));
mavlink/include/mavlink/v1.0/pixhawk/version.h
+1 -1
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
nuttx-configs/px4fmu-v2/nsh/defconfig
+4 -3
View File
@@ -299,15 +299,16 @@ CONFIG_STM32_USART=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_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
src/drivers/airspeed/airspeed.h
+1 -1
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;
src/drivers/ardrone_interface/ardrone_motor_control.c
-23
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) */
src/drivers/boards/px4fmu-v2/board_config.h
+24 -4
View File
@@ -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
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+4 -3
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");
src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+18
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:
src/drivers/device/cdev.cpp
+36
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()
{
src/drivers/device/device.h
+22
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 */
src/drivers/device/spi.cpp
+6
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
src/drivers/device/spi.h
+11
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.
*/
src/drivers/drv_gpio.h
+3 -1
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 */
src/drivers/drv_hrt.h
+14
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.
*/
src/drivers/drv_rc_input.h
+5 -2
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
@@ -60,7 +60,7 @@
/**
* Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
*/
#define RC_INPUT_MAX_CHANNELS 18
#define RC_INPUT_MAX_CHANNELS 20
/**
* Input signal type, value is a control position from zero to 100
@@ -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;
src/drivers/hmc5883/hmc5883.cpp
+27 -15
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 ");
src/drivers/l3gd20/l3gd20.cpp
+100 -17
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);
}
src/drivers/lsm303d/lsm303d.cpp
+428 -47
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'");
}
src/drivers/mkblctrl/mkblctrl.cpp
+141 -57
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);
}
}
}
src/drivers/mpu6000/mpu6000.cpp
+135 -43
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);
}
src/drivers/ms5611/ms5611.cpp
+5 -2
View File
@@ -420,8 +420,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:
src/drivers/ms5611/ms5611_spi.cpp
+16 -8
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));
src/drivers/px4fmu/fmu.cpp
+331 -169
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);
}
src/drivers/px4io/px4io.cpp
+268 -100
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();
@@ -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'");
}
src/drivers/px4io/px4io_uploader.cpp
+4 -1
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) {
src/drivers/stm32/drv_hrt.c
+118 -44
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 */
src/lib/external_lgpl/tecs/tecs.cpp
+1 -1
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,
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+8
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)) {
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+11 -9
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");
src/modules/attitude_estimator_so3/README
+3
View File
@@ -0,0 +1,3 @@
Synopsis
nsh> attitude_estimator_so3_comp start
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp → src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+171 -328
View File
@@ -1,16 +1,49 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
/****************************************************************************
*
* @file attitude_estimator_so3_comp_main.c
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* 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 attitude_estimator_so3_main.cpp
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
@@ -46,94 +79,91 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "attitude_estimator_so3_comp_params.h"
#include "attitude_estimator_so3_params.h"
#ifdef __cplusplus
}
#endif
extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]);
extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */
static int attitude_estimator_so3_task; /**< Handle of deamon task / thread */
//! Auxiliary variables to reduce number of repeated operations
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
static bool bFilterInit = false;
//! Auxiliary variables to reduce number of repeated operations
static float q0q0, q0q1, q0q2, q0q3;
static float q1q1, q1q2, q1q3;
static float q2q2, q2q3;
static float q3q3;
//! Serial packet related
static int uart;
static int baudrate;
static bool bFilterInit = false;
/**
* Mainloop of attitude_estimator_so3_comp.
* Mainloop of attitude_estimator_so3.
*/
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]);
int attitude_estimator_so3_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Function prototypes */
float invSqrt(float number);
void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz);
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n"
"-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n"
"ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n");
fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n");
exit(1);
}
/**
* The attitude_estimator_so3_comp app only briefly exists to start
* The attitude_estimator_so3 app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int attitude_estimator_so3_comp_main(int argc, char *argv[])
int attitude_estimator_so3_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("attitude_estimator_so3_comp already running\n");
warnx("already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp",
attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12400,
attitude_estimator_so3_comp_thread_main,
(const char **)argv);
14000,
attitude_estimator_so3_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
while(thread_running){
while (thread_running){
usleep(200000);
printf(".");
}
printf("terminated.");
warnx("stopped");
exit(0);
}
@@ -157,7 +187,8 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float number) {
float invSqrt(float number)
{
volatile long i;
volatile float x, y;
volatile const float f = 1.5F;
@@ -221,48 +252,47 @@ void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, floa
q3q3 = q3 * q3;
}
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) {
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt)
{
float recipNorm;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
//! Make filter converge to initial solution faster
//! This function assumes you are in static position.
//! WARNING : in case air reboot, this can cause problem. But this is very
//! unlikely happen.
if(bFilterInit == false)
{
// Make filter converge to initial solution faster
// This function assumes you are in static position.
// WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
if(bFilterInit == false) {
NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
bFilterInit = true;
}
//! If magnetometer measurement is available, use it.
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
float hx, hy, hz, bx, bz;
float halfwx, halfwy, halfwz;
// Normalise magnetometer measurement
// Will sqrt work better? PX4 system is powerful enough?
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2);
bx = sqrt(hx * hx + hy * hy);
bz = hz;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
bx = sqrt(hx * hx + hy * hy);
bz = hz;
// Estimated direction of magnetic field
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Estimated direction of magnetic field
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (my * halfwz - mz * halfwy);
halfey += (mz * halfwx - mx * halfwz);
halfez += (mx * halfwy - my * halfwx);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (my * halfwz - mz * halfwy);
halfey += (mz * halfwx - mx * halfwz);
halfez += (mx * halfwy - my * halfwx);
}
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
@@ -293,7 +323,9 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
gyro_bias[1] += twoKi * halfey * dt;
gyro_bias[2] += twoKi * halfez * dt;
gx += gyro_bias[0]; // apply integral feedback
// apply integral feedback
gx += gyro_bias[0];
gy += gyro_bias[1];
gz += gyro_bias[2];
}
@@ -337,208 +369,43 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
q3 *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
void send_uart_byte(char c)
{
write(uart,&c,1);
}
void send_uart_bytes(uint8_t *data, int length)
{
write(uart,data,(size_t)(sizeof(uint8_t)*length));
}
void send_uart_float(float f) {
uint8_t * b = (uint8_t *) &f;
//! Assume float is 4-bytes
for(int i=0; i<4; i++) {
uint8_t b1 = (b[i] >> 4) & 0x0f;
uint8_t b2 = (b[i] & 0x0f);
uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
send_uart_bytes(&c1,1);
send_uart_bytes(&c2,1);
}
}
void send_uart_float_arr(float *arr, int length)
{
for(int i=0;i<length;++i)
{
send_uart_float(arr[i]);
send_uart_byte(',');
}
}
int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
int speed;
switch (baud) {
case 0: speed = B0; break;
case 50: speed = B50; break;
case 75: speed = B75; break;
case 110: speed = B110; break;
case 134: speed = B134; break;
case 150: speed = B150; break;
case 200: speed = B200; break;
case 300: speed = B300; break;
case 600: speed = B600; break;
case 1200: speed = B1200; break;
case 1800: speed = B1800; break;
case 2400: speed = B2400; break;
case 4800: speed = B4800; break;
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
case 460800: speed = B460800; break;
case 921600: speed = B921600; break;
default:
printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
return -EINVAL;
}
printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
*is_usb = false;
/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
printf("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) {
printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
} else {
*is_usb = true;
}
return uart;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
/*
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
*/
/*
* EKF Attitude Estimator main function.
* Nonliner complementary filter on SO(3), attitude estimator main function.
*
* Estimates the attitude recursively once started.
* Estimates the attitude once started.
*
* @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments
*/
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
int attitude_estimator_so3_thread_main(int argc, char *argv[])
{
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Serial debug related
int ch;
struct termios uart_config_original;
bool usb_uart;
bool debug_mode = false;
char *device_name = "/dev/ttyS2";
baudrate = 115200;
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Time constant
float dt = 0.005f;
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
/* Initialization */
float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */
float acc[3] = {0.0f, 0.0f, 0.0f};
float gyro[3] = {0.0f, 0.0f, 0.0f};
float mag[3] = {0.0f, 0.0f, 0.0f};
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
//! -d <device_name>, default : /dev/ttyS2
//! -b <baud_rate>, default : 115200
while ((ch = getopt(argc,argv,"d:b:")) != EOF){
switch(ch){
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if(baudrate == 0)
printf("invalid baud rate '%s'",optarg);
break;
case 'd':
device_name = optarg;
debug_mode = true;
break;
default:
usage("invalid argument");
}
}
if(debug_mode){
printf("Opening debugging port for 3D visualization\n");
uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
printf("could not open %s", device_name);
else
printf("Open port success\n");
}
// print text
printf("Nonlinear SO3 Attitude Estimator initialized..\n\n");
fflush(stdout);
int overloadcounter = 19;
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
warnx("main thread started");
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
@@ -555,8 +422,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */
orb_set_interval(sub_raw, 4);
/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
orb_set_interval(sub_raw, 3);
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
@@ -565,17 +432,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
//orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
//orb_advert_t att_pub = -1;
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
/* advertise debug value */
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
@@ -583,20 +448,22 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_so3_comp_params so3_comp_params;
struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles;
struct attitude_estimator_so3_params so3_comp_params;
struct attitude_estimator_so3_param_handles so3_comp_param_handles;
/* initialize parameter handles */
parameters_init(&so3_comp_param_handles);
parameters_update(&so3_comp_param_handles, &so3_comp_params);
uint64_t start_time = hrt_absolute_time();
bool initialized = false;
bool state_initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0;
/* register the perf counter */
perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp");
perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3");
/* Main loop*/
while (!thread_should_exit) {
@@ -615,12 +482,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr,
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
warnx("WARNING: Not getting sensors - sensor app running?");
}
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
@@ -644,11 +508,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
gyro_offsets[2] += raw.gyro_rad_s[2];
offset_count++;
if (hrt_absolute_time() - start_time > 3000000LL) {
if (hrt_absolute_time() > start_time + 3000000l) {
initialized = true;
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
}
} else {
@@ -668,9 +533,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
sensor_last_timestamp[0] = raw.timestamp;
}
gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
@@ -696,31 +561,14 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
mag[1] = raw.magnetometer_ga[1];
mag[2] = raw.magnetometer_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
// if (overloadcounter == 20) {
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
// overloadcounter = 0;
// }
overloadcounter++;
}
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
dt = 0.005f;
parameters_update(&so3_comp_param_handles, &so3_comp_params);
const_initialized = true;
if (!state_initialized && dt < 0.05f && dt > 0.001f) {
state_initialized = true;
warnx("state initialized");
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
if (!state_initialized) {
continue;
}
@@ -728,18 +576,23 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
-acc[0], -acc[1], -acc[2],
mag[0], mag[1], mag[2],
so3_comp_params.Kp,
so3_comp_params.Ki,
dt);
// Convert q->R, This R converts inertial frame to body frame.
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
//1-2-3 Representation.
//Equation (290)
@@ -747,29 +600,42 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll
euler[1] = -asinf(Rot_matrix[2]); //! Pitch
euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]); //! Yaw
euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
/* Do something */
// Publish only finite euler angles
att.roll = euler[0] - so3_comp_params.roll_off;
att.pitch = euler[1] - so3_comp_params.pitch_off;
att.yaw = euler[2] - so3_comp_params.yaw_off;
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
// Due to inputs or numerical failure the output is invalid
warnx("infinite euler angles, rotation matrix:");
warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
// Don't publish anything
continue;
}
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data);
if (last_data > 0 && raw.timestamp > last_data + 12000) {
warnx("sensor data missed");
}
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
// Quaternion
att.q[0] = q0;
att.q[1] = q1;
att.q[2] = q2;
att.q[3] = q3;
att.q_valid = true;
// XXX Apply the same transformation to the rotation matrix
att.roll = euler[0] - so3_comp_params.roll_off;
att.pitch = euler[1] - so3_comp_params.pitch_off;
att.yaw = euler[2] - so3_comp_params.yaw_off;
//! Euler angle rate. But it needs to be investigated again.
// Euler angle rate. But it needs to be investigated again.
/*
att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
@@ -783,53 +649,30 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
att.pitchacc = 0;
att.yawacc = 0;
//! Quaternion
att.q[0] = q0;
att.q[1] = q1;
att.q[2] = q2;
att.q[3] = q3;
att.q_valid = true;
/* TODO: Bias estimation required */
memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(float)*9);
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
// Publish
if (att_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
} else {
warnx("NaN in roll/pitch/yaw estimate!");
orb_advertise(ORB_ID(vehicle_attitude), &att);
}
perf_end(so3_comp_loop_perf);
//! This will print out debug packet to visualization software
if(debug_mode)
{
float quat[4];
quat[0] = q0;
quat[1] = q1;
quat[2] = q2;
quat[3] = q3;
send_uart_float_arr(quat,4);
send_uart_byte('\n');
}
}
}
}
loopcounter++;
}// while
}
thread_running = false;
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
return 0;
}
src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c
Executable
+86
View File
@@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* 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 attitude_estimator_so3_params.c
*
* Parameters for nonlinear complementary filters on the SO(3).
*/
#include "attitude_estimator_so3_params.h"
/* This is filter gain for nonlinear SO3 complementary filter */
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
will compensate gyro bias which depends on temperature and vibration of your vehicle */
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
//! You can set this gain higher if you want more fast response.
//! But note that higher gain will give you also higher overshoot.
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
//! This gain is depend on your vehicle status.
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_so3_param_handles *h)
{
/* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP");
h->Ki = param_find("SO3_COMP_KI");
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
h->roll_off = param_find("SO3_ROLL_OFFS");
h->pitch_off = param_find("SO3_PITCH_OFFS");
h->yaw_off = param_find("SO3_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p)
{
/* Update filter gain */
param_get(h->Kp, &(p->Kp));
param_get(h->Ki, &(p->Ki));
/* Update attitude offset */
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}
src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h
Executable
+67
View File
@@ -0,0 +1,67 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* 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 attitude_estimator_so3_params.h
*
* Parameters for nonlinear complementary filters on the SO(3).
*/
#include <systemlib/param/param.h>
struct attitude_estimator_so3_params {
float Kp;
float Ki;
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_so3_param_handles {
param_t Kp, Ki;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_so3_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p);
src/modules/attitude_estimator_so3/module.mk
+8
View File
@@ -0,0 +1,8 @@
#
# Attitude estimator (Nonlinear SO(3) complementary Filter)
#
MODULE_COMMAND = attitude_estimator_so3
SRCS = attitude_estimator_so3_main.cpp \
attitude_estimator_so3_params.c
src/modules/attitude_estimator_so3_comp/README
-5
View File
@@ -1,5 +0,0 @@
Synopsis
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
Option -d is for debugging packet. See code for detailed packet structure.
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
-63
View File
@@ -1,63 +0,0 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.c
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include "attitude_estimator_so3_comp_params.h"
/* This is filter gain for nonlinear SO3 complementary filter */
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
will compensate gyro bias which depends on temperature and vibration of your vehicle */
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
//! You can set this gain higher if you want more fast response.
//! But note that higher gain will give you also higher overshoot.
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
//! This gain is depend on your vehicle status.
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
{
/* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP");
h->Ki = param_find("SO3_COMP_KI");
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
{
/* Update filter gain */
param_get(h->Kp, &(p->Kp));
param_get(h->Ki, &(p->Ki));
/* Update attitude offset */
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h
-44
View File
@@ -1,44 +0,0 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.h
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include <systemlib/param/param.h>
struct attitude_estimator_so3_comp_params {
float Kp;
float Ki;
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_so3_comp_param_handles {
param_t Kp, Ki;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);
src/modules/attitude_estimator_so3_comp/module.mk
-8
View File
@@ -1,8 +0,0 @@
#
# Attitude estimator (Nonlinear SO3 complementary Filter)
#
MODULE_COMMAND = attitude_estimator_so3_comp
SRCS = attitude_estimator_so3_comp_main.cpp \
attitude_estimator_so3_comp_params.c
src/modules/commander/commander.cpp
+6 -5
View File
@@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[])
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
if (status.condition_local_altitude_valid) {
if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
status_changed = true;
@@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
// TODO AUTO_LAND handling
if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't switch to other states until takeoff not completed */
if (local_pos->z > -takeoff_alt || status->condition_landed) {
// XXX: only respect the condition_landed when the local position is actually valid
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
return TRANSITION_NOT_CHANGED;
}
}
@@ -1549,7 +1550,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
/* possibly on ground, switch to TAKEOFF if needed */
if (local_pos->z > -takeoff_alt || status->condition_landed) {
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
return res;
}
@@ -1597,8 +1598,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
/* switch to failsafe mode */
bool manual_control_old = control_mode->flag_control_manual_enabled;
if (!status->condition_landed) {
/* in air: try to hold position */
if (!status->condition_landed && status->condition_local_position_valid) {
/* in air: try to hold position if possible */
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
} else {
src/modules/fw_att_control/fw_att_control_params.c
+2 -2
View File
@@ -33,9 +33,9 @@
****************************************************************************/
/**
* @file fw_pos_control_l1_params.c
* @file fw_att_control_params.c
*
* Parameters defined by the L1 position control task
* Parameters defined by the fixed-wing attitude control task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
src/modules/mavlink/mavlink_receiver.cpp
+4 -7
View File
@@ -382,16 +382,15 @@ handle_message(mavlink_message_t *msg)
/* hil gyro */
static const float mrad2rad = 1.0e-3f;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
hil_sensors.gyro_counter = hil_counter;
/* accelerometer */
hil_sensors.accelerometer_counter = hil_counter;
static const float mg2ms2 = 9.8f / 1000.0f;
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
@@ -401,6 +400,7 @@ handle_message(mavlink_message_t *msg)
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_mode = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
hil_sensors.accelerometer_counter = hil_counter;
/* adc */
hil_sensors.adc_voltage_v[0] = 0.0f;
@@ -409,7 +409,6 @@ handle_message(mavlink_message_t *msg)
/* magnetometer */
float mga2ga = 1.0e-3f;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
@@ -419,15 +418,13 @@ handle_message(mavlink_message_t *msg)
hil_sensors.magnetometer_range_ga = 32.7f; // int16
hil_sensors.magnetometer_mode = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
hil_sensors.magnetometer_counter = hil_counter;
/* baro */
hil_sensors.baro_pres_mbar = imu.abs_pressure;
hil_sensors.baro_alt_meter = imu.pressure_alt;
hil_sensors.baro_temp_celcius = imu.temperature;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.accelerometer_counter = hil_counter;
hil_sensors.baro_counter = hil_counter;
/* differential pressure */
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
src/modules/mavlink/missionlib.c
+4 -4
View File
@@ -301,7 +301,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[last_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[last_setpoint_index].param1,
wpm->waypoints[last_setpoint_index].param2,
wpm->waypoints[last_setpoint_index].param3,
@@ -317,7 +317,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[next_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[next_setpoint_index].param1,
wpm->waypoints[next_setpoint_index].param2,
wpm->waypoints[next_setpoint_index].param3,
@@ -343,7 +343,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z;
sp.altitude_is_relative = true;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize publication if necessary */
@@ -364,7 +364,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.x = param5_lat_x;
sp.y = param6_lon_y;
sp.z = param7_alt_z;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
/* Initialize publication if necessary */
if (local_position_setpoint_pub < 0) {
src/modules/mavlink/orb_listener.c
+31 -67
View File
@@ -54,6 +54,7 @@
#include <sys/prctl.h>
#include <stdlib.h>
#include <poll.h>
#include <lib/geo/geo.h>
#include <mavlink/mavlink_log.h>
@@ -72,7 +73,6 @@ struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_effective_0;
struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att;
struct airspeed_s airspeed;
@@ -119,7 +119,6 @@ static void l_attitude_setpoint(const struct listener *l);
static void l_actuator_outputs(const struct listener *l);
static void l_actuator_armed(const struct listener *l);
static void l_manual_control_setpoint(const struct listener *l);
static void l_vehicle_attitude_controls_effective(const struct listener *l);
static void l_vehicle_attitude_controls(const struct listener *l);
static void l_debug_key_value(const struct listener *l);
static void l_optical_flow(const struct listener *l);
@@ -147,7 +146,6 @@ static const struct listener listeners[] = {
{l_actuator_armed, &mavlink_subs.armed_sub, 0},
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
{l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0},
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
{l_optical_flow, &mavlink_subs.optical_flow, 0},
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
@@ -242,16 +240,29 @@ l_vehicle_attitude(const struct listener *l)
att.rollspeed,
att.pitchspeed,
att.yawspeed);
/* limit VFR message rate to 10Hz */
hrt_abstime t = hrt_absolute_time();
if (t >= last_sent_vfr + 100000) {
last_sent_vfr = t;
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
}
/* send quaternion values if it exists */
if(att.q_valid) {
mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
att.q[0],
att.q[1],
att.q[2],
att.q[3],
att.rollspeed,
att.pitchspeed,
att.yawspeed);
}
}
attitude_counter++;
@@ -266,13 +277,7 @@ l_vehicle_gps_position(const struct listener *l)
orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
/* GPS COG is 0..2PI in degrees * 1e2 */
float cog_deg = gps.cog_rad;
if (cog_deg > M_PI_F)
cog_deg -= 2.0f * M_PI_F;
cog_deg *= M_RAD_TO_DEG_F;
float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
/* GPS position */
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
@@ -365,28 +370,16 @@ l_global_position(const struct listener *l)
/* copy global position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
uint64_t timestamp = global_pos.timestamp;
int32_t lat = global_pos.lat;
int32_t lon = global_pos.lon;
int32_t alt = (int32_t)(global_pos.alt * 1000);
int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
int16_t vx = (int16_t)(global_pos.vx * 100.0f);
int16_t vy = (int16_t)(global_pos.vy * 100.0f);
int16_t vz = (int16_t)(global_pos.vz * 100.0f);
/* heading in degrees * 10, from 0 to 36.000) */
uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
timestamp / 1000,
lat,
lon,
alt,
relative_alt,
vx,
vy,
vz,
hdg);
global_pos.timestamp / 1000,
global_pos.lat,
global_pos.lon,
global_pos.alt * 1000.0f,
global_pos.relative_alt * 1000.0f,
global_pos.vx * 100.0f,
global_pos.vy * 100.0f,
global_pos.vz * 100.0f,
_wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
void
@@ -424,8 +417,8 @@ l_global_position_setpoint(const struct listener *l)
coordinate_frame,
global_sp.lat,
global_sp.lon,
global_sp.altitude,
global_sp.yaw);
global_sp.altitude * 1000.0f,
global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
}
void
@@ -506,8 +499,8 @@ l_actuator_outputs(const struct listener *l)
act_outputs.output[6],
act_outputs.output[7]);
/* only send in HIL mode */
if (mavlink_hil_enabled && armed.armed) {
/* only send in HIL mode and only send first group for HIL */
if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
@@ -603,32 +596,6 @@ l_manual_control_setpoint(const struct listener *l)
0);
}
void
l_vehicle_attitude_controls_effective(const struct listener *l)
{
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
if (gcs_link) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl0 ",
actuators_effective_0.control_effective[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl1 ",
actuators_effective_0.control_effective[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl2 ",
actuators_effective_0.control_effective[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl3 ",
actuators_effective_0.control_effective[3]);
}
}
void
l_vehicle_attitude_controls(const struct listener *l)
{
@@ -839,9 +806,6 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
/* --- ACTUATOR CONTROL VALUE --- */
mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */
mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
src/modules/position_estimator_inav/position_estimator_inav_main.c
+5 -6
View File
@@ -236,13 +236,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) {
/* poll error */
errx(1, "subscriptions poll error on init.");
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
} else if (ret > 0) {
if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (wait_baro && sensor.baro_counter > baro_counter) {
if (wait_baro && sensor.baro_counter != baro_counter) {
baro_counter = sensor.baro_counter;
/* mean calculation over several measurements */
@@ -320,8 +320,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) {
/* poll error */
warnx("subscriptions poll error.");
thread_should_exit = true;
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
continue;
} else if (ret > 0) {
@@ -355,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fds[4].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter > accel_counter) {
if (sensor.accelerometer_counter != accel_counter) {
if (att.R_valid) {
/* correct accel bias, now only for Z */
sensor.accelerometer_m_s2[2] -= accel_bias[2];
@@ -381,7 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
accel_updates++;
}
if (sensor.baro_counter > baro_counter) {
if (sensor.baro_counter != baro_counter) {
baro_corr = - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter;
baro_updates++;
src/modules/px4iofirmware/controls.c
+35 -16
View File
@@ -50,7 +50,7 @@
#define RC_CHANNEL_HIGH_THRESH 5000
#define RC_CHANNEL_LOW_THRESH -5000
static bool ppm_input(uint16_t *values, uint16_t *num_values);
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
@@ -66,7 +66,7 @@ controls_init(void)
sbus_init("/dev/ttyS2");
/* default to a 1:1 input map, all enabled */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
@@ -94,6 +94,9 @@ controls_tick() {
* other. Don't do that.
*/
/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
uint16_t rssi = 0;
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
@@ -104,14 +107,15 @@ controls_tick() {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
rssi = 255;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
r_raw_rc_count = 8;
}
perf_end(c_gather_sbus);
@@ -121,11 +125,20 @@ controls_tick() {
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
if (ppm_updated)
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
if (ppm_updated) {
/* XXX sample RSSI properly here */
rssi = 255;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
}
perf_end(c_gather_ppm);
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
/*
* In some cases we may have received a frame, but input has still
* been lost.
@@ -197,14 +210,16 @@ controls_tick() {
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
}
}
@@ -221,7 +236,7 @@ controls_tick() {
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
if (assigned_channels == 0) {
if (assigned_channels == 0 || rssi == 0) {
rc_input_lost = true;
} else {
/* set RC OK flag */
@@ -306,7 +321,7 @@ controls_tick() {
}
static bool
ppm_input(uint16_t *values, uint16_t *num_values)
ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
{
bool result = false;
@@ -321,8 +336,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* PPM data exists, copy it */
*num_values = ppm_decoded_channels;
if (*num_values > PX4IO_CONTROL_CHANNELS)
*num_values = PX4IO_CONTROL_CHANNELS;
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
*num_values = PX4IO_RC_INPUT_CHANNELS;
for (unsigned i = 0; i < *num_values; i++)
values[i] = ppm_buffer[i];
@@ -330,6 +345,10 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* clear validity */
ppm_last_valid_decode = 0;
/* store PPM frame length */
if (num_values)
*frame_len = ppm_frame_length;
/* good if we got any channels */
result = (*num_values > 0);
}
src/modules/px4iofirmware/dsm.c
+1 -1
View File
@@ -355,7 +355,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
continue;
/* ignore channels out of range */
if (channel >= PX4IO_INPUT_CHANNELS)
if (channel >= PX4IO_RC_INPUT_CHANNELS)
continue;
/* update the decoded channel count */
src/modules/px4iofirmware/mixer.cpp
+32 -7
View File
@@ -77,7 +77,8 @@ enum mixer_source {
MIX_NONE,
MIX_FMU,
MIX_OVERRIDE,
MIX_FAILSAFE
MIX_FAILSAFE,
MIX_OVERRIDE_FMU_OK
};
static mixer_source source;
@@ -135,10 +136,19 @@ mixer_tick(void)
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
} else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* if allowed, mix from RC inputs directly up to available rc channels */
source = MIX_OVERRIDE_FMU_OK;
}
}
@@ -185,7 +195,7 @@ mixer_tick(void)
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
}
@@ -201,6 +211,10 @@ mixer_tick(void)
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
@@ -236,24 +250,35 @@ mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control)
{
if (control_group != 0)
if (control_group > 3)
return -1;
switch (source) {
case MIX_FMU:
if (control_index < PX4IO_CONTROL_CHANNELS) {
control = REG_TO_FLOAT(r_page_controls[control_index]);
if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
break;
}
return -1;
case MIX_OVERRIDE:
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
break;
}
return -1;
case MIX_OVERRIDE_FMU_OK:
/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
break;
} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
break;
}
return -1;
case MIX_FAILSAFE:
case MIX_NONE:
control = 0.0f;
src/modules/px4iofirmware/protocol.h
+41 -20
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
@@ -63,7 +63,7 @@
* readable pages to be densely packed. Page numbers do not need to be
* packed.
*
* Definitions marked 1 are only valid on PX4IOv1 boards. Likewise,
* Definitions marked [1] are only valid on PX4IOv1 boards. Likewise,
* [2] denotes definitions specific to the PX4IOv2 board.
*/
@@ -76,6 +76,9 @@
#define PX4IO_PROTOCOL_VERSION 4
/* maximum allowable sizes on this protocol version */
#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */
@@ -87,6 +90,7 @@
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */
#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT 8 /**< hardcoded # of control groups*/
/* dynamic status page */
#define PX4IO_PAGE_STATUS 1
@@ -124,6 +128,8 @@
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -184,44 +190,59 @@ enum { /* DSM bind states */
dsm_bind_reinit_uart
};
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_VALID 64
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */
/* raw text load to the mixer parser - ignores offset */
#define PX4IO_PAGE_MIXERLOAD 52
#define PX4IO_PAGE_MIXERLOAD 52
/* R/C channel config */
#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */
#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */
#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */
#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */
#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */
#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */
/* PWM output - overrides mixer */
#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */
#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */
#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* Debug and test page - not used in normal operation */
#define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */
#define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */
/* PWM minimum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */
#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
src/modules/px4iofirmware/px4io.c
+33
View File
@@ -45,6 +45,7 @@
#include <string.h>
#include <poll.h>
#include <signal.h>
#include <crc32.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
@@ -117,6 +118,29 @@ show_debug_messages(void)
}
}
static void
heartbeat_blink(void)
{
static bool heartbeat = false;
LED_BLUE(heartbeat = !heartbeat);
}
static void
calculate_fw_crc(void)
{
#define APP_SIZE_MAX 0xf000
#define APP_LOAD_ADDRESS 0x08001000
// compute CRC of the current firmware
uint32_t sum = 0;
for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
}
r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF;
r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
}
int
user_start(int argc, char *argv[])
{
@@ -129,6 +153,9 @@ user_start(int argc, char *argv[])
/* configure the high-resolution time/callout interface */
hrt_init();
/* calculate our fw CRC so FMU can decide if we need to update */
calculate_fw_crc();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
@@ -201,6 +228,7 @@ user_start(int argc, char *argv[])
*/
uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0;
for (;;) {
/* track the rate at which the loop is running */
@@ -216,6 +244,11 @@ user_start(int argc, char *argv[])
controls_tick();
perf_end(controls_perf);
if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
#if 0
/* check for debug activity */
show_debug_messages();
src/modules/px4iofirmware/px4io.h
+6 -2
View File
@@ -53,7 +53,9 @@
*/
#define PX4IO_SERVO_COUNT 8
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels
#define PX4IO_CONTROL_GROUPS 2
#define PX4IO_RC_INPUT_CHANNELS 18
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */
/*
* Debug logging
@@ -169,6 +171,8 @@ extern pwm_limit_t pwm_limit;
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
/*
* Mixer
*/
@@ -209,7 +213,7 @@ extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
src/modules/px4iofirmware/registers.c
+44 -11
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
@@ -45,6 +45,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/systemlib.h>
#include <stm32_pwr.h>
#include "px4io.h"
#include "protocol.h"
@@ -68,7 +70,7 @@ static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS,
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
@@ -87,7 +89,9 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_IBATT] = 0,
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
[PX4IO_P_STATUS_PRSSI] = 0
[PX4IO_P_STATUS_PRSSI] = 0,
[PX4IO_P_STATUS_NRSSI] = 0,
[PX4IO_P_STATUS_RC_DATA] = 0
};
/**
@@ -112,7 +116,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT];
uint16_t r_page_raw_rc_input[] =
{
[PX4IO_P_RAW_RC_COUNT] = 0,
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
};
/**
@@ -122,7 +126,7 @@ uint16_t r_page_raw_rc_input[] =
*/
uint16_t r_page_rc_input[] = {
[PX4IO_P_RC_VALID] = 0,
[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
};
/**
@@ -154,6 +158,8 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
@@ -172,7 +178,7 @@ volatile uint16_t r_page_setup[] =
*
* Control values from the FMU.
*/
volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
volatile uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
/*
* PAGE 102 does not have a buffer.
@@ -183,7 +189,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
*
* R/C channel input configuration.
*/
uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
/* valid options */
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
@@ -235,7 +241,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
case PX4IO_PAGE_CONTROLS:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_controls[offset] = *values;
@@ -501,6 +507,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
break;
case PX4IO_P_SETUP_REBOOT_BL:
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
// don't allow reboot while armed
break;
}
// check the magic value
if (value != PX4IO_REBOOT_BL_MAGIC)
break;
// note that we don't set BL_WAIT_MAGIC in
// BKP_DR1 as that is not necessary given the
// timing of the forceupdate command. The
// bootloader on px4io waits for enough time
// anyway, and this method works with older
// bootloader versions (tested with both
// revision 3 and revision 4).
up_systemreset();
break;
case PX4IO_P_SETUP_DSM:
dsm_bind(value & 0x0f, (value >> 4) & 7);
break;
@@ -525,7 +554,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
if (channel >= PX4IO_CONTROL_CHANNELS)
if (channel >= PX4IO_RC_INPUT_CHANNELS)
return -1;
/* disable the channel until we have a chance to sanity-check it */
@@ -555,6 +584,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* this option is normally set last */
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint8_t count = 0;
bool disabled = false;
/* assert min..center..max ordering */
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
@@ -573,7 +603,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}
@@ -581,7 +614,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (count) {
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
} else {
} else if (!disabled) {
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
}
src/modules/px4iofirmware/safety.c
+1 -15
View File
@@ -77,7 +77,6 @@ static unsigned blink_counter = 0;
static bool safety_button_pressed;
static void safety_check_button(void *arg);
static void heartbeat_blink(void *arg);
static void failsafe_blink(void *arg);
void
@@ -86,9 +85,6 @@ safety_init(void)
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
/* arrange for the heartbeat handler to be called at 4Hz */
hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
@@ -163,16 +159,6 @@ safety_check_button(void *arg)
}
}
static void
heartbeat_blink(void *arg)
{
static bool heartbeat = false;
/* XXX add flags here that need to be frobbed by various loops */
LED_BLUE(heartbeat = !heartbeat);
}
static void
failsafe_blink(void *arg)
{
@@ -192,4 +178,4 @@ failsafe_blink(void *arg)
}
LED_AMBER(failsafe);
}
}
src/modules/px4iofirmware/sbus.c
+49 -18
View File
@@ -54,6 +54,27 @@
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
/*
Measured values with Futaba FX-30/R6108SB:
-+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV)
-+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV)
-+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks)
*/
/* define range mapping here, -+100% -> 1000..2000 */
#define SBUS_RANGE_MIN 200.0f
#define SBUS_RANGE_MAX 1800.0f
#define SBUS_TARGET_MIN 1000.0f
#define SBUS_TARGET_MAX 2000.0f
/* pre-calculate the floating point stuff as far as possible at compile time */
#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
static int sbus_fd = -1;
@@ -66,7 +87,7 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels);
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
int
sbus_init(const char *device)
@@ -97,7 +118,7 @@ sbus_init(const char *device)
}
bool
sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
@@ -154,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
* decode it.
*/
partial_frame_count = 0;
return sbus_decode(now, values, num_values, max_channels);
return sbus_decode(now, values, num_values, rssi, max_channels);
}
/*
@@ -194,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
};
static bool
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values)
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
@@ -202,15 +223,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
return false;
}
/* if the failsafe or connection lost bit is set, we consider the frame invalid */
if ((frame[23] & (1 << 2)) && /* signal lost */
(frame[23] & (1 << 3))) { /* failsafe */
/* actively announce signal loss */
*values = 0;
return false;
}
/* we have received something we think is a frame */
last_frame_time = frame_time;
@@ -234,22 +246,41 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
}
}
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
values[channel] = (value / 2) + 998;
/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
}
/* decode switch channels if data fields are wide enough */
if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) {
chancount = 18;
/* channel 17 (index 16) */
values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
}
/* note the number of channels decoded */
*num_values = chancount;
/* decode and handle failsafe and frame-lost flags */
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
/* report that we failed to read anything valid off the receiver */
*rssi = 0;
return false;
}
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag or try to calculate some kind of RSSI information - to be implemented
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issueing return-to-launch!!! */
*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
}
*rssi = 255;
return true;
}
src/modules/sdlog2/sdlog2.c
+4 -18
View File
@@ -68,7 +68,6 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -691,7 +690,6 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct actuator_controls_effective_s act_controls_effective;
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
@@ -717,7 +715,6 @@ int sdlog2_thread_main(int argc, char *argv[])
int rates_sp_sub;
int act_outputs_sub;
int act_controls_sub;
int act_controls_effective_sub;
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
@@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 20;
/* Sanity check variable and index */
/* number of subscriptions */
const ssize_t fdsc = 19;
/* sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
@@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL EFFECTIVE --- */
subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
fds[fdsc_count].fd = subs.act_controls_effective_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL POSITION --- */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
fds[fdsc_count].fd = subs.local_pos_sub;
@@ -1114,12 +1105,6 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
/* --- ACTUATOR CONTROL EFFECTIVE --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective);
// TODO not implemented yet
}
/* --- LOCAL POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
@@ -1208,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_RC_MSG;
/* Copy only the first 8 channels of 14 */
memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
log_msg.body.log_RC.channel_count = buf.rc.chan_count;
LOGBUFFER_WRITE_AND_COUNT(RC);
}
src/modules/sdlog2/sdlog2_messages.h
+2 -1
View File
@@ -159,6 +159,7 @@ struct log_STAT_s {
#define LOG_RC_MSG 11
struct log_RC_s {
float channel[8];
uint8_t channel_count;
};
/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
@@ -281,7 +282,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),

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