mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 03:43:34 +08:00
Merge branch 'beta' into offboard2
This commit is contained in:
Anton Babushkin
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# HILStar / X-Plane
|
||||
#
|
||||
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
|
||||
# Lorenz Meier <lm@inf.ethz.ch>
|
||||
#
|
||||
|
||||
echo "HIL Rascal 110 starting.."
|
||||
@@ -26,17 +26,12 @@ then
|
||||
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
|
||||
|
||||
set HIL yes
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Team Blacksheep Discovery Quadcopter
|
||||
#
|
||||
# Maintainers: Simon Wilks <sjwilks@gmail.com>
|
||||
# Simon Wilks <sjwilks@gmail.com>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# 3DR Iris Quadcopter
|
||||
#
|
||||
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# HIL Quadcopter X
|
||||
#
|
||||
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
@@ -1,45 +0,0 @@
|
||||
#!nsh
|
||||
#
|
||||
# HIL Rascal 110 (Flightgear)
|
||||
#
|
||||
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
|
||||
#
|
||||
|
||||
echo "HIL Rascal 110 starting.."
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
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
|
||||
|
||||
set HIL yes
|
||||
|
||||
set VEHICLE_TYPE fw
|
||||
set MIXER FMU_AERT
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# HIL Quadcopter +
|
||||
#
|
||||
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
sh /etc/init.d/1001_rc_quad_x.hil
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# HIL Rascal 110 (Flightgear)
|
||||
#
|
||||
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
|
||||
# Thomas Gubler <thomasgubler@gmail.com>
|
||||
#
|
||||
|
||||
echo "HIL Rascal 110 starting.."
|
||||
@@ -34,9 +34,6 @@ then
|
||||
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
|
||||
|
||||
set HIL yes
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
#!nsh
|
||||
#
|
||||
# HIL Malolo 1 (Flightgear)
|
||||
#
|
||||
# Thomas Gubler <thomasgubler@gmail.com>
|
||||
#
|
||||
|
||||
echo "HIL Malolo 1 starting.."
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
# Set all params here, then disable autoconfig
|
||||
|
||||
param set FW_AIRSPD_MIN 12
|
||||
param set FW_AIRSPD_TRIM 25
|
||||
param set FW_ATT_TC 0.3
|
||||
param set FW_L1_DAMPING 0.74
|
||||
param set FW_L1_PERIOD 15
|
||||
param set FW_PR_FF 0.8
|
||||
param set FW_PR_I 0.05
|
||||
param set FW_PR_IMAX 0.2
|
||||
param set FW_PR_P 0.1
|
||||
param set FW_P_LIM_MAX 45
|
||||
param set FW_P_LIM_MIN -45
|
||||
param set FW_P_RMAX_NEG 0
|
||||
param set FW_P_RMAX_POS 0
|
||||
param set FW_P_ROLLFF 0
|
||||
param set FW_RR_FF 0.6
|
||||
param set FW_RR_I 0.02
|
||||
param set FW_RR_IMAX 0.2
|
||||
param set FW_RR_P 0.1
|
||||
param set FW_R_LIM 45
|
||||
param set FW_R_RMAX 0
|
||||
param set FW_THR_CRUISE 0.6
|
||||
param set FW_THR_MAX 1
|
||||
param set FW_THR_MIN 0
|
||||
param set FW_T_CLMB_MAX 5
|
||||
param set FW_T_HRATE_P 0.02
|
||||
param set FW_T_PTCH_DAMP 0
|
||||
param set FW_T_RLL2THR 15
|
||||
param set FW_T_SINK_MAX 5
|
||||
param set FW_T_SINK_MIN 2
|
||||
param set FW_T_SRATE_P 0.01
|
||||
param set FW_T_TIME_CONST 3
|
||||
param set FW_T_VERT_ACC 7
|
||||
param set FW_YCO_VMIN 1000
|
||||
param set FW_YR_FF 0.0
|
||||
param set FW_YR_I 0
|
||||
param set FW_YR_IMAX 0.2
|
||||
param set FW_YR_P 0.0
|
||||
param set FW_Y_RMAX 0
|
||||
param set NAV_LAND_ALT 90
|
||||
param set NAV_RTL_ALT 100
|
||||
param set NAV_RTL_LAND_T -1
|
||||
|
||||
param set SYS_AUTOCONFIG 0
|
||||
param save
|
||||
fi
|
||||
|
||||
set HIL yes
|
||||
|
||||
set VEHICLE_TYPE fw
|
||||
# Set the AERT mixer for HIL (even if the malolo is a flying wing)
|
||||
set MIXER FMU_AERT
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Generic 10” Octo coaxial geometry
|
||||
#
|
||||
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
|
||||
# Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
sh /etc/init.d/8001_octo_x
|
||||
|
||||
@@ -1,10 +1,5 @@
|
||||
#!nsh
|
||||
|
||||
echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
|
||||
|
||||
#
|
||||
# Load default params for this platform
|
||||
#
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
# Set all params here, then disable autoconfig
|
||||
|
||||
@@ -1,10 +1,5 @@
|
||||
#!nsh
|
||||
|
||||
echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
|
||||
|
||||
#
|
||||
# Load default params for this platform
|
||||
#
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
# Set all params here, then disable autoconfig
|
||||
@@ -23,8 +18,6 @@ then
|
||||
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
|
||||
|
||||
@@ -1,7 +1,5 @@
|
||||
#!nsh
|
||||
|
||||
echo "[init] PX4FMU v1, v2 with or without IO on Camflyer"
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
#
|
||||
@@ -26,7 +24,7 @@ then
|
||||
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_MAX 1.0
|
||||
param set FW_THR_MIN 0
|
||||
param set FW_T_SINK_MAX 5
|
||||
param set FW_T_SINK_MIN 2
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Phantom FPV Flying Wing
|
||||
#
|
||||
# Maintainers: Simon Wilks <sjwilks@gmail.com>
|
||||
# Simon Wilks <sjwilks@gmail.com>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Skywalker X5 Flying Wing
|
||||
#
|
||||
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
|
||||
# Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Wing Wing (aka Z-84) Flying Wing
|
||||
#
|
||||
# Maintainers: Simon Wilks <sjwilks@gmail.com>
|
||||
# Simon Wilks <sjwilks@gmail.com>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# FX-79 Buffalo Flying Wing
|
||||
#
|
||||
# Maintainers: Simon Wilks <sjwilks@gmail.com>
|
||||
# Simon Wilks <sjwilks@gmail.com>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Generic 10” Quad X geometry
|
||||
#
|
||||
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
|
||||
# Lorenz Meier <lm@inf.ethz.ch>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# DJI Flame Wheel F330 Quadcopter
|
||||
#
|
||||
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# DJI Flame Wheel F450 Quadcopter
|
||||
#
|
||||
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
|
||||
# Lorenz Meier <lm@inf.ethz.ch>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
|
||||
@@ -2,48 +2,10 @@
|
||||
#
|
||||
# HobbyKing X550 Quadcopter
|
||||
#
|
||||
# Maintainers: Todd Stellanova <tstellanova@gmail.com>
|
||||
# Todd Stellanova <tstellanova@gmail.com>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
#
|
||||
# Default parameters for this platform
|
||||
#
|
||||
param set MC_ROLL_P 5.5
|
||||
param set MC_ROLLRATE_P 0.14
|
||||
param set MC_ROLLRATE_I 0
|
||||
param set MC_ROLLRATE_D 0.006
|
||||
param set MC_PITCH_P 5.5
|
||||
param set MC_PITCHRATE_P 0.14
|
||||
param set MC_PITCHRATE_I 0
|
||||
param set MC_PITCHRATE_D 0.006
|
||||
param set MC_YAW_P 0.6
|
||||
param set MC_YAWRATE_P 0.08
|
||||
param set MC_YAWRATE_I 0
|
||||
param set MC_YAWRATE_D 0
|
||||
|
||||
param set MPC_THR_MAX 1.0
|
||||
param set MPC_THR_MIN 0.1
|
||||
param set MPC_XY_P 1.0
|
||||
param set MPC_XY_VEL_P 0.1
|
||||
param set MPC_XY_VEL_I 0.02
|
||||
param set MPC_XY_VEL_D 0.01
|
||||
param set MPC_XY_VEL_MAX 5
|
||||
param set MPC_XY_FF 0.5
|
||||
param set MPC_Z_P 1.0
|
||||
param set MPC_Z_VEL_P 0.1
|
||||
param set MPC_Z_VEL_I 0.02
|
||||
param set MPC_Z_VEL_D 0.0
|
||||
param set MPC_Z_VEL_MAX 3
|
||||
param set MPC_Z_FF 0.5
|
||||
param set MPC_TILT_MAX 1.0
|
||||
param set MPC_LAND_SPEED 1.0
|
||||
param set MPC_LAND_TILT 0.3
|
||||
fi
|
||||
|
||||
set VEHICLE_TYPE mc
|
||||
set MIXER FMU_quad_x
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
set PWM_OUTPUTS 1234
|
||||
set PWM_RATE 400
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Generic 10” Quad + geometry
|
||||
#
|
||||
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
@@ -2,11 +2,13 @@
|
||||
#
|
||||
# Generic 10” Hexa X geometry
|
||||
#
|
||||
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
set MIXER FMU_hexa_x
|
||||
|
||||
set PWM_OUTPUTS 123456
|
||||
# We only can run one channel group with one rate,
|
||||
# so all 8 at 400 Hz
|
||||
set PWM_OUTPUTS 12345678
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Generic 10” Hexa + geometry
|
||||
#
|
||||
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
sh /etc/init.d/6001_hexa_x
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Generic 10” Octo X geometry
|
||||
#
|
||||
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# Generic 10” Octo + geometry
|
||||
#
|
||||
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
|
||||
# Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
sh /etc/init.d/8001_octo_x
|
||||
|
||||
@@ -23,7 +23,7 @@
|
||||
|
||||
if param compare SYS_AUTOSTART 1000
|
||||
then
|
||||
#sh /etc/init.d/1000_rc_fw_easystar.hil
|
||||
sh /etc/init.d/1000_rc_fw_easystar.hil
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 1001
|
||||
@@ -31,11 +31,6 @@ then
|
||||
sh /etc/init.d/1001_rc_quad_x.hil
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 1002
|
||||
then
|
||||
sh /etc/init.d/1002_rc_fw_state.hil
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 1003
|
||||
then
|
||||
sh /etc/init.d/1003_rc_quad_+.hil
|
||||
@@ -46,6 +41,11 @@ then
|
||||
sh /etc/init.d/1004_rc_fw_Rascal110.hil
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 1005
|
||||
then
|
||||
sh /etc/init.d/1005_rc_fw_Malolo1.hil
|
||||
fi
|
||||
|
||||
#
|
||||
# Standard plane
|
||||
#
|
||||
|
||||
@@ -3,17 +3,6 @@
|
||||
# Standard apps for fixed wing
|
||||
#
|
||||
|
||||
#
|
||||
# Start the attitude and position estimator
|
||||
#
|
||||
att_pos_estimator_ekf start
|
||||
|
||||
#
|
||||
# Start attitude controller
|
||||
#
|
||||
fw_att_control start
|
||||
|
||||
#
|
||||
# Start the position controller
|
||||
#
|
||||
fw_pos_control_l1 start
|
||||
|
||||
@@ -4,7 +4,6 @@
|
||||
|
||||
#
|
||||
# Allow PX4IO to recover from midair restarts.
|
||||
# this is very unlikely, but quite safe and robust.
|
||||
#
|
||||
px4io recovery
|
||||
|
||||
|
||||
@@ -1,24 +1,11 @@
|
||||
#!nsh
|
||||
#
|
||||
# Standard apps for multirotors
|
||||
# Standard apps for multirotors:
|
||||
# att & pos estimator, att & pos control.
|
||||
#
|
||||
|
||||
#
|
||||
# Start the attitude estimator
|
||||
#
|
||||
attitude_estimator_ekf start
|
||||
|
||||
#
|
||||
# Start position estimator
|
||||
#
|
||||
position_estimator_inav start
|
||||
|
||||
#
|
||||
# Start attitude control
|
||||
#
|
||||
mc_att_control start
|
||||
|
||||
#
|
||||
# Start position control
|
||||
#
|
||||
mc_pos_control start
|
||||
|
||||
@@ -3,10 +3,6 @@
|
||||
# Standard startup script for PX4FMU onboard sensor drivers.
|
||||
#
|
||||
|
||||
#
|
||||
# Start sensor drivers here.
|
||||
#
|
||||
|
||||
ms5611 start
|
||||
adc start
|
||||
|
||||
|
||||
@@ -3,8 +3,7 @@
|
||||
# PX4FMU startup script.
|
||||
|
||||
#
|
||||
# Default to auto-start mode. An init script on the microSD card
|
||||
# can change this to prevent automatic startup of the flight script.
|
||||
# Default to auto-start mode.
|
||||
#
|
||||
set MODE autostart
|
||||
|
||||
|
||||
@@ -25,13 +25,13 @@ 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
|
||||
S: 0 0 -8000 -8000 0 -10000 10000
|
||||
S: 0 1 6000 6000 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
|
||||
S: 0 0 -8000 -8000 0 -10000 10000
|
||||
S: 0 1 -6000 -6000 0 -10000 10000
|
||||
|
||||
Output 2
|
||||
--------
|
||||
|
||||
@@ -5,14 +5,3 @@ This file defines a single mixer for a hexacopter in the + configuration. All co
|
||||
are mixed 100%.
|
||||
|
||||
R: 6+ 10000 10000 10000 0
|
||||
|
||||
Gimbal / payload mixer for last two channels
|
||||
-----------------------------------------------------
|
||||
|
||||
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
|
||||
|
||||
@@ -5,14 +5,3 @@ This file defines a single mixer for a hexacopter in the X configuration. All c
|
||||
are mixed 100%.
|
||||
|
||||
R: 6x 10000 10000 10000 0
|
||||
|
||||
Gimbal / payload mixer for last two channels
|
||||
-----------------------------------------------------
|
||||
|
||||
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
|
||||
|
||||
@@ -21,7 +21,6 @@ MODULES += drivers/px4fmu
|
||||
MODULES += drivers/boards/px4fmu-v1
|
||||
MODULES += drivers/ardrone_interface
|
||||
MODULES += drivers/l3gd20
|
||||
#MODULES += drivers/bma180
|
||||
MODULES += drivers/mpu6000
|
||||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
@@ -44,7 +43,6 @@ MODULES += modules/sensors
|
||||
#
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/bl_update
|
||||
MODULES += systemcmds/boardinfo
|
||||
MODULES += systemcmds/i2c
|
||||
MODULES += systemcmds/mixer
|
||||
MODULES += systemcmds/param
|
||||
@@ -65,14 +63,12 @@ MODULES += systemcmds/hw_ver
|
||||
MODULES += modules/commander
|
||||
MODULES += modules/navigator
|
||||
MODULES += modules/mavlink
|
||||
MODULES += modules/mavlink_onboard
|
||||
MODULES += modules/gpio_led
|
||||
|
||||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/attitude_estimator_so3
|
||||
MODULES += modules/att_pos_estimator_ekf
|
||||
MODULES += modules/position_estimator_inav
|
||||
#MODULES += examples/flow_position_estimator
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,287 @@
|
||||
// MESSAGE AHRS2 PACKING
|
||||
|
||||
#define MAVLINK_MSG_ID_AHRS2 178
|
||||
|
||||
typedef struct __mavlink_ahrs2_t
|
||||
{
|
||||
float roll; ///< Roll angle (rad)
|
||||
float pitch; ///< Pitch angle (rad)
|
||||
float yaw; ///< Yaw angle (rad)
|
||||
float altitude; ///< Altitude (MSL)
|
||||
int32_t lat; ///< Latitude in degrees * 1E7
|
||||
int32_t lng; ///< Longitude in degrees * 1E7
|
||||
} mavlink_ahrs2_t;
|
||||
|
||||
#define MAVLINK_MSG_ID_AHRS2_LEN 24
|
||||
#define MAVLINK_MSG_ID_178_LEN 24
|
||||
|
||||
#define MAVLINK_MSG_ID_AHRS2_CRC 47
|
||||
#define MAVLINK_MSG_ID_178_CRC 47
|
||||
|
||||
|
||||
|
||||
#define MAVLINK_MESSAGE_INFO_AHRS2 { \
|
||||
"AHRS2", \
|
||||
6, \
|
||||
{ { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs2_t, roll) }, \
|
||||
{ "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs2_t, pitch) }, \
|
||||
{ "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs2_t, yaw) }, \
|
||||
{ "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs2_t, altitude) }, \
|
||||
{ "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_ahrs2_t, lat) }, \
|
||||
{ "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_ahrs2_t, lng) }, \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Pack a ahrs2 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 roll Roll angle (rad)
|
||||
* @param pitch Pitch angle (rad)
|
||||
* @param yaw Yaw angle (rad)
|
||||
* @param altitude Altitude (MSL)
|
||||
* @param lat Latitude in degrees * 1E7
|
||||
* @param lng Longitude in degrees * 1E7
|
||||
* @return length of the message in bytes (excluding serial stream start sign)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_ahrs2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
|
||||
float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_AHRS2_LEN];
|
||||
_mav_put_float(buf, 0, roll);
|
||||
_mav_put_float(buf, 4, pitch);
|
||||
_mav_put_float(buf, 8, yaw);
|
||||
_mav_put_float(buf, 12, altitude);
|
||||
_mav_put_int32_t(buf, 16, lat);
|
||||
_mav_put_int32_t(buf, 20, lng);
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN);
|
||||
#else
|
||||
mavlink_ahrs2_t packet;
|
||||
packet.roll = roll;
|
||||
packet.pitch = pitch;
|
||||
packet.yaw = yaw;
|
||||
packet.altitude = altitude;
|
||||
packet.lat = lat;
|
||||
packet.lng = lng;
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN);
|
||||
#endif
|
||||
|
||||
msg->msgid = MAVLINK_MSG_ID_AHRS2;
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
|
||||
#else
|
||||
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Pack a ahrs2 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 roll Roll angle (rad)
|
||||
* @param pitch Pitch angle (rad)
|
||||
* @param yaw Yaw angle (rad)
|
||||
* @param altitude Altitude (MSL)
|
||||
* @param lat Latitude in degrees * 1E7
|
||||
* @param lng Longitude in degrees * 1E7
|
||||
* @return length of the message in bytes (excluding serial stream start sign)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_ahrs2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
|
||||
mavlink_message_t* msg,
|
||||
float roll,float pitch,float yaw,float altitude,int32_t lat,int32_t lng)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_AHRS2_LEN];
|
||||
_mav_put_float(buf, 0, roll);
|
||||
_mav_put_float(buf, 4, pitch);
|
||||
_mav_put_float(buf, 8, yaw);
|
||||
_mav_put_float(buf, 12, altitude);
|
||||
_mav_put_int32_t(buf, 16, lat);
|
||||
_mav_put_int32_t(buf, 20, lng);
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN);
|
||||
#else
|
||||
mavlink_ahrs2_t packet;
|
||||
packet.roll = roll;
|
||||
packet.pitch = pitch;
|
||||
packet.yaw = yaw;
|
||||
packet.altitude = altitude;
|
||||
packet.lat = lat;
|
||||
packet.lng = lng;
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN);
|
||||
#endif
|
||||
|
||||
msg->msgid = MAVLINK_MSG_ID_AHRS2;
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
|
||||
#else
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Encode a ahrs2 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 ahrs2 C-struct to read the message contents from
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_ahrs2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2)
|
||||
{
|
||||
return mavlink_msg_ahrs2_pack(system_id, component_id, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Encode a ahrs2 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 ahrs2 C-struct to read the message contents from
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_ahrs2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2)
|
||||
{
|
||||
return mavlink_msg_ahrs2_pack_chan(system_id, component_id, chan, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send a ahrs2 message
|
||||
* @param chan MAVLink channel to send the message
|
||||
*
|
||||
* @param roll Roll angle (rad)
|
||||
* @param pitch Pitch angle (rad)
|
||||
* @param yaw Yaw angle (rad)
|
||||
* @param altitude Altitude (MSL)
|
||||
* @param lat Latitude in degrees * 1E7
|
||||
* @param lng Longitude in degrees * 1E7
|
||||
*/
|
||||
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
|
||||
static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_AHRS2_LEN];
|
||||
_mav_put_float(buf, 0, roll);
|
||||
_mav_put_float(buf, 4, pitch);
|
||||
_mav_put_float(buf, 8, yaw);
|
||||
_mav_put_float(buf, 12, altitude);
|
||||
_mav_put_int32_t(buf, 16, lat);
|
||||
_mav_put_int32_t(buf, 20, lng);
|
||||
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
|
||||
#else
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN);
|
||||
#endif
|
||||
#else
|
||||
mavlink_ahrs2_t packet;
|
||||
packet.roll = roll;
|
||||
packet.pitch = pitch;
|
||||
packet.yaw = yaw;
|
||||
packet.altitude = altitude;
|
||||
packet.lat = lat;
|
||||
packet.lng = lng;
|
||||
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
|
||||
#else
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// MESSAGE AHRS2 UNPACKING
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get field roll from ahrs2 message
|
||||
*
|
||||
* @return Roll angle (rad)
|
||||
*/
|
||||
static inline float mavlink_msg_ahrs2_get_roll(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_float(msg, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field pitch from ahrs2 message
|
||||
*
|
||||
* @return Pitch angle (rad)
|
||||
*/
|
||||
static inline float mavlink_msg_ahrs2_get_pitch(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_float(msg, 4);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field yaw from ahrs2 message
|
||||
*
|
||||
* @return Yaw angle (rad)
|
||||
*/
|
||||
static inline float mavlink_msg_ahrs2_get_yaw(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_float(msg, 8);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field altitude from ahrs2 message
|
||||
*
|
||||
* @return Altitude (MSL)
|
||||
*/
|
||||
static inline float mavlink_msg_ahrs2_get_altitude(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_float(msg, 12);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field lat from ahrs2 message
|
||||
*
|
||||
* @return Latitude in degrees * 1E7
|
||||
*/
|
||||
static inline int32_t mavlink_msg_ahrs2_get_lat(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_int32_t(msg, 16);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field lng from ahrs2 message
|
||||
*
|
||||
* @return Longitude in degrees * 1E7
|
||||
*/
|
||||
static inline int32_t mavlink_msg_ahrs2_get_lng(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_int32_t(msg, 20);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Decode a ahrs2 message into a struct
|
||||
*
|
||||
* @param msg The message to decode
|
||||
* @param ahrs2 C-struct to decode the message contents into
|
||||
*/
|
||||
static inline void mavlink_msg_ahrs2_decode(const mavlink_message_t* msg, mavlink_ahrs2_t* ahrs2)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP
|
||||
ahrs2->roll = mavlink_msg_ahrs2_get_roll(msg);
|
||||
ahrs2->pitch = mavlink_msg_ahrs2_get_pitch(msg);
|
||||
ahrs2->yaw = mavlink_msg_ahrs2_get_yaw(msg);
|
||||
ahrs2->altitude = mavlink_msg_ahrs2_get_altitude(msg);
|
||||
ahrs2->lat = mavlink_msg_ahrs2_get_lat(msg);
|
||||
ahrs2->lng = mavlink_msg_ahrs2_get_lng(msg);
|
||||
#else
|
||||
memcpy(ahrs2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS2_LEN);
|
||||
#endif
|
||||
}
|
||||
@@ -1398,6 +1398,59 @@ static void mavlink_test_rally_fetch_point(uint8_t system_id, uint8_t component_
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
static void mavlink_test_ahrs2(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_ahrs2_t packet_in = {
|
||||
17.0,
|
||||
}45.0,
|
||||
}73.0,
|
||||
}101.0,
|
||||
}963498296,
|
||||
}963498504,
|
||||
};
|
||||
mavlink_ahrs2_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
packet1.roll = packet_in.roll;
|
||||
packet1.pitch = packet_in.pitch;
|
||||
packet1.yaw = packet_in.yaw;
|
||||
packet1.altitude = packet_in.altitude;
|
||||
packet1.lat = packet_in.lat;
|
||||
packet1.lng = packet_in.lng;
|
||||
|
||||
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_ahrs2_encode(system_id, component_id, &msg, &packet1);
|
||||
mavlink_msg_ahrs2_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_ahrs2_pack(system_id, component_id, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng );
|
||||
mavlink_msg_ahrs2_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_ahrs2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng );
|
||||
mavlink_msg_ahrs2_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_ahrs2_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_ahrs2_send(MAVLINK_COMM_1 , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng );
|
||||
mavlink_msg_ahrs2_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);
|
||||
@@ -1426,6 +1479,7 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
|
||||
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);
|
||||
mavlink_test_ahrs2(system_id, component_id, last_msg);
|
||||
}
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
@@ -5,8 +5,8 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:56:32 2013"
|
||||
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:27:33 2014"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
|
||||
|
||||
#endif // MAVLINK_VERSION_H
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -5,8 +5,8 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Mon Dec 16 09:03:20 2013"
|
||||
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:27:43 2014"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
|
||||
|
||||
#endif // MAVLINK_VERSION_H
|
||||
|
||||
File diff suppressed because one or more lines are too long
+34
-34
@@ -1,23 +1,23 @@
|
||||
// MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING
|
||||
|
||||
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 193
|
||||
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 130
|
||||
|
||||
typedef struct __mavlink_data_transmission_handshake_t
|
||||
{
|
||||
uint32_t size; ///< total data size in bytes (set on ACK only)
|
||||
uint16_t width; ///< Width of a matrix or image
|
||||
uint16_t height; ///< Height of a matrix or image
|
||||
uint16_t packets; ///< number of packets beeing sent (set on ACK only)
|
||||
uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h)
|
||||
uint8_t packets; ///< number of packets beeing sent (set on ACK only)
|
||||
uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only)
|
||||
uint8_t jpg_quality; ///< JPEG quality out of [1,100]
|
||||
} mavlink_data_transmission_handshake_t;
|
||||
|
||||
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 12
|
||||
#define MAVLINK_MSG_ID_193_LEN 12
|
||||
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 13
|
||||
#define MAVLINK_MSG_ID_130_LEN 13
|
||||
|
||||
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC 23
|
||||
#define MAVLINK_MSG_ID_193_CRC 23
|
||||
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC 29
|
||||
#define MAVLINK_MSG_ID_130_CRC 29
|
||||
|
||||
|
||||
|
||||
@@ -27,10 +27,10 @@ typedef struct __mavlink_data_transmission_handshake_t
|
||||
{ { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_data_transmission_handshake_t, size) }, \
|
||||
{ "width", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_data_transmission_handshake_t, width) }, \
|
||||
{ "height", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_data_transmission_handshake_t, height) }, \
|
||||
{ "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_data_transmission_handshake_t, type) }, \
|
||||
{ "packets", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_data_transmission_handshake_t, packets) }, \
|
||||
{ "payload", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_data_transmission_handshake_t, payload) }, \
|
||||
{ "jpg_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \
|
||||
{ "packets", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_data_transmission_handshake_t, packets) }, \
|
||||
{ "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_data_transmission_handshake_t, type) }, \
|
||||
{ "payload", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_data_transmission_handshake_t, payload) }, \
|
||||
{ "jpg_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \
|
||||
} \
|
||||
}
|
||||
|
||||
@@ -51,17 +51,17 @@ typedef struct __mavlink_data_transmission_handshake_t
|
||||
* @return length of the message in bytes (excluding serial stream start sign)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
|
||||
uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint8_t packets, uint8_t payload, uint8_t jpg_quality)
|
||||
uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN];
|
||||
_mav_put_uint32_t(buf, 0, size);
|
||||
_mav_put_uint16_t(buf, 4, width);
|
||||
_mav_put_uint16_t(buf, 6, height);
|
||||
_mav_put_uint8_t(buf, 8, type);
|
||||
_mav_put_uint8_t(buf, 9, packets);
|
||||
_mav_put_uint8_t(buf, 10, payload);
|
||||
_mav_put_uint8_t(buf, 11, jpg_quality);
|
||||
_mav_put_uint16_t(buf, 8, packets);
|
||||
_mav_put_uint8_t(buf, 10, type);
|
||||
_mav_put_uint8_t(buf, 11, payload);
|
||||
_mav_put_uint8_t(buf, 12, jpg_quality);
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
|
||||
#else
|
||||
@@ -69,8 +69,8 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst
|
||||
packet.size = size;
|
||||
packet.width = width;
|
||||
packet.height = height;
|
||||
packet.type = type;
|
||||
packet.packets = packets;
|
||||
packet.type = type;
|
||||
packet.payload = payload;
|
||||
packet.jpg_quality = jpg_quality;
|
||||
|
||||
@@ -102,17 +102,17 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
|
||||
mavlink_message_t* msg,
|
||||
uint8_t type,uint32_t size,uint16_t width,uint16_t height,uint8_t packets,uint8_t payload,uint8_t jpg_quality)
|
||||
uint8_t type,uint32_t size,uint16_t width,uint16_t height,uint16_t packets,uint8_t payload,uint8_t jpg_quality)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN];
|
||||
_mav_put_uint32_t(buf, 0, size);
|
||||
_mav_put_uint16_t(buf, 4, width);
|
||||
_mav_put_uint16_t(buf, 6, height);
|
||||
_mav_put_uint8_t(buf, 8, type);
|
||||
_mav_put_uint8_t(buf, 9, packets);
|
||||
_mav_put_uint8_t(buf, 10, payload);
|
||||
_mav_put_uint8_t(buf, 11, jpg_quality);
|
||||
_mav_put_uint16_t(buf, 8, packets);
|
||||
_mav_put_uint8_t(buf, 10, type);
|
||||
_mav_put_uint8_t(buf, 11, payload);
|
||||
_mav_put_uint8_t(buf, 12, jpg_quality);
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
|
||||
#else
|
||||
@@ -120,8 +120,8 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t
|
||||
packet.size = size;
|
||||
packet.width = width;
|
||||
packet.height = height;
|
||||
packet.type = type;
|
||||
packet.packets = packets;
|
||||
packet.type = type;
|
||||
packet.payload = payload;
|
||||
packet.jpg_quality = jpg_quality;
|
||||
|
||||
@@ -177,17 +177,17 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode_chan(uint8
|
||||
*/
|
||||
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
|
||||
static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint8_t packets, uint8_t payload, uint8_t jpg_quality)
|
||||
static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN];
|
||||
_mav_put_uint32_t(buf, 0, size);
|
||||
_mav_put_uint16_t(buf, 4, width);
|
||||
_mav_put_uint16_t(buf, 6, height);
|
||||
_mav_put_uint8_t(buf, 8, type);
|
||||
_mav_put_uint8_t(buf, 9, packets);
|
||||
_mav_put_uint8_t(buf, 10, payload);
|
||||
_mav_put_uint8_t(buf, 11, jpg_quality);
|
||||
_mav_put_uint16_t(buf, 8, packets);
|
||||
_mav_put_uint8_t(buf, 10, type);
|
||||
_mav_put_uint8_t(buf, 11, payload);
|
||||
_mav_put_uint8_t(buf, 12, jpg_quality);
|
||||
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC);
|
||||
@@ -199,8 +199,8 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_
|
||||
packet.size = size;
|
||||
packet.width = width;
|
||||
packet.height = height;
|
||||
packet.type = type;
|
||||
packet.packets = packets;
|
||||
packet.type = type;
|
||||
packet.payload = payload;
|
||||
packet.jpg_quality = jpg_quality;
|
||||
|
||||
@@ -224,7 +224,7 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 8);
|
||||
return _MAV_RETURN_uint8_t(msg, 10);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -262,9 +262,9 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_get_height(const
|
||||
*
|
||||
* @return number of packets beeing sent (set on ACK only)
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg)
|
||||
static inline uint16_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 9);
|
||||
return _MAV_RETURN_uint16_t(msg, 8);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -274,7 +274,7 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 10);
|
||||
return _MAV_RETURN_uint8_t(msg, 11);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -284,7 +284,7 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 11);
|
||||
return _MAV_RETURN_uint8_t(msg, 12);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -299,8 +299,8 @@ static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_
|
||||
data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg);
|
||||
data_transmission_handshake->width = mavlink_msg_data_transmission_handshake_get_width(msg);
|
||||
data_transmission_handshake->height = mavlink_msg_data_transmission_handshake_get_height(msg);
|
||||
data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg);
|
||||
data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg);
|
||||
data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg);
|
||||
data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg);
|
||||
data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg);
|
||||
#else
|
||||
+3
-3
@@ -1,6 +1,6 @@
|
||||
// MESSAGE ENCAPSULATED_DATA PACKING
|
||||
|
||||
#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 194
|
||||
#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 131
|
||||
|
||||
typedef struct __mavlink_encapsulated_data_t
|
||||
{
|
||||
@@ -9,10 +9,10 @@ typedef struct __mavlink_encapsulated_data_t
|
||||
} mavlink_encapsulated_data_t;
|
||||
|
||||
#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255
|
||||
#define MAVLINK_MSG_ID_194_LEN 255
|
||||
#define MAVLINK_MSG_ID_131_LEN 255
|
||||
|
||||
#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC 223
|
||||
#define MAVLINK_MSG_ID_194_CRC 223
|
||||
#define MAVLINK_MSG_ID_131_CRC 223
|
||||
|
||||
#define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253
|
||||
|
||||
@@ -0,0 +1,419 @@
|
||||
// MESSAGE GPS2_RAW PACKING
|
||||
|
||||
#define MAVLINK_MSG_ID_GPS2_RAW 124
|
||||
|
||||
typedef struct __mavlink_gps2_raw_t
|
||||
{
|
||||
uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||
int32_t lat; ///< Latitude (WGS84), in degrees * 1E7
|
||||
int32_t lon; ///< Longitude (WGS84), in degrees * 1E7
|
||||
int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up)
|
||||
uint32_t dgps_age; ///< Age of DGPS info
|
||||
uint16_t eph; ///< GPS HDOP 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.
|
||||
uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255
|
||||
uint8_t dgps_numch; ///< Number of DGPS satellites
|
||||
} mavlink_gps2_raw_t;
|
||||
|
||||
#define MAVLINK_MSG_ID_GPS2_RAW_LEN 35
|
||||
#define MAVLINK_MSG_ID_124_LEN 35
|
||||
|
||||
#define MAVLINK_MSG_ID_GPS2_RAW_CRC 87
|
||||
#define MAVLINK_MSG_ID_124_CRC 87
|
||||
|
||||
|
||||
|
||||
#define MAVLINK_MESSAGE_INFO_GPS2_RAW { \
|
||||
"GPS2_RAW", \
|
||||
12, \
|
||||
{ { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \
|
||||
{ "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \
|
||||
{ "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_raw_t, lon) }, \
|
||||
{ "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_raw_t, alt) }, \
|
||||
{ "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \
|
||||
{ "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps2_raw_t, eph) }, \
|
||||
{ "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps2_raw_t, epv) }, \
|
||||
{ "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_raw_t, vel) }, \
|
||||
{ "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_gps2_raw_t, cog) }, \
|
||||
{ "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \
|
||||
{ "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \
|
||||
{ "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Pack a gps2_raw 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||
* @param 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.
|
||||
* @param lat Latitude (WGS84), in degrees * 1E7
|
||||
* @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 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
|
||||
* @param dgps_numch Number of DGPS satellites
|
||||
* @param dgps_age Age of DGPS info
|
||||
* @return length of the message in bytes (excluding serial stream start sign)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
|
||||
uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN];
|
||||
_mav_put_uint64_t(buf, 0, time_usec);
|
||||
_mav_put_int32_t(buf, 8, lat);
|
||||
_mav_put_int32_t(buf, 12, lon);
|
||||
_mav_put_int32_t(buf, 16, alt);
|
||||
_mav_put_uint32_t(buf, 20, dgps_age);
|
||||
_mav_put_uint16_t(buf, 24, eph);
|
||||
_mav_put_uint16_t(buf, 26, epv);
|
||||
_mav_put_uint16_t(buf, 28, vel);
|
||||
_mav_put_uint16_t(buf, 30, cog);
|
||||
_mav_put_uint8_t(buf, 32, fix_type);
|
||||
_mav_put_uint8_t(buf, 33, satellites_visible);
|
||||
_mav_put_uint8_t(buf, 34, dgps_numch);
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN);
|
||||
#else
|
||||
mavlink_gps2_raw_t packet;
|
||||
packet.time_usec = time_usec;
|
||||
packet.lat = lat;
|
||||
packet.lon = lon;
|
||||
packet.alt = alt;
|
||||
packet.dgps_age = dgps_age;
|
||||
packet.eph = eph;
|
||||
packet.epv = epv;
|
||||
packet.vel = vel;
|
||||
packet.cog = cog;
|
||||
packet.fix_type = fix_type;
|
||||
packet.satellites_visible = satellites_visible;
|
||||
packet.dgps_numch = dgps_numch;
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN);
|
||||
#endif
|
||||
|
||||
msg->msgid = MAVLINK_MSG_ID_GPS2_RAW;
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
|
||||
#else
|
||||
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Pack a gps2_raw 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_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||
* @param 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.
|
||||
* @param lat Latitude (WGS84), in degrees * 1E7
|
||||
* @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 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
|
||||
* @param dgps_numch Number of DGPS satellites
|
||||
* @param dgps_age Age of DGPS info
|
||||
* @return length of the message in bytes (excluding serial stream start sign)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
|
||||
mavlink_message_t* msg,
|
||||
uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN];
|
||||
_mav_put_uint64_t(buf, 0, time_usec);
|
||||
_mav_put_int32_t(buf, 8, lat);
|
||||
_mav_put_int32_t(buf, 12, lon);
|
||||
_mav_put_int32_t(buf, 16, alt);
|
||||
_mav_put_uint32_t(buf, 20, dgps_age);
|
||||
_mav_put_uint16_t(buf, 24, eph);
|
||||
_mav_put_uint16_t(buf, 26, epv);
|
||||
_mav_put_uint16_t(buf, 28, vel);
|
||||
_mav_put_uint16_t(buf, 30, cog);
|
||||
_mav_put_uint8_t(buf, 32, fix_type);
|
||||
_mav_put_uint8_t(buf, 33, satellites_visible);
|
||||
_mav_put_uint8_t(buf, 34, dgps_numch);
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN);
|
||||
#else
|
||||
mavlink_gps2_raw_t packet;
|
||||
packet.time_usec = time_usec;
|
||||
packet.lat = lat;
|
||||
packet.lon = lon;
|
||||
packet.alt = alt;
|
||||
packet.dgps_age = dgps_age;
|
||||
packet.eph = eph;
|
||||
packet.epv = epv;
|
||||
packet.vel = vel;
|
||||
packet.cog = cog;
|
||||
packet.fix_type = fix_type;
|
||||
packet.satellites_visible = satellites_visible;
|
||||
packet.dgps_numch = dgps_numch;
|
||||
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN);
|
||||
#endif
|
||||
|
||||
msg->msgid = MAVLINK_MSG_ID_GPS2_RAW;
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
|
||||
#else
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Encode a gps2_raw 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 gps2_raw C-struct to read the message contents from
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw)
|
||||
{
|
||||
return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Encode a gps2_raw 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 gps2_raw C-struct to read the message contents from
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw)
|
||||
{
|
||||
return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send a gps2_raw message
|
||||
* @param chan MAVLink channel to send the message
|
||||
*
|
||||
* @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||
* @param 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.
|
||||
* @param lat Latitude (WGS84), in degrees * 1E7
|
||||
* @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 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
|
||||
* @param dgps_numch Number of DGPS satellites
|
||||
* @param dgps_age Age of DGPS info
|
||||
*/
|
||||
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
|
||||
static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN];
|
||||
_mav_put_uint64_t(buf, 0, time_usec);
|
||||
_mav_put_int32_t(buf, 8, lat);
|
||||
_mav_put_int32_t(buf, 12, lon);
|
||||
_mav_put_int32_t(buf, 16, alt);
|
||||
_mav_put_uint32_t(buf, 20, dgps_age);
|
||||
_mav_put_uint16_t(buf, 24, eph);
|
||||
_mav_put_uint16_t(buf, 26, epv);
|
||||
_mav_put_uint16_t(buf, 28, vel);
|
||||
_mav_put_uint16_t(buf, 30, cog);
|
||||
_mav_put_uint8_t(buf, 32, fix_type);
|
||||
_mav_put_uint8_t(buf, 33, satellites_visible);
|
||||
_mav_put_uint8_t(buf, 34, dgps_numch);
|
||||
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
|
||||
#else
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN);
|
||||
#endif
|
||||
#else
|
||||
mavlink_gps2_raw_t packet;
|
||||
packet.time_usec = time_usec;
|
||||
packet.lat = lat;
|
||||
packet.lon = lon;
|
||||
packet.alt = alt;
|
||||
packet.dgps_age = dgps_age;
|
||||
packet.eph = eph;
|
||||
packet.epv = epv;
|
||||
packet.vel = vel;
|
||||
packet.cog = cog;
|
||||
packet.fix_type = fix_type;
|
||||
packet.satellites_visible = satellites_visible;
|
||||
packet.dgps_numch = dgps_numch;
|
||||
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
|
||||
#else
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// MESSAGE GPS2_RAW UNPACKING
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get field time_usec from gps2_raw message
|
||||
*
|
||||
* @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||
*/
|
||||
static inline uint64_t mavlink_msg_gps2_raw_get_time_usec(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint64_t(msg, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field fix_type from gps2_raw message
|
||||
*
|
||||
* @return 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.
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_gps2_raw_get_fix_type(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 32);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field lat from gps2_raw message
|
||||
*
|
||||
* @return Latitude (WGS84), in degrees * 1E7
|
||||
*/
|
||||
static inline int32_t mavlink_msg_gps2_raw_get_lat(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_int32_t(msg, 8);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field lon from gps2_raw message
|
||||
*
|
||||
* @return Longitude (WGS84), in degrees * 1E7
|
||||
*/
|
||||
static inline int32_t mavlink_msg_gps2_raw_get_lon(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_int32_t(msg, 12);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field alt from gps2_raw message
|
||||
*
|
||||
* @return Altitude (WGS84), in meters * 1000 (positive for up)
|
||||
*/
|
||||
static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_int32_t(msg, 16);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field eph from gps2_raw message
|
||||
*
|
||||
* @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint16_t(msg, 24);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field epv from gps2_raw message
|
||||
*
|
||||
* @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint16_t(msg, 26);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field vel from gps2_raw message
|
||||
*
|
||||
* @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps2_raw_get_vel(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint16_t(msg, 28);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field cog from gps2_raw message
|
||||
*
|
||||
* @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps2_raw_get_cog(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint16_t(msg, 30);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field satellites_visible from gps2_raw message
|
||||
*
|
||||
* @return Number of satellites visible. If unknown, set to 255
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_gps2_raw_get_satellites_visible(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 33);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field dgps_numch from gps2_raw message
|
||||
*
|
||||
* @return Number of DGPS satellites
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_gps2_raw_get_dgps_numch(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 34);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field dgps_age from gps2_raw message
|
||||
*
|
||||
* @return Age of DGPS info
|
||||
*/
|
||||
static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint32_t(msg, 20);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Decode a gps2_raw message into a struct
|
||||
*
|
||||
* @param msg The message to decode
|
||||
* @param gps2_raw C-struct to decode the message contents into
|
||||
*/
|
||||
static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mavlink_gps2_raw_t* gps2_raw)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP
|
||||
gps2_raw->time_usec = mavlink_msg_gps2_raw_get_time_usec(msg);
|
||||
gps2_raw->lat = mavlink_msg_gps2_raw_get_lat(msg);
|
||||
gps2_raw->lon = mavlink_msg_gps2_raw_get_lon(msg);
|
||||
gps2_raw->alt = mavlink_msg_gps2_raw_get_alt(msg);
|
||||
gps2_raw->dgps_age = mavlink_msg_gps2_raw_get_dgps_age(msg);
|
||||
gps2_raw->eph = mavlink_msg_gps2_raw_get_eph(msg);
|
||||
gps2_raw->epv = mavlink_msg_gps2_raw_get_epv(msg);
|
||||
gps2_raw->vel = mavlink_msg_gps2_raw_get_vel(msg);
|
||||
gps2_raw->cog = mavlink_msg_gps2_raw_get_cog(msg);
|
||||
gps2_raw->fix_type = mavlink_msg_gps2_raw_get_fix_type(msg);
|
||||
gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg);
|
||||
gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg);
|
||||
#else
|
||||
memcpy(gps2_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RAW_LEN);
|
||||
#endif
|
||||
}
|
||||
@@ -0,0 +1,237 @@
|
||||
// MESSAGE GPS_INJECT_DATA PACKING
|
||||
|
||||
#define MAVLINK_MSG_ID_GPS_INJECT_DATA 123
|
||||
|
||||
typedef struct __mavlink_gps_inject_data_t
|
||||
{
|
||||
uint8_t target_system; ///< System ID
|
||||
uint8_t target_component; ///< Component ID
|
||||
uint8_t len; ///< data length
|
||||
uint8_t data[110]; ///< raw data (110 is enough for 12 satellites of RTCMv2)
|
||||
} mavlink_gps_inject_data_t;
|
||||
|
||||
#define MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN 113
|
||||
#define MAVLINK_MSG_ID_123_LEN 113
|
||||
|
||||
#define MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC 250
|
||||
#define MAVLINK_MSG_ID_123_CRC 250
|
||||
|
||||
#define MAVLINK_MSG_GPS_INJECT_DATA_FIELD_DATA_LEN 110
|
||||
|
||||
#define MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA { \
|
||||
"GPS_INJECT_DATA", \
|
||||
4, \
|
||||
{ { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_inject_data_t, target_system) }, \
|
||||
{ "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_inject_data_t, target_component) }, \
|
||||
{ "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gps_inject_data_t, len) }, \
|
||||
{ "data", NULL, MAVLINK_TYPE_UINT8_T, 110, 3, offsetof(mavlink_gps_inject_data_t, data) }, \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Pack a gps_inject_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 len data length
|
||||
* @param data raw data (110 is enough for 12 satellites of RTCMv2)
|
||||
* @return length of the message in bytes (excluding serial stream start sign)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps_inject_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
|
||||
uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN];
|
||||
_mav_put_uint8_t(buf, 0, target_system);
|
||||
_mav_put_uint8_t(buf, 1, target_component);
|
||||
_mav_put_uint8_t(buf, 2, len);
|
||||
_mav_put_uint8_t_array(buf, 3, data, 110);
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
|
||||
#else
|
||||
mavlink_gps_inject_data_t packet;
|
||||
packet.target_system = target_system;
|
||||
packet.target_component = target_component;
|
||||
packet.len = len;
|
||||
mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110);
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
|
||||
#endif
|
||||
|
||||
msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA;
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
|
||||
#else
|
||||
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Pack a gps_inject_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 len data length
|
||||
* @param data raw data (110 is enough for 12 satellites of RTCMv2)
|
||||
* @return length of the message in bytes (excluding serial stream start sign)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps_inject_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,uint8_t len,const uint8_t *data)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN];
|
||||
_mav_put_uint8_t(buf, 0, target_system);
|
||||
_mav_put_uint8_t(buf, 1, target_component);
|
||||
_mav_put_uint8_t(buf, 2, len);
|
||||
_mav_put_uint8_t_array(buf, 3, data, 110);
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
|
||||
#else
|
||||
mavlink_gps_inject_data_t packet;
|
||||
packet.target_system = target_system;
|
||||
packet.target_component = target_component;
|
||||
packet.len = len;
|
||||
mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110);
|
||||
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
|
||||
#endif
|
||||
|
||||
msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA;
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
|
||||
#else
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Encode a gps_inject_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 gps_inject_data C-struct to read the message contents from
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps_inject_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data)
|
||||
{
|
||||
return mavlink_msg_gps_inject_data_pack(system_id, component_id, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Encode a gps_inject_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 gps_inject_data C-struct to read the message contents from
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps_inject_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data)
|
||||
{
|
||||
return mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, chan, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send a gps_inject_data message
|
||||
* @param chan MAVLink channel to send the message
|
||||
*
|
||||
* @param target_system System ID
|
||||
* @param target_component Component ID
|
||||
* @param len data length
|
||||
* @param data raw data (110 is enough for 12 satellites of RTCMv2)
|
||||
*/
|
||||
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
|
||||
static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
||||
char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN];
|
||||
_mav_put_uint8_t(buf, 0, target_system);
|
||||
_mav_put_uint8_t(buf, 1, target_component);
|
||||
_mav_put_uint8_t(buf, 2, len);
|
||||
_mav_put_uint8_t_array(buf, 3, data, 110);
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
|
||||
#else
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
|
||||
#endif
|
||||
#else
|
||||
mavlink_gps_inject_data_t packet;
|
||||
packet.target_system = target_system;
|
||||
packet.target_component = target_component;
|
||||
packet.len = len;
|
||||
mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110);
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
|
||||
#else
|
||||
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// MESSAGE GPS_INJECT_DATA UNPACKING
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get field target_system from gps_inject_data message
|
||||
*
|
||||
* @return System ID
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_gps_inject_data_get_target_system(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field target_component from gps_inject_data message
|
||||
*
|
||||
* @return Component ID
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_gps_inject_data_get_target_component(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field len from gps_inject_data message
|
||||
*
|
||||
* @return data length
|
||||
*/
|
||||
static inline uint8_t mavlink_msg_gps_inject_data_get_len(const mavlink_message_t* msg)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t(msg, 2);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get field data from gps_inject_data message
|
||||
*
|
||||
* @return raw data (110 is enough for 12 satellites of RTCMv2)
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_gps_inject_data_get_data(const mavlink_message_t* msg, uint8_t *data)
|
||||
{
|
||||
return _MAV_RETURN_uint8_t_array(msg, data, 110, 3);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Decode a gps_inject_data message into a struct
|
||||
*
|
||||
* @param msg The message to decode
|
||||
* @param gps_inject_data C-struct to decode the message contents into
|
||||
*/
|
||||
static inline void mavlink_msg_gps_inject_data_decode(const mavlink_message_t* msg, mavlink_gps_inject_data_t* gps_inject_data)
|
||||
{
|
||||
#if MAVLINK_NEED_BYTE_SWAP
|
||||
gps_inject_data->target_system = mavlink_msg_gps_inject_data_get_target_system(msg);
|
||||
gps_inject_data->target_component = mavlink_msg_gps_inject_data_get_target_component(msg);
|
||||
gps_inject_data->len = mavlink_msg_gps_inject_data_get_len(msg);
|
||||
mavlink_msg_gps_inject_data_get_data(msg, gps_inject_data->data);
|
||||
#else
|
||||
memcpy(gps_inject_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
|
||||
#endif
|
||||
}
|
||||
@@ -4839,6 +4839,220 @@ static void mavlink_test_log_request_end(uint8_t system_id, uint8_t component_id
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
static void mavlink_test_gps_inject_data(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_gps_inject_data_t packet_in = {
|
||||
5,
|
||||
}72,
|
||||
}139,
|
||||
}{ 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 },
|
||||
};
|
||||
mavlink_gps_inject_data_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
packet1.target_system = packet_in.target_system;
|
||||
packet1.target_component = packet_in.target_component;
|
||||
packet1.len = packet_in.len;
|
||||
|
||||
mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*110);
|
||||
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_gps_inject_data_encode(system_id, component_id, &msg, &packet1);
|
||||
mavlink_msg_gps_inject_data_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_gps_inject_data_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data );
|
||||
mavlink_msg_gps_inject_data_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data );
|
||||
mavlink_msg_gps_inject_data_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_gps_inject_data_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_gps_inject_data_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.len , packet1.data );
|
||||
mavlink_msg_gps_inject_data_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
static void mavlink_test_gps2_raw(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_gps2_raw_t packet_in = {
|
||||
93372036854775807ULL,
|
||||
}963497880,
|
||||
}963498088,
|
||||
}963498296,
|
||||
}963498504,
|
||||
}18483,
|
||||
}18587,
|
||||
}18691,
|
||||
}18795,
|
||||
}101,
|
||||
}168,
|
||||
}235,
|
||||
};
|
||||
mavlink_gps2_raw_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
packet1.time_usec = packet_in.time_usec;
|
||||
packet1.lat = packet_in.lat;
|
||||
packet1.lon = packet_in.lon;
|
||||
packet1.alt = packet_in.alt;
|
||||
packet1.dgps_age = packet_in.dgps_age;
|
||||
packet1.eph = packet_in.eph;
|
||||
packet1.epv = packet_in.epv;
|
||||
packet1.vel = packet_in.vel;
|
||||
packet1.cog = packet_in.cog;
|
||||
packet1.fix_type = packet_in.fix_type;
|
||||
packet1.satellites_visible = packet_in.satellites_visible;
|
||||
packet1.dgps_numch = packet_in.dgps_numch;
|
||||
|
||||
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_gps2_raw_encode(system_id, component_id, &msg, &packet1);
|
||||
mavlink_msg_gps2_raw_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_gps2_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age );
|
||||
mavlink_msg_gps2_raw_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_gps2_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age );
|
||||
mavlink_msg_gps2_raw_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_gps2_raw_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_gps2_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age );
|
||||
mavlink_msg_gps2_raw_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
static void mavlink_test_data_transmission_handshake(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_data_transmission_handshake_t packet_in = {
|
||||
963497464,
|
||||
}17443,
|
||||
}17547,
|
||||
}17651,
|
||||
}163,
|
||||
}230,
|
||||
}41,
|
||||
};
|
||||
mavlink_data_transmission_handshake_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
packet1.size = packet_in.size;
|
||||
packet1.width = packet_in.width;
|
||||
packet1.height = packet_in.height;
|
||||
packet1.packets = packet_in.packets;
|
||||
packet1.type = packet_in.type;
|
||||
packet1.payload = packet_in.payload;
|
||||
packet1.jpg_quality = packet_in.jpg_quality;
|
||||
|
||||
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_data_transmission_handshake_encode(system_id, component_id, &msg, &packet1);
|
||||
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_data_transmission_handshake_pack(system_id, component_id, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
|
||||
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
|
||||
mavlink_msg_data_transmission_handshake_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_data_transmission_handshake_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_data_transmission_handshake_send(MAVLINK_COMM_1 , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
|
||||
mavlink_msg_data_transmission_handshake_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
static void mavlink_test_encapsulated_data(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_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 },
|
||||
};
|
||||
mavlink_encapsulated_data_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
packet1.seqnr = packet_in.seqnr;
|
||||
|
||||
mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*253);
|
||||
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_encapsulated_data_encode(system_id, component_id, &msg, &packet1);
|
||||
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_encapsulated_data_pack(system_id, component_id, &msg , packet1.seqnr , packet1.data );
|
||||
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seqnr , packet1.data );
|
||||
mavlink_msg_encapsulated_data_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_encapsulated_data_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_1 , packet1.seqnr , packet1.data );
|
||||
mavlink_msg_encapsulated_data_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
@@ -5393,6 +5607,10 @@ static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink
|
||||
mavlink_test_log_data(system_id, component_id, last_msg);
|
||||
mavlink_test_log_erase(system_id, component_id, last_msg);
|
||||
mavlink_test_log_request_end(system_id, component_id, last_msg);
|
||||
mavlink_test_gps_inject_data(system_id, component_id, last_msg);
|
||||
mavlink_test_gps2_raw(system_id, component_id, last_msg);
|
||||
mavlink_test_data_transmission_handshake(system_id, component_id, last_msg);
|
||||
mavlink_test_encapsulated_data(system_id, component_id, last_msg);
|
||||
mavlink_test_battery_status(system_id, component_id, last_msg);
|
||||
mavlink_test_setpoint_8dof(system_id, component_id, last_msg);
|
||||
mavlink_test_setpoint_6dof(system_id, component_id, last_msg);
|
||||
|
||||
@@ -5,8 +5,8 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:59:18 2013"
|
||||
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:29 2014"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
|
||||
|
||||
#endif // MAVLINK_VERSION_H
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -5,8 +5,8 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:57:49 2013"
|
||||
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:04 2014"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
|
||||
|
||||
#endif // MAVLINK_VERSION_H
|
||||
|
||||
@@ -48,7 +48,7 @@ typedef struct __mavlink_system {
|
||||
uint8_t type; ///< Unused, can be used by user to store the system's type
|
||||
uint8_t state; ///< Unused, can be used by user to store the system's state
|
||||
uint8_t mode; ///< Unused, can be used by user to store the system's mode
|
||||
uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode
|
||||
uint32_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode
|
||||
} mavlink_system_t;
|
||||
|
||||
typedef struct __mavlink_message {
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -853,106 +853,6 @@ static void mavlink_test_point_of_interest_connection(uint8_t system_id, uint8_t
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
static void mavlink_test_data_transmission_handshake(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_data_transmission_handshake_t packet_in = {
|
||||
963497464,
|
||||
}17443,
|
||||
}17547,
|
||||
}29,
|
||||
}96,
|
||||
}163,
|
||||
}230,
|
||||
};
|
||||
mavlink_data_transmission_handshake_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
packet1.size = packet_in.size;
|
||||
packet1.width = packet_in.width;
|
||||
packet1.height = packet_in.height;
|
||||
packet1.type = packet_in.type;
|
||||
packet1.packets = packet_in.packets;
|
||||
packet1.payload = packet_in.payload;
|
||||
packet1.jpg_quality = packet_in.jpg_quality;
|
||||
|
||||
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_data_transmission_handshake_encode(system_id, component_id, &msg, &packet1);
|
||||
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_data_transmission_handshake_pack(system_id, component_id, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
|
||||
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
|
||||
mavlink_msg_data_transmission_handshake_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_data_transmission_handshake_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_data_transmission_handshake_send(MAVLINK_COMM_1 , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
|
||||
mavlink_msg_data_transmission_handshake_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
static void mavlink_test_encapsulated_data(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_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 },
|
||||
};
|
||||
mavlink_encapsulated_data_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
packet1.seqnr = packet_in.seqnr;
|
||||
|
||||
mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*253);
|
||||
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_encapsulated_data_encode(system_id, component_id, &msg, &packet1);
|
||||
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_encapsulated_data_pack(system_id, component_id, &msg , packet1.seqnr , packet1.data );
|
||||
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seqnr , packet1.data );
|
||||
mavlink_msg_encapsulated_data_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_encapsulated_data_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
|
||||
memset(&packet2, 0, sizeof(packet2));
|
||||
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_1 , packet1.seqnr , packet1.data );
|
||||
mavlink_msg_encapsulated_data_decode(last_msg, &packet2);
|
||||
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
|
||||
}
|
||||
|
||||
static void mavlink_test_brief_feature(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
@@ -1086,8 +986,6 @@ static void mavlink_test_pixhawk(uint8_t system_id, uint8_t component_id, mavlin
|
||||
mavlink_test_pattern_detected(system_id, component_id, last_msg);
|
||||
mavlink_test_point_of_interest(system_id, component_id, last_msg);
|
||||
mavlink_test_point_of_interest_connection(system_id, component_id, last_msg);
|
||||
mavlink_test_data_transmission_handshake(system_id, component_id, last_msg);
|
||||
mavlink_test_encapsulated_data(system_id, component_id, last_msg);
|
||||
mavlink_test_brief_feature(system_id, component_id, last_msg);
|
||||
mavlink_test_attitude_control(system_id, component_id, last_msg);
|
||||
}
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:58:02 2013"
|
||||
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:16 2014"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
|
||||
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -31,8 +31,8 @@ static void mavlink_test_obs_position(uint8_t system_id, uint8_t component_id, m
|
||||
uint16_t i;
|
||||
mavlink_obs_position_t packet_in = {
|
||||
963497464,
|
||||
963497672,
|
||||
963497880,
|
||||
}963497672,
|
||||
}963497880,
|
||||
};
|
||||
mavlink_obs_position_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
@@ -207,8 +207,8 @@ static void mavlink_test_obs_air_velocity(uint8_t system_id, uint8_t component_i
|
||||
uint16_t i;
|
||||
mavlink_obs_air_velocity_t packet_in = {
|
||||
17.0,
|
||||
45.0,
|
||||
73.0,
|
||||
}45.0,
|
||||
}73.0,
|
||||
};
|
||||
mavlink_obs_air_velocity_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
@@ -254,7 +254,7 @@ static void mavlink_test_obs_bias(uint8_t system_id, uint8_t component_id, mavli
|
||||
uint16_t i;
|
||||
mavlink_obs_bias_t packet_in = {
|
||||
{ 17.0, 18.0, 19.0 },
|
||||
{ 101.0, 102.0, 103.0 },
|
||||
}{ 101.0, 102.0, 103.0 },
|
||||
};
|
||||
mavlink_obs_bias_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
@@ -428,7 +428,7 @@ static void mavlink_test_llc_out(uint8_t system_id, uint8_t component_id, mavlin
|
||||
uint16_t i;
|
||||
mavlink_llc_out_t packet_in = {
|
||||
{ 17235, 17236, 17237, 17238 },
|
||||
{ 17651, 17652 },
|
||||
}{ 17651, 17652 },
|
||||
};
|
||||
mavlink_llc_out_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
@@ -473,8 +473,8 @@ static void mavlink_test_pm_elec(uint8_t system_id, uint8_t component_id, mavlin
|
||||
uint16_t i;
|
||||
mavlink_pm_elec_t packet_in = {
|
||||
17.0,
|
||||
45.0,
|
||||
{ 73.0, 74.0, 75.0 },
|
||||
}45.0,
|
||||
}{ 73.0, 74.0, 75.0 },
|
||||
};
|
||||
mavlink_pm_elec_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
@@ -520,9 +520,9 @@ static void mavlink_test_sys_stat(uint8_t system_id, uint8_t component_id, mavli
|
||||
uint16_t i;
|
||||
mavlink_sys_stat_t packet_in = {
|
||||
5,
|
||||
72,
|
||||
139,
|
||||
206,
|
||||
}72,
|
||||
}139,
|
||||
}206,
|
||||
};
|
||||
mavlink_sys_stat_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
@@ -569,7 +569,7 @@ static void mavlink_test_cmd_airspeed_chng(uint8_t system_id, uint8_t component_
|
||||
uint16_t i;
|
||||
mavlink_cmd_airspeed_chng_t packet_in = {
|
||||
17.0,
|
||||
17,
|
||||
}17,
|
||||
};
|
||||
mavlink_cmd_airspeed_chng_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
@@ -614,7 +614,7 @@ static void mavlink_test_cmd_airspeed_ack(uint8_t system_id, uint8_t component_i
|
||||
uint16_t i;
|
||||
mavlink_cmd_airspeed_ack_t packet_in = {
|
||||
17.0,
|
||||
17,
|
||||
}17,
|
||||
};
|
||||
mavlink_cmd_airspeed_ack_t packet1, packet2;
|
||||
memset(&packet1, 0, sizeof(packet1));
|
||||
|
||||
@@ -5,8 +5,8 @@
|
||||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Tue Sep 10 23:50:02 2013"
|
||||
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:27 2014"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
|
||||
|
||||
#endif // MAVLINK_VERSION_H
|
||||
|
||||
+36
-18
@@ -436,22 +436,22 @@ __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, flo
|
||||
__EXPORT float _wrap_pi(float bearing)
|
||||
{
|
||||
/* value is inf or NaN */
|
||||
if (!isfinite(bearing) || bearing == 0) {
|
||||
if (!isfinite(bearing)) {
|
||||
return bearing;
|
||||
}
|
||||
|
||||
int c = 0;
|
||||
|
||||
while (bearing > M_PI_F && c < 30) {
|
||||
while (bearing > M_PI_F) {
|
||||
bearing -= M_TWOPI_F;
|
||||
c++;
|
||||
if (c++ > 3)
|
||||
return NAN;
|
||||
}
|
||||
|
||||
c = 0;
|
||||
|
||||
while (bearing <= -M_PI_F && c < 30) {
|
||||
while (bearing <= -M_PI_F) {
|
||||
bearing += M_TWOPI_F;
|
||||
c++;
|
||||
if (c++ > 3)
|
||||
return NAN;
|
||||
}
|
||||
|
||||
return bearing;
|
||||
@@ -464,12 +464,18 @@ __EXPORT float _wrap_2pi(float bearing)
|
||||
return bearing;
|
||||
}
|
||||
|
||||
while (bearing >= M_TWOPI_F) {
|
||||
bearing = bearing - M_TWOPI_F;
|
||||
int c = 0;
|
||||
while (bearing > M_TWOPI_F) {
|
||||
bearing -= M_TWOPI_F;
|
||||
if (c++ > 3)
|
||||
return NAN;
|
||||
}
|
||||
|
||||
while (bearing < 0.0f) {
|
||||
bearing = bearing + M_TWOPI_F;
|
||||
c = 0;
|
||||
while (bearing <= 0.0f) {
|
||||
bearing += M_TWOPI_F;
|
||||
if (c++ > 3)
|
||||
return NAN;
|
||||
}
|
||||
|
||||
return bearing;
|
||||
@@ -482,12 +488,18 @@ __EXPORT float _wrap_180(float bearing)
|
||||
return bearing;
|
||||
}
|
||||
|
||||
int c = 0;
|
||||
while (bearing > 180.0f) {
|
||||
bearing = bearing - 360.0f;
|
||||
bearing -= 360.0f;
|
||||
if (c++ > 3)
|
||||
return NAN;
|
||||
}
|
||||
|
||||
while (bearing <= -180.0f) {
|
||||
bearing = bearing + 360.0f;
|
||||
c = 0;
|
||||
while (bearing <= -180.0f) {
|
||||
bearing += 360.0f;
|
||||
if (c++ > 3)
|
||||
return NAN;
|
||||
}
|
||||
|
||||
return bearing;
|
||||
@@ -500,12 +512,18 @@ __EXPORT float _wrap_360(float bearing)
|
||||
return bearing;
|
||||
}
|
||||
|
||||
while (bearing >= 360.0f) {
|
||||
bearing = bearing - 360.0f;
|
||||
int c = 0;
|
||||
while (bearing > 360.0f) {
|
||||
bearing -= 360.0f;
|
||||
if (c++ > 3)
|
||||
return NAN;
|
||||
}
|
||||
|
||||
while (bearing < 0.0f) {
|
||||
bearing = bearing + 360.0f;
|
||||
c = 0;
|
||||
while (bearing <= 0.0f) {
|
||||
bearing += 360.0f;
|
||||
if (c++ > 3)
|
||||
return NAN;
|
||||
}
|
||||
|
||||
return bearing;
|
||||
|
||||
@@ -83,14 +83,10 @@ bool LaunchDetector::getLaunchDetected()
|
||||
|
||||
void LaunchDetector::updateParams() {
|
||||
|
||||
warnx(" LaunchDetector::updateParams()");
|
||||
launchdetection_on.update();
|
||||
throttlePreTakeoff.update();
|
||||
|
||||
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
|
||||
launchMethods[i]->updateParams();
|
||||
warnx("updating component %d", i);
|
||||
}
|
||||
|
||||
warnx(" LaunchDetector::updateParams() ended");
|
||||
}
|
||||
|
||||
@@ -211,7 +211,7 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status);
|
||||
|
||||
void set_control_mode();
|
||||
|
||||
void print_reject_mode(const char *msg);
|
||||
void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
|
||||
|
||||
void print_reject_arm(const char *msg);
|
||||
|
||||
@@ -370,6 +370,7 @@ int arm()
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
@@ -382,6 +383,7 @@ int disarm()
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
@@ -661,6 +663,8 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* vehicle status topic */
|
||||
memset(&status, 0, sizeof(status));
|
||||
status.condition_landed = true; // initialize to safe value
|
||||
// We want to accept RC inputs as default
|
||||
status.rc_input_blocked = false;
|
||||
status.main_state = MAIN_STATE_MANUAL;
|
||||
status.set_nav_state = NAV_STATE_NONE;
|
||||
status.set_nav_state_timestamp = 0;
|
||||
@@ -939,10 +943,12 @@ 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);
|
||||
|
||||
static bool published_condition_landed_fw = false;
|
||||
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;
|
||||
published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes
|
||||
|
||||
if (status.condition_landed) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: LANDED");
|
||||
@@ -951,6 +957,12 @@ int commander_thread_main(int argc, char *argv[])
|
||||
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (!published_condition_landed_fw) {
|
||||
status.condition_landed = false; // Fixedwing does not have a landing detector currently
|
||||
published_condition_landed_fw = true;
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* update battery status */
|
||||
@@ -1109,7 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
/* start RC input check */
|
||||
if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
|
||||
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
|
||||
/* handle the case where RC signal was regained */
|
||||
if (!status.rc_signal_found_once) {
|
||||
status.rc_signal_found_once = true;
|
||||
@@ -1544,7 +1556,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
|
||||
{
|
||||
/* main mode switch */
|
||||
if (!isfinite(sp_man->mode_switch)) {
|
||||
warnx("mode sw not finite");
|
||||
/* default to manual if signal is invalid */
|
||||
status->mode_switch = MODE_SWITCH_MANUAL;
|
||||
|
||||
} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
|
||||
@@ -1634,7 +1646,7 @@ set_main_state_rc(struct vehicle_status_s *status)
|
||||
break; // changed successfully or already in this state
|
||||
|
||||
// else fallback to SEATBELT
|
||||
print_reject_mode("EASY");
|
||||
print_reject_mode(status, "EASY");
|
||||
}
|
||||
|
||||
res = main_state_transition(status, MAIN_STATE_SEATBELT);
|
||||
@@ -1643,7 +1655,7 @@ set_main_state_rc(struct vehicle_status_s *status)
|
||||
break; // changed successfully or already in this mode
|
||||
|
||||
if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
|
||||
print_reject_mode("SEATBELT");
|
||||
print_reject_mode(status, "SEATBELT");
|
||||
|
||||
// else fallback to MANUAL
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
@@ -1657,7 +1669,7 @@ set_main_state_rc(struct vehicle_status_s *status)
|
||||
break; // changed successfully or already in this state
|
||||
|
||||
// else fallback to SEATBELT (EASY likely will not work too)
|
||||
print_reject_mode("AUTO");
|
||||
print_reject_mode(status, "AUTO");
|
||||
res = main_state_transition(status, MAIN_STATE_SEATBELT);
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
@@ -1676,6 +1688,7 @@ set_main_state_rc(struct vehicle_status_s *status)
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
set_control_mode()
|
||||
{
|
||||
/* set vehicle_control_mode according to main state and failsafe state */
|
||||
@@ -1772,16 +1785,25 @@ set_control_mode()
|
||||
}
|
||||
|
||||
void
|
||||
print_reject_mode(const char *msg)
|
||||
print_reject_mode(struct vehicle_status_s *status, const char *msg)
|
||||
{
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
|
||||
last_print_mode_reject_time = t;
|
||||
char s[80];
|
||||
sprintf(s, "#audio: warning: reject %s", msg);
|
||||
sprintf(s, "#audio: REJECT %s", msg);
|
||||
mavlink_log_critical(mavlink_fd, s);
|
||||
tune_negative();
|
||||
|
||||
// only buzz if armed, because else we're driving people nuts indoors
|
||||
// they really need to look at the leds as well.
|
||||
if (status->arming_state == ARMING_STATE_ARMED) {
|
||||
tune_negative();
|
||||
} else {
|
||||
|
||||
// Always show the led indication
|
||||
led_negative();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1929,7 +1951,15 @@ void *commander_low_prio_loop(void *arg)
|
||||
} else if ((int)(cmd.param4) == 1) {
|
||||
/* RC calibration */
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
calib_ret = do_rc_calibration(mavlink_fd);
|
||||
/* disable RC control input completely */
|
||||
status.rc_input_blocked = true;
|
||||
calib_ret = OK;
|
||||
mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
|
||||
|
||||
} else if ((int)(cmd.param4) == 2) {
|
||||
/* RC trim calibration */
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
calib_ret = do_trim_calibration(mavlink_fd);
|
||||
|
||||
} else if ((int)(cmd.param5) == 1) {
|
||||
/* accelerometer calibration */
|
||||
@@ -1940,6 +1970,18 @@ void *commander_low_prio_loop(void *arg)
|
||||
/* airspeed calibration */
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
calib_ret = do_airspeed_calibration(mavlink_fd);
|
||||
} else if ((int)(cmd.param4) == 0) {
|
||||
/* RC calibration ended - have we been in one worth confirming? */
|
||||
if (status.rc_input_blocked) {
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
/* enable RC control input */
|
||||
status.rc_input_blocked = false;
|
||||
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
|
||||
}
|
||||
|
||||
/* this always succeeds */
|
||||
calib_ret = OK;
|
||||
|
||||
}
|
||||
|
||||
if (calib_ret == OK)
|
||||
@@ -1998,6 +2040,10 @@ void *commander_low_prio_loop(void *arg)
|
||||
break;
|
||||
}
|
||||
|
||||
case VEHICLE_CMD_START_RX_PAIR:
|
||||
/* handled in the IO driver */
|
||||
break;
|
||||
|
||||
default:
|
||||
/* don't answer on unsupported commands, it will be done in main loop */
|
||||
break;
|
||||
|
||||
@@ -123,11 +123,16 @@ void tune_neutral()
|
||||
}
|
||||
|
||||
void tune_negative()
|
||||
{
|
||||
led_negative();
|
||||
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
|
||||
}
|
||||
|
||||
void led_negative()
|
||||
{
|
||||
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
|
||||
rgbled_set_color(RGBLED_COLOR_RED);
|
||||
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
|
||||
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
|
||||
}
|
||||
|
||||
int tune_arm()
|
||||
|
||||
@@ -62,6 +62,9 @@ int tune_arm(void);
|
||||
int tune_low_bat(void);
|
||||
int tune_critical_bat(void);
|
||||
void tune_stop(void);
|
||||
|
||||
void led_negative();
|
||||
|
||||
int blink_msg_state();
|
||||
|
||||
int led_init(void);
|
||||
|
||||
@@ -53,17 +53,16 @@
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
int do_rc_calibration(int mavlink_fd)
|
||||
int do_trim_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "trim calibration starting");
|
||||
|
||||
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
usleep(200000);
|
||||
struct manual_control_setpoint_s sp;
|
||||
bool changed;
|
||||
orb_check(sub_man, &changed);
|
||||
|
||||
if (!changed) {
|
||||
mavlink_log_critical(mavlink_fd, "no manual control, aborting");
|
||||
mavlink_log_critical(mavlink_fd, "no inputs, aborting");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
@@ -82,12 +81,12 @@ int do_rc_calibration(int mavlink_fd)
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
|
||||
mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
|
||||
close(sub_man);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "trim calibration done");
|
||||
mavlink_log_info(mavlink_fd, "trim cal done");
|
||||
close(sub_man);
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -41,6 +41,6 @@
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
int do_rc_calibration(int mavlink_fd);
|
||||
int do_trim_calibration(int mavlink_fd);
|
||||
|
||||
#endif /* RC_CALIBRATION_H_ */
|
||||
|
||||
@@ -394,6 +394,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
|
||||
break;
|
||||
|
||||
case FAILSAFE_STATE_RTL:
|
||||
|
||||
/* global position and home position required for RTL */
|
||||
if (status->condition_global_position_valid && status->condition_home_position_valid) {
|
||||
status->set_nav_state = NAV_STATE_RTL;
|
||||
@@ -404,6 +405,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
|
||||
break;
|
||||
|
||||
case FAILSAFE_STATE_LAND:
|
||||
|
||||
/* at least relative altitude estimate required for landing */
|
||||
if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
|
||||
status->set_nav_state = NAV_STATE_LAND;
|
||||
|
||||
+108
-70
@@ -126,45 +126,46 @@ static const char *k_data_manager_device_path = "/fs/microsd/dataman";
|
||||
/* The data manager work queues */
|
||||
|
||||
typedef struct {
|
||||
sq_queue_t q;
|
||||
sem_t mutex; /* Mutual exclusion on work queue adds and deletes */
|
||||
unsigned size;
|
||||
unsigned max_size;
|
||||
sq_queue_t q; /* Nuttx queue */
|
||||
sem_t mutex; /* Mutual exclusion on work queue adds and deletes */
|
||||
unsigned size; /* Current size of queue */
|
||||
unsigned max_size; /* Maximum queue size reached */
|
||||
} work_q_t;
|
||||
|
||||
static work_q_t g_free_q;
|
||||
static work_q_t g_work_q;
|
||||
static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/
|
||||
static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */
|
||||
|
||||
sem_t g_work_queued_sema;
|
||||
sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */
|
||||
sem_t g_init_sema;
|
||||
|
||||
static bool g_task_should_exit; /**< if true, dataman task should exit */
|
||||
|
||||
#define DM_SECTOR_HDR_SIZE 4
|
||||
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE;
|
||||
#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */
|
||||
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
|
||||
|
||||
static void init_q(work_q_t *q)
|
||||
{
|
||||
sq_init(&(q->q));
|
||||
sem_init(&(q->mutex), 1, 1);
|
||||
q->size = q->max_size = 0;
|
||||
sq_init(&(q->q)); /* Initialize the NuttX queue structure */
|
||||
sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */
|
||||
q->size = q->max_size = 0; /* Queue is initially empty */
|
||||
}
|
||||
|
||||
static void destroy_q(work_q_t *q)
|
||||
static inline void
|
||||
destroy_q(work_q_t *q)
|
||||
{
|
||||
sem_destroy(&(q->mutex));
|
||||
sem_destroy(&(q->mutex)); /* Destroy the queue lock */
|
||||
}
|
||||
|
||||
static inline void
|
||||
lock_queue(work_q_t *q)
|
||||
{
|
||||
sem_wait(&(q->mutex));
|
||||
sem_wait(&(q->mutex)); /* Acquire the queue lock */
|
||||
}
|
||||
|
||||
static inline void
|
||||
unlock_queue(work_q_t *q)
|
||||
{
|
||||
sem_post(&(q->mutex));
|
||||
sem_post(&(q->mutex)); /* Release the queue lock */
|
||||
}
|
||||
|
||||
static work_q_item_t *
|
||||
@@ -172,54 +173,47 @@ create_work_item(void)
|
||||
{
|
||||
work_q_item_t *item;
|
||||
|
||||
/* Try to reuse item from free item queue */
|
||||
lock_queue(&g_free_q);
|
||||
if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
|
||||
g_free_q.size--;
|
||||
unlock_queue(&g_free_q);
|
||||
|
||||
/* If we there weren't any free items then obtain memory for a new one */
|
||||
if (item == NULL)
|
||||
item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
|
||||
|
||||
/* If we got one then lock the item*/
|
||||
if (item)
|
||||
sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */
|
||||
|
||||
/* return the item pointer, or NULL if all failed */
|
||||
return item;
|
||||
}
|
||||
|
||||
/* Work queue management functions */
|
||||
static void
|
||||
enqueue_work_item(work_q_item_t *item)
|
||||
{
|
||||
/* put the work item on the work queue */
|
||||
lock_queue(&g_work_q);
|
||||
sq_addlast(&item->link, &(g_work_q.q));
|
||||
|
||||
if (++g_work_q.size > g_work_q.max_size)
|
||||
g_work_q.max_size = g_work_q.size;
|
||||
|
||||
unlock_queue(&g_work_q);
|
||||
|
||||
/* tell the work thread that work is available */
|
||||
sem_post(&g_work_queued_sema);
|
||||
}
|
||||
|
||||
static void
|
||||
static inline void
|
||||
destroy_work_item(work_q_item_t *item)
|
||||
{
|
||||
sem_destroy(&item->wait_sem);
|
||||
sem_destroy(&item->wait_sem); /* Destroy the item lock */
|
||||
/* Return the item to the free item queue for later reuse */
|
||||
lock_queue(&g_free_q);
|
||||
sq_addfirst(&item->link, &(g_free_q.q));
|
||||
|
||||
/* Update the queue size and potentially the maximum queue size */
|
||||
if (++g_free_q.size > g_free_q.max_size)
|
||||
g_free_q.max_size = g_free_q.size;
|
||||
|
||||
unlock_queue(&g_free_q);
|
||||
}
|
||||
|
||||
static work_q_item_t *
|
||||
static inline work_q_item_t *
|
||||
dequeue_work_item(void)
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
/* retrieve the 1st item on the work queue */
|
||||
lock_queue(&g_work_q);
|
||||
|
||||
if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q)))
|
||||
@@ -229,6 +223,32 @@ dequeue_work_item(void)
|
||||
return work;
|
||||
}
|
||||
|
||||
static int
|
||||
enqueue_work_item_and_wait_for_result(work_q_item_t *item)
|
||||
{
|
||||
/* put the work item at the end of the work queue */
|
||||
lock_queue(&g_work_q);
|
||||
sq_addlast(&item->link, &(g_work_q.q));
|
||||
|
||||
/* Adjust the queue size and potentially the maximum queue size */
|
||||
if (++g_work_q.size > g_work_q.max_size)
|
||||
g_work_q.max_size = g_work_q.size;
|
||||
|
||||
unlock_queue(&g_work_q);
|
||||
|
||||
/* tell the work thread that work is available */
|
||||
sem_post(&g_work_queued_sema);
|
||||
|
||||
/* wait for the result */
|
||||
sem_wait(&item->wait_sem);
|
||||
|
||||
int result = item->result;
|
||||
|
||||
destroy_work_item(item);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* Calculate the offset in file of specific item */
|
||||
static int
|
||||
calculate_offset(dm_item_t item, unsigned char index)
|
||||
@@ -250,6 +270,8 @@ calculate_offset(dm_item_t item, unsigned char index)
|
||||
*
|
||||
* byte 0: Length of user data item
|
||||
* byte 1: Persistence of this data item
|
||||
* byte 2: Unused (for future use)
|
||||
* byte 3: Unused (for future use)
|
||||
* byte DM_SECTOR_HDR_SIZE... : data item value
|
||||
*
|
||||
* The total size must not exceed k_sector_size
|
||||
@@ -266,6 +288,7 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
|
||||
/* Get the offset for this item */
|
||||
offset = calculate_offset(item, index);
|
||||
|
||||
/* If item type or index out of range, return error */
|
||||
if (offset < 0)
|
||||
return -1;
|
||||
|
||||
@@ -283,10 +306,12 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
|
||||
|
||||
len = -1;
|
||||
|
||||
/* Seek to the right spot in the data manager file and write the data item */
|
||||
if (lseek(g_task_fd, offset, SEEK_SET) == offset)
|
||||
if ((len = write(g_task_fd, buffer, count)) == count)
|
||||
fsync(g_task_fd);
|
||||
fsync(g_task_fd); /* Make sure data is written to physical media */
|
||||
|
||||
/* Make sure the write succeeded */
|
||||
if (len != count)
|
||||
return -1;
|
||||
|
||||
@@ -304,6 +329,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count)
|
||||
/* Get the offset for this item */
|
||||
offset = calculate_offset(item, index);
|
||||
|
||||
/* If item type or index out of range, return error */
|
||||
if (offset < 0)
|
||||
return -1;
|
||||
|
||||
@@ -316,14 +342,17 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count)
|
||||
if (lseek(g_task_fd, offset, SEEK_SET) == offset)
|
||||
len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
|
||||
|
||||
/* Check for length issues */
|
||||
/* Check for read error */
|
||||
if (len < 0)
|
||||
return -1;
|
||||
|
||||
/* A zero length entry is a empty entry */
|
||||
if (len == 0)
|
||||
buffer[0] = 0;
|
||||
|
||||
/* See if we got data */
|
||||
if (buffer[0] > 0) {
|
||||
/* We got more than requested!!! */
|
||||
if (buffer[0] > count)
|
||||
return -1;
|
||||
|
||||
@@ -340,11 +369,14 @@ _clear(dm_item_t item)
|
||||
{
|
||||
int i, result = 0;
|
||||
|
||||
/* Get the offset of 1st item of this type */
|
||||
int offset = calculate_offset(item, 0);
|
||||
|
||||
/* Check for item type out of range */
|
||||
if (offset < 0)
|
||||
return -1;
|
||||
|
||||
/* Clear all items of this type */
|
||||
for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) {
|
||||
char buf[1];
|
||||
|
||||
@@ -353,9 +385,11 @@ _clear(dm_item_t item)
|
||||
break;
|
||||
}
|
||||
|
||||
/* Avoid SD flash wear by only doing writes where necessary */
|
||||
if (read(g_task_fd, buf, 1) < 1)
|
||||
break;
|
||||
|
||||
/* If item has length greater than 0 it needs to be overwritten */
|
||||
if (buf[0]) {
|
||||
if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
|
||||
result = -1;
|
||||
@@ -373,6 +407,7 @@ _clear(dm_item_t item)
|
||||
offset += k_sector_size;
|
||||
}
|
||||
|
||||
/* Make sure data is actually written to physical media */
|
||||
fsync(g_task_fd);
|
||||
return result;
|
||||
}
|
||||
@@ -452,12 +487,13 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
/* Make sure data manager has been started and is not shutting down */
|
||||
if ((g_fd < 0) || g_task_should_exit)
|
||||
return -1;
|
||||
|
||||
/* Will return with queues locked */
|
||||
/* get a work item and queue up a write request */
|
||||
if ((work = create_work_item()) == NULL)
|
||||
return -1; /* queues unlocked on failure */
|
||||
return -1;
|
||||
|
||||
work->func = dm_write_func;
|
||||
work->write_params.item = item;
|
||||
@@ -465,12 +501,9 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
|
||||
work->write_params.persistence = persistence;
|
||||
work->write_params.buf = buf;
|
||||
work->write_params.count = count;
|
||||
enqueue_work_item(work);
|
||||
|
||||
sem_wait(&work->wait_sem);
|
||||
ssize_t result = work->result;
|
||||
destroy_work_item(work);
|
||||
return result;
|
||||
/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
|
||||
return (ssize_t)enqueue_work_item_and_wait_for_result(work);
|
||||
}
|
||||
|
||||
/* Retrieve from the data manager file */
|
||||
@@ -479,24 +512,22 @@ dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
/* Make sure data manager has been started and is not shutting down */
|
||||
if ((g_fd < 0) || g_task_should_exit)
|
||||
return -1;
|
||||
|
||||
/* Will return with queues locked */
|
||||
/* get a work item and queue up a read request */
|
||||
if ((work = create_work_item()) == NULL)
|
||||
return -1; /* queues unlocked on failure */
|
||||
return -1;
|
||||
|
||||
work->func = dm_read_func;
|
||||
work->read_params.item = item;
|
||||
work->read_params.index = index;
|
||||
work->read_params.buf = buf;
|
||||
work->read_params.count = count;
|
||||
enqueue_work_item(work);
|
||||
|
||||
sem_wait(&work->wait_sem);
|
||||
ssize_t result = work->result;
|
||||
destroy_work_item(work);
|
||||
return result;
|
||||
/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
|
||||
return (ssize_t)enqueue_work_item_and_wait_for_result(work);
|
||||
}
|
||||
|
||||
__EXPORT int
|
||||
@@ -504,21 +535,19 @@ dm_clear(dm_item_t item)
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
/* Make sure data manager has been started and is not shutting down */
|
||||
if ((g_fd < 0) || g_task_should_exit)
|
||||
return -1;
|
||||
|
||||
/* Will return with queues locked */
|
||||
/* get a work item and queue up a clear request */
|
||||
if ((work = create_work_item()) == NULL)
|
||||
return -1; /* queues unlocked on failure */
|
||||
return -1;
|
||||
|
||||
work->func = dm_clear_func;
|
||||
work->clear_params.item = item;
|
||||
enqueue_work_item(work);
|
||||
|
||||
sem_wait(&work->wait_sem);
|
||||
int result = work->result;
|
||||
destroy_work_item(work);
|
||||
return result;
|
||||
/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
|
||||
return enqueue_work_item_and_wait_for_result(work);
|
||||
}
|
||||
|
||||
/* Tell the data manager about the type of the last reset */
|
||||
@@ -527,21 +556,19 @@ dm_restart(dm_reset_reason reason)
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
/* Make sure data manager has been started and is not shutting down */
|
||||
if ((g_fd < 0) || g_task_should_exit)
|
||||
return -1;
|
||||
|
||||
/* Will return with queues locked */
|
||||
/* get a work item and queue up a restart request */
|
||||
if ((work = create_work_item()) == NULL)
|
||||
return -1; /* queues unlocked on failure */
|
||||
return -1;
|
||||
|
||||
work->func = dm_restart_func;
|
||||
work->restart_params.reason = reason;
|
||||
enqueue_work_item(work);
|
||||
|
||||
sem_wait(&work->wait_sem);
|
||||
int result = work->result;
|
||||
destroy_work_item(work);
|
||||
return result;
|
||||
/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
|
||||
return enqueue_work_item_and_wait_for_result(work);
|
||||
}
|
||||
|
||||
static int
|
||||
@@ -570,24 +597,29 @@ task_main(int argc, char *argv[])
|
||||
|
||||
sem_init(&g_work_queued_sema, 1, 0);
|
||||
|
||||
/* Open or create the data manager file */
|
||||
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
|
||||
if (g_task_fd < 0) {
|
||||
warnx("Could not open data manager file %s", k_data_manager_device_path);
|
||||
sem_post(&g_init_sema);
|
||||
sem_post(&g_init_sema); /* Don't want to hang startup */
|
||||
return -1;
|
||||
}
|
||||
if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
|
||||
close(g_task_fd);
|
||||
warnx("Could not seek data manager file %s", k_data_manager_device_path);
|
||||
sem_post(&g_init_sema);
|
||||
sem_post(&g_init_sema); /* Don't want to hang startup */
|
||||
return -1;
|
||||
}
|
||||
fsync(g_task_fd);
|
||||
|
||||
/* We use two file descriptors, one for the caller context and one for the worker thread */
|
||||
/* They are actually the same but we need to some way to reject caller request while the */
|
||||
/* worker thread is shutting down but still processing requests */
|
||||
g_fd = g_task_fd;
|
||||
|
||||
warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
|
||||
|
||||
/* Tell startup that the worker thread has completed its initialization */
|
||||
sem_post(&g_init_sema);
|
||||
|
||||
/* Start the endless loop, waiting for then processing work requests */
|
||||
@@ -595,7 +627,7 @@ task_main(int argc, char *argv[])
|
||||
|
||||
/* do we need to exit ??? */
|
||||
if ((g_task_should_exit) && (g_fd >= 0)) {
|
||||
/* Close the file handle to stop further queueing */
|
||||
/* Close the file handle to stop further queuing */
|
||||
g_fd = -1;
|
||||
}
|
||||
|
||||
@@ -607,6 +639,7 @@ task_main(int argc, char *argv[])
|
||||
/* Empty the work queue */
|
||||
while ((work = dequeue_work_item())) {
|
||||
|
||||
/* handle each work item with the appropriate handler */
|
||||
switch (work->func) {
|
||||
case dm_write_func:
|
||||
g_func_counts[dm_write_func]++;
|
||||
@@ -647,7 +680,7 @@ task_main(int argc, char *argv[])
|
||||
close(g_task_fd);
|
||||
g_task_fd = -1;
|
||||
|
||||
/* Empty the work queue */
|
||||
/* The work queue is now empty, empty the free queue */
|
||||
for (;;) {
|
||||
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
|
||||
break;
|
||||
@@ -669,7 +702,7 @@ start(void)
|
||||
|
||||
sem_init(&g_init_sema, 1, 0);
|
||||
|
||||
/* start the task */
|
||||
/* start the worker thread */
|
||||
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
|
||||
warn("task start failed");
|
||||
return -1;
|
||||
@@ -704,7 +737,7 @@ stop(void)
|
||||
static void
|
||||
usage(void)
|
||||
{
|
||||
errx(1, "usage: dataman {start|stop|status}");
|
||||
errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}");
|
||||
}
|
||||
|
||||
int
|
||||
@@ -726,6 +759,7 @@ dataman_main(int argc, char *argv[])
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/* Worker thread should be running for all other commands */
|
||||
if (g_fd < 0)
|
||||
errx(1, "not running");
|
||||
|
||||
@@ -733,6 +767,10 @@ dataman_main(int argc, char *argv[])
|
||||
stop();
|
||||
else if (!strcmp(argv[1], "status"))
|
||||
status();
|
||||
else if (!strcmp(argv[1], "poweronrestart"))
|
||||
dm_restart(DM_INIT_REASON_POWER_ON);
|
||||
else if (!strcmp(argv[1], "inflightrestart"))
|
||||
dm_restart(DM_INIT_REASON_IN_FLIGHT);
|
||||
else
|
||||
usage();
|
||||
|
||||
|
||||
@@ -785,26 +785,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
_tecs.set_speed_weight(_parameters.speed_weight);
|
||||
|
||||
/* current waypoint (the one currently heading for) */
|
||||
math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
|
||||
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
|
||||
|
||||
/* current waypoint (the one currently heading for) */
|
||||
math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
|
||||
math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
|
||||
|
||||
|
||||
/* previous waypoint */
|
||||
math::Vector<2> prev_wp;
|
||||
|
||||
if (pos_sp_triplet.previous.valid) {
|
||||
prev_wp(0) = pos_sp_triplet.previous.lat;
|
||||
prev_wp(1) = pos_sp_triplet.previous.lon;
|
||||
prev_wp(0) = (float)pos_sp_triplet.previous.lat;
|
||||
prev_wp(1) = (float)pos_sp_triplet.previous.lon;
|
||||
|
||||
} else {
|
||||
/*
|
||||
* No valid previous waypoint, go for the current wp.
|
||||
* This is automatically handled by the L1 library.
|
||||
*/
|
||||
prev_wp(0) = pos_sp_triplet.current.lat;
|
||||
prev_wp(1) = pos_sp_triplet.current.lon;
|
||||
prev_wp(0) = (float)pos_sp_triplet.current.lat;
|
||||
prev_wp(1) = (float)pos_sp_triplet.current.lon;
|
||||
|
||||
}
|
||||
|
||||
@@ -1263,7 +1263,7 @@ FixedwingPositionControl::task_main()
|
||||
// vehicle_baro_poll();
|
||||
|
||||
math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
|
||||
math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
|
||||
math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
|
||||
|
||||
/*
|
||||
* Attempt to control position, on success (= sensors present and not in manual mode),
|
||||
|
||||
@@ -489,14 +489,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
|
||||
//}
|
||||
} else {
|
||||
float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
|
||||
|
||||
if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
|
||||
/* move yaw setpoint */
|
||||
yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
|
||||
|
||||
if (_manual_control_sp.yaw > 0.0f) {
|
||||
yaw_sp_move_rate -= YAW_DEADZONE;
|
||||
|
||||
} else {
|
||||
yaw_sp_move_rate += YAW_DEADZONE;
|
||||
}
|
||||
|
||||
yaw_sp_move_rate *= _params.rc_scale_yaw;
|
||||
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
|
||||
_v_att_sp.R_valid = false;
|
||||
@@ -660,7 +664,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
|
||||
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
|
||||
|
||||
if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
|
||||
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
|
||||
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
|
||||
_rates_int(i) = rate_i;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -479,9 +479,11 @@ MulticopterPositionControl::select_alt(bool global)
|
||||
{
|
||||
if (global != _use_global_alt) {
|
||||
_use_global_alt = global;
|
||||
|
||||
if (global) {
|
||||
/* switch from barometric to global altitude */
|
||||
_alt_sp += _global_pos.alt - _global_pos.baro_alt;
|
||||
|
||||
} else {
|
||||
/* switch from global to barometric altitude */
|
||||
_alt_sp += _global_pos.baro_alt - _global_pos.alt;
|
||||
@@ -589,6 +591,7 @@ MulticopterPositionControl::task_main()
|
||||
if (_control_mode.flag_control_manual_enabled) {
|
||||
/* select altitude source and update setpoint */
|
||||
select_alt(_global_pos.global_valid);
|
||||
|
||||
if (!_use_global_alt) {
|
||||
alt = _global_pos.baro_alt;
|
||||
}
|
||||
@@ -845,9 +848,10 @@ MulticopterPositionControl::task_main()
|
||||
|
||||
/* adjust limits for landing mode */
|
||||
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
|
||||
_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
|
||||
_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
|
||||
/* limit max tilt and min lift when landing */
|
||||
tilt_max = _params.land_tilt_max;
|
||||
|
||||
if (thr_min < 0.0f)
|
||||
thr_min = 0.0f;
|
||||
}
|
||||
@@ -863,9 +867,11 @@ MulticopterPositionControl::task_main()
|
||||
if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
|
||||
/* absolute horizontal thrust */
|
||||
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
|
||||
|
||||
if (thrust_sp_xy_len > 0.01f) {
|
||||
/* max horizontal thrust for given vertical thrust*/
|
||||
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
|
||||
|
||||
if (thrust_sp_xy_len > thrust_xy_max) {
|
||||
float k = thrust_xy_max / thrust_sp_xy_len;
|
||||
thrust_sp(0) *= k;
|
||||
@@ -874,15 +880,18 @@ MulticopterPositionControl::task_main()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/* thrust compensation for altitude only control mode */
|
||||
float att_comp;
|
||||
|
||||
if (_att.R[2][2] > TILT_COS_MAX) {
|
||||
att_comp = 1.0f / _att.R[2][2];
|
||||
|
||||
} else if (_att.R[2][2] > 0.0f) {
|
||||
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
|
||||
saturation_z = true;
|
||||
|
||||
} else {
|
||||
att_comp = 1.0f;
|
||||
saturation_z = true;
|
||||
|
||||
@@ -688,8 +688,9 @@ Navigator::task_main()
|
||||
/* RETURN switch, overrides MISSION switch */
|
||||
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
|
||||
/* switch to RTL if not already landed after RTL and home position set */
|
||||
if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
|
||||
_vstatus.condition_home_position_valid) {
|
||||
if (!(_rtl_state == RTL_STATE_DESCEND &&
|
||||
(myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
|
||||
_vstatus.condition_home_position_valid) {
|
||||
dispatch(EVENT_RTL_REQUESTED);
|
||||
}
|
||||
|
||||
@@ -746,8 +747,9 @@ Navigator::task_main()
|
||||
break;
|
||||
|
||||
case NAV_STATE_RTL:
|
||||
if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
|
||||
_vstatus.condition_home_position_valid) {
|
||||
if (!(_rtl_state == RTL_STATE_DESCEND &&
|
||||
(myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
|
||||
_vstatus.condition_home_position_valid) {
|
||||
dispatch(EVENT_RTL_REQUESTED);
|
||||
}
|
||||
|
||||
@@ -1078,7 +1080,7 @@ Navigator::start_loiter()
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
|
||||
}
|
||||
|
||||
_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
|
||||
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
|
||||
}
|
||||
|
||||
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
|
||||
@@ -1443,14 +1445,7 @@ Navigator::check_mission_item_reached()
|
||||
}
|
||||
|
||||
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
|
||||
if (_vstatus.is_rotary_wing) {
|
||||
return _vstatus.condition_landed;
|
||||
|
||||
} else {
|
||||
/* For fw there is currently no landing detector:
|
||||
* make sure control is not stopped when overshooting the landing waypoint */
|
||||
return false;
|
||||
}
|
||||
return _vstatus.condition_landed;
|
||||
}
|
||||
|
||||
/* XXX TODO count turns */
|
||||
@@ -1575,10 +1570,12 @@ Navigator::on_mission_item_reached()
|
||||
if (_rtl_state == RTL_STATE_DESCEND) {
|
||||
/* hovering above home position, land if needed or loiter */
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
|
||||
|
||||
if (_mission_item.autocontinue) {
|
||||
dispatch(EVENT_LAND_REQUESTED);
|
||||
|
||||
} else {
|
||||
_reset_loiter_pos = false;
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
|
||||
|
||||
@@ -52,8 +52,8 @@
|
||||
static const int ERROR = -1;
|
||||
|
||||
|
||||
Mission::Mission() :
|
||||
|
||||
Mission::Mission() :
|
||||
|
||||
_offboard_dataman_id(-1),
|
||||
_current_offboard_mission_index(0),
|
||||
_current_onboard_mission_index(0),
|
||||
@@ -65,7 +65,7 @@ Mission::Mission() :
|
||||
|
||||
Mission::~Mission()
|
||||
{
|
||||
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
@@ -126,33 +126,39 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool
|
||||
{
|
||||
/* try onboard mission first */
|
||||
if (current_onboard_mission_available()) {
|
||||
|
||||
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
|
||||
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
_current_mission_type = MISSION_TYPE_ONBOARD;
|
||||
*onboard = true;
|
||||
*index = _current_onboard_mission_index;
|
||||
|
||||
/* otherwise fallback to offboard */
|
||||
/* otherwise fallback to offboard */
|
||||
|
||||
} else if (current_offboard_mission_available()) {
|
||||
|
||||
dm_item_t dm_current;
|
||||
|
||||
if (_offboard_dataman_id == 0) {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
|
||||
} else {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
||||
}
|
||||
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
|
||||
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
|
||||
_current_mission_type = MISSION_TYPE_NONE;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
_current_mission_type = MISSION_TYPE_OFFBOARD;
|
||||
*onboard = false;
|
||||
*index = _current_offboard_mission_index;
|
||||
@@ -171,25 +177,29 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
|
||||
{
|
||||
/* try onboard mission first */
|
||||
if (next_onboard_mission_available()) {
|
||||
|
||||
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
|
||||
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* otherwise fallback to offboard */
|
||||
/* otherwise fallback to offboard */
|
||||
|
||||
} else if (next_offboard_mission_available()) {
|
||||
|
||||
dm_item_t dm_current;
|
||||
|
||||
if (_offboard_dataman_id == 0) {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
|
||||
} else {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
||||
}
|
||||
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
|
||||
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
|
||||
return ERROR;
|
||||
@@ -244,14 +254,16 @@ void
|
||||
Mission::move_to_next()
|
||||
{
|
||||
switch (_current_mission_type) {
|
||||
case MISSION_TYPE_ONBOARD:
|
||||
_current_onboard_mission_index++;
|
||||
break;
|
||||
case MISSION_TYPE_OFFBOARD:
|
||||
_current_offboard_mission_index++;
|
||||
break;
|
||||
case MISSION_TYPE_NONE:
|
||||
default:
|
||||
break;
|
||||
case MISSION_TYPE_ONBOARD:
|
||||
_current_onboard_mission_index++;
|
||||
break;
|
||||
|
||||
case MISSION_TYPE_OFFBOARD:
|
||||
_current_offboard_mission_index++;
|
||||
break;
|
||||
|
||||
case MISSION_TYPE_NONE:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -60,4 +60,4 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
|
||||
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude
|
||||
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing
|
||||
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode
|
||||
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing
|
||||
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -633,37 +632,39 @@ Sensors::parameters_update()
|
||||
if (!rc_valid)
|
||||
warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
|
||||
|
||||
const char *paramerr = "FAIL PARM LOAD";
|
||||
|
||||
/* channel mapping */
|
||||
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
|
||||
warnx("Failed getting roll chan index");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
|
||||
warnx("Failed getting pitch chan index");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
|
||||
warnx("Failed getting yaw chan index");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
|
||||
warnx("Failed getting throttle chan index");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
|
||||
warnx("Failed getting mode sw chan index");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
|
||||
warnx("Failed getting return sw chan index");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
|
||||
warnx("Failed getting assisted sw chan index");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
|
||||
warnx("Failed getting mission sw chan index");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) {
|
||||
@@ -671,7 +672,7 @@ Sensors::parameters_update()
|
||||
}
|
||||
|
||||
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
|
||||
warnx("Failed getting flaps chan index");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
|
||||
@@ -738,12 +739,12 @@ Sensors::parameters_update()
|
||||
|
||||
/* scaling of ADC ticks to battery voltage */
|
||||
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
|
||||
warnx("Failed updating voltage scaling param");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
/* scaling of ADC ticks to battery current */
|
||||
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
|
||||
warnx("Failed updating current scaling param");
|
||||
warnx(paramerr);
|
||||
}
|
||||
|
||||
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
|
||||
@@ -1647,17 +1648,17 @@ int sensors_main(int argc, char *argv[])
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (sensors::g_sensors != nullptr)
|
||||
errx(0, "sensors task already running");
|
||||
errx(0, "already running");
|
||||
|
||||
sensors::g_sensors = new Sensors;
|
||||
|
||||
if (sensors::g_sensors == nullptr)
|
||||
errx(1, "sensors task alloc failed");
|
||||
errx(1, "alloc failed");
|
||||
|
||||
if (OK != sensors::g_sensors->start()) {
|
||||
delete sensors::g_sensors;
|
||||
sensors::g_sensors = nullptr;
|
||||
err(1, "sensors task start failed");
|
||||
err(1, "start failed");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
@@ -1665,7 +1666,7 @@ int sensors_main(int argc, char *argv[])
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
if (sensors::g_sensors == nullptr)
|
||||
errx(1, "sensors task not running");
|
||||
errx(1, "not running");
|
||||
|
||||
delete sensors::g_sensors;
|
||||
sensors::g_sensors = nullptr;
|
||||
@@ -1674,10 +1675,10 @@ int sensors_main(int argc, char *argv[])
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (sensors::g_sensors) {
|
||||
errx(0, "task is running");
|
||||
errx(0, "is running");
|
||||
|
||||
} else {
|
||||
errx(1, "task is not running");
|
||||
errx(1, "not running");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -216,6 +216,7 @@ struct vehicle_status_s
|
||||
|
||||
bool rc_signal_found_once;
|
||||
bool rc_signal_lost; /**< true if RC reception lost */
|
||||
bool rc_input_blocked; /**< set if RC input should be ignored */
|
||||
|
||||
bool offboard_control_signal_found_once;
|
||||
bool offboard_control_signal_lost;
|
||||
|
||||
@@ -257,7 +257,6 @@ mtd_start(char *partition_names[], unsigned n_partitions)
|
||||
|
||||
/* Now create MTD FLASH partitions */
|
||||
|
||||
warnx("Creating partitions");
|
||||
FAR struct mtd_dev_s *part[n_partitions];
|
||||
char blockname[32];
|
||||
|
||||
@@ -266,9 +265,6 @@ mtd_start(char *partition_names[], unsigned n_partitions)
|
||||
|
||||
for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) {
|
||||
|
||||
warnx(" Partition %d. Block offset=%lu, size=%lu",
|
||||
i, (unsigned long)offset, (unsigned long)nblocks);
|
||||
|
||||
/* Create the partition */
|
||||
|
||||
part[i] = mtd_partition(mtd_dev, offset, nblocks);
|
||||
|
||||
@@ -89,7 +89,7 @@ task_main(int argc, char *argv[])
|
||||
unsigned hash = i ^ my_id;
|
||||
unsigned len = (hash & 63) + 2;
|
||||
|
||||
int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
|
||||
int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
|
||||
warnx("ret: %d", ret);
|
||||
if (ret != len) {
|
||||
warnx("%d write failed, index %d, length %d", my_id, hash, len);
|
||||
@@ -103,7 +103,7 @@ task_main(int argc, char *argv[])
|
||||
for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
|
||||
unsigned hash = i ^ my_id;
|
||||
unsigned len2, len = (hash & 63) + 2;
|
||||
if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) {
|
||||
if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) {
|
||||
warnx("%d read failed length test, index %d", my_id, hash);
|
||||
goto fail;
|
||||
}
|
||||
@@ -163,7 +163,7 @@ int test_dataman(int argc, char *argv[])
|
||||
free(sems);
|
||||
dm_restart(DM_INIT_REASON_IN_FLIGHT);
|
||||
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
|
||||
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0)
|
||||
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0)
|
||||
break;
|
||||
}
|
||||
if (i >= NUM_MISSIONS_SUPPORTED) {
|
||||
@@ -173,7 +173,7 @@ int test_dataman(int argc, char *argv[])
|
||||
}
|
||||
dm_restart(DM_INIT_REASON_POWER_ON);
|
||||
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
|
||||
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) {
|
||||
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
|
||||
warnx("Restart power-on failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user