forked from Archive/PX4-Autopilot
Merge branch 'beta' into ready_rtl_fix
This commit is contained in:
commit
1eecc73483
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
#
|
||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
||||
|
|
Binary file not shown.
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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.
|
|
@ -0,0 +1,3 @@
|
|||
# Hexa coaxial
|
||||
|
||||
R: 6c 10000 10000 10000 0
|
|
@ -1,88 +0,0 @@
|
|||
#!nsh
|
||||
#
|
||||
# PX4FMU startup script for logging purposes
|
||||
#
|
||||
|
||||
#
|
||||
# Try to mount the microSD card.
|
||||
#
|
||||
echo "[init] looking for microSD..."
|
||||
if mount -t vfat /dev/mmcsd0 /fs/microsd
|
||||
then
|
||||
echo "[init] card mounted at /fs/microsd"
|
||||
# Start playing the startup tune
|
||||
tone_alarm start
|
||||
else
|
||||
echo "[init] no microSD card found"
|
||||
# Play SOS
|
||||
tone_alarm error
|
||||
fi
|
||||
|
||||
uorb start
|
||||
|
||||
#
|
||||
# Start sensor drivers here.
|
||||
#
|
||||
|
||||
ms5611 start
|
||||
adc start
|
||||
|
||||
# mag might be external
|
||||
if hmc5883 start
|
||||
then
|
||||
echo "using HMC5883"
|
||||
fi
|
||||
|
||||
if mpu6000 start
|
||||
then
|
||||
echo "using MPU6000"
|
||||
fi
|
||||
|
||||
if l3gd20 start
|
||||
then
|
||||
echo "using L3GD20(H)"
|
||||
fi
|
||||
|
||||
if lsm303d start
|
||||
then
|
||||
set BOARD fmuv2
|
||||
else
|
||||
set BOARD fmuv1
|
||||
fi
|
||||
|
||||
# Start airspeed sensors
|
||||
if meas_airspeed start
|
||||
then
|
||||
echo "using MEAS airspeed sensor"
|
||||
else
|
||||
if ets_airspeed start
|
||||
then
|
||||
echo "using ETS airspeed sensor (bus 3)"
|
||||
else
|
||||
if ets_airspeed start -b 1
|
||||
then
|
||||
echo "Using ETS airspeed sensor (bus 1)"
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the sensor collection task.
|
||||
# IMPORTANT: this also loads param offsets
|
||||
# ALWAYS start this task before the
|
||||
# preflight_check.
|
||||
#
|
||||
if sensors start
|
||||
then
|
||||
echo "SENSORS STARTED"
|
||||
fi
|
||||
|
||||
sdlog2 start -r 250 -e -b 16
|
||||
|
||||
if sercon
|
||||
then
|
||||
echo "[init] USB interface connected"
|
||||
|
||||
# Try to get an USB console
|
||||
nshterm /dev/ttyACM0 &
|
||||
fi
|
Binary file not shown.
|
@ -1,3 +1,4 @@
|
|||
parameters.wiki
|
||||
parameters.xml
|
||||
parameters.wikirpc.xml
|
||||
cookies.txt
|
|
@ -1,62 +0,0 @@
|
|||
import output
|
||||
from xml.sax.saxutils import escape
|
||||
|
||||
class DokuWikiOutput(output.Output):
|
||||
def Generate(self, groups):
|
||||
pre_text = """<?xml version='1.0'?>
|
||||
<methodCall>
|
||||
<methodName>wiki.putPage</methodName>
|
||||
<params>
|
||||
<param>
|
||||
<value>
|
||||
<string>:firmware:parameters</string>
|
||||
</value>
|
||||
</param>
|
||||
<param>
|
||||
<value>
|
||||
<string>"""
|
||||
result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values."
|
||||
for group in groups:
|
||||
result += "==== %s ====\n\n" % group.GetName()
|
||||
result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n"
|
||||
result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n"
|
||||
for param in group.GetParams():
|
||||
code = param.GetFieldValue("code")
|
||||
name = param.GetFieldValue("short_desc")
|
||||
name = name.replace("\n", "")
|
||||
result += "| %s | %s " % (code, name)
|
||||
min_val = param.GetFieldValue("min")
|
||||
if min_val is not None:
|
||||
result += " | %s " % min_val
|
||||
else:
|
||||
result += " | "
|
||||
max_val = param.GetFieldValue("max")
|
||||
if max_val is not None:
|
||||
result += " | %s " % max_val
|
||||
else:
|
||||
result += " | "
|
||||
def_val = param.GetFieldValue("default")
|
||||
if def_val is not None:
|
||||
result += "| %s " % def_val
|
||||
else:
|
||||
result += " | "
|
||||
long_desc = param.GetFieldValue("long_desc")
|
||||
if long_desc is not None:
|
||||
long_desc = long_desc.replace("\n", "")
|
||||
result += "| %s " % long_desc
|
||||
else:
|
||||
result += " | "
|
||||
result += " |\n"
|
||||
result += "\n"
|
||||
post_text = """</string>
|
||||
</value>
|
||||
</param>
|
||||
<param>
|
||||
<value>
|
||||
<name>sum</name>
|
||||
<string>Updated parameters automagically from code.</string>
|
||||
</value>
|
||||
</param>
|
||||
</params>
|
||||
</methodCall>"""
|
||||
return pre_text + escape(result) + post_text
|
|
@ -1,5 +0,0 @@
|
|||
class Output(object):
|
||||
def Save(self, groups, fn):
|
||||
data = self.Generate(groups)
|
||||
with open(fn, 'w') as f:
|
||||
f.write(data)
|
|
@ -1,7 +1,7 @@
|
|||
import output
|
||||
import codecs
|
||||
|
||||
class DokuWikiOutput(output.Output):
|
||||
def Generate(self, groups):
|
||||
class DokuWikiListingsOutput():
|
||||
def __init__(self, groups):
|
||||
result = ""
|
||||
for group in groups:
|
||||
result += "==== %s ====\n\n" % group.GetName()
|
||||
|
@ -24,4 +24,8 @@ class DokuWikiOutput(output.Output):
|
|||
if def_val is not None:
|
||||
result += "* Default value: %s\n" % def_val
|
||||
result += "\n"
|
||||
return result
|
||||
self.output = result
|
||||
|
||||
def Save(self, filename):
|
||||
with codecs.open(filename, 'w', 'utf-8') as f:
|
||||
f.write(self.output)
|
|
@ -0,0 +1,76 @@
|
|||
from xml.sax.saxutils import escape
|
||||
import codecs
|
||||
|
||||
class DokuWikiTablesOutput():
|
||||
def __init__(self, groups):
|
||||
result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values.\n\n"
|
||||
for group in groups:
|
||||
result += "==== %s ====\n\n" % group.GetName()
|
||||
result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n"
|
||||
result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n"
|
||||
for param in group.GetParams():
|
||||
code = param.GetFieldValue("code")
|
||||
name = param.GetFieldValue("short_desc")
|
||||
min_val = param.GetFieldValue("min")
|
||||
max_val = param.GetFieldValue("max")
|
||||
def_val = param.GetFieldValue("default")
|
||||
long_desc = param.GetFieldValue("long_desc")
|
||||
|
||||
name = name.replace("\n", " ")
|
||||
result += "| %s | %s |" % (code, name)
|
||||
|
||||
if min_val is not None:
|
||||
result += " %s |" % min_val
|
||||
else:
|
||||
result += " |"
|
||||
|
||||
if max_val is not None:
|
||||
result += " %s |" % max_val
|
||||
else:
|
||||
result += " |"
|
||||
|
||||
if def_val is not None:
|
||||
result += " %s |" % def_val
|
||||
else:
|
||||
result += " |"
|
||||
|
||||
if long_desc is not None:
|
||||
long_desc = long_desc.replace("\n", " ")
|
||||
result += " %s |" % long_desc
|
||||
else:
|
||||
result += " |"
|
||||
|
||||
result += "\n"
|
||||
result += "\n"
|
||||
self.output = result;
|
||||
|
||||
def Save(self, filename):
|
||||
with codecs.open(filename, 'w', 'utf-8') as f:
|
||||
f.write(self.output)
|
||||
|
||||
def SaveRpc(self, filename):
|
||||
with codecs.open(filename, 'w', 'utf-8') as f:
|
||||
f.write("""<?xml version='1.0'?>
|
||||
<methodCall>
|
||||
<methodName>wiki.putPage</methodName>
|
||||
<params>
|
||||
<param>
|
||||
<value>
|
||||
<string>:firmware:parameters</string>
|
||||
</value>
|
||||
</param>
|
||||
<param>
|
||||
<value>
|
||||
<string>""")
|
||||
f.write(escape(self.output))
|
||||
f.write("""</string>
|
||||
</value>
|
||||
</param>
|
||||
<param>
|
||||
<value>
|
||||
<name>sum</name>
|
||||
<string>Updated parameters automagically from code.</string>
|
||||
</value>
|
||||
</param>
|
||||
</params>
|
||||
</methodCall>""")
|
|
@ -1,8 +1,8 @@
|
|||
import output
|
||||
from xml.dom.minidom import getDOMImplementation
|
||||
import codecs
|
||||
|
||||
class XMLOutput(output.Output):
|
||||
def Generate(self, groups):
|
||||
class XMLOutput():
|
||||
def __init__(self, groups):
|
||||
impl = getDOMImplementation()
|
||||
xml_document = impl.createDocument(None, "parameters", None)
|
||||
xml_parameters = xml_document.documentElement
|
||||
|
@ -19,4 +19,8 @@ class XMLOutput(output.Output):
|
|||
xml_param.appendChild(xml_field)
|
||||
xml_value = xml_document.createTextNode(value)
|
||||
xml_field.appendChild(xml_value)
|
||||
return xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8")
|
||||
self.xml_document = xml_document
|
||||
|
||||
def Save(self, filename):
|
||||
with codecs.open(filename, 'w', 'utf-8') as f:
|
||||
self.xml_document.writexml(f, indent=" ", addindent=" ", newl="\n")
|
|
@ -40,22 +40,28 @@
|
|||
#
|
||||
|
||||
import scanner
|
||||
import parser
|
||||
import xmlout
|
||||
import dokuwikiout
|
||||
import srcparser
|
||||
import output_xml
|
||||
import output_dokuwiki_tables
|
||||
import output_dokuwiki_listings
|
||||
|
||||
# Initialize parser
|
||||
prs = parser.Parser()
|
||||
prs = srcparser.Parser()
|
||||
|
||||
# Scan directories, and parse the files
|
||||
sc = scanner.Scanner()
|
||||
sc.ScanDir("../../src", prs)
|
||||
output = prs.GetParamGroups()
|
||||
groups = prs.GetParamGroups()
|
||||
|
||||
# Output into XML
|
||||
out = xmlout.XMLOutput()
|
||||
out.Save(output, "parameters.xml")
|
||||
out = output_xml.XMLOutput(groups)
|
||||
out.Save("parameters.xml")
|
||||
|
||||
# Output into DokuWiki
|
||||
out = dokuwikiout.DokuWikiOutput()
|
||||
out.Save(output, "parameters.wiki")
|
||||
# Output to DokuWiki listings
|
||||
#out = output_dokuwiki_listings.DokuWikiListingsOutput(groups)
|
||||
#out.Save("parameters.wiki")
|
||||
|
||||
# Output to DokuWiki tables
|
||||
out = output_dokuwiki_tables.DokuWikiTablesOutput(groups)
|
||||
out.Save("parameters.wiki")
|
||||
out.SaveRpc("parameters.wikirpc.xml")
|
||||
|
|
|
@ -1,5 +1,6 @@
|
|||
import os
|
||||
import re
|
||||
import codecs
|
||||
|
||||
class Scanner(object):
|
||||
"""
|
||||
|
@ -29,6 +30,6 @@ class Scanner(object):
|
|||
Scans provided file and passes its contents to the parser using
|
||||
parser.Parse method.
|
||||
"""
|
||||
with open(path, 'r') as f:
|
||||
with codecs.open(path, 'r', 'utf-8') as f:
|
||||
contents = f.read()
|
||||
parser.Parse(contents)
|
||||
|
|
|
@ -28,8 +28,7 @@ class ParameterGroup(object):
|
|||
state of the parser.
|
||||
"""
|
||||
return sorted(self.params,
|
||||
cmp=lambda x, y: cmp(x.GetFieldValue("code"),
|
||||
y.GetFieldValue("code")))
|
||||
key=lambda x: x.GetFieldValue("code"))
|
||||
|
||||
class Parameter(object):
|
||||
"""
|
||||
|
@ -61,9 +60,10 @@ class Parameter(object):
|
|||
"""
|
||||
Return list of existing field codes in convenient order
|
||||
"""
|
||||
return sorted(self.fields.keys(),
|
||||
cmp=lambda x, y: cmp(self.priority.get(y, 0),
|
||||
self.priority.get(x, 0)) or cmp(x, y))
|
||||
keys = self.fields.keys()
|
||||
keys = sorted(keys)
|
||||
keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
|
||||
return keys
|
||||
|
||||
def GetFieldValue(self, code):
|
||||
"""
|
||||
|
@ -197,7 +197,7 @@ class Parser(object):
|
|||
if tag == "group":
|
||||
group = tags[tag]
|
||||
elif tag not in self.valid_tags:
|
||||
sys.stderr.write("Skipping invalid"
|
||||
sys.stderr.write("Skipping invalid "
|
||||
"documentation tag: '%s'\n" % tag)
|
||||
else:
|
||||
param.SetField(tag, tags[tag])
|
||||
|
@ -214,7 +214,7 @@ class Parser(object):
|
|||
object. Note that returned object is not a copy. Modifications affect
|
||||
state of the parser.
|
||||
"""
|
||||
return sorted(self.param_groups.values(),
|
||||
cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0),
|
||||
self.priority.get(x.GetName(), 0)) or cmp(x.GetName(),
|
||||
y.GetName()))
|
||||
groups = self.param_groups.values()
|
||||
groups = sorted(groups, key=lambda x: x.GetName())
|
||||
groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True)
|
||||
return groups
|
|
@ -2,4 +2,4 @@ python px_process_params.py
|
|||
|
||||
rm cookies.txt
|
||||
curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login
|
||||
curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wiki "https://pixhawk.org/lib/exe/xmlrpc.php"
|
||||
curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wikirpc.xml "https://pixhawk.org/lib/exe/xmlrpc.php"
|
||||
|
|
|
@ -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()
|
||||
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -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.
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -141,9 +141,9 @@
|
|||
|
||||
#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY)
|
||||
|
||||
/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
|
||||
* otherwise frequency is 2xAPBx.
|
||||
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
@ -76,8 +76,8 @@
|
|||
|
||||
#include <drivers/airspeed/airspeed.h>
|
||||
|
||||
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
|
||||
I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
|
||||
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
|
||||
I2C("Airspeed", path, bus, address, 100000),
|
||||
_reports(nullptr),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
|
||||
_max_differential_pressure_pa(0),
|
||||
|
@ -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 */
|
||||
|
|
|
@ -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
|
||||
|
@ -90,7 +90,7 @@ static const int ERROR = -1;
|
|||
class __EXPORT Airspeed : public device::I2C
|
||||
{
|
||||
public:
|
||||
Airspeed(int bus, int address, unsigned conversion_interval);
|
||||
Airspeed(int bus, int address, unsigned conversion_interval, const char* path);
|
||||
virtual ~Airspeed();
|
||||
|
||||
virtual int init();
|
||||
|
@ -127,6 +127,8 @@ protected:
|
|||
|
||||
orb_advert_t _airspeed_pub;
|
||||
|
||||
int _class_instance;
|
||||
|
||||
unsigned _conversion_interval;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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 */
|
|
@ -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
|
||||
|
@ -77,6 +77,7 @@
|
|||
|
||||
/* I2C bus address */
|
||||
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
|
||||
#define ETS_PATH "/dev/ets_airspeed"
|
||||
|
||||
/* Register address */
|
||||
#define READ_CMD 0x07 /* Read the data */
|
||||
|
@ -93,7 +94,7 @@
|
|||
class ETSAirspeed : public Airspeed
|
||||
{
|
||||
public:
|
||||
ETSAirspeed(int bus, int address = I2C_ADDRESS);
|
||||
ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH);
|
||||
|
||||
protected:
|
||||
|
||||
|
@ -112,8 +113,8 @@ protected:
|
|||
*/
|
||||
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
|
||||
|
||||
ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address,
|
||||
CONVERSION_INTERVAL)
|
||||
ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
|
||||
CONVERSION_INTERVAL, path)
|
||||
{
|
||||
|
||||
}
|
||||
|
@ -184,8 +185,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);
|
||||
|
||||
|
|
|
@ -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++;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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_
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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_
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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++;
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
@ -50,6 +50,7 @@
|
|||
* - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
|
||||
*/
|
||||
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
@ -89,8 +90,10 @@
|
|||
|
||||
/* I2C bus address is 1010001x */
|
||||
#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */
|
||||
#define PATH_MS4525 "/dev/ms4525"
|
||||
/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
|
||||
#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */
|
||||
#define PATH_MS5525 "/dev/ms5525"
|
||||
|
||||
/* Register address */
|
||||
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */
|
||||
|
@ -101,7 +104,7 @@
|
|||
class MEASAirspeed : public Airspeed
|
||||
{
|
||||
public:
|
||||
MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO);
|
||||
MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525);
|
||||
|
||||
protected:
|
||||
|
||||
|
@ -120,8 +123,8 @@ protected:
|
|||
*/
|
||||
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
|
||||
|
||||
MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
|
||||
CONVERSION_INTERVAL)
|
||||
MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
|
||||
CONVERSION_INTERVAL, path)
|
||||
{
|
||||
|
||||
}
|
||||
|
@ -208,8 +211,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);
|
||||
|
||||
|
@ -302,7 +307,7 @@ start(int i2c_bus)
|
|||
errx(1, "already started");
|
||||
|
||||
/* create the driver, try the MS4525DO first */
|
||||
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO);
|
||||
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
|
||||
|
||||
/* check if the MS4525DO was instantiated */
|
||||
if (g_dev == nullptr)
|
||||
|
@ -311,7 +316,7 @@ start(int i2c_bus)
|
|||
/* try the MS5525DSO next if init fails */
|
||||
if (OK != g_dev->Airspeed::init()) {
|
||||
delete g_dev;
|
||||
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO);
|
||||
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
|
||||
|
||||
/* check if the MS5525DSO was instantiated */
|
||||
if (g_dev == nullptr)
|
||||
|
@ -384,7 +389,7 @@ test()
|
|||
err(1, "immediate read failed");
|
||||
|
||||
warnx("single read");
|
||||
warnx("diff pressure: %d pa", (double)report.differential_pressure_pa);
|
||||
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
|
||||
|
@ -409,7 +414,7 @@ test()
|
|||
err(1, "periodic read failed");
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("diff pressure: %d pa", report.differential_pressure_pa);
|
||||
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
|
||||
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
|
||||
}
|
||||
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
@ -765,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
|
|||
Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
|
||||
foundMotorCount++;
|
||||
|
||||
if (Motor[i].MaxPWM == 250) {
|
||||
if ((Motor[i].MaxPWM & 252) == 248) {
|
||||
Motor[i].Version = BLCTRL_NEW;
|
||||
|
||||
} else {
|
||||
|
@ -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);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
@ -1331,6 +1353,7 @@ MPU6000::print_info()
|
|||
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
|
||||
CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
|
||||
_parent(parent),
|
||||
_gyro_topic(-1),
|
||||
_gyro_class_instance(-1)
|
||||
{
|
||||
}
|
||||
|
@ -1356,11 +1379,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;
|
||||
|
|
|
@ -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))
|
||||
|
|
|
@ -45,28 +45,46 @@
|
|||
#include <systemlib/param/param.h>
|
||||
|
||||
/*
|
||||
* Launch detection parameters, accessible via MAVLink
|
||||
* Catapult launch detection parameters, accessible via MAVLink
|
||||
*
|
||||
*/
|
||||
|
||||
/* Catapult Launch detection */
|
||||
|
||||
// @DisplayName Switch to enable launchdetection
|
||||
// @Description if set to 1 launchdetection is enabled
|
||||
// @Range 0 or 1
|
||||
/**
|
||||
* Enable launch detection.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Launch detection
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
|
||||
|
||||
// @DisplayName Catapult Accelerometer Threshold
|
||||
// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
|
||||
// @Range > 0
|
||||
/**
|
||||
* Catapult accelerometer theshold.
|
||||
*
|
||||
* LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
|
||||
*
|
||||
* @min 0
|
||||
* @group Launch detection
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
|
||||
|
||||
// @DisplayName Catapult Time Threshold
|
||||
// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
|
||||
// @Range > 0, in seconds
|
||||
/**
|
||||
* Catapult time theshold.
|
||||
*
|
||||
* LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
|
||||
*
|
||||
* @min 0
|
||||
* @group Launch detection
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
|
||||
|
||||
// @DisplayName Throttle setting while detecting the launch
|
||||
// @Description The throttle is set to this value while the system is waiting for the takeoff
|
||||
// @Range 0 to 1
|
||||
/**
|
||||
* Throttle setting while detecting launch.
|
||||
*
|
||||
* The throttle is set to this value while the system is waiting for the take-off.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Launch detection
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);
|
||||
|
|
|
@ -152,6 +152,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;
|
||||
|
@ -429,7 +430,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
arming_res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
|
||||
if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
|
||||
if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
arming_res = TRANSITION_DENIED;
|
||||
|
||||
|
@ -515,7 +516,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
|
||||
if (safety->safety_switch_available && !safety->safety_off) {
|
||||
if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
arming_res = TRANSITION_DENIED;
|
||||
|
||||
|
@ -563,7 +564,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;
|
||||
|
@ -615,6 +618,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");
|
||||
|
@ -860,10 +864,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, ¶chute_enabled);
|
||||
}
|
||||
|
||||
orb_check(sp_man_sub, &updated);
|
||||
|
@ -1152,7 +1156,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
if (status.arming_state == ARMING_STATE_STANDBY &&
|
||||
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
if (safety.safety_switch_available && !safety.safety_off) {
|
||||
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
|
||||
} else if (status.main_state != MAIN_STATE_MANUAL) {
|
||||
|
@ -1251,7 +1255,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();
|
||||
}
|
||||
|
|
|
@ -48,7 +48,39 @@
|
|||
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
|
||||
|
||||
/**
|
||||
* Empty cell voltage.
|
||||
*
|
||||
* Defines the voltage where a single cell of the battery is considered empty.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
|
||||
|
||||
/**
|
||||
* Full cell voltage.
|
||||
*
|
||||
* Defines the voltage where a single cell of the battery is considered full.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
|
||||
|
||||
/**
|
||||
* Number of cells.
|
||||
*
|
||||
* Defines the number of cells the attached battery consists of.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
|
||||
|
||||
/**
|
||||
* Battery capacity.
|
||||
*
|
||||
* Defines the capacity of the attached battery.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
|
||||
|
|
|
@ -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"
|
||||
|
@ -332,6 +335,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;
|
||||
|
|
|
@ -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> ¤t_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> ¤t_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> ¤t_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> ¤t_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> ¤t_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)
|
||||
|
|
|
@ -40,12 +40,10 @@
|
|||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/*
|
||||
* Controller parameters, accessible via MAVLink
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
|
@ -119,58 +117,268 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
|
||||
|
||||
|
||||
/**
|
||||
* Controller roll limit
|
||||
*
|
||||
* The maximum roll the controller will output.
|
||||
*
|
||||
* @unit degrees
|
||||
* @min 0.0
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
|
||||
|
||||
|
||||
/**
|
||||
* Throttle limit max
|
||||
*
|
||||
* This is the maximum throttle % that can be used by the controller.
|
||||
* For overpowered aircraft, this should be reduced to a value that
|
||||
* provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Throttle limit min
|
||||
*
|
||||
* This is the minimum throttle % that can be used by the controller.
|
||||
* For electric aircraft this will normally be set to zero, but can be set
|
||||
* to a small non-zero value if a folding prop is fitted to prevent the
|
||||
* prop from folding and unfolding repeatedly in-flight or to provide
|
||||
* some aerodynamic drag from a turning prop to improve the descent rate.
|
||||
*
|
||||
* For aircraft with internal combustion engine this parameter should be set
|
||||
* for desired idle rpm.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
|
||||
|
||||
/**
|
||||
* Throttle limit value before flare
|
||||
*
|
||||
* This throttle value will be set as throttle limit at FW_LND_TLALT,
|
||||
* before arcraft will flare.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Maximum climb rate
|
||||
*
|
||||
* This is the best climb rate that the aircraft can achieve with
|
||||
* the throttle set to THR_MAX and the airspeed set to the
|
||||
* default value. For electric aircraft make sure this number can be
|
||||
* achieved towards the end of flight when the battery voltage has reduced.
|
||||
* The setting of this parameter can be checked by commanding a positive
|
||||
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
|
||||
* required to climb is close to THR_MAX and the aircraft is maintaining
|
||||
* airspeed, then this parameter is set correctly. If the airspeed starts
|
||||
* to reduce, then the parameter is set to high, and if the throttle
|
||||
* demand required to climb and maintain speed is noticeably less than
|
||||
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
|
||||
* FW_THR_MAX reduced.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
|
||||
|
||||
|
||||
/**
|
||||
* Minimum descent rate
|
||||
*
|
||||
* This is the sink rate of the aircraft with the throttle
|
||||
* set to THR_MIN and flown at the same airspeed as used
|
||||
* to measure FW_T_CLMB_MAX.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
|
||||
|
||||
|
||||
/**
|
||||
* Maximum descent rate
|
||||
*
|
||||
* This sets the maximum descent rate that the controller will use.
|
||||
* If this value is too large, the aircraft can over-speed on descent.
|
||||
* This should be set to a value that can be achieved without
|
||||
* exceeding the lower pitch angle limit and without over-speeding
|
||||
* the aircraft.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
|
||||
|
||||
/**
|
||||
* TECS time constant
|
||||
*
|
||||
* This is the time constant of the TECS control algorithm (in seconds).
|
||||
* Smaller values make it faster to respond, larger values make it slower
|
||||
* to respond.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
|
||||
|
||||
/**
|
||||
* Throttle damping factor
|
||||
*
|
||||
* This is the damping gain for the throttle demand loop.
|
||||
* Increase to add damping to correct for oscillations in speed and height.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
|
||||
|
||||
/**
|
||||
* Integrator gain
|
||||
*
|
||||
* This is the integrator gain on the control loop.
|
||||
* Increasing this gain increases the speed at which speed
|
||||
* and height offsets are trimmed out, but reduces damping and
|
||||
* increases overshoot.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
|
||||
|
||||
/**
|
||||
* Maximum vertical acceleration
|
||||
*
|
||||
* This is the maximum vertical acceleration (in metres/second^2)
|
||||
* either up or down that the controller will use to correct speed
|
||||
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
|
||||
* allows for reasonably aggressive pitch changes if required to recover
|
||||
* from under-speed conditions.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
|
||||
|
||||
/**
|
||||
* Complementary filter "omega" parameter for height
|
||||
*
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse vertical acceleration and barometric height to obtain
|
||||
* an estimate of height rate and height. Increasing this frequency weights
|
||||
* the solution more towards use of the barometer, whilst reducing it weights
|
||||
* the solution more towards use of the accelerometer data.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
|
||||
|
||||
/**
|
||||
* Complementary filter "omega" parameter for speed
|
||||
*
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse longitudinal acceleration and airspeed to obtain an
|
||||
* improved airspeed estimate. Increasing this frequency weights the solution
|
||||
* more towards use of the arispeed sensor, whilst reducing it weights the
|
||||
* solution more towards use of the accelerometer data.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
|
||||
|
||||
/**
|
||||
* Roll -> Throttle feedforward
|
||||
*
|
||||
* Increasing this gain turn increases the amount of throttle that will
|
||||
* be used to compensate for the additional drag created by turning.
|
||||
* Ideally this should be set to approximately 10 x the extra sink rate
|
||||
* in m/s created by a 45 degree bank turn. Increase this gain if
|
||||
* the aircraft initially loses energy in turns and reduce if the
|
||||
* aircraft initially gains energy in turns. Efficient high aspect-ratio
|
||||
* aircraft (eg powered sailplanes) can use a lower value, whereas
|
||||
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
|
||||
|
||||
/**
|
||||
* Speed <--> Altitude priority
|
||||
*
|
||||
* This parameter adjusts the amount of weighting that the pitch control
|
||||
* applies to speed vs height errors. Setting it to 0.0 will cause the
|
||||
* pitch control to control height and ignore speed errors. This will
|
||||
* normally improve height accuracy but give larger airspeed errors.
|
||||
* Setting it to 2.0 will cause the pitch control loop to control speed
|
||||
* and ignore height errors. This will normally reduce airspeed errors,
|
||||
* but give larger height errors. The default value of 1.0 allows the pitch
|
||||
* control to simultaneously control height and speed.
|
||||
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
|
||||
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
|
||||
|
||||
/**
|
||||
* Pitch damping factor
|
||||
*
|
||||
* This is the damping gain for the pitch demand loop. Increase to add
|
||||
* damping to correct for oscillations in height. The default value of 0.0
|
||||
* will work well provided the pitch to servo controller has been tuned
|
||||
* properly.
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
|
||||
|
||||
/**
|
||||
* Height rate P factor
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
|
||||
|
||||
/**
|
||||
* Speed rate P factor
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
|
||||
/**
|
||||
* Landing slope angle
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
|
||||
|
||||
/**
|
||||
* Landing slope length
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
|
||||
|
||||
/**
|
||||
*
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
|
||||
|
||||
/**
|
||||
* Landing flare altitude (relative)
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
|
||||
|
||||
/**
|
||||
* Landing throttle limit altitude (relative)
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
|
||||
|
||||
/**
|
||||
* Landing heading hold horizontal distance
|
||||
*
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;}
|
||||
|
|
|
@ -76,8 +76,20 @@
|
|||
#include <uORB/topics/mission_result.h>
|
||||
|
||||
/* define MAVLink specific parameters */
|
||||
/**
|
||||
* MAVLink system ID
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
|
||||
/**
|
||||
* MAVLink component ID
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
|
||||
/**
|
||||
* MAVLink type
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
|
||||
|
||||
__EXPORT int mavlink_main(int argc, char *argv[]);
|
||||
|
|
|
@ -351,7 +351,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 {
|
||||
|
|
|
@ -53,11 +53,9 @@
|
|||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <time.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
@ -71,7 +69,6 @@
|
|||
#include <uORB/topics/parameter_update.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
@ -84,9 +81,9 @@
|
|||
*/
|
||||
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
|
||||
|
||||
#define MIN_TAKEOFF_THROTTLE 0.3f
|
||||
#define YAW_DEADZONE 0.05f
|
||||
#define RATES_I_LIMIT 0.5f
|
||||
#define MIN_TAKEOFF_THRUST 0.2f
|
||||
#define RATES_I_LIMIT 0.3f
|
||||
|
||||
class MulticopterAttitudeControl
|
||||
{
|
||||
|
@ -135,15 +132,13 @@ private:
|
|||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */
|
||||
math::Matrix<3, 3> _R; /**< rotation matrix for current state */
|
||||
math::Vector<3> _rates_prev; /**< angular rates on previous step */
|
||||
math::Vector<3> _rates_sp; /**< angular rates setpoint */
|
||||
math::Vector<3> _rates_int; /**< angular rates integral error */
|
||||
float _thrust_sp; /**< thrust setpoint */
|
||||
math::Vector<3> _att_control; /**< attitude control vector */
|
||||
|
||||
math::Matrix<3, 3> I; /**< identity matrix */
|
||||
math::Matrix<3, 3> _I; /**< identity matrix */
|
||||
|
||||
bool _reset_yaw_sp; /**< reset yaw setpoint flag */
|
||||
|
||||
|
@ -262,7 +257,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
_actuators_0_pub(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control"))
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
|
||||
|
||||
{
|
||||
memset(&_v_att, 0, sizeof(_v_att));
|
||||
|
@ -276,15 +271,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
_params.rate_i.zero();
|
||||
_params.rate_d.zero();
|
||||
|
||||
_R_sp.identity();
|
||||
_R.identity();
|
||||
_rates_prev.zero();
|
||||
_rates_sp.zero();
|
||||
_rates_int.zero();
|
||||
_thrust_sp = 0.0f;
|
||||
_att_control.zero();
|
||||
|
||||
I.identity();
|
||||
_I.identity();
|
||||
|
||||
_params_handles.roll_p = param_find("MC_ROLL_P");
|
||||
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
|
||||
|
@ -535,16 +528,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
|
|||
_thrust_sp = _v_att_sp.thrust;
|
||||
|
||||
/* construct attitude setpoint rotation matrix */
|
||||
math::Matrix<3, 3> R_sp;
|
||||
|
||||
if (_v_att_sp.R_valid) {
|
||||
/* rotation matrix in _att_sp is valid, use it */
|
||||
_R_sp.set(&_v_att_sp.R_body[0][0]);
|
||||
R_sp.set(&_v_att_sp.R_body[0][0]);
|
||||
|
||||
} else {
|
||||
/* rotation matrix in _att_sp is not valid, use euler angles instead */
|
||||
_R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
|
||||
R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
|
||||
|
||||
/* copy rotation matrix back to setpoint struct */
|
||||
memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body));
|
||||
memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
|
||||
_v_att_sp.R_valid = true;
|
||||
}
|
||||
|
||||
|
@ -561,23 +556,24 @@ MulticopterAttitudeControl::control_attitude(float dt)
|
|||
}
|
||||
|
||||
/* rotation matrix for current state */
|
||||
_R.set(_v_att.R);
|
||||
math::Matrix<3, 3> R;
|
||||
R.set(_v_att.R);
|
||||
|
||||
/* all input data is ready, run controller itself */
|
||||
|
||||
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
|
||||
math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2));
|
||||
math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2));
|
||||
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
|
||||
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
|
||||
|
||||
/* axis and sin(angle) of desired rotation */
|
||||
math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z);
|
||||
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
|
||||
|
||||
/* calculate angle error */
|
||||
float e_R_z_sin = e_R.length();
|
||||
float e_R_z_cos = R_z * R_sp_z;
|
||||
|
||||
/* calculate weight for yaw control */
|
||||
float yaw_w = _R_sp(2, 2) * _R_sp(2, 2);
|
||||
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
|
||||
|
||||
/* calculate rotation matrix after roll/pitch only rotation */
|
||||
math::Matrix<3, 3> R_rp;
|
||||
|
@ -600,15 +596,15 @@ MulticopterAttitudeControl::control_attitude(float dt)
|
|||
e_R_cp(2, 1) = e_R_z_axis(0);
|
||||
|
||||
/* rotation matrix for roll/pitch only rotation */
|
||||
R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
|
||||
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
|
||||
|
||||
} else {
|
||||
/* zero roll/pitch rotation */
|
||||
R_rp = _R;
|
||||
R_rp = R;
|
||||
}
|
||||
|
||||
/* R_rp and _R_sp has the same Z axis, calculate yaw error */
|
||||
math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0));
|
||||
/* R_rp and R_sp has the same Z axis, calculate yaw error */
|
||||
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
|
||||
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
|
||||
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
|
||||
|
||||
|
@ -616,7 +612,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
|
|||
/* for large thrust vector rotations use another rotation method:
|
||||
* calculate angle and axis for R -> R_sp rotation directly */
|
||||
math::Quaternion q;
|
||||
q.from_dcm(_R.transposed() * _R_sp);
|
||||
q.from_dcm(R.transposed() * R_sp);
|
||||
math::Vector<3> e_R_d = q.imag();
|
||||
e_R_d.normalize();
|
||||
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
|
||||
|
@ -658,7 +654,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
|
|||
_rates_prev = rates;
|
||||
|
||||
/* update integral only if not saturated on low limit */
|
||||
if (_thrust_sp > 0.1f) {
|
||||
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (fabsf(_att_control(i)) < _thrust_sp) {
|
||||
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
|
||||
|
@ -695,9 +691,6 @@ MulticopterAttitudeControl::task_main()
|
|||
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
|
||||
/* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */
|
||||
orb_set_interval(_v_att_sub, 5);
|
||||
|
||||
/* initialize parameters cache */
|
||||
parameters_update();
|
||||
|
||||
|
@ -767,10 +760,12 @@ MulticopterAttitudeControl::task_main()
|
|||
}
|
||||
|
||||
} else {
|
||||
/* attitude controller disabled */
|
||||
// TODO poll 'attitude_rates_setpoint' topic
|
||||
_rates_sp.zero();
|
||||
_thrust_sp = 0.0f;
|
||||
/* attitude controller disabled, poll rates setpoint topic */
|
||||
vehicle_rates_setpoint_poll();
|
||||
_rates_sp(0) = _v_rates_sp.roll;
|
||||
_rates_sp(1) = _v_rates_sp.pitch;
|
||||
_rates_sp(2) = _v_rates_sp.yaw;
|
||||
_thrust_sp = _v_rates_sp.thrust;
|
||||
}
|
||||
|
||||
if (_v_control_mode.flag_control_rates_enabled) {
|
||||
|
|
|
@ -41,16 +41,135 @@
|
|||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* Roll P gain
|
||||
*
|
||||
* Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
|
||||
|
||||
/**
|
||||
* Roll rate P gain
|
||||
*
|
||||
* Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
|
||||
|
||||
/**
|
||||
* Roll rate I gain
|
||||
*
|
||||
* Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
|
||||
|
||||
/**
|
||||
* Roll rate D gain
|
||||
*
|
||||
* Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
|
||||
|
||||
/**
|
||||
* Pitch P gain
|
||||
*
|
||||
* Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
|
||||
*
|
||||
* @unit 1/s
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
|
||||
|
||||
/**
|
||||
* Pitch rate P gain
|
||||
*
|
||||
* Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
|
||||
|
||||
/**
|
||||
* Pitch rate I gain
|
||||
*
|
||||
* Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
|
||||
|
||||
/**
|
||||
* Pitch rate D gain
|
||||
*
|
||||
* Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
|
||||
|
||||
/**
|
||||
* Yaw P gain
|
||||
*
|
||||
* Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
|
||||
*
|
||||
* @unit 1/s
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate P gain
|
||||
*
|
||||
* Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
|
||||
|
||||
/**
|
||||
* Yaw rate I gain
|
||||
*
|
||||
* Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate D gain
|
||||
*
|
||||
* Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Yaw feed forward
|
||||
*
|
||||
* Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
|
||||
|
|
|
@ -51,7 +51,6 @@
|
|||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <time.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
@ -68,7 +67,6 @@
|
|||
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
|
|
@ -39,20 +39,164 @@
|
|||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f);
|
||||
/**
|
||||
* Minimum thrust
|
||||
*
|
||||
* Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
|
||||
|
||||
/**
|
||||
* Maximum thrust
|
||||
*
|
||||
* Limit max allowed thrust.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Proportional gain for vertical position error
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
|
||||
|
||||
/**
|
||||
* Proportional gain for vertical velocity error
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
|
||||
|
||||
/**
|
||||
* Integral gain for vertical velocity error
|
||||
*
|
||||
* Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f);
|
||||
|
||||
/**
|
||||
* Differential gain for vertical velocity error
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Maximum vertical velocity
|
||||
*
|
||||
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
|
||||
|
||||
/**
|
||||
* Vertical velocity feed forward
|
||||
*
|
||||
* Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
|
||||
|
||||
/**
|
||||
* Proportional gain for horizontal position error
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f);
|
||||
|
||||
/**
|
||||
* Proportional gain for horizontal velocity error
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
|
||||
|
||||
/**
|
||||
* Integral gain for horizontal velocity error
|
||||
*
|
||||
* Non-zero value allows to resist wind.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
|
||||
|
||||
/**
|
||||
* Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
|
||||
|
||||
/**
|
||||
* Maximum horizontal velocity
|
||||
*
|
||||
* Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
|
||||
|
||||
/**
|
||||
* Horizontal velocity feed forward
|
||||
*
|
||||
* Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
|
||||
|
||||
/**
|
||||
* Maximum tilt
|
||||
*
|
||||
* Limits maximum tilt in AUTO and EASY modes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.57
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Landing descend rate
|
||||
*
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
|
||||
|
||||
/**
|
||||
* Maximum landing tilt
|
||||
*
|
||||
* Limits maximum tilt on landing.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.57
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f);
|
||||
|
|
|
@ -45,11 +45,17 @@
|
|||
#include <systemlib/param/param.h>
|
||||
|
||||
/*
|
||||
* geofence parameters, accessible via MAVLink
|
||||
*
|
||||
* Geofence parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
// @DisplayName Switch to enable geofence
|
||||
// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present
|
||||
// @Range 0 or 1
|
||||
/**
|
||||
* Enable geofence.
|
||||
*
|
||||
* Set to 1 to enable geofence.
|
||||
* Defaults to 1 because geofence is only enabled when the geofence.txt file is present.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Geofence
|
||||
*/
|
||||
PARAM_DEFINE_INT32(GF_ON, 1);
|
||||
|
|
|
@ -305,6 +305,12 @@ private:
|
|||
void start_land();
|
||||
void start_land_home();
|
||||
|
||||
/**
|
||||
* Fork for state transitions
|
||||
*/
|
||||
void request_loiter_or_ready();
|
||||
void request_mission_if_available();
|
||||
|
||||
/**
|
||||
* Guards offboard mission
|
||||
*/
|
||||
|
@ -699,24 +705,17 @@ Navigator::task_main()
|
|||
} else {
|
||||
/* MISSION switch */
|
||||
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
request_loiter_or_ready();
|
||||
stick_mode = true;
|
||||
|
||||
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
|
||||
/* switch to mission only if available */
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
|
||||
request_mission_if_available();
|
||||
stick_mode = true;
|
||||
}
|
||||
|
||||
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
|
||||
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
request_mission_if_available();
|
||||
stick_mode = true;
|
||||
}
|
||||
}
|
||||
|
@ -733,17 +732,11 @@ Navigator::task_main()
|
|||
break;
|
||||
|
||||
case NAV_STATE_LOITER:
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
request_loiter_or_ready();
|
||||
break;
|
||||
|
||||
case NAV_STATE_MISSION:
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
|
||||
request_mission_if_available();
|
||||
break;
|
||||
|
||||
case NAV_STATE_RTL:
|
||||
|
@ -768,12 +761,7 @@ Navigator::task_main()
|
|||
} else {
|
||||
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
|
||||
if (myState == NAV_STATE_NONE) {
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
request_mission_if_available();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -1069,7 +1057,7 @@ Navigator::start_loiter()
|
|||
float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
|
||||
|
||||
/* use current altitude if above min altitude set by parameter */
|
||||
if (_global_pos.alt < min_alt_amsl) {
|
||||
if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
|
||||
_pos_sp_triplet.current.alt = min_alt_amsl;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
|
||||
|
||||
|
@ -1078,9 +1066,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;
|
||||
|
@ -1396,6 +1383,28 @@ Navigator::set_rtl_item()
|
|||
_pos_sp_triplet_updated = true;
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::request_loiter_or_ready()
|
||||
{
|
||||
if (_vstatus.condition_landed) {
|
||||
dispatch(EVENT_READY_REQUESTED);
|
||||
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::request_mission_if_available()
|
||||
{
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
|
||||
} else {
|
||||
request_loiter_or_ready();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
|
||||
{
|
||||
|
@ -1554,13 +1563,7 @@ Navigator::on_mission_item_reached()
|
|||
/* loiter at last waypoint */
|
||||
_reset_loiter_pos = false;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
|
||||
|
||||
if (_vstatus.condition_landed) {
|
||||
dispatch(EVENT_READY_REQUESTED);
|
||||
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
request_loiter_or_ready();
|
||||
}
|
||||
|
||||
} else if (myState == NAV_STATE_RTL) {
|
||||
|
|
|
@ -50,14 +50,91 @@
|
|||
|
||||
/*
|
||||
* Navigator parameters, accessible via MAVLink
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* Minimum altitude (fixed wing only)
|
||||
*
|
||||
* Minimum altitude above home for LOITER.
|
||||
*
|
||||
* @unit meters
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
|
||||
|
||||
/**
|
||||
* Waypoint acceptance radius
|
||||
*
|
||||
* Default value of acceptance radius (if not specified in mission item).
|
||||
*
|
||||
* @unit meters
|
||||
* @min 0.0
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);
|
||||
|
||||
/**
|
||||
* Loiter radius (fixed wing only)
|
||||
*
|
||||
* Default value of loiter radius (if not specified in mission item).
|
||||
*
|
||||
* @unit meters
|
||||
* @min 0.0
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
|
||||
|
||||
/**
|
||||
* Enable onboard mission
|
||||
*
|
||||
* @group Navigation
|
||||
*/
|
||||
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
|
||||
|
||||
/**
|
||||
* Take-off altitude
|
||||
*
|
||||
* Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
|
||||
*
|
||||
* @unit meters
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
|
||||
|
||||
/**
|
||||
* Landing altitude
|
||||
*
|
||||
* Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
|
||||
*
|
||||
* @unit meters
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
|
||||
|
||||
/**
|
||||
* Return-To-Launch altitude
|
||||
*
|
||||
* Minimum altitude above home position for going home in RTL mode.
|
||||
*
|
||||
* @unit meters
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
|
||||
|
||||
/**
|
||||
* Return-To-Launch delay
|
||||
*
|
||||
* Delay after descend before landing in RTL mode.
|
||||
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
|
||||
*
|
||||
* @unit seconds
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
|
||||
|
||||
/**
|
||||
* Enable parachute deployment
|
||||
*
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);
|
||||
|
|
|
@ -42,14 +42,11 @@
|
|||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <float.h>
|
||||
#include <string.h>
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <limits.h>
|
||||
#include <math.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
@ -527,13 +524,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 +586,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 {
|
||||
|
|
|
@ -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);
|
||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue