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

Merge branch 'beta' into offboard2

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-14 13:41:03 +01:00
parent 66f270dd7e 51b7c27363
commit 66f00d056f
80 changed files with 923 additions and 1295 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+2 -28
View File
@@ -5,36 +5,10 @@
# Lorenz Meier <lm@inf.ethz.ch>
#
echo "HIL Rascal 110 starting.."
sh /etc/init.d/rc.fw_defaults
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_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
fi
echo "HIL Rascal 110 starting.."
set HIL yes
set VEHICLE_TYPE fw
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+14 -34
View File
@@ -2,48 +2,28 @@
#
# Team Blacksheep Discovery Quadcopter
#
# Simon Wilks <sjwilks@gmail.com>
# Anton Babushkin <anton.babushkin@me.com>, Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
#
sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ROLL_P 5.0
param set MC_ROLLRATE_P 0.17
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.006
param set MC_PITCH_P 5.0
param set MC_PITCHRATE_P 0.17
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.006
param set MC_YAW_P 0.5
param set MC_YAWRATE_P 0.2
# TODO review MC_YAWRATE_I
param set MC_ROLL_P 8.0
param set MC_ROLLRATE_P 0.07
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.0017
param set MC_PITCH_P 8.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.1
param set MC_PITCHRATE_D 0.0025
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.28
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.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_w
set PWM_OUTPUTS 1234
set PWM_RATE 400
ROMFS/px4fmu_common/init.d/10016_3dr_iris
+3 -26
View File
@@ -5,11 +5,11 @@
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
# TODO tune roll/pitch separately
param set MC_ROLL_P 9.0
param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_I 0.0
@@ -22,34 +22,11 @@ then
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.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
param set BAT_V_SCALING 0.00989
param set BAT_C_SCALING 0.0124
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_RATE 400
set PWM_DISARMED 900
set PWM_MIN 1000
set PWM_MAX 2000
ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
+3 -1
View File
@@ -5,6 +5,8 @@
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/4001_quad_x
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_x
set HIL yes
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
+3 -1
View File
@@ -5,6 +5,8 @@
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/1001_rc_quad_x.hil
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_+
set HIL yes
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
+2 -30
View File
@@ -5,38 +5,10 @@
# Thomas Gubler <thomasgubler@gmail.com>
#
echo "HIL Rascal 110 starting.."
sh /etc/init.d/rc.fw_defaults
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
fi
echo "HIL Rascal 110 starting.."
set HIL yes
set VEHICLE_TYPE fw
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
+3 -20
View File
@@ -5,14 +5,13 @@
# Thomas Gubler <thomasgubler@gmail.com>
#
echo "HIL Malolo 1 starting.."
sh /etc/init.d/rc.fw_defaults
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_AIRSPD_MAX 40
param set FW_ATT_TC 0.3
param set FW_L1_DAMPING 0.74
param set FW_L1_PERIOD 15
@@ -20,10 +19,6 @@ then
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
@@ -31,9 +26,6 @@ then
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
@@ -43,22 +35,13 @@ then
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
param set FW_YR_P 0.0
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
ROMFS/px4fmu_common/init.d/11001_hexa_cox
+15
View File
@@ -0,0 +1,15 @@
#!nsh
#
# UNTESTED UNTESTED!
#
# Generic 10” Hexa coaxial geometry
#
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/rc.mc_defaults
set MIXER hexa_cox
# We only can run one channel group with one rate, so set all 8 channels
set PWM_OUTPUTS 12345678
ROMFS/px4fmu_common/init.d/12001_octo_cox
+5 -3
View File
@@ -2,9 +2,11 @@
#
# Generic 10” Octo coaxial geometry
#
# Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/8001_octo_x
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_cox
set MIXER octo_cox
set PWM_OUTPUTS 12345678
ROMFS/px4fmu_common/init.d/2100_mpx_easystar
+1 -32
View File
@@ -2,38 +2,7 @@
#
# MPX EasyStar Plane
#
# Maintainers: ???
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
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
fi
sh /etc/init.d/rc.fw_defaults
set VEHICLE_TYPE fw
set MIXER FMU_RET
ROMFS/px4fmu_common/init.d/2101_hk_bixler
+1 -31
View File
@@ -1,35 +1,5 @@
#!nsh
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
sh /etc/init.d/rc.fw_defaults
set VEHICLE_TYPE fw
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
+1 -29
View File
@@ -1,33 +1,5 @@
#!nsh
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_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
sh /etc/init.d/rc.fw_defaults
set VEHICLE_TYPE fw
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/3030_io_camflyer
+1 -34
View File
@@ -1,38 +1,5 @@
#!nsh
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1.0
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
sh /etc/init.d/rc.fw_defaults
set VEHICLE_TYPE fw
set MIXER FMU_Q
ROMFS/px4fmu_common/init.d/3031_phantom
-36
View File
@@ -5,40 +5,4 @@
# Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 11.4
param set FW_AIRSPD_TRIM 14
param set FW_AIRSPD_MAX 22
param set FW_L1_PERIOD 15
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 45
param set FW_P_LIM_MIN -45
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 15
param set FW_R_P 80
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.8
param set FW_THR_LND_MAX 0
param set FW_THR_MAX 1
param set FW_THR_MIN 0.5
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 2.0
param set FW_Y_ROLLFF 1.0
param set RC_SCALE_ROLL 0.6
param set RC_SCALE_PITCH 0.6
param set TRIM_PITCH 0.1
fi
set VEHICLE_TYPE fw
set MIXER FMU_Q
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
+25 -29
View File
@@ -5,39 +5,35 @@
# Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
#
sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_AIRSPD_MIN 15
param set FW_AIRSPD_TRIM 20
param set FW_AIRSPD_MAX 40
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.3
param set FW_PR_I 0
param set FW_PR_IMAX 0.2
param set FW_PR_P 0.03
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 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set FW_P_ROLLFF 0
param set FW_RR_FF 0.3
param set FW_RR_I 0
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.03
param set FW_R_LIM 60
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set VEHICLE_TYPE fw
set MIXER FMU_X5
ROMFS/px4fmu_common/init.d/3033_wingwing
+1 -34
View File
@@ -5,39 +5,6 @@
# Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
sh /etc/init.d/rc.fw_defaults
set VEHICLE_TYPE fw
set MIXER FMU_Q
ROMFS/px4fmu_common/init.d/3034_fx79
+1 -34
View File
@@ -5,39 +5,6 @@
# Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MAX 20
param set FW_AIRSPD_TRIM 12
param set FW_AIRSPD_MIN 15
param set FW_L1_PERIOD 12
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 80
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.75
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 1.1
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
sh /etc/init.d/rc.fw_defaults
set VEHICLE_TYPE fw
set MIXER FMU_FX79
ROMFS/px4fmu_common/init.d/4001_quad_x
+1 -41
View File
@@ -5,48 +5,8 @@
# Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.12
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.004
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.12
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
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
sh /etc/init.d/rc.mc_defaults
set VEHICLE_TYPE mc
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
set PWM_DISARMED 900
set PWM_MIN 1100
set PWM_MAX 2000
ROMFS/px4fmu_common/init.d/4010_dji_f330
+8 -34
View File
@@ -5,49 +5,23 @@
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.12
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.004
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.12
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.004
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.05
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.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
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MIN 1175
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/4011_dji_f450
+12 -37
View File
@@ -5,49 +5,24 @@
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
# TODO REVIEW
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.12
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.004
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.12
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
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
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MIN 1175
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/4012_hk_x550
-14
View File
@@ -1,14 +0,0 @@
#!nsh
#
# HobbyKing X550 Quadcopter
#
# Todd Stellanova <tstellanova@gmail.com>
#
sh /etc/init.d/4001_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
set PWM_DISARMED 900
set PWM_MIN 1000
set PWM_MAX 2000
ROMFS/px4fmu_common/init.d/5001_quad_+
+3 -1
View File
@@ -5,6 +5,8 @@
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/4001_quad_x
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_+
set PWM_OUTPUTS 1234
ROMFS/px4fmu_common/init.d/6001_hexa_x
+2 -3
View File
@@ -5,10 +5,9 @@
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/4001_quad_x
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_x
# We only can run one channel group with one rate,
# so all 8 at 400 Hz
# We only can run one channel group with one rate, so set all 8 channels
set PWM_OUTPUTS 12345678
ROMFS/px4fmu_common/init.d/7001_hexa_+
+4 -1
View File
@@ -5,6 +5,9 @@
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/6001_hexa_x
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_+
# We only can run one channel group with one rate, so set all 8 channels
set PWM_OUTPUTS 12345678
ROMFS/px4fmu_common/init.d/8001_octo_x
+1 -1
View File
@@ -5,7 +5,7 @@
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/4001_quad_x
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_x
ROMFS/px4fmu_common/init.d/9001_octo_+
+3 -1
View File
@@ -5,6 +5,8 @@
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/8001_octo_x
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_+
set PWM_OUTPUTS 12345678
ROMFS/px4fmu_common/init.d/rc.autostart
+9 -5
View File
@@ -116,11 +116,6 @@ then
sh /etc/init.d/4011_dji_f450
fi
if param compare SYS_AUTOSTART 4012 12
then
sh /etc/init.d/4012_hk_x550
fi
#
# Quad +
#
@@ -180,6 +175,15 @@ then
sh /etc/init.d/10016_3dr_iris
fi
#
# Hexa Coaxial
#
if param compare SYS_AUTOSTART 11001
then
sh /etc/init.d/11001_hexa_cox
fi
#
# Octo Coaxial
#
ROMFS/px4fmu_common/init.d/rc.fw_defaults
+14
View File
@@ -0,0 +1,14 @@
#!nsh
set VEHICLE_TYPE fw
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for FW
#
param set NAV_LAND_ALT 90
param set NAV_RTL_ALT 100
param set NAV_RTL_LAND_T -1
param set NAV_ACCEPT_RAD 50
fi
ROMFS/px4fmu_common/init.d/rc.mc_defaults
+43
View File
@@ -0,0 +1,43 @@
#!nsh
set VEHICLE_TYPE mc
if [ $DO_AUTOCONFIG == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.5
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 PWM_RATE 400
set PWM_DISARMED 900
set PWM_MIN 1075
set PWM_MAX 2000
ROMFS/px4fmu_common/init.d/rcS
+22 -10
View File
@@ -464,20 +464,28 @@ then
if [ $MIXER == none ]
then
# Set default mixer for multicopter if not defined
set MIXER quad_x
echo "Default mixer for multicopter not defined"
fi
if [ $MAV_TYPE == none ]
then
# Use MAV_TYPE = 2 (quadcopter) if not defined
set MAV_TYPE 2
# Use mixer to detect vehicle type
if [ $MIXER == FMU_quad_x -o $MIXER == FMU_quad_+ ]
then
set MAV_TYPE 2
fi
if [ $MIXER == FMU_quad_w ]
then
set MAV_TYPE 2
fi
if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ]
then
set MAV_TYPE 13
fi
if [ $MIXER == hexa_cox ]
then
set MAV_TYPE 13
fi
if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ]
then
set MAV_TYPE 14
@@ -487,8 +495,14 @@ then
set MAV_TYPE 14
fi
fi
param set MAV_TYPE $MAV_TYPE
# Still no MAV_TYPE found
if [ $MAV_TYPE == none ]
then
echo "Unknown MAV_TYPE"
else
param set MAV_TYPE $MAV_TYPE
fi
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
@@ -502,10 +516,8 @@ then
#
if [ $VEHICLE_TYPE == none ]
then
echo "[init] Vehicle type: GENERIC"
echo "[init] Vehicle type: No autostart ID found"
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
fi
# Start any custom addons
ROMFS/px4fmu_common/mixers/FMU_X5.mix
+5 -6
View File
@@ -23,13 +23,13 @@ for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -3000 -5000 0 -10000 10000
S: 0 1 -5000 -5000 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 -5000 -3000 0 -10000 10000
S: 0 1 5000 5000 0 -10000 10000
S: 0 0 -8000 -8000 0 -10000 10000
S: 0 1 -6000 -6000 0 -10000 10000
Output 2
--------
@@ -48,7 +48,7 @@ M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / payload mixer for last four channels
Gimbal / flaps / payload mixer for last four channels
-----------------------------------------------------
M: 1
@@ -66,4 +66,3 @@ S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix
+1 -5
View File
@@ -1,7 +1,3 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a hexacopter in the + configuration. All controls
are mixed 100%.
# Hexa +
R: 6+ 10000 10000 10000 0
ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix
+1 -5
View File
@@ -1,7 +1,3 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a hexacopter in the X configuration. All controls
are mixed 100%.
# Hexa X
R: 6x 10000 10000 10000 0
ROMFS/px4fmu_common/mixers/FMU_octo_+.mix
+1 -5
View File
@@ -1,7 +1,3 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a octocopter in the + configuration. All controls
are mixed 100%.
# Octo +
R: 8+ 10000 10000 10000 0
ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix
+1 -5
View File
@@ -1,7 +1,3 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a Coaxial Octocopter in the X configuration. All controls
are mixed 100%.
# Octo coaxial
R: 8c 10000 10000 10000 0
ROMFS/px4fmu_common/mixers/FMU_octo_x.mix
+1 -5
View File
@@ -1,7 +1,3 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a octocopter in the X configuration. All controls
are mixed 100%.
# Octo X
R: 8x 10000 10000 10000 0
ROMFS/px4fmu_common/mixers/README
-154
View File
@@ -1,154 +0,0 @@
PX4 mixer definitions
=====================
Files in this directory implement example mixers that can be used as a basis
for customisation, or for general testing purposes.
Mixer basics
------------
Mixers combine control values from various sources (control tasks, user inputs,
etc.) and produce output values suitable for controlling actuators; servos,
motors, switches and so on.
An actuator derives its value from the combination of one or more control
values. Each of the control values is scaled according to the actuator's
configuration and then combined to produce the actuator value, which may then be
further scaled to suit the specific output type.
Internally, all scaling is performed using floating point values. Inputs and
outputs are clamped to the range -1.0 to 1.0.
control control control
| | |
v v v
scale scale scale
| | |
| v |
+-------> mix <------+
|
scale
|
v
out
Scaling
-------
Basic scalers provide linear scaling of the input to the output.
Each scaler allows the input value to be scaled independently for inputs
greater/less than zero. An offset can be applied to the output, and lower and
upper boundary constraints can be applied. Negative scaling factors cause the
output to be inverted (negative input produces positive output).
Scaler pseudocode:
if (input < 0)
output = (input * NEGATIVE_SCALE) + OFFSET
else
output = (input * POSITIVE_SCALE) + OFFSET
if (output < LOWER_LIMIT)
output = LOWER_LIMIT
if (output > UPPER_LIMIT)
output = UPPER_LIMIT
Syntax
------
Mixer definitions are text files; lines beginning with a single capital letter
followed by a colon are significant. All other lines are ignored, meaning that
explanatory text can be freely mixed with the definitions.
Each file may define more than one mixer; the allocation of mixers to actuators
is specific to the device reading the mixer definition, and the number of
actuator outputs generated by a mixer is specific to the mixer.
A mixer begins with a line of the form
<tag>: <mixer arguments>
The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a
multirotor mixer, etc.
Null Mixer
..........
A null mixer consumes no controls and generates a single actuator output whose
value is always zero. Typically a null mixer is used as a placeholder in a
collection of mixers in order to achieve a specific pattern of actuator outputs.
The null mixer definition has the form:
Z:
Simple Mixer
............
A simple mixer combines zero or more control inputs into a single actuator
output. Inputs are scaled, and the mixing function sums the result before
applying an output scaler.
A simple mixer definition begins with:
M: <control count>
O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
If <control count> is zero, the sum is effectively zero and the mixer will
output a fixed value that is <offset> constrained by <lower limit> and <upper
limit>.
The second line defines the output scaler with scaler parameters as discussed
above. Whilst the calculations are performed as floating-point operations, the
values stored in the definition file are scaled by a factor of 10000; i.e. an
offset of -0.5 is encoded as -5000.
The definition continues with <control count> entries describing the control
inputs and their scaling, in the form:
S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
The <group> value identifies the control group from which the scaler will read,
and the <index> value an offset within that group. These values are specific to
the device reading the mixer definition.
When used to mix vehicle controls, mixer group zero is the vehicle attitude
control group, and index values zero through three are normally roll, pitch,
yaw and thrust respectively.
The remaining fields on the line configure the control scaler with parameters as
discussed above. Whilst the calculations are performed as floating-point
operations, the values stored in the definition file are scaled by a factor of
10000; i.e. an offset of -0.5 is encoded as -5000.
Multirotor Mixer
................
The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
into a set of actuator outputs intended to drive motor speed controllers.
The mixer definition is a single line of the form:
R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
The supported geometries include:
4x - quadrotor in X configuration
4+ - quadrotor in + configuration
6x - hexcopter in X configuration
6+ - hexcopter in + configuration
8x - octocopter in X configuration
8+ - octocopter in + configuration
Each of the roll, pitch and yaw scale values determine scaling of the roll,
pitch and yaw controls relative to the thrust control. Whilst the calculations
are performed as floating-point operations, the values stored in the definition
file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000.
Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the
thrust input ranges from 0.0 to 1.0. Output for each actuator is in the
range -1.0 to 1.0.
In the case where an actuator saturates, all actuator values are rescaled so that
the saturating actuator is limited to 1.0.
ROMFS/px4fmu_common/mixers/hexa_cox.mix
+3
View File
@@ -0,0 +1,3 @@
# Hexa coaxial
R: 6c 10000 10000 10000 0
Tools/px_uploader.py
+48
View File
@@ -50,6 +50,9 @@
# Currently only used for informational purposes.
#
# for python2.7 compatibility
from __future__ import print_function
import sys
import argparse
import binascii
@@ -154,6 +157,8 @@ class uploader(object):
PROG_MULTI = b'\x27'
READ_MULTI = b'\x28' # rev2 only
GET_CRC = b'\x29' # rev3+
GET_OTP = b'\x2a' # rev4+ , get a word from OTP area
GET_SN = b'\x2b' # rev4+ , get a word from SN area
REBOOT = b'\x30'
INFO_BL_REV = b'\x01' # bootloader protocol revision
@@ -175,6 +180,8 @@ class uploader(object):
def __init__(self, portname, baudrate):
# open the port, keep the default timeout short so we can poll quickly
self.port = serial.Serial(portname, baudrate, timeout=0.5)
self.otp = b''
self.sn = b''
def close(self):
if self.port is not None:
@@ -237,6 +244,22 @@ class uploader(object):
self.__getSync()
return value
# send the GET_OTP command and wait for an info parameter
def __getOTP(self, param):
t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array.
self.__send(uploader.GET_OTP + t + uploader.EOC)
value = self.__recv(4)
self.__getSync()
return value
# send the GET_OTP command and wait for an info parameter
def __getSN(self, param):
t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array.
self.__send(uploader.GET_SN + t + uploader.EOC)
value = self.__recv(4)
self.__getSync()
return value
# send the CHIP_ERASE command and wait for the bootloader to become ready
def __erase(self):
self.__send(uploader.CHIP_ERASE
@@ -353,6 +376,31 @@ class uploader(object):
if self.fw_maxsize < fw.property('image_size'):
raise RuntimeError("Firmware image is too large for this board")
# OTP added in v4:
if self.bl_rev > 3:
for byte in range(0,32*6,4):
x = self.__getOTP(byte)
self.otp = self.otp + x
print(binascii.hexlify(x).decode('Latin-1') + ' ', end='')
# see src/modules/systemlib/otp.h in px4 code:
self.otp_id = self.otp[0:4]
self.otp_idtype = self.otp[4:5]
self.otp_vid = self.otp[8:4:-1]
self.otp_pid = self.otp[12:8:-1]
self.otp_coa = self.otp[32:160]
# show user:
print("type: " + self.otp_id.decode('Latin-1'))
print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
print("sn: ", end='')
for byte in range(0,12,4):
x = self.__getSN(byte)
x = x[::-1] # reverse the bytes
self.sn = self.sn + x
print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
print('')
print("erase...")
self.__erase()
makefiles/config_px4fmu-v2_default.mk
+2
View File
@@ -38,6 +38,8 @@ MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
MODULES += drivers/mkblctrl
# Needs to be burned to the ground and re-written; for now,
# just don't build it.
makefiles/config_px4fmu-v2_test.mk
+9
View File
@@ -18,6 +18,12 @@ MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/boards/px4fmu-v2
MODULES += drivers/px4io
MODULES += drivers/rgbled
MODULES += drivers/mpu6000
MODULES += drivers/lsm303d
MODULES += drivers/l3gd20
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += systemcmds/perf
MODULES += systemcmds/reboot
MODULES += systemcmds/tests
@@ -31,6 +37,9 @@ MODULES += systemcmds/hw_ver
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/uORB
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
#
# Libraries
nuttx-configs/px4fmu-v2/include/board.h
+2 -2
View File
@@ -209,8 +209,8 @@
#define GPIO_USART3_RX GPIO_USART3_RX_3
#define GPIO_USART3_TX GPIO_USART3_TX_3
#define GPIO_USART2_RTS GPIO_USART2_RTS_2
#define GPIO_USART2_CTS GPIO_USART2_CTS_2
#define GPIO_USART3_RTS GPIO_USART3_RTS_2
#define GPIO_USART3_CTS GPIO_USART3_CTS_2
#define GPIO_UART4_RX GPIO_UART4_RX_1
#define GPIO_UART4_TX GPIO_UART4_TX_1
src/drivers/airspeed/airspeed.cpp
+21 -7
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -86,6 +86,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
_collect_phase(false),
_diff_pres_offset(0.0f),
_airspeed_pub(-1),
_class_instance(-1),
_conversion_interval(conversion_interval),
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
@@ -102,6 +103,9 @@ Airspeed::~Airspeed()
/* make sure we are truly inactive */
stop();
if (_class_instance != -1)
unregister_class_devname(AIRSPEED_DEVICE_PATH, _class_instance);
/* free any existing reports */
if (_reports != nullptr)
delete _reports;
@@ -126,13 +130,23 @@ Airspeed::init()
if (_reports == nullptr)
goto out;
/* get a publish handle on the airspeed topic */
differential_pressure_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report);
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(AIRSPEED_DEVICE_PATH);
if (_airspeed_pub < 0)
warnx("failed to create airspeed sensor object. Did you start uOrb?");
/* publication init */
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct differential_pressure_s arp;
measure();
_reports->get(&arp);
/* measurement will have generated a report, publish */
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
if (_airspeed_pub < 0)
warnx("failed to create airspeed sensor object. uORB started?");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
src/drivers/airspeed/airspeed.h
+3 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -127,6 +127,8 @@ protected:
orb_advert_t _airspeed_pub;
int _class_instance;
unsigned _conversion_interval;
perf_counter_t _sample_perf;
src/drivers/bma180/bma180.cpp
+18 -7
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -153,6 +153,7 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _class_instance;
unsigned _current_lowpass;
unsigned _current_range;
@@ -238,6 +239,7 @@ BMA180::BMA180(int bus, spi_dev_e device) :
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_class_instance(-1),
_current_lowpass(0),
_current_range(0),
_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
@@ -282,11 +284,6 @@ BMA180::init()
if (_reports == nullptr)
goto out;
/* advertise sensor topic */
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
@@ -322,6 +319,19 @@ BMA180::init()
ret = ERROR;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
measure();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
struct accel_report arp;
_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
}
out:
return ret;
}
@@ -723,7 +733,8 @@ BMA180::measure()
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
if (_accel_topic > 0 && !(_pub_blocked))
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
/* stop the perf counter */
perf_end(_sample_perf);
src/drivers/device/cdev.cpp
+11 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -38,6 +38,7 @@
*/
#include "device.h"
#include "drivers/drv_device.h"
#include <sys/ioctl.h>
#include <arch/irq.h>
@@ -93,6 +94,7 @@ CDev::CDev(const char *name,
Device(name, irq),
// public
// protected
_pub_blocked(false),
// private
_devname(devname),
_registered(false),
@@ -256,6 +258,14 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
case DIOC_GETPRIV:
*(void **)(uintptr_t)arg = (void *)this;
return OK;
break;
case DEVIOCSPUBBLOCK:
_pub_blocked = (arg != 0);
return OK;
break;
case DEVIOCGPUBBLOCK:
return _pub_blocked;
break;
}
return -ENOTTY;
src/drivers/device/device.cpp
+1 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
src/drivers/device/device.h
+3 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -415,6 +415,8 @@ protected:
*/
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
bool _pub_blocked; /**< true if publishing should be blocked */
private:
static const unsigned _max_pollwaiters = 8;
src/drivers/drv_device.h
+62
View File
@@ -0,0 +1,62 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_device.h
*
* Generic device / sensor interface.
*/
#ifndef _DRV_DEVICE_H
#define _DRV_DEVICE_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
/*
* ioctl() definitions
*/
#define _DEVICEIOCBASE (0x100)
#define _DEVICEIOC(_n) (_IOC(_DEVICEIOCBASE, _n))
/** ask device to stop publishing */
#define DEVIOCSPUBBLOCK _DEVICEIOC(0)
/** check publication block status */
#define DEVIOCGPUBBLOCK _DEVICEIOC(1)
#endif /* _DRV_DEVICE_H */
src/drivers/ets_airspeed/ets_airspeed.cpp
+5 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -184,8 +184,10 @@ ETSAirspeed::collect()
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
if (_airspeed_pub > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
src/drivers/gps/gps.cpp
+13 -8
View File
@@ -289,11 +289,13 @@ GPS::task_main()
//no time and satellite information simulated
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
if (!(_pub_blocked)) {
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
}
usleep(2e5);
@@ -330,11 +332,14 @@ GPS::task_main()
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
if (!(_pub_blocked)) {
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
}
last_rate_count++;
src/drivers/gps/gps_helper.cpp
+4 -3
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.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
@@ -41,6 +39,9 @@
/**
* @file gps_helper.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
float
src/drivers/gps/gps_helper.h
+3 -3
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.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
@@ -35,6 +33,8 @@
/**
* @file gps_helper.h
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef GPS_HELPER_H
src/drivers/gps/module.mk
+1 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
src/drivers/gps/mtk.cpp
+7 -4
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,7 +31,12 @@
*
****************************************************************************/
/* @file mkt.cpp */
/**
* @file mtk.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <unistd.h>
#include <stdio.h>
src/drivers/gps/mtk.h
+7 -4
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,7 +31,12 @@
*
****************************************************************************/
/* @file mtk.h */
/**
* @file mtk.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef MTK_H_
#define MTK_H_
src/drivers/gps/ubx.cpp
+6 -4
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -40,8 +37,13 @@
* U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
*/
#include <assert.h>
#include <math.h>
#include <poll.h>
src/drivers/gps/ubx.h
+12 -5
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,7 +31,17 @@
*
****************************************************************************/
/* @file U-Blox protocol definitions */
/**
* @file ubx.h
*
* U-Blox protocol definition. Following u-blox 6/7 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
*/
#ifndef UBX_H_
#define UBX_H_
src/drivers/hmc5883/hmc5883.cpp
+18 -18
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -381,16 +381,6 @@ HMC5883::init()
reset();
_class_instance = register_class_devname(MAG_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the mag topic if we are
* the primary mag */
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
}
ret = OK;
/* sensor is ok, but not calibrated */
@@ -867,9 +857,17 @@ HMC5883::collect()
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag publication");
}
}
/* post a report to the ring */
@@ -1140,10 +1138,12 @@ int HMC5883::check_calibration()
SUBSYSTEM_TYPE_MAG};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
if (!(_pub_blocked)) {
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
}
src/drivers/l3gd20/l3gd20.cpp
+19 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -379,15 +379,24 @@ L3GD20::init()
goto out;
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic */
struct gyro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
}
reset();
measure();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
}
ret = OK;
out:
return ret;
@@ -894,8 +903,10 @@ L3GD20::measure()
poll_notify(POLLIN);
/* publish for subscribers */
if (_gyro_topic > 0)
if (_gyro_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;
src/drivers/lsm303d/lsm303d.cpp
+44 -27
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -277,15 +277,15 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
int _class_instance;
int _accel_class_instance;
unsigned _accel_read;
unsigned _mag_read;
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
perf_counter_t _reg7_resets;
perf_counter_t _reg1_resets;
perf_counter_t _reg7_resets;
perf_counter_t _extreme_values;
perf_counter_t _accel_reschedules;
@@ -295,8 +295,8 @@ private:
// expceted values of reg1 and reg7 to catch in-flight
// brownouts of the sensor
uint8_t _reg7_expected;
uint8_t _reg1_expected;
uint8_t _reg7_expected;
// accel logging
int _accel_log_fd;
@@ -500,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(-1),
_class_instance(-1),
_accel_class_instance(-1),
_accel_read(0),
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
@@ -551,8 +551,8 @@ LSM303D::~LSM303D()
if (_mag_reports != nullptr)
delete _mag_reports;
if (_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
if (_accel_class_instance != -1)
unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
delete _mag;
@@ -562,13 +562,13 @@ LSM303D::~LSM303D()
perf_free(_reg1_resets);
perf_free(_reg7_resets);
perf_free(_extreme_values);
perf_free(_accel_reschedules);
}
int
LSM303D::init()
{
int ret = ERROR;
int mag_ret;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
@@ -597,13 +597,37 @@ LSM303D::init()
goto out;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
// we are the primary accel device, so advertise to
// the ORB
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
/* fill report structures */
measure();
if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
if (_mag->_mag_topic < 0)
debug("failed to create sensor_mag publication");
}
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
}
out:
@@ -727,7 +751,7 @@ LSM303D::check_extremes(const accel_report *arb)
_last_log_us = now;
::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
(unsigned long long)arb->timestamp,
arb->x, arb->y, arb->z,
(double)arb->x, (double)arb->y, (double)arb->z,
(int)arb->x_raw,
(int)arb->y_raw,
(int)arb->z_raw,
@@ -1517,8 +1541,8 @@ LSM303D::measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_accel_topic != -1) {
/* publish for subscribers */
if (_accel_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
@@ -1591,8 +1615,8 @@ LSM303D::mag_measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_mag->_mag_topic != -1) {
/* publish for subscribers */
if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
@@ -1707,13 +1731,6 @@ LSM303D_mag::init()
goto out;
_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
if (_mag_class_instance == CLASS_DEVICE_PRIMARY) {
// we are the primary mag device, so advertise to
// the ORB
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
}
out:
return ret;
src/drivers/meas_airspeed/meas_airspeed.cpp
+5 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -208,8 +208,10 @@ MEASAirspeed::collect()
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
if (_airspeed_pub > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
src/drivers/mkblctrl/mkblctrl.cpp
+94 -364
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Marco Bauer <marco@wtns.de>
*
* Redistribution and use in source and binary forms, with or without
@@ -65,7 +65,6 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
@@ -93,16 +92,12 @@
#define MOTOR_SPINUP_COUNTER 30
#define ESC_UORB_PUBLISH_DELAY 500000
class MK : public device::I2C
{
public:
enum Mode {
MODE_NONE,
MODE_2PWM,
MODE_4PWM,
MODE_6PWM,
};
enum MappingMode {
MAPPING_MK = 0,
MAPPING_PX4,
@@ -120,8 +115,7 @@ public:
virtual int init(unsigned motors);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
int set_mode(Mode mode);
int set_pwm_rate(unsigned rate);
int set_update_rate(unsigned rate);
int set_motor_count(unsigned count);
int set_motor_test(bool motortest);
int set_overrideSecurityChecks(bool overrideSecurityChecks);
@@ -133,7 +127,6 @@ private:
static const unsigned _max_actuators = MAX_MOTORS;
static const bool showDebug = false;
Mode _mode;
int _update_rate;
int _current_update_rate;
int _task;
@@ -180,33 +173,15 @@ private:
static const GPIOConfig _gpio_tab[];
static const unsigned _ngpio;
void gpio_reset(void);
void gpio_set_function(uint32_t gpios, int function);
void gpio_write(uint32_t gpios, int function);
uint32_t gpio_read(void);
int gpio_ioctl(file *filp, int cmd, unsigned long arg);
int mk_servo_arm(bool status);
int mk_servo_set(unsigned int chan, short val);
int mk_servo_set_value(unsigned int chan, short val);
int mk_servo_test(unsigned int chan);
short scaling(float val, float inMin, float inMax, float outMin, float outMax);
};
const MK::GPIOConfig MK::_gpio_tab[] = {
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
};
const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]);
const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad + configuration
const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 }; // Addresstranslator for Hexa + configuration
@@ -247,8 +222,7 @@ MK *g_mk;
MK::MK(int bus, const char *_device_path) :
I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
_mode(MODE_NONE),
_update_rate(50),
_update_rate(400),
_task(-1),
_t_actuators(-1),
_t_actuator_armed(-1),
@@ -317,26 +291,23 @@ MK::init(unsigned motors)
usleep(500000);
if (sizeof(_device) > 0) {
ret = register_driver(_device, &fops, 0666, (void *)this);
if (sizeof(_device) > 0) {
ret = register_driver(_device, &fops, 0666, (void *)this);
if (ret == OK) {
if (ret == OK) {
log("creating alternate output device");
_primary_pwm_device = true;
}
}
/* reset GPIOs */
gpio_reset();
}
/* start the IO interface task */
_task = task_spawn_cmd("mkblctrl",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
(main_t)&MK::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
(main_t)&MK::task_main_trampoline,
nullptr);
if (_task < 0) {
@@ -354,43 +325,7 @@ MK::task_main_trampoline(int argc, char *argv[])
}
int
MK::set_mode(Mode mode)
{
/*
* Configure for PWM output.
*
* Note that regardless of the configured mode, the task is always
* listening and mixing; the mode just selects which of the channels
* are presented on the output pins.
*/
switch (mode) {
case MODE_2PWM:
up_pwm_servo_deinit();
_update_rate = UPDATE_RATE; /* default output rate */
break;
case MODE_4PWM:
up_pwm_servo_deinit();
_update_rate = UPDATE_RATE; /* default output rate */
break;
case MODE_NONE:
debug("MODE_NONE");
/* disable servo outputs and set a very low update rate */
up_pwm_servo_deinit();
_update_rate = UPDATE_RATE;
break;
default:
return -EINVAL;
}
_mode = mode;
return OK;
}
int
MK::set_pwm_rate(unsigned rate)
MK::set_update_rate(unsigned rate)
{
if ((rate > 500) || (rate < 10))
return -EINVAL;
@@ -621,11 +556,13 @@ MK::task_main()
}
}
if(!_overrideSecurityChecks) {
if (!_overrideSecurityChecks) {
/* don't go under BLCTRL_MIN_VALUE */
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
outputs.output[i] = BLCTRL_MIN_VALUE;
}
}
/* output to BLCtrl's */
@@ -675,21 +612,24 @@ MK::task_main()
esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
esc.esc[i].esc_rpm = (uint16_t) 0;
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
if (Motor[i].Version == 1) {
// BLCtrl 2.0 (11Bit)
esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits;
} else {
// BLCtrl < 2.0 (8Bit)
esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
}
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
esc.esc[i].esc_errorcount = (uint16_t) 0;
// if motortest is requested - do it...
if (_motortest == true) {
mk_servo_test(i);
}
// if motortest is requested - do it...
if (_motortest == true) {
mk_servo_test(i);
}
}
@@ -728,7 +668,7 @@ MK::mk_servo_arm(bool status)
unsigned int
MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
{
if(initI2C) {
if (initI2C) {
I2C::init();
}
@@ -781,8 +721,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
}
if(!_overrideSecurityChecks) {
if (!_overrideSecurityChecks) {
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
_task_should_exit = true;
}
@@ -811,8 +751,8 @@ MK::mk_servo_set(unsigned int chan, short val)
tmpVal = 0;
}
Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff;
Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07;
if (_armed == false) {
Motor[chan].SetPoint = 0;
@@ -1019,28 +959,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret;
// XXX disabled, confusing users
/* try it as a GPIO ioctl first */
ret = gpio_ioctl(filp, cmd, arg);
if (ret != -ENOTTY)
return ret;
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
/*
switch (_mode) {
case MODE_2PWM:
case MODE_4PWM:
case MODE_6PWM:
ret = pwm_ioctl(filp, cmd, arg);
break;
default:
debug("not in a PWM mode");
break;
}
*/
ret = pwm_ioctl(filp, cmd, arg);
/* if nobody wants it, let CDev have it */
@@ -1075,6 +993,10 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = OK;
break;
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = _update_rate;
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = OK;
break;
@@ -1084,6 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
if (arg < 2150) {
Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
} else {
ret = -EINVAL;
}
@@ -1198,139 +1121,10 @@ MK::write(file *filp, const char *buffer, size_t len)
return count * 2;
}
void
MK::gpio_reset(void)
{
/*
* Setup default GPIO config - all pins as GPIOs, GPIO driver chip
* to input mode.
*/
for (unsigned i = 0; i < _ngpio; i++)
stm32_configgpio(_gpio_tab[i].input);
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
stm32_configgpio(GPIO_GPIO_DIR);
}
void
MK::gpio_set_function(uint32_t gpios, int function)
{
/*
* GPIOs 0 and 1 must have the same direction as they are buffered
* by a shared 2-port driver. Any attempt to set either sets both.
*/
if (gpios & 3) {
gpios |= 3;
/* flip the buffer to output mode if required */
if (GPIO_SET_OUTPUT == function)
stm32_gpiowrite(GPIO_GPIO_DIR, 1);
}
/* configure selected GPIOs as required */
for (unsigned i = 0; i < _ngpio; i++) {
if (gpios & (1 << i)) {
switch (function) {
case GPIO_SET_INPUT:
stm32_configgpio(_gpio_tab[i].input);
break;
case GPIO_SET_OUTPUT:
stm32_configgpio(_gpio_tab[i].output);
break;
case GPIO_SET_ALT_1:
if (_gpio_tab[i].alt != 0)
stm32_configgpio(_gpio_tab[i].alt);
break;
}
}
}
/* flip buffer to input mode if required */
if ((GPIO_SET_INPUT == function) && (gpios & 3))
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
}
void
MK::gpio_write(uint32_t gpios, int function)
{
int value = (function == GPIO_SET) ? 1 : 0;
for (unsigned i = 0; i < _ngpio; i++)
if (gpios & (1 << i))
stm32_gpiowrite(_gpio_tab[i].output, value);
}
uint32_t
MK::gpio_read(void)
{
uint32_t bits = 0;
for (unsigned i = 0; i < _ngpio; i++)
if (stm32_gpioread(_gpio_tab[i].input))
bits |= (1 << i);
return bits;
}
int
MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = OK;
lock();
switch (cmd) {
case GPIO_RESET:
gpio_reset();
break;
case GPIO_SET_OUTPUT:
case GPIO_SET_INPUT:
case GPIO_SET_ALT_1:
gpio_set_function(arg, cmd);
break;
case GPIO_SET_ALT_2:
case GPIO_SET_ALT_3:
case GPIO_SET_ALT_4:
ret = -EINVAL;
break;
case GPIO_SET:
case GPIO_CLEAR:
gpio_write(arg, cmd);
break;
case GPIO_GET:
*(uint32_t *)arg = gpio_read();
break;
default:
ret = -ENOTTY;
}
unlock();
return ret;
}
namespace
{
enum PortMode {
PORT_MODE_UNSET = 0,
PORT_FULL_GPIO,
PORT_FULL_SERIAL,
PORT_FULL_PWM,
PORT_GPIO_AND_SERIAL,
PORT_PWM_AND_SERIAL,
PORT_PWM_AND_GPIO,
};
enum MappingMode {
MAPPING_MK = 0,
MAPPING_PX4,
@@ -1341,20 +1135,11 @@ enum FrameType {
FRAME_X,
};
PortMode g_port_mode;
int
mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
{
uint32_t gpio_bits;
int shouldStop = 0;
MK::Mode servo_mode;
/* reset to all-inputs */
g_mk->ioctl(0, GPIO_RESET, 0);
gpio_bits = 0;
servo_mode = MK::MODE_NONE;
/* native PX4 addressing) */
g_mk->set_px4mode(px4mode);
@@ -1368,7 +1153,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
/* ovveride security checks if enabled */
g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
/* count used motors */
do {
if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
@@ -1383,86 +1167,54 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
/* (re)set the PWM output mode */
g_mk->set_mode(servo_mode);
if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
g_mk->set_pwm_rate(update_rate);
g_mk->set_update_rate(update_rate);
return OK;
}
int
mk_start(unsigned bus, unsigned motors, char *device_path)
{
int ret = OK;
if (g_mk == nullptr) {
g_mk = new MK(bus, device_path);
if (g_mk == nullptr) {
ret = -ENOMEM;
} else {
ret = g_mk->init(motors);
if (ret != OK) {
delete g_mk;
g_mk = nullptr;
}
}
}
return ret;
}
int
mk_check_for_i2c_esc_bus(char *device_path, int motors)
mk_start(unsigned motors, char *device_path)
{
int ret;
if (g_mk == nullptr) {
// try i2c3 first
g_mk = new MK(3, device_path);
g_mk = new MK(3, device_path);
if (!g_mk)
return -ENOMEM;
if (g_mk == nullptr) {
return -1;
} else {
ret = g_mk->mk_check_for_blctrl(8, false, true);
delete g_mk;
g_mk = nullptr;
if (ret > 0) {
return 3;
}
}
g_mk = new MK(1, device_path);
if (g_mk == nullptr) {
return -1;
} else {
ret = g_mk->mk_check_for_blctrl(8, false, true);
delete g_mk;
g_mk = nullptr;
if (ret > 0) {
return 1;
}
if (OK == g_mk->init(motors)) {
warnx("[mkblctrl] scanning i2c3...\n");
ret = g_mk->mk_check_for_blctrl(8, false, true);
if (ret > 0) {
return OK;
}
}
return -1;
}
delete g_mk;
g_mk = nullptr;
// fallback to bus 1
g_mk = new MK(1, device_path);
if (!g_mk)
return -ENOMEM;
if (OK == g_mk->init(motors)) {
warnx("[mkblctrl] scanning i2c1...\n");
ret = g_mk->mk_check_for_blctrl(8, false, true);
if (ret > 0) {
return OK;
}
}
delete g_mk;
g_mk = nullptr;
return -ENXIO;
}
} // namespace
@@ -1472,10 +1224,8 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
int
mkblctrl_main(int argc, char *argv[])
{
PortMode port_mode = PORT_FULL_PWM;
int pwm_update_rate_in_hz = UPDATE_RATE;
int motorcount = 8;
int bus = -1;
int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default
bool motortest = false;
@@ -1489,18 +1239,6 @@ mkblctrl_main(int argc, char *argv[])
*/
for (int i = 1; i < argc; i++) {
/* look for the optional i2c bus parameter */
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
if (argc > i + 1) {
bus = atoi(argv[i + 1]);
newMode = true;
} else {
errx(1, "missing argument for i2c bus (-b)");
return 1;
}
}
/* look for the optional frame parameter */
if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
if (argc > i + 1) {
@@ -1560,51 +1298,43 @@ mkblctrl_main(int argc, char *argv[])
fprintf(stderr, "mkblctrl: help:\n");
fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n");
fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n");
fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
fprintf(stderr, "\n");
fprintf(stderr, "Motortest:\n");
fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
fprintf(stderr, "mkblctrl -t\n");
fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
exit(1);
}
if (!motortest) {
if (g_mk == nullptr) {
if (bus == -1) {
bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
}
if (g_mk == nullptr) {
if (mk_start(motorcount, devicepath) != OK) {
errx(1, "failed to start the MK-BLCtrl driver");
}
if (bus != -1) {
if (mk_start(bus, motorcount, devicepath) != OK) {
errx(1, "failed to start the MK-BLCtrl driver");
}
} else {
errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)");
}
/* parameter set ? */
if (newMode) {
/* switch parameter */
return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
}
/* parameter set ? */
if (newMode) {
/* switch parameter */
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
}
exit(0);
exit(0);
} else {
errx(1, "MK-BLCtrl driver already running");
}
} else {
errx(1, "MK-BLCtrl driver already running");
}
} else {
if (g_mk == nullptr) {
errx(1, "MK-BLCtrl driver not running. You have to start it first.");
} else {
if (g_mk == nullptr) {
errx(1, "MK-BLCtrl driver not running. You have to start it first.");
} else {
g_mk->set_motor_test(motortest);
exit(0);
} else {
g_mk->set_motor_test(motortest);
exit(0);
}
}
}
}
}
src/drivers/mpu6000/mpu6000.cpp
+33 -16
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -443,7 +443,6 @@ int
MPU6000::init()
{
int ret;
int gyro_ret;
/* do SPI init (and probe) first */
ret = SPI::init();
@@ -488,16 +487,36 @@ MPU6000::init()
return ret;
}
/* fetch an initial set of measurements for advertisement */
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
measure();
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise accel topic */
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
}
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
}
if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro->_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
}
out:
return ret;
@@ -1307,10 +1326,13 @@ MPU6000::measure()
poll_notify(POLLIN);
_gyro->parent_poll_notify();
if (_accel_topic != -1) {
if (_accel_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (_gyro->_gyro_topic != -1) {
if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
@@ -1356,11 +1378,6 @@ MPU6000_gyro::init()
}
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
gyro_report gr;
memset(&gr, 0, sizeof(gr));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
out:
return ret;
src/drivers/ms5611/ms5611.cpp
+68 -19
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -90,6 +90,7 @@ static const int ERROR = -1;
/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */
#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */
#define MS5611_BARO_DEVICE_PATH "/dev/ms5611"
class MS5611 : public device::CDev
{
@@ -132,6 +133,8 @@ protected:
orb_advert_t _baro_topic;
int _class_instance;
perf_counter_t _sample_perf;
perf_counter_t _measure_perf;
perf_counter_t _comms_errors;
@@ -192,7 +195,7 @@ protected:
extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
CDev("MS5611", BARO_DEVICE_PATH),
CDev("MS5611", MS5611_BARO_DEVICE_PATH),
_interface(interface),
_prom(prom_buf.s),
_measure_ticks(0),
@@ -204,6 +207,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
_SENS(0),
_msl_pressure(101325),
_baro_topic(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
@@ -218,6 +222,9 @@ MS5611::~MS5611()
/* make sure we are truly inactive */
stop_cycle();
if (_class_instance != -1)
unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance);
/* free any existing reports */
if (_reports != nullptr)
delete _reports;
@@ -251,18 +258,57 @@ MS5611::init()
goto out;
}
/* get a publish handle on the baro topic */
struct baro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report);
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_DEVICE_PATH);
if (_baro_topic < 0) {
debug("failed to create sensor_baro object");
ret = -ENOSPC;
goto out;
}
struct baro_report brp;
/* do a first measurement cycle to populate reports with valid data */
_measure_phase = 0;
_reports->flush();
/* this do..while is goto without goto */
do {
/* do temperature first */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(MS5611_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* now do a pressure measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(MS5611_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
_reports->get(&brp);
ret = OK;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
if (_baro_topic < 0)
debug("failed to create sensor_baro publication");
}
} while (0);
ret = OK;
out:
return ret;
}
@@ -670,7 +716,10 @@ MS5611::collect()
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
if (_baro_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
@@ -812,7 +861,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(BARO_DEVICE_PATH, O_RDONLY);
fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
warnx("can't open baro device");
goto fail;
@@ -846,10 +895,10 @@ test()
ssize_t sz;
int ret;
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
@@ -905,7 +954,7 @@ test()
void
reset()
{
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@@ -944,10 +993,10 @@ calibrate(unsigned altitude)
float pressure;
float p1;
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
/* start the sensor polling at max */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
src/modules/commander/commander.cpp
+9 -5
View File
@@ -153,6 +153,7 @@ static uint64_t last_print_mode_reject_time = 0;
static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static int parachute_enabled = 0;
static struct vehicle_status_s status;
static struct actuator_armed_s armed;
@@ -568,7 +569,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
//XXX: to enable the parachute, a param needs to be set
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
@@ -620,6 +623,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_system_id = param_find("MAV_SYS_ID");
param_t _param_component_id = param_find("MAV_COMP_ID");
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
/* welcome user */
warnx("starting");
@@ -866,10 +870,10 @@ int commander_thread_main(int argc, char *argv[])
/* re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
}
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled);
}
orb_check(sp_man_sub, &updated);
@@ -1321,7 +1325,7 @@ int commander_thread_main(int argc, char *argv[])
// TODO remove this hack
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive();
}
src/modules/commander/state_machine_helper.cpp
+30
View File
@@ -42,6 +42,8 @@
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <dirent.h>
#include <fcntl.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -50,6 +52,7 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
@@ -344,6 +347,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
// Disable publication of all attached sensors
/* list directory */
DIR *d;
struct dirent *direntry;
d = opendir("/dev");
if (d) {
while ((direntry = readdir(d)) != NULL) {
int sensfd = ::open(direntry->d_name, 0);
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
close(sensfd);
printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
}
closedir(d);
warnx("directory listing ok (FS mounted and readable)");
} else {
/* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
return 1;
}
}
break;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+13 -12
View File
@@ -236,7 +236,7 @@ private:
float land_slope_length;
float land_H1_virt;
float land_flare_alt_relative;
float land_thrust_lim_horizontal_distance;
float land_thrust_lim_alt_relative;
float land_heading_hold_horizontal_distance;
} _parameters; /**< local copies of interesting parameters */
@@ -281,7 +281,7 @@ private:
param_t land_slope_length;
param_t land_H1_virt;
param_t land_flare_alt_relative;
param_t land_thrust_lim_horizontal_distance;
param_t land_thrust_lim_alt_relative;
param_t land_heading_hold_horizontal_distance;
} _parameter_handles; /**< handles for interesting parameters */
@@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST");
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
@@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
_l1_control.set_l1_damping(_parameters.l1_damping);
@@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update()
}
/* Update the landing slope */
landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
/* Update and publish the navigation capabilities */
_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
@@ -836,6 +836,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -846,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
if (pos_sp_triplet.previous.valid) {
target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
target_bearing = bearing_lastwp_currwp;
} else {
target_bearing = _att.yaw;
}
@@ -888,10 +890,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float airspeed_approach = 1.3f * _parameters.airspeed_min;
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
@@ -903,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* kill the throttle if param requests it */
throttle_max = _parameters.throttle_max;
if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) {
if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
if (!land_motor_lim) {
land_motor_lim = true;
@@ -912,7 +913,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt);
float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
/* avoid climbout */
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+2 -2
View File
@@ -168,9 +168,9 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
src/modules/fw_pos_control_l1/landingslope.cpp
+17 -5
View File
@@ -48,13 +48,13 @@
void Landingslope::update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_horizontal_distance,
float motor_lim_relative_alt,
float H1_virt)
{
_landing_slope_angle_rad = landing_slope_angle_rad;
_flare_relative_alt = flare_relative_alt;
_motor_lim_horizontal_distance = motor_lim_horizontal_distance;
_motor_lim_relative_alt = motor_lim_relative_alt;
_H1_virt = H1_virt;
calculateSlopeValues();
@@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
}
float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
{
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
else
return wp_altitude;
}
float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
{
/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
else
return wp_landing_altitude;
}
src/modules/fw_pos_control_l1/landingslope.h
+16 -5
View File
@@ -49,7 +49,7 @@ private:
/* see Documentation/fw_landing.png for a plot of the landing slope */
float _landing_slope_angle_rad; /**< phi in the plot */
float _flare_relative_alt; /**< h_flare,rel in the plot */
float _motor_lim_horizontal_distance;
float _motor_lim_relative_alt;
float _H1_virt; /**< H1 in the plot */
float _H0; /**< h_flare,rel + H1 in the plot */
float _d1; /**< d1 in the plot */
@@ -63,7 +63,18 @@ public:
Landingslope() {}
~Landingslope() {}
float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
* Performs check if aircraft is in front of waypoint to avoid climbout
*/
float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
/**
*
@@ -85,17 +96,17 @@ public:
}
float getFlareCurveAltitude(float wp_distance, float wp_altitude);
float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
void update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_horizontal_distance,
float motor_lim_relative_alt,
float H1_virt);
inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
inline float flare_relative_alt() {return _flare_relative_alt;}
inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;}
inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;}
inline float H1_virt() {return _H1_virt;}
inline float H0() {return _H0;}
inline float d1() {return _d1;}
src/modules/mavlink/mavlink_receiver.cpp
+1 -1
View File
@@ -381,7 +381,7 @@ handle_message(mavlink_message_t *msg)
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (telemetry_status_pub == 0) {
if (telemetry_status_pub <= 0) {
telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
src/modules/navigator/navigator_main.cpp
+1 -2
View File
@@ -1080,9 +1080,8 @@ Navigator::start_loiter()
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.previous.valid = false;
src/modules/navigator/navigator_params.c
+2 -1
View File
@@ -55,9 +55,10 @@
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
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, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); // enable parachute deployment
src/modules/position_estimator_inav/position_estimator_inav_main.c
+4 -4
View File
@@ -527,13 +527,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (gps.fix_type >= 3) {
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) {
if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) {
if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) {
gps_valid = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
}
@@ -589,8 +589,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_gps[2][1] = 0.0f;
}
w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m);
w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m);
w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m);
w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m);
}
} else {
src/modules/position_estimator_inav/position_estimator_inav_params.c
+1 -1
View File
@@ -40,7 +40,7 @@
#include "position_estimator_inav_params.h"
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
src/modules/sdlog2/sdlog2.c
+24
View File
@@ -82,6 +82,7 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/telemetry_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -758,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
struct telemetry_status_s telemetry;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -783,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
int telemetry_sub;
} subs;
/* log message buffer: header + body */
@@ -811,6 +814,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
struct log_DIST_s log_DIST;
struct log_TELE_s log_TELE;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -946,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- TELEMETRY STATUS --- */
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
fds[fdsc_count].fd = subs.telemetry_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -1347,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* --- TELEMETRY --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry);
log_msg.msg_type = LOG_TELE_MSG;
log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
log_msg.body.log_TELE.noise = buf.telemetry.noise;
log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
LOGBUFFER_WRITE_AND_COUNT(TELE);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
src/modules/sdlog2/sdlog2_messages.h
+14
View File
@@ -264,6 +264,18 @@ struct log_DIST_s {
uint8_t flags;
};
/* --- TELE - TELEMETRY STATUS --- */
#define LOG_TELE_MSG 22
struct log_TELE_s {
uint8_t rssi;
uint8_t remote_rssi;
uint8_t noise;
uint8_t remote_noise;
uint16_t rxerrors;
uint16_t fixed;
uint8_t txbuf;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@@ -311,6 +323,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),
src/modules/uORB/topics/telemetry_status.h
+5 -5
View File
@@ -58,10 +58,10 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
struct telemetry_status_s {
uint64_t timestamp;
enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */
unsigned rssi; /**< local signal strength */
unsigned remote_rssi; /**< remote signal strength */
unsigned rxerrors; /**< receive errors */
unsigned fixed; /**< count of error corrected packets */
uint8_t rssi; /**< local signal strength */
uint8_t remote_rssi; /**< remote signal strength */
uint16_t rxerrors; /**< receive errors */
uint16_t fixed; /**< count of error corrected packets */
uint8_t noise; /**< background noise level */
uint8_t remote_noise; /**< remote background noise level */
uint8_t txbuf; /**< how full the tx buffer is as a percentage */
@@ -73,4 +73,4 @@ struct telemetry_status_s {
ORB_DECLARE(telemetry_status);
#endif /* TOPIC_TELEMETRY_STATUS_H */
#endif /* TOPIC_TELEMETRY_STATUS_H */
src/systemcmds/config/config.c
+49 -7
View File
@@ -56,6 +56,7 @@
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
@@ -65,6 +66,7 @@ __EXPORT int config_main(int argc, char *argv[]);
static void do_gyro(int argc, char *argv[]);
static void do_accel(int argc, char *argv[]);
static void do_mag(int argc, char *argv[]);
static void do_device(int argc, char *argv[]);
int
config_main(int argc, char *argv[])
@@ -72,20 +74,60 @@ config_main(int argc, char *argv[])
if (argc >= 2) {
if (!strcmp(argv[1], "gyro")) {
do_gyro(argc - 2, argv + 2);
}
if (!strcmp(argv[1], "accel")) {
} else if (!strcmp(argv[1], "accel")) {
do_accel(argc - 2, argv + 2);
}
if (!strcmp(argv[1], "mag")) {
} else if (!strcmp(argv[1], "mag")) {
do_mag(argc - 2, argv + 2);
} else {
do_device(argc - 1, argv + 1);
}
}
errx(1, "expected a command, try 'gyro', 'accel', 'mag'");
}
static void
do_device(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "no device path provided and command provided.");
}
int fd;
int ret;
fd = open(argv[0], 0);
if (fd < 0) {
warn("%s", argv[0]);
errx(1, "FATAL: no device found");
} else {
if (argc == 2 && !strcmp(argv[1], "block")) {
/* disable the device publications */
ret = ioctl(fd, DEVIOCSPUBBLOCK, 1);
if (ret)
errx(ret,"uORB publications could not be blocked");
} else if (argc == 2 && !strcmp(argv[1], "unblock")) {
/* enable the device publications */
ret = ioctl(fd, DEVIOCSPUBBLOCK, 0);
if (ret)
errx(ret,"uORB publications could not be unblocked");
} else {
errx("no valid command: %s", argv[1]);
}
}
exit(0);
}
static void
do_gyro(int argc, char *argv[])
{
@@ -124,7 +166,7 @@ do_gyro(int argc, char *argv[])
if (ret)
errx(ret,"range could not be set");
} else if(argc == 1 && !strcmp(argv[0], "check")) {
} else if (argc == 1 && !strcmp(argv[0], "check")) {
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret) {