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

Merged beta into paul_estimator

Browse Source
This commit is contained in:
Lorenz Meier
2014-02-16 17:07:02 +01:00
parent 30612eb32d 5bb004a711
commit 8016032a40
206 changed files with 12072 additions and 9311 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Documentation/fw_landing.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 17 KiB

ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+3 -34
View File
@@ -2,44 +2,13 @@
#
# HILStar / X-Plane
#
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/rc.fw_defaults
echo "HIL Rascal 110 starting.."
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
set HIL yes
set VEHICLE_TYPE fw
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+14 -16
View File
@@ -2,30 +2,28 @@
#
# Team Blacksheep Discovery Quadcopter
#
# Maintainers: 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_ATT_P 5.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.17
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.006
param set MC_YAWPOS_P 0.5
param set MC_YAWPOS_I 0.15
param set MC_YAWPOS_D 0.0
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
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_RATE 400
ROMFS/px4fmu_common/init.d/10016_3dr_iris
+13 -34
View File
@@ -2,52 +2,31 @@
#
# 3DR Iris Quadcopter
#
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 9.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.13
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 0.5
param set MC_YAWPOS_I 0.15
param set MC_YAWPOS_D 0.0
# 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
param set MC_ROLLRATE_D 0.004
param set MC_PITCH_P 9.0
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 0.5
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.8
param set MPC_THR_MIN 0.2
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param set BAT_V_SCALING 0.00989
param set BAT_C_SCALING 0.0124
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_RATE 400
set PWM_DISARMED 900
set PWM_MIN 1000
set PWM_MAX 2000
ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
+4 -38
View File
@@ -2,45 +2,11 @@
#
# HIL Quadcopter X
#
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
# Anton Babushkin <anton.babushkin@me.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.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_TILT_MAX 0.5
param set MPC_THR_MAX 0.8
param set MPC_THR_MIN 0.2
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
fi
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_x
set HIL yes
set VEHICLE_TYPE mc
set MIXER FMU_quad_x
ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
-45
View File
@@ -1,45 +0,0 @@
#!nsh
#
# HIL Rascal 110 (Flightgear)
#
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
#
echo "HIL Rascal 110 starting.."
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
set HIL yes
set VEHICLE_TYPE fw
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
+4 -38
View File
@@ -2,45 +2,11 @@
#
# HIL Quadcopter +
#
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
# Anton Babushkin <anton.babushkin@me.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.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_TILT_MAX 0.5
param set MPC_THR_MAX 0.8
param set MPC_THR_MIN 0.2
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
fi
sh /etc/init.d/rc.mc_defaults
set HIL yes
set VEHICLE_TYPE mc
set MIXER FMU_quad_+
set HIL yes
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
+3 -34
View File
@@ -2,44 +2,13 @@
#
# HIL Rascal 110 (Flightgear)
#
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
# Thomas Gubler <thomasgubler@gmail.com>
#
sh /etc/init.d/rc.fw_defaults
echo "HIL Rascal 110 starting.."
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
set HIL yes
set VEHICLE_TYPE fw
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
+47
View File
@@ -0,0 +1,47 @@
#!nsh
#
# HIL Malolo 1 (Flightgear)
#
# Thomas Gubler <thomasgubler@gmail.com>
#
sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
then
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
param set FW_PR_FF 0.8
param set FW_PR_I 0.05
param set FW_PR_IMAX 0.2
param set FW_PR_P 0.1
param set FW_P_ROLLFF 0
param set FW_RR_FF 0.6
param set FW_RR_I 0.02
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.1
param set FW_R_LIM 45
param set FW_R_RMAX 0
param set FW_T_CLMB_MAX 5
param set FW_T_HRATE_P 0.02
param set FW_T_PTCH_DAMP 0
param set FW_T_RLL2THR 15
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 3
param set FW_T_VERT_ACC 7
param set FW_YR_FF 0.0
param set FW_YR_I 0
param set FW_YR_IMAX 0.2
param set FW_YR_P 0.0
fi
set HIL yes
# Set the AERT mixer for HIL (even if the malolo is a flying wing)
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/11001_hexa_cox
+2 -26
View File
@@ -7,33 +7,9 @@
# Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
sh /etc/init.d/rc.mc_defaults
set VEHICLE_TYPE mc
set MIXER hexa_cox
# We only can run one channel group with one rate, so set all 8 channels
set PWM_OUTPUTS 12345678
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/12001_octo_cox
+2 -27
View File
@@ -5,33 +5,8 @@
# Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
sh /etc/init.d/rc.mc_defaults
set VEHICLE_TYPE mc
set MIXER FMU_octo_cox
set MIXER octo_cox
set PWM_OUTPUTS 12345678
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/2100_mpx_easystar
+1 -32
View File
@@ -2,38 +2,7 @@
#
# MPX EasyStar Plane
#
# Maintainers: ???
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
sh /etc/init.d/rc.fw_defaults
set VEHICLE_TYPE fw
set MIXER FMU_RET
ROMFS/px4fmu_common/init.d/2101_hk_bixler
+1 -36
View File
@@ -1,40 +1,5 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
sh /etc/init.d/rc.fw_defaults
#
# Load default params for this platform
#
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
set VEHICLE_TYPE fw
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
+1 -36
View File
@@ -1,40 +1,5 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
sh /etc/init.d/rc.fw_defaults
#
# Load default params for this platform
#
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
set VEHICLE_TYPE fw
set MIXER FMU_AERT
ROMFS/px4fmu_common/init.d/3030_io_camflyer
+1 -36
View File
@@ -1,40 +1,5 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on Camflyer"
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_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
set VEHICLE_TYPE fw
set MIXER FMU_Q
ROMFS/px4fmu_common/init.d/3031_phantom
+1 -37
View File
@@ -2,43 +2,7 @@
#
# Phantom FPV Flying Wing
#
# Maintainers: Simon Wilks <sjwilks@gmail.com>
# Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 11.4
param set FW_AIRSPD_TRIM 14
param set FW_AIRSPD_MAX 22
param set FW_L1_PERIOD 15
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 45
param set FW_P_LIM_MIN -45
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 15
param set FW_R_P 80
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.8
param set FW_THR_LND_MAX 0
param set FW_THR_MAX 1
param set FW_THR_MIN 0.5
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 2.0
param set FW_Y_ROLLFF 1.0
param set RC_SCALE_ROLL 0.6
param set RC_SCALE_PITCH 0.6
param set TRIM_PITCH 0.1
fi
set VEHICLE_TYPE fw
set MIXER FMU_Q
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
+26 -30
View File
@@ -2,42 +2,38 @@
#
# Skywalker X5 Flying Wing
#
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
# Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
#
sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_AIRSPD_MIN 15
param set FW_AIRSPD_TRIM 20
param set FW_AIRSPD_MAX 40
param set FW_ATT_TC 0.3
param set FW_L1_DAMPING 0.74
param set FW_L1_PERIOD 15
param set FW_PR_FF 0.3
param set FW_PR_I 0
param set FW_PR_IMAX 0.2
param set FW_PR_P 0.03
param set FW_P_LIM_MAX 45
param set FW_P_LIM_MIN -45
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set FW_P_ROLLFF 0
param set FW_RR_FF 0.3
param set FW_RR_I 0
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.03
param set FW_R_LIM 60
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set VEHICLE_TYPE fw
set MIXER FMU_X5
ROMFS/px4fmu_common/init.d/3033_wingwing
+2 -35
View File
@@ -2,42 +2,9 @@
#
# Wing Wing (aka Z-84) Flying Wing
#
# Maintainers: Simon Wilks <sjwilks@gmail.com>
# Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
sh /etc/init.d/rc.fw_defaults
set VEHICLE_TYPE fw
set MIXER FMU_Q
ROMFS/px4fmu_common/init.d/3034_fx79
+2 -35
View File
@@ -2,42 +2,9 @@
#
# FX-79 Buffalo Flying Wing
#
# Maintainers: Simon Wilks <sjwilks@gmail.com>
# Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MAX 20
param set FW_AIRSPD_TRIM 12
param set FW_AIRSPD_MIN 15
param set FW_L1_PERIOD 12
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 80
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.75
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 1.1
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
sh /etc/init.d/rc.fw_defaults
set VEHICLE_TYPE fw
set MIXER FMU_FX79
ROMFS/px4fmu_common/init.d/4001_quad_x
+12
View File
@@ -0,0 +1,12 @@
#!nsh
#
# Generic 10” Quad X geometry
#
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
ROMFS/px4fmu_common/init.d/4008_ardrone
-57
View File
@@ -1,57 +0,0 @@
#!nsh
echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set MC_ATTRATE_D 0
param set MC_ATTRATE_I 0
param set MC_ATTRATE_P 0.13
param set MC_ATT_D 0
param set MC_ATT_I 0.3
param set MC_ATT_P 5
param set MC_YAWPOS_D 0.1
param set MC_YAWPOS_I 0.15
param set MC_YAWPOS_P 1
param set MC_YAWRATE_D 0
param set MC_YAWRATE_I 0
param set MC_YAWRATE_P 0.15
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
param set BAT_V_SCALING 0.008381
#
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Configure PX4FMU for operation with PX4IOAR
#
fmu mode_gpio_serial
#
# Fire up the AR.Drone interface.
#
ardrone_interface start -d /dev/ttyS1
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
# Exit, because /dev/ttyS0 is needed for MAVLink
exit
ROMFS/px4fmu_common/init.d/4009_ardrone_flow
-83
View File
@@ -1,83 +0,0 @@
#!nsh
echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set MC_ATTRATE_D 0
param set MC_ATTRATE_I 0
param set MC_ATTRATE_P 0.13
param set MC_ATT_D 0
param set MC_ATT_I 0.3
param set MC_ATT_P 5
param set MC_YAWPOS_D 0.1
param set MC_YAWPOS_I 0.15
param set MC_YAWPOS_P 1
param set MC_YAWRATE_D 0
param set MC_YAWRATE_I 0
param set MC_YAWRATE_P 0.15
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
param set BAT_V_SCALING 0.008381
#
# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor)
#
mavlink start -d /dev/ttyS0 -b 57600
mavlink_onboard start -d /dev/ttyS3 -b 115200
usleep 5000
#
# Configure PX4FMU for operation with PX4IOAR
#
fmu mode_gpio_serial
#
# Fire up the AR.Drone interface.
#
ardrone_interface start -d /dev/ttyS1
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors
#
# Start the attitude estimator
#
attitude_estimator_ekf start
#
# Start the position estimator
#
flow_position_estimator start
#
# Fire up the multi rotor attitude controller
#
multirotor_att_control start
#
# Fire up the flow position controller
#
flow_position_control start
#
# Fire up the flow speed controller
#
flow_speed_control start
# Exit, because /dev/ttyS0 is needed for MAVLink
exit
ROMFS/px4fmu_common/init.d/4010_dji_f330
+14 -38
View File
@@ -2,50 +2,26 @@
#
# DJI Flame Wheel F330 Quadcopter
#
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
# 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_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.8
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
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.05
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.8
param set MPC_THR_MIN 0.2
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MIN 1175
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/4011_dji_f450
+17 -26
View File
@@ -2,36 +2,27 @@
#
# DJI Flame Wheel F450 Quadcopter
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
# TODO REVIEW
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
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MIN 1175
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/4012_hk_x550
-33
View File
@@ -1,33 +0,0 @@
#!nsh
#
# HobbyKing X550 Quadcopter
#
# Maintainers: Todd Stellanova <tstellanova@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 5.5
param set MC_ATT_I 0
param set MC_ATT_D 0
param set MC_ATTRATE_P 0.14
param set MC_ATTRATE_I 0
param set MC_ATTRATE_D 0.006
param set MC_YAWPOS_P 0.6
param set MC_YAWPOS_I 0
param set MC_YAWPOS_D 0
param set MC_YAWRATE_P 0.08
param set MC_YAWRATE_I 0
param set MC_YAWRATE_D 0
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
ROMFS/px4fmu_common/init.d/5001_quad_+
+12
View File
@@ -0,0 +1,12 @@
#!nsh
#
# Generic 10” Quad + geometry
#
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_+
set PWM_OUTPUTS 1234
ROMFS/px4fmu_common/init.d/5001_quad_+_pwm
-37
View File
@@ -1,37 +0,0 @@
#!nsh
#
# Generic 10” Quad + geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_+
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/6001_hexa_x
+13
View File
@@ -0,0 +1,13 @@
#!nsh
#
# Generic 10” Hexa X geometry
#
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_x
# We only can run one channel group with one rate, so set all 8 channels
set PWM_OUTPUTS 12345678
ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm
-37
View File
@@ -1,37 +0,0 @@
#!nsh
#
# Generic 10” Hexa X geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_hexa_x
set PWM_OUTPUTS 12345678
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/7001_hexa_+
+13
View File
@@ -0,0 +1,13 @@
#!nsh
#
# Generic 10” Hexa + geometry
#
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_+
# We only can run one channel group with one rate, so set all 8 channels
set PWM_OUTPUTS 12345678
ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm
-37
View File
@@ -1,37 +0,0 @@
#!nsh
#
# Generic 10” Hexa + geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_hexa_+
set PWM_OUTPUTS 12345678
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/8001_octo_x
+12
View File
@@ -0,0 +1,12 @@
#!nsh
#
# Generic 10” Octo X geometry
#
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_x
set PWM_OUTPUTS 12345678
ROMFS/px4fmu_common/init.d/8001_octo_x_pwm
-37
View File
@@ -1,37 +0,0 @@
#!nsh
#
# Generic 10” Octo X geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_octo_x
set PWM_OUTPUTS 12345678
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/9001_octo_+
+12
View File
@@ -0,0 +1,12 @@
#!nsh
#
# Generic 10” Octo + geometry
#
# Anton Babushkin <anton.babushkin@me.com>
#
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_+
set PWM_OUTPUTS 12345678
ROMFS/px4fmu_common/init.d/9001_octo_+_pwm
-37
View File
@@ -1,37 +0,0 @@
#!nsh
#
# Generic 10” Octo + geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_octo_+
set PWM_OUTPUTS 12345678
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900
ROMFS/px4fmu_common/init.d/rc.autostart
+12 -22
View File
@@ -31,11 +31,6 @@ then
sh /etc/init.d/1001_rc_quad_x.hil
fi
if param compare SYS_AUTOSTART 1002
then
sh /etc/init.d/1002_rc_fw_state.hil
fi
if param compare SYS_AUTOSTART 1003
then
sh /etc/init.d/1003_rc_quad_+.hil
@@ -46,6 +41,11 @@ then
sh /etc/init.d/1004_rc_fw_Rascal110.hil
fi
if param compare SYS_AUTOSTART 1005
then
sh /etc/init.d/1005_rc_fw_Malolo1.hil
fi
#
# Standard plane
#
@@ -101,14 +101,9 @@ fi
# Quad X
#
if param compare SYS_AUTOSTART 4008 8
if param compare SYS_AUTOSTART 4001
then
#sh /etc/init.d/4008_ardrone
fi
if param compare SYS_AUTOSTART 4009 9
then
#sh /etc/init.d/4009_ardrone_flow
sh /etc/init.d/4001_quad_x
fi
if param compare SYS_AUTOSTART 4010 10
@@ -121,18 +116,13 @@ 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 +
#
if param compare SYS_AUTOSTART 5001
then
sh /etc/init.d/5001_quad_+_pwm
sh /etc/init.d/5001_quad_+
fi
#
@@ -141,7 +131,7 @@ fi
if param compare SYS_AUTOSTART 6001
then
sh /etc/init.d/6001_hexa_x_pwm
sh /etc/init.d/6001_hexa_x
fi
#
@@ -150,7 +140,7 @@ fi
if param compare SYS_AUTOSTART 7001
then
sh /etc/init.d/7001_hexa_+_pwm
sh /etc/init.d/7001_hexa_+
fi
#
@@ -159,7 +149,7 @@ fi
if param compare SYS_AUTOSTART 8001
then
sh /etc/init.d/8001_octo_x_pwm
sh /etc/init.d/8001_octo_x
fi
#
@@ -168,7 +158,7 @@ fi
if param compare SYS_AUTOSTART 9001
then
sh /etc/init.d/9001_octo_+_pwm
sh /etc/init.d/9001_octo_+
fi
#
ROMFS/px4fmu_common/init.d/rc.fw_apps
-4
View File
@@ -12,8 +12,4 @@ fw_att_pos_estimator start
# Start attitude controller
#
fw_att_control start
#
# Start the position controller
#
fw_pos_control_l1 start
ROMFS/px4fmu_common/init.d/rc.fw_defaults
+14
View File
@@ -0,0 +1,14 @@
#!nsh
set VEHICLE_TYPE fw
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for FW
#
param set NAV_LAND_ALT 90
param set NAV_RTL_ALT 100
param set NAV_RTL_LAND_T -1
param set NAV_ACCEPT_RAD 50
fi
ROMFS/px4fmu_common/init.d/rc.io
-1
View File
@@ -4,7 +4,6 @@
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
#
px4io recovery
ROMFS/px4fmu_common/init.d/rc.logging
+3 -1
View File
@@ -7,8 +7,10 @@ if [ -d /fs/microsd ]
then
if hw_ver compare PX4FMU_V1
then
sdlog2 start -r 50 -a -b 16 -t
echo "Start sdlog2 at 50Hz"
sdlog2 start -r 50 -a -b 8 -t
else
echo "Start sdlog2 at 200Hz"
sdlog2 start -r 200 -a -b 16 -t
fi
fi
ROMFS/px4fmu_common/init.d/rc.mc_apps
+4 -17
View File
@@ -1,24 +1,11 @@
#!nsh
#
# Standard apps for multirotors
# Standard apps for multirotors:
# att & pos estimator, att & pos control.
#
#
# Start the attitude estimator
#
attitude_estimator_ekf start
#
# Start position estimator
#
position_estimator_inav start
#
# Start attitude control
#
multirotor_att_control start
#
# Start position control
#
multirotor_pos_control start
mc_att_control start
mc_pos_control start
ROMFS/px4fmu_common/init.d/rc.mc_defaults
+43
View File
@@ -0,0 +1,43 @@
#!nsh
set VEHICLE_TYPE mc
if [ $DO_AUTOCONFIG == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.5
param set MPC_THR_MAX 1.0
param set MPC_THR_MIN 0.1
param set MPC_XY_P 1.0
param set MPC_XY_VEL_P 0.1
param set MPC_XY_VEL_I 0.02
param set MPC_XY_VEL_D 0.01
param set MPC_XY_VEL_MAX 5
param set MPC_XY_FF 0.5
param set MPC_Z_P 1.0
param set MPC_Z_VEL_P 0.1
param set MPC_Z_VEL_I 0.02
param set MPC_Z_VEL_D 0.0
param set MPC_Z_VEL_MAX 3
param set MPC_Z_FF 0.5
param set MPC_TILT_MAX 1.0
param set MPC_LAND_SPEED 1.0
param set MPC_LAND_TILT 0.3
fi
set PWM_RATE 400
set PWM_DISARMED 900
set PWM_MIN 1075
set PWM_MAX 2000
ROMFS/px4fmu_common/init.d/rc.sensors
-4
View File
@@ -3,10 +3,6 @@
# Standard startup script for PX4FMU onboard sensor drivers.
#
#
# Start sensor drivers here.
#
ms5611 start
adc start
ROMFS/px4fmu_common/init.d/rcS
+31 -14
View File
@@ -3,8 +3,7 @@
# PX4FMU startup script.
#
# Default to auto-start mode. An init script on the microSD card
# can change this to prevent automatic startup of the flight script.
# Default to auto-start mode.
#
set MODE autostart
@@ -326,7 +325,7 @@ then
if [ $OUTPUT_MODE == hil ]
then
echo "[init] Use HIL as primary output"
if hil mode_pwm
if hil mode_port2_pwm8
then
echo "[init] HIL output started"
else
@@ -404,6 +403,16 @@ then
fi
fi
#
# Start the datamanager
#
dataman start
#
# Start the navigator
#
navigator start
#
# Sensors, Logging, GPS
#
@@ -456,17 +465,21 @@ 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_hex_x -o $MIXER == FMU_hex_+ ]
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
@@ -483,8 +496,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
@@ -498,10 +517,8 @@ then
#
if [ $VEHICLE_TYPE == none ]
then
echo "[init] Vehicle type: GENERIC"
echo "[init] Vehicle type: No autostart ID found"
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
fi
# Start any custom addons
ROMFS/px4fmu_common/mixers/FMU_hex_+.mix → ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix
View File
ROMFS/px4fmu_common/mixers/FMU_hex_x.mix → ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix
View File
ROMFS/px4fmu_common/mixers/FMU_octo_+.mix
+1 -5
View File
@@ -1,7 +1,3 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a octocopter in the + configuration. All controls
are mixed 100%.
# Octo +
R: 8+ 10000 10000 10000 0
Tools/fsm_visualisation.py
Executable
+201
View File
@@ -0,0 +1,201 @@
#!/usr/bin/env python3
"""fsm_visualisation.py: Create dot code and dokuwiki table from a state transition table
convert dot code to png using graphviz:
dot fsm.dot -Tpng -o fsm.png
"""
import argparse
import re
__author__ = "Julian Oes"
def get_dot_header():
return """digraph finite_state_machine {
graph [ dpi = 300 ];
ratio = 1.5
node [shape = circle];"""
def get_dot_footer():
return """}\n"""
def main():
# parse input arguments
parser = argparse.ArgumentParser(description='Create dot code and dokuwiki table from a state transition table.')
parser.add_argument("-i", "--input-file", default=None, help="choose file to parse")
parser.add_argument("-d", "--dot-file", default=None, help="choose file for output dot file")
parser.add_argument("-t", "--table-file", default=None, help="choose file for output of table")
args = parser.parse_args()
# open source file
if args.input_file == None:
exit('please specify file')
f = open(args.input_file,'r')
source = f.read()
# search for state transition table and extract the table itself
# first look for StateTable::Tran
# then accept anything including newline until {
# but don't accept the definition (without ;)
# then extract anything inside the brackets until };
match = re.search(r'StateTable::Tran(?:.|\n!;)*\{((?:.|\n)*?)\};', source)
if not match:
exit('no state transition table found')
table_source = match.group(1)
# bookkeeping for error checking
num_errors_found = 0
states = []
events = []
# first get all states and events
for table_line in table_source.split('\n'):
match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line)
if match:
states.append(str(match.group(1)))
# go to next line
continue
if len(states) == 1:
match = re.search(r'/\*\s+EVENT_(\w+)\s+\*/', table_line)
if match:
events.append(str(match.group(1)))
print('Found %d states and %d events' % (len(states), len(events)))
# keep track of origin state
state = None
# fill dot code in here
dot_code = ''
# create table len(states)xlen(events)
transition_table = [[[] for x in range(len(states))] for y in range(len(events))]
# now fill the transition table and write the dot code
for table_line in table_source.split('\n'):
# get states
# from: /* NAV_STATE_NONE */
# extract only "NONE"
match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line)
if match:
state = match.group(1)
state_index = states.index(state)
# go to next line
continue
# can't advance without proper state
if state == None:
continue
# get event and next state
# from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}
# extract "READY_REQUESTED" and "READY" if there is ACTION
match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*\w+_STATE_(\w+)', table_line)
# get event and next state
# from /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
# extract "NONE_REQUESTED" and "NAV_STATE_NONE" if there is NO_ACTION
match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*\w+_STATE_(\w+)', table_line)
# ignore lines with brackets only
if match_action or match_no_action:
# only write arrows for actions
if match_action:
event = match_action.group(1)
new_state = match_action.group(2)
dot_code += ' ' + state + ' -> ' + new_state + '[ label = "' + event + '"];\n'
elif match_no_action:
event = match_no_action.group(1)
new_state = match_no_action.group(2)
# check for state changes without action
if state != new_state:
print('Error: no action but state change:')
print('State: ' + state + ' changed to: ' + new_state)
print(table_line)
num_errors_found += 1
# check for wrong events
if event not in events:
print('Error: unknown event: ' + event)
print(table_line)
num_errors_found += 1
# check for wrong new states
if new_state not in states:
print('Error: unknown new state: ' + new_state)
print(table_line)
num_errors_found += 1
# save new state in transition table
event_index = events.index(event)
# bold for action
if match_action:
transition_table[event_index][state_index] = '**' + new_state + '**'
else:
transition_table[event_index][state_index] = new_state
# assemble dot code
dot_code = get_dot_header() + dot_code + get_dot_footer()
# write or print dot file
if args.dot_file:
f = open(args.dot_file,'w')
f.write(dot_code)
print('Wrote dot file')
else:
print('##########Dot-start##########')
print(dot_code)
print('##########Dot-end############')
# assemble doku wiki table
table_code = '| ^ '
# start with header of all states
for state in states:
table_code += state + ' ^ '
table_code += '\n'
# add events and new states
for event, row in zip(events, transition_table):
table_code += '^ ' + event + ' | '
for new_state in row:
table_code += new_state + ' | '
table_code += '\n'
# write or print wiki table
if args.table_file:
f = open(args.table_file,'w')
f.write(table_code)
print('Wrote table file')
else:
print('##########Table-start########')
print(table_code)
print('##########Table-end##########')
# report obvous errors
if num_errors_found:
print('Obvious errors found: %d' % num_errors_found)
else:
print('No obvious errors found')
if __name__ == '__main__':
main()
Tools/px4params/.gitignore
+1
View File
@@ -1,3 +1,4 @@
parameters.wiki
parameters.xml
parameters.wikirpc.xml
cookies.txt
Tools/px4params/dokuwikiout.py
-62
View File
@@ -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
Tools/px4params/output.py
-5
View File
@@ -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)
Tools/px4params/dokuwikiout_listings.py → Tools/px4params/output_dokuwiki_listings.py
+8 -4
View File
@@ -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)
Tools/px4params/output_dokuwiki_tables.py
+76
View File
@@ -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>""")
Tools/px4params/xmlout.py → Tools/px4params/output_xml.py
+8 -4
View File
@@ -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")
Tools/px4params/px_process_params.py
+16 -10
View File
@@ -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")
Tools/px4params/scanner.py
+2 -1
View File
@@ -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)
Tools/px4params/parser.py → Tools/px4params/srcparser.py
+10 -10
View File
@@ -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
Tools/px4params/xmlrpc.sh
+1 -1
View File
@@ -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"
makefiles/config_px4fmu-v1_default.mk
+4 -6
View File
@@ -21,7 +21,6 @@ MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu-v1
MODULES += drivers/ardrone_interface
MODULES += drivers/l3gd20
#MODULES += drivers/bma180
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
@@ -44,7 +43,6 @@ MODULES += modules/sensors
#
MODULES += systemcmds/mtd
MODULES += systemcmds/bl_update
MODULES += systemcmds/boardinfo
MODULES += systemcmds/i2c
MODULES += systemcmds/mixer
MODULES += systemcmds/param
@@ -65,14 +63,12 @@ MODULES += systemcmds/hw_ver
MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led
#
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/fw_att_pos_estimator
MODULES += modules/position_estimator_inav
#MODULES += examples/flow_position_estimator
@@ -82,8 +78,8 @@ MODULES += modules/position_estimator_inav
#
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
MODULES += modules/multirotor_pos_control
MODULES += modules/mc_att_control
MODULES += modules/mc_pos_control
#MODULES += examples/flow_position_control
#MODULES += examples/flow_speed_control
@@ -105,6 +101,7 @@ MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries
@@ -116,6 +113,7 @@ MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# Demo apps
makefiles/config_px4fmu-v2_default.mk
+4 -2
View File
@@ -87,8 +87,8 @@ MODULES += examples/flow_position_estimator
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
MODULES += modules/multirotor_pos_control
MODULES += modules/mc_att_control
MODULES += modules/mc_pos_control
#
# Logging
@@ -108,6 +108,7 @@ MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries
@@ -119,6 +120,7 @@ MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
MODULES += lib/launchdetection
#
# Demo apps
makefiles/config_px4fmu-v2_test.mk
+5
View File
@@ -41,6 +41,11 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
#
# Libraries
#
LIBRARIES += lib/mathlib/CMSIS
#
# Transitional support - add commands from the NuttX export archive.
#
nuttx-configs/px4fmu-v2/include/board.h
+3 -3
View File
@@ -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.
src/drivers/airspeed/airspeed.cpp
+2 -2
View File
@@ -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),
src/drivers/airspeed/airspeed.h
+1 -1
View File
@@ -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();
src/drivers/ets_airspeed/ets_airspeed.cpp
+4 -3
View File
@@ -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)
{
}
src/drivers/frsky_telemetry/frsky_data.c
+4 -4
View File
@@ -225,16 +225,16 @@ void frsky_send_frame2(int uart)
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
if (global_pos.valid) {
if (global_pos.global_valid) {
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f);
lat = frsky_format_gps(abs(global_pos.lat));
lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f);
lon = frsky_format_gps(abs(global_pos.lon));
lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy)
speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e)
* 25.0f / 46.0f;
alt = global_pos.alt;
sec = tm_gps->tm_sec;
src/drivers/hott/messages.cpp
+2 -2
View File
@@ -295,8 +295,8 @@ build_gps_response(uint8_t *buffer, size_t *size)
memset(&home, 0, sizeof(home));
orb_copy(ORB_ID(home_position), _home_sub, &home);
_home_lat = ((double)(home.lat))*1e-7d;
_home_lon = ((double)(home.lon))*1e-7d;
_home_lat = home.lat;
_home_lon = home.lon;
_home_position_set = true;
}
src/drivers/meas_airspeed/meas_airspeed.cpp
+10 -7
View File
@@ -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)
{
}
@@ -304,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)
@@ -313,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)
@@ -386,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))
@@ -411,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);
}
src/drivers/mpu6000/mpu6000.cpp
+1
View File
@@ -1353,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)
{
}
src/examples/fixedwing_control/main.c
+1 -5
View File
@@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
* Calculate heading error of current position to desired position
*/
/*
* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution,
* so they need to be scaled by 1e7 and converted to IEEE double precision floating point.
*/
float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon);
/* calculate heading error */
float yaw_err = att->yaw - bearing;
src/examples/flow_position_control/flow_position_control_main.c
-1
View File
@@ -61,7 +61,6 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
src/lib/conversion/rotation.cpp
+2 -13
View File
@@ -41,22 +41,11 @@
#include "rotation.h"
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix)
{
/* first set to zero */
rot_matrix->Matrix::zero(3, 3);
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
math::EulerAngles euler(roll, pitch, yaw);
math::Dcm R(euler);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
(*rot_matrix)(i, j) = R(i, j);
}
}
rot_matrix->from_euler(roll, pitch, yaw);
}
src/lib/conversion/rotation.h
+1 -1
View File
@@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = {
* Get the rotation matrix
*/
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix);
#endif /* ROTATION_H_ */
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+72 -35
View File
@@ -45,39 +45,29 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
ECL_PitchController::ECL_PitchController() :
_last_run(0),
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate_pos(0.0f),
_max_rate_neg(0.0f),
_roll_ff(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_max_deflection_rad(math::radians(45.0f))
_bodyrate_setpoint(0.0f)
{
}
float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
/* flying inverted (wings upside down) ? */
bool inverted = false;
@@ -100,34 +90,78 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
}
/* calculate the offset in the rate resulting from rolling */
//xxx needs explanation and conversion to body angular rates or should be removed
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
if (inverted)
turn_offset = -turn_offset;
/* Calculate the error */
float pitch_error = pitch_setpoint - pitch;
/* rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
/* add turn offset */
_rate_setpoint += turn_offset;
_rate_error = _rate_setpoint - pitch_rate;
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
}
float ilimit_scaled = _integrator_max * scaler;
}
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
return _rate_setpoint;
}
float id = _rate_error * k_i_rate * dt * scaler;
float ECL_PitchController::control_bodyrate(float roll, float pitch,
float pitch_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
/* Transform setpoint to body angular rates */
_bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian
/* Transform estimation to body angular rates */
float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase into the
* wrong direction if actuator is at limit
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -_max_deflection_rad) {
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > _max_deflection_rad) {
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
@@ -136,11 +170,14 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
}
/* integrator limit */
_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
/* store non-limited output */
_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_PitchController::reset_integrator()
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+23 -7
View File
@@ -36,6 +36,7 @@
* Definition of a simple orthogonal pitch PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
@@ -51,13 +52,18 @@
#include <stdbool.h>
#include <stdint.h>
class __EXPORT ECL_PitchController
class __EXPORT ECL_PitchController //XXX: create controller superclass
{
public:
ECL_PitchController();
float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
float control_bodyrate(float roll, float pitch,
float pitch_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
void reset_integrator();
@@ -67,21 +73,27 @@ public:
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate_pos(float max_rate_pos) {
_max_rate_pos = max_rate_pos;
}
void set_max_rate_neg(float max_rate_neg) {
_max_rate_neg = max_rate_neg;
}
void set_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
@@ -94,13 +106,17 @@ public:
return _rate_setpoint;
}
float get_desired_bodyrate() {
return _bodyrate_setpoint;
}
private:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate_pos;
float _max_rate_neg;
@@ -109,7 +125,7 @@ private:
float _integrator;
float _rate_error;
float _rate_setpoint;
float _max_deflection_rad;
float _bodyrate_setpoint;
};
#endif // ECL_PITCH_CONTROLLER_H
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+50 -26
View File
@@ -45,21 +45,46 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
ECL_RollController::ECL_RollController() :
_last_run(0),
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_max_deflection_rad(math::radians(45.0f))
_bodyrate_setpoint(0.0f)
{
}
float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
float ECL_RollController::control_attitude(float roll_setpoint, float roll)
{
/* Calculate error */
float roll_error = roll_setpoint - roll;
/* Apply P controller */
_rate_setpoint = roll_error / _tc;
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
return _rate_setpoint;
}
float ECL_RollController::control_bodyrate(float pitch,
float roll_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
@@ -70,10 +95,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
if (dt_micros > 500000)
lock_integrator = true;
float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0; //xxx: param
/* input conditioning */
// warnx("airspeed pre %.4f", (double)airspeed);
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
@@ -81,32 +107,27 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
airspeed = airspeed_min;
}
float roll_error = roll_setpoint - roll;
_rate_setpoint = roll_error / _tc;
/* limit the rate */
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
/* Transform setpoint to body angular rates */
_bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
_rate_error = _rate_setpoint - roll_rate;
/* Transform estimation to body angular rates */
float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
float ilimit_scaled = _integrator_max * scaler;
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * k_i_rate * dt * scaler;
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase into the
* wrong direction if actuator is at limit
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -_max_deflection_rad) {
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > _max_deflection_rad) {
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
@@ -115,11 +136,14 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
}
/* integrator limit */
_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
/* store non-limited output */
_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_RollController::reset_integrator()
src/lib/ecl/attitude_fw/ecl_roll_controller.h
+21 -7
View File
@@ -36,6 +36,7 @@
* Definition of a simple orthogonal roll PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
@@ -51,13 +52,17 @@
#include <stdbool.h>
#include <stdint.h>
class __EXPORT ECL_RollController
class __EXPORT ECL_RollController //XXX: create controller superclass
{
public:
ECL_RollController();
float control(float roll_setpoint, float roll, float roll_rate,
float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
float control_attitude(float roll_setpoint, float roll);
float control_bodyrate(float pitch,
float roll_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
void reset_integrator();
@@ -66,18 +71,23 @@ public:
_tc = time_constant;
}
}
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
@@ -90,19 +100,23 @@ public:
return _rate_setpoint;
}
float get_desired_bodyrate() {
return _bodyrate_setpoint;
}
private:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate;
float _last_output;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _max_deflection_rad;
float _bodyrate_setpoint;
};
#endif // ECL_ROLL_CONTROLLER_H
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+106 -10
View File
@@ -44,29 +44,125 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
ECL_YawController::ECL_YawController() :
_last_run(0),
_last_error(0.0f),
_k_p(0.0f),
_k_i(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
_last_output(0.0f),
_last_rate_hp_out(0.0f),
_last_rate_hp_in(0.0f),
_k_d_last(0.0f),
_integrator(0.0f)
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f),
_coordinated_min_speed(1.0f)
{
}
float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
float airspeed_min, float airspeed_max, float aspeed)
float ECL_YawController::control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint)
{
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
if(denumerator != 0.0f) { //XXX: floating point comparison
_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
}
// if(counter % 20 == 0) {
// warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
// }
}
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
// counter++;
if(!isfinite(_rate_setpoint)){
warnx("yaw rate sepoint not finite");
_rate_setpoint = 0.0f;
}
return _rate_setpoint;
}
float ECL_YawController::control_bodyrate(float roll, float pitch,
float pitch_rate, float yaw_rate,
float pitch_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
return 0.0f;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
/* Transform setpoint to body angular rates */
_bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
/* Transform estimation to body angular rates */
float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_YawController::reset_integrator()
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+54 -19
View File
@@ -35,6 +35,15 @@
* @file ecl_yaw_controller.h
* Definition of a simple orthogonal coordinated turn yaw PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
* The control design is based on a design
* by Paul Riseborough and Andrew Tridgell, 2013,
* which in turn is based on initial work of
* Jonathan Challinger, 2012.
*/
#ifndef ECL_YAW_CONTROLLER_H
#define ECL_YAW_CONTROLLER_H
@@ -42,47 +51,73 @@
#include <stdbool.h>
#include <stdint.h>
class __EXPORT ECL_YawController
class __EXPORT ECL_YawController //XXX: create controller superclass
{
public:
ECL_YawController();
float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
float control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint);
float control_bodyrate( float roll, float pitch,
float pitch_rate, float yaw_rate,
float pitch_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator);
void reset_integrator();
void set_k_side(float k_a) {
_k_side = k_a;
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_roll_ff(float k_roll_ff) {
_k_roll_ff = k_roll_ff;
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
void set_coordinated_min_speed(float coordinated_min_speed) {
_coordinated_min_speed = coordinated_min_speed;
}
float get_rate_error() {
return _rate_error;
}
float get_desired_rate() {
return _rate_setpoint;
}
float get_desired_bodyrate() {
return _bodyrate_setpoint;
}
private:
uint64_t _last_run;
float _k_side;
float _k_p;
float _k_i;
float _k_d;
float _k_roll_ff;
float _k_ff;
float _integrator_max;
float _last_error;
float _max_rate;
float _roll_ff;
float _last_output;
float _last_rate_hp_out;
float _last_rate_hp_in;
float _k_d_last;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
float _coordinated_min_speed;
};
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+22 -25
View File
@@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing()
float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
{
/* following [2], switching on L1 distance */
return math::max(wp_radius, _L1_distance);
return math::min(wp_radius, _L1_distance);
}
bool ECL_L1_Pos_Controller::reached_loiter_target(void)
@@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
return _crosstrack_error;
}
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
const math::Vector2f &ground_speed_vector)
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
const math::Vector<2> &ground_speed_vector)
{
/* this follows the logic presented in [1] */
@@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float ltrack_vel;
/* get the direction between the last (visited) and next waypoint */
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1));
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
_L1_distance = _L1_ratio * ground_speed;
/* calculate vector from A to B */
math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B);
/*
* check if waypoints are on top of each other. If yes,
@@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
vector_AB.normalize();
/* calculate the vector from waypoint A to the aircraft */
math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
/* calculate crosstrack error (output only) */
_crosstrack_error = vector_AB % vector_A_to_airplane;
@@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float alongTrackDist = vector_A_to_airplane * vector_AB;
/* estimate airplane position WRT to B */
math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
/* calculate angle of airplane position vector relative to line) */
@@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
/* calculate eta to fly to waypoint A */
/* unit vector from waypoint A to current position */
math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
/* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
/*
* If the AB vector and the vector from B to airplane point in the same
@@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
_nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0));
} else {
@@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float eta1 = asinf(sine_eta1);
eta = eta1 + eta2;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
}
@@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
_bearing_error = eta;
}
void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector2f &ground_speed_vector)
void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector<2> &ground_speed_vector)
{
/* the complete guidance logic in this section was proposed by [2] */
@@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
float K_velocity = 2.0f * _L1_damping * omega;
/* update bearing to next waypoint */
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1));
/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
@@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
_L1_distance = _L1_ratio * ground_speed;
/* calculate the vector from waypoint A to current position */
math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
/* store the normalized vector from waypoint A to current position */
math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
/* calculate eta angle towards the loiter center */
@@ -280,26 +280,26 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
*/
// XXX check switch over
if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
_lateral_accel = lateral_accel_sp_center;
_circle_mode = false;
/* angle between requested and current velocity vector */
_bearing_error = eta;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
} else {
_lateral_accel = lateral_accel_sp_circle;
_circle_mode = true;
_bearing_error = 0.0f;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
}
}
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector)
{
/* the complete guidance logic in this section was proposed by [2] */
@@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
}
math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
{
/* this is an approximation for small angles, proposed by [2] */
math::Vector2f out;
out.setX(math::radians((target.getX() - origin.getX())));
out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
}
src/lib/ecl/l1/ecl_l1_pos_controller.h
+6 -6
View File
@@ -160,8 +160,8 @@ public:
*
* @return sets _lateral_accel setpoint
*/
void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
const math::Vector2f &ground_speed);
void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
const math::Vector<2> &ground_speed);
/**
@@ -172,8 +172,8 @@ public:
*
* @return sets _lateral_accel setpoint
*/
void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector2f &ground_speed_vector);
void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector<2> &ground_speed_vector);
/**
@@ -185,7 +185,7 @@ public:
*
* @return sets _lateral_accel setpoint
*/
void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed);
/**
@@ -260,7 +260,7 @@ private:
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
* @return The vector in meters pointing from the reference position to the coordinates
*/
math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
};
src/lib/external_lgpl/tecs/tecs.cpp
+89 -60
View File
@@ -2,6 +2,7 @@
#include "tecs.h"
#include <ecl/ecl.h>
#include <systemlib/err.h>
using namespace math;
@@ -29,7 +30,7 @@ using namespace math;
*
*/
void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth)
{
// Implement third order complementary filter for height and height rate
// estimted height rate = _integ2_state
@@ -168,64 +169,88 @@ void TECS::_update_speed_demand(void)
// calculate velocity rate limits based on physical performance limits
// provision to use a different rate limit if bad descent or underspeed condition exists
// Use 50% of maximum energy rate to allow margin for total energy contgroller
float velRateMax;
float velRateMin;
// float velRateMax;
// float velRateMin;
//
// if ((_badDescent) || (_underspeed)) {
// velRateMax = 0.5f * _STEdot_max / _integ5_state;
// velRateMin = 0.5f * _STEdot_min / _integ5_state;
//
// } else {
// velRateMax = 0.5f * _STEdot_max / _integ5_state;
// velRateMin = 0.5f * _STEdot_min / _integ5_state;
// }
//
// // Apply rate limit
// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
// _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
// _TAS_rate_dem = velRateMax;
//
// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
// _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
// _TAS_rate_dem = velRateMin;
//
// } else {
// _TAS_dem_adj = _TAS_dem;
//
//
// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
// }
if ((_badDescent) || (_underspeed)) {
velRateMax = 0.5f * _STEdot_max / _integ5_state;
velRateMin = 0.5f * _STEdot_min / _integ5_state;
} else {
velRateMax = 0.5f * _STEdot_max / _integ5_state;
velRateMin = 0.5f * _STEdot_min / _integ5_state;
}
// Apply rate limit
if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
_TAS_rate_dem = velRateMax;
} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
_TAS_rate_dem = velRateMin;
} else {
_TAS_dem_adj = _TAS_dem;
_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
}
_TAS_dem_adj = _TAS_dem;
_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
// Constrain speed demand again to protect against bad values on initialisation.
_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
_TAS_dem_last = _TAS_dem;
// _TAS_dem_last = _TAS_dem;
// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin);
// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u",
// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent , _underspeed);
}
void TECS::_update_height_demand(float demand)
void TECS::_update_height_demand(float demand, float state)
{
// Apply 2 point moving average to demanded height
// This is required because height demand is only updated at 5Hz
_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
_hgt_dem_in_old = _hgt_dem;
// // Apply 2 point moving average to demanded height
// // This is required because height demand is only updated at 5Hz
// _hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
// _hgt_dem_in_old = _hgt_dem;
//
// // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
// // _maxClimbRate);
//
// // Limit height rate of change
// if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
// _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
//
// } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
// _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
// }
//
// _hgt_dem_prev = _hgt_dem;
//
// // Apply first order lag to height demand
// _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
// _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
// _hgt_dem_adj_last = _hgt_dem_adj;
//
// // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
// // _hgt_rate_dem);
// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
// _maxClimbRate);
// Limit height rate of change
if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
}
_hgt_dem_prev = _hgt_dem;
// Apply first order lag to height demand
_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
_hgt_dem_adj_last = _hgt_dem_adj;
// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
// _hgt_rate_dem);
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
// Limit height rate of change
if (_hgt_rate_dem > _maxClimbRate) {
_hgt_rate_dem = _maxClimbRate;
} else if (_hgt_rate_dem < -_maxSinkRate) {
_hgt_rate_dem = -_maxSinkRate;
}
//warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
}
void TECS::_detect_underspeed(void)
@@ -257,7 +282,7 @@ void TECS::_update_energies(void)
_SKEdot = _integ5_state * _vel_dot;
}
void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat)
{
// Calculate total energy values
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
@@ -285,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
if (STEdot_dem >= 0) {
ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
@@ -353,14 +378,18 @@ void TECS::_detect_bad_descent(void)
// 1) Underspeed protection not active
// 2) Specific total energy error > 0
// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
float STEdot = _SPEdot + _SKEdot;
// float STEdot = _SPEdot + _SKEdot;
//
// if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
//
// warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem);
// _badDescent = true;
//
// } else {
// _badDescent = false;
// }
if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
_badDescent = true;
} else {
_badDescent = false;
}
_badDescent = false;
}
void TECS::_update_pitch(void)
@@ -476,7 +505,7 @@ void TECS::_update_STE_rate_lim(void)
_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
}
void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
float throttle_min, float throttle_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max)
{
@@ -508,7 +537,7 @@ void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float bar
_update_speed_demand();
// Calculate the height demand
_update_height_demand(hgt_dem);
_update_height_demand(hgt_dem, baro_altitude);
// Detect underspeed condition
_detect_underspeed();
src/lib/external_lgpl/tecs/tecs.h
+14 -4
View File
@@ -71,10 +71,10 @@ public:
// Update of the estimated height and height rate internal state
// Update of the inertial speed rate internal state
// Should be called at 50Hz or greater
void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth);
// Update the control loop calculations
void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
float throttle_min, float throttle_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max);
// demanded throttle in percentage
@@ -180,6 +180,14 @@ public:
_indicated_airspeed_max = airspeed;
}
void set_heightrate_p(float heightrate_p) {
_heightrate_p = heightrate_p;
}
void set_speedrate_p(float speedrate_p) {
_speedrate_p = speedrate_p;
}
private:
// Last time update_50Hz was called
uint64_t _update_50hz_last_usec;
@@ -203,6 +211,8 @@ private:
float _vertAccLim;
float _rollComp;
float _spdWeight;
float _heightrate_p;
float _speedrate_p;
// throttle demand in the range from 0.0 to 1.0
float _throttle_dem;
@@ -329,7 +339,7 @@ private:
void _update_speed_demand(void);
// Update the demanded height
void _update_height_demand(float demand);
void _update_height_demand(float demand, float state);
// Detect an underspeed condition
void _detect_underspeed(void);
@@ -338,7 +348,7 @@ private:
void _update_energies(void);
// Update Demanded Throttle
void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat);
// Detect Bad Descent
void _detect_bad_descent(void);
src/lib/geo/geo.c
+92 -30
View File
@@ -72,7 +72,6 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
const double r_earth = 6371000;
double lat1 = phi_1;
double lon1 = lambda_0;
@@ -81,7 +80,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
double lon2 = lambda_0 + 0.5 / 180 * M_PI;
double sin_lat_2 = sin(lat2);
double cos_lat_2 = cos(lat2);
double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH;
/* 2) calculate distance rho on plane */
double k_bar = 0;
@@ -188,8 +187,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
const double radius_earth = 6371000.0d;
return radius_earth * c;
return CONSTANTS_RADIUS_OF_EARTH * c;
}
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
@@ -209,7 +207,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
return theta;
}
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -220,11 +218,11 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon);
*v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
}
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -235,8 +233,17 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
*v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat;
*v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad);
}
__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
*lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
}
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
@@ -386,25 +393,64 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
return return_value;
}
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z)
{
double current_x_rad = lat_next / 180.0 * M_PI;
double current_y_rad = lon_next / 180.0 * M_PI;
double x_rad = lat_now / 180.0 * M_PI;
double y_rad = lon_now / 180.0 * M_PI;
double d_lat = x_rad - current_x_rad;
double d_lon = y_rad - current_y_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
float dz = alt_now - alt_next;
*dist_xy = fabsf(dxy);
*dist_z = fabsf(dz);
return sqrtf(dxy * dxy + dz * dz);
}
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z)
{
float dx = x_now - x_next;
float dy = y_now - y_next;
float dz = z_now - z_next;
*dist_xy = sqrtf(dx * dx + dy * dy);
*dist_z = fabsf(dz);
return sqrtf(dx * dx + dy * dy + dz * dz);
}
__EXPORT float _wrap_pi(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing) || bearing == 0) {
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing > M_PI_F && c < 30) {
while (bearing > M_PI_F) {
bearing -= M_TWOPI_F;
c++;
if (c++ > 3)
return NAN;
}
c = 0;
while (bearing <= -M_PI_F && c < 30) {
while (bearing <= -M_PI_F) {
bearing += M_TWOPI_F;
c++;
if (c++ > 3)
return NAN;
}
return bearing;
@@ -417,12 +463,18 @@ __EXPORT float _wrap_2pi(float bearing)
return bearing;
}
while (bearing >= M_TWOPI_F) {
bearing = bearing - M_TWOPI_F;
int c = 0;
while (bearing > M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3)
return NAN;
}
while (bearing < 0.0f) {
bearing = bearing + M_TWOPI_F;
c = 0;
while (bearing <= 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3)
return NAN;
}
return bearing;
@@ -435,12 +487,18 @@ __EXPORT float _wrap_180(float bearing)
return bearing;
}
int c = 0;
while (bearing > 180.0f) {
bearing = bearing - 360.0f;
bearing -= 360.0f;
if (c++ > 3)
return NAN;
}
while (bearing <= -180.0f) {
bearing = bearing + 360.0f;
c = 0;
while (bearing <= -180.0f) {
bearing += 360.0f;
if (c++ > 3)
return NAN;
}
return bearing;
@@ -453,15 +511,19 @@ __EXPORT float _wrap_360(float bearing)
return bearing;
}
while (bearing >= 360.0f) {
bearing = bearing - 360.0f;
int c = 0;
while (bearing > 360.0f) {
bearing -= 360.0f;
if (c++ > 3)
return NAN;
}
while (bearing < 0.0f) {
bearing = bearing + 360.0f;
c = 0;
while (bearing <= 0.0f) {
bearing += 360.0f;
if (c++ > 3)
return NAN;
}
return bearing;
}
src/lib/geo/geo.h
+21 -2
View File
@@ -47,6 +47,9 @@
#pragma once
#include "uORB/topics/fence.h"
#include "uORB/topics/vehicle_global_position.h"
__BEGIN_DECLS
#include <stdbool.h>
@@ -112,15 +115,31 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
*/
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep);
/*
* Calculate distance in global frame
*/
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z);
/*
* Calculate distance in local frame (NED)
*/
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z);
__EXPORT float _wrap_180(float bearing);
__EXPORT float _wrap_360(float bearing);
__EXPORT float _wrap_pi(float bearing);
src/lib/launchdetection/CatapultLaunchMethod.cpp
+90
View File
@@ -0,0 +1,90 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
* distribution.
* 3. Neither the name ECL 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 CatapultLaunchMethod.cpp
* Catpult Launch detection
*
* Authors and acknowledgements in header.
*/
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
CatapultLaunchMethod::CatapultLaunchMethod() :
last_timestamp(0),
integrator(0.0f),
launchDetected(false),
threshold_accel(NULL, "LAUN_CAT_A", false),
threshold_time(NULL, "LAUN_CAT_T", false)
{
}
CatapultLaunchMethod::~CatapultLaunchMethod() {
}
void CatapultLaunchMethod::update(float accel_x)
{
float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
last_timestamp = hrt_absolute_time();
if (accel_x > threshold_accel.get()) {
integrator += accel_x * dt;
// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
if (integrator > threshold_accel.get() * threshold_time.get()) {
launchDetected = true;
}
} else {
// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
/* reset integrator */
integrator = 0.0f;
launchDetected = false;
}
}
bool CatapultLaunchMethod::getLaunchDetected()
{
return launchDetected;
}
void CatapultLaunchMethod::updateParams()
{
threshold_accel.update();
threshold_time.update();
}
src/lib/launchdetection/CatapultLaunchMethod.h
+71
View File
@@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
* distribution.
* 3. Neither the name ECL 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 CatapultLaunchMethod.h
* Catpult Launch detection
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef CATAPULTLAUNCHMETHOD_H_
#define CATAPULTLAUNCHMETHOD_H_
#include "LaunchMethod.h"
#include <drivers/drv_hrt.h>
#include <controllib/block/BlockParam.hpp>
class CatapultLaunchMethod : public LaunchMethod
{
public:
CatapultLaunchMethod();
~CatapultLaunchMethod();
void update(float accel_x);
bool getLaunchDetected();
void updateParams();
private:
hrt_abstime last_timestamp;
// float threshold_accel_raw;
// float threshold_time;
float integrator;
bool launchDetected;
control::BlockParamFloat threshold_accel;
control::BlockParamFloat threshold_time;
};
#endif /* CATAPULTLAUNCHMETHOD_H_ */
src/lib/mathlib/math/Vector.cpp → src/lib/launchdetection/LaunchDetector.cpp
+40 -48
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
* 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
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -32,69 +32,61 @@
****************************************************************************/
/**
* @file Vector.cpp
* @file launchDetection.cpp
* Auto Detection for different launch methods (e.g. catapult)
*
* math vector
* Authors and acknowledgements in header.
*/
#include "test/test.hpp"
#include "LaunchDetector.h"
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
#include "Vector.hpp"
namespace math
LaunchDetector::LaunchDetector() :
launchdetection_on(NULL, "LAUN_ALL_ON", false),
throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
{
/* init all detectors */
launchMethods[0] = new CatapultLaunchMethod();
static const float data_testA[] = {1, 3};
static const float data_testB[] = {4, 1};
static Vector testA(2, data_testA);
static Vector testB(2, data_testB);
int __EXPORT vectorTest()
{
vectorAddTest();
vectorSubTest();
return 0;
/* update all parameters of all detectors */
updateParams();
}
int vectorAddTest()
LaunchDetector::~LaunchDetector()
{
printf("Test Vector Add\t\t: ");
Vector r = testA + testB;
float data_test[] = {5.0f, 4.0f};
ASSERT(vectorEqual(Vector(2, data_test), r));
printf("PASS\n");
return 0;
}
int vectorSubTest()
void LaunchDetector::update(float accel_x)
{
printf("Test Vector Sub\t\t: ");
Vector r(2);
r = testA - testB;
float data_test[] = { -3.0f, 2.0f};
ASSERT(vectorEqual(Vector(2, data_test), r));
printf("PASS\n");
return 0;
}
bool vectorEqual(const Vector &a, const Vector &b, float eps)
{
if (a.getRows() != b.getRows()) {
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
return false;
if (launchdetection_on.get() == 1) {
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->update(accel_x);
}
}
}
bool ret = true;
for (size_t i = 0; i < a.getRows(); i++) {
if (!equal(a(i), b(i), eps)) {
printf("element mismatch (%d)\n", i);
ret = false;
bool LaunchDetector::getLaunchDetected()
{
if (launchdetection_on.get() == 1) {
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
if(launchMethods[i]->getLaunchDetected()) {
return true;
}
}
}
return ret;
return false;
}
} // namespace math
void LaunchDetector::updateParams() {
launchdetection_on.update();
throttlePreTakeoff.update();
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->updateParams();
}
}
src/lib/launchdetection/LaunchDetector.h
+75
View File
@@ -0,0 +1,75 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
* distribution.
* 3. Neither the name ECL 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 LaunchDetector.h
* Auto Detection for different launch methods (e.g. catapult)
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef LAUNCHDETECTOR_H
#define LAUNCHDETECTOR_H
#include <stdbool.h>
#include <stdint.h>
#include "LaunchMethod.h"
#include <controllib/block/BlockParam.hpp>
class __EXPORT LaunchDetector
{
public:
LaunchDetector();
~LaunchDetector();
void update(float accel_x);
bool getLaunchDetected();
void updateParams();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
// virtual bool getLaunchDetected();
protected:
private:
LaunchMethod* launchMethods[1];
control::BlockParamInt launchdetection_on;
control::BlockParamFloat throttlePreTakeoff;
};
#endif // LAUNCHDETECTOR_H
src/lib/mathlib/math/arm/Matrix.cpp → src/lib/launchdetection/LaunchMethod.h
+20 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -10,9 +10,9 @@
* 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
* the documentation4 and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -32,9 +32,23 @@
****************************************************************************/
/**
* @file Matrix.cpp
* @file LaunchMethod.h
* Base class for different launch methods
*
* matrix code
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "Matrix.hpp"
#ifndef LAUNCHMETHOD_H_
#define LAUNCHMETHOD_H_
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
virtual bool getLaunchDetected() = 0;
virtual void updateParams() = 0;
protected:
private:
};
#endif /* LAUNCHMETHOD_H_ */
src/lib/mathlib/math/Vector2f.cpp → src/lib/launchdetection/launchdetection_params.c
+48 -61
View File
@@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -17,7 +18,7 @@
* 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
* 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,
@@ -32,72 +33,58 @@
****************************************************************************/
/**
* @file Vector2f.cpp
* @file launchdetection_params.c
*
* math vector
* Parameters for launchdetection
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "test/test.hpp"
#include <nuttx/config.h>
#include "Vector2f.hpp"
#include <systemlib/param/param.h>
namespace math
{
/*
* Catapult launch detection parameters, accessible via MAVLink
*
*/
Vector2f::Vector2f() :
Vector(2)
{
}
/**
* Enable launch detection.
*
* @min 0
* @max 1
* @group Launch detection
*/
PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
Vector2f::Vector2f(const Vector &right) :
Vector(right)
{
#ifdef VECTOR_ASSERT
ASSERT(right.getRows() == 2);
#endif
}
/**
* 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);
Vector2f::Vector2f(float x, float y) :
Vector(2)
{
setX(x);
setY(y);
}
/**
* 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);
Vector2f::Vector2f(const float *data) :
Vector(2, data)
{
}
Vector2f::~Vector2f()
{
}
float Vector2f::cross(const Vector2f &b) const
{
const Vector2f &a = *this;
return a(0)*b(1) - a(1)*b(0);
}
float Vector2f::operator %(const Vector2f &v) const
{
return cross(v);
}
float Vector2f::operator *(const Vector2f &v) const
{
return dot(v);
}
int __EXPORT vector2fTest()
{
printf("Test Vector2f\t\t: ");
// test float ctor
Vector2f v(1, 2);
ASSERT(equal(v(0), 1));
ASSERT(equal(v(1), 2));
printf("PASS\n");
return 0;
}
} // namespace math
/**
* 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);
src/lib/launchdetection/module.mk
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2013 Estimation and Control Library (ECL). 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.
#
############################################################################
#
# Launchdetection Library
#
SRCS = LaunchDetector.cpp \
CatapultLaunchMethod.cpp \
launchdetection_params.c
src/lib/mathlib/math/Dcm.cpp
-174
View File
@@ -1,174 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 Dcm.cpp
*
* math direction cosine matrix
*/
#include <mathlib/math/test/test.hpp>
#include "Dcm.hpp"
#include "Quaternion.hpp"
#include "EulerAngles.hpp"
#include "Vector3.hpp"
namespace math
{
Dcm::Dcm() :
Matrix(Matrix::identity(3))
{
}
Dcm::Dcm(float c00, float c01, float c02,
float c10, float c11, float c12,
float c20, float c21, float c22) :
Matrix(3, 3)
{
Dcm &dcm = *this;
dcm(0, 0) = c00;
dcm(0, 1) = c01;
dcm(0, 2) = c02;
dcm(1, 0) = c10;
dcm(1, 1) = c11;
dcm(1, 2) = c12;
dcm(2, 0) = c20;
dcm(2, 1) = c21;
dcm(2, 2) = c22;
}
Dcm::Dcm(const float data[3][3]) :
Matrix(3, 3)
{
Dcm &dcm = *this;
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
dcm(i, j) = data[i][j];
}
Dcm::Dcm(const float *data) :
Matrix(3, 3, data)
{
}
Dcm::Dcm(const Quaternion &q) :
Matrix(3, 3)
{
Dcm &dcm = *this;
double a = q.getA();
double b = q.getB();
double c = q.getC();
double d = q.getD();
double aSq = a * a;
double bSq = b * b;
double cSq = c * c;
double dSq = d * d;
dcm(0, 0) = aSq + bSq - cSq - dSq;
dcm(0, 1) = 2.0 * (b * c - a * d);
dcm(0, 2) = 2.0 * (a * c + b * d);
dcm(1, 0) = 2.0 * (b * c + a * d);
dcm(1, 1) = aSq - bSq + cSq - dSq;
dcm(1, 2) = 2.0 * (c * d - a * b);
dcm(2, 0) = 2.0 * (b * d - a * c);
dcm(2, 1) = 2.0 * (a * b + c * d);
dcm(2, 2) = aSq - bSq - cSq + dSq;
}
Dcm::Dcm(const EulerAngles &euler) :
Matrix(3, 3)
{
Dcm &dcm = *this;
double cosPhi = cos(euler.getPhi());
double sinPhi = sin(euler.getPhi());
double cosThe = cos(euler.getTheta());
double sinThe = sin(euler.getTheta());
double cosPsi = cos(euler.getPsi());
double sinPsi = sin(euler.getPsi());
dcm(0, 0) = cosThe * cosPsi;
dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
dcm(1, 0) = cosThe * sinPsi;
dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
dcm(2, 0) = -sinThe;
dcm(2, 1) = sinPhi * cosThe;
dcm(2, 2) = cosPhi * cosThe;
}
Dcm::Dcm(const Dcm &right) :
Matrix(right)
{
}
Dcm::~Dcm()
{
}
int __EXPORT dcmTest()
{
printf("Test DCM\t\t: ");
// default ctor
ASSERT(matrixEqual(Dcm(),
Matrix::identity(3)));
// quaternion ctor
ASSERT(matrixEqual(
Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
0.2896295f, 0.9564251f, -0.0369570f,
-0.1986693f, 0.0978434f, 0.9751703f)));
// euler angle ctor
ASSERT(matrixEqual(
Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
0.2896295f, 0.9564251f, -0.0369570f,
-0.1986693f, 0.0978434f, 0.9751703f)));
// rotations
Vector3 vB(1, 2, 3);
ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
Dcm(EulerAngles(
M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
printf("PASS\n");
return 0;
}
} // namespace math
src/lib/mathlib/math/EulerAngles.cpp
-126
View File
@@ -1,126 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 Vector.cpp
*
* math vector
*/
#include "test/test.hpp"
#include "EulerAngles.hpp"
#include "Quaternion.hpp"
#include "Dcm.hpp"
#include "Vector3.hpp"
namespace math
{
EulerAngles::EulerAngles() :
Vector(3)
{
setPhi(0.0f);
setTheta(0.0f);
setPsi(0.0f);
}
EulerAngles::EulerAngles(float phi, float theta, float psi) :
Vector(3)
{
setPhi(phi);
setTheta(theta);
setPsi(psi);
}
EulerAngles::EulerAngles(const Quaternion &q) :
Vector(3)
{
(*this) = EulerAngles(Dcm(q));
}
EulerAngles::EulerAngles(const Dcm &dcm) :
Vector(3)
{
setTheta(asinf(-dcm(2, 0)));
if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) + getPhi());
} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) - getPhi());
} else {
setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
}
}
EulerAngles::~EulerAngles()
{
}
int __EXPORT eulerAnglesTest()
{
printf("Test EulerAngles\t: ");
EulerAngles euler(0.1f, 0.2f, 0.3f);
// test ctor
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
ASSERT(equal(euler.getPhi(), 0.1f));
ASSERT(equal(euler.getTheta(), 0.2f));
ASSERT(equal(euler.getPsi(), 0.3f));
// test dcm ctor
euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
// test quat ctor
euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
// test assignment
euler.setPhi(0.4f);
euler.setTheta(0.5f);
euler.setPsi(0.6f);
ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
printf("PASS\n");
return 0;
}
} // namespace math
src/lib/mathlib/math/Matrix.cpp
-193
View File
@@ -1,193 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 Matrix.cpp
*
* matrix code
*/
#include "test/test.hpp"
#include <math.h>
#include "Matrix.hpp"
namespace math
{
static const float data_testA[] = {
1, 2, 3,
4, 5, 6
};
static Matrix testA(2, 3, data_testA);
static const float data_testB[] = {
0, 1, 3,
7, -1, 2
};
static Matrix testB(2, 3, data_testB);
static const float data_testC[] = {
0, 1,
2, 1,
3, 2
};
static Matrix testC(3, 2, data_testC);
static const float data_testD[] = {
0, 1, 2,
2, 1, 4,
5, 2, 0
};
static Matrix testD(3, 3, data_testD);
static const float data_testE[] = {
1, -1, 2,
0, 2, 3,
2, -1, 1
};
static Matrix testE(3, 3, data_testE);
static const float data_testF[] = {
3.777e006f, 2.915e007f, 0.000e000f,
2.938e007f, 2.267e008f, 0.000e000f,
0.000e000f, 0.000e000f, 6.033e008f
};
static Matrix testF(3, 3, data_testF);
int __EXPORT matrixTest()
{
matrixAddTest();
matrixSubTest();
matrixMultTest();
matrixInvTest();
matrixDivTest();
return 0;
}
int matrixAddTest()
{
printf("Test Matrix Add\t\t: ");
Matrix r = testA + testB;
float data_test[] = {
1.0f, 3.0f, 6.0f,
11.0f, 4.0f, 8.0f
};
ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
printf("PASS\n");
return 0;
}
int matrixSubTest()
{
printf("Test Matrix Sub\t\t: ");
Matrix r = testA - testB;
float data_test[] = {
1.0f, 1.0f, 0.0f,
-3.0f, 6.0f, 4.0f
};
ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
printf("PASS\n");
return 0;
}
int matrixMultTest()
{
printf("Test Matrix Mult\t: ");
Matrix r = testC * testB;
float data_test[] = {
7.0f, -1.0f, 2.0f,
7.0f, 1.0f, 8.0f,
14.0f, 1.0f, 13.0f
};
ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
printf("PASS\n");
return 0;
}
int matrixInvTest()
{
printf("Test Matrix Inv\t\t: ");
Matrix origF = testF;
Matrix r = testF.inverse();
float data_test[] = {
-0.0012518f, 0.0001610f, 0.0000000f,
0.0001622f, -0.0000209f, 0.0000000f,
0.0000000f, 0.0000000f, 1.6580e-9f
};
ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
// make sure F in unchanged
ASSERT(matrixEqual(origF, testF));
printf("PASS\n");
return 0;
}
int matrixDivTest()
{
printf("Test Matrix Div\t\t: ");
Matrix r = testD / testE;
float data_test[] = {
0.2222222f, 0.5555556f, -0.1111111f,
0.0f, 1.0f, 1.0,
-4.1111111f, 1.2222222f, 4.5555556f
};
ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
printf("PASS\n");
return 0;
}
bool matrixEqual(const Matrix &a, const Matrix &b, float eps)
{
if (a.getRows() != b.getRows()) {
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
return false;
} else if (a.getCols() != b.getCols()) {
printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
return false;
}
bool ret = true;
for (size_t i = 0; i < a.getRows(); i++)
for (size_t j = 0; j < a.getCols(); j++) {
if (!equal(a(i, j), b(i, j), eps)) {
printf("element mismatch (%d, %d)\n", i, j);
ret = false;
}
}
return ret;
}
} // namespace math
src/lib/mathlib/math/Matrix.hpp
+395 -21
View File
@@ -1,6 +1,9 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,30 +35,401 @@
****************************************************************************/
/**
* @file Matrix.h
* @file Matrix.hpp
*
* matrix code
* Matrix class
*/
#pragma once
#ifndef MATRIX_HPP
#define MATRIX_HPP
#include <nuttx/config.h>
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
#include "arm/Matrix.hpp"
#else
#include "generic/Matrix.hpp"
#endif
#include <stdio.h>
#include "../CMSIS/Include/arm_math.h"
namespace math
{
class Matrix;
int matrixTest();
int matrixAddTest();
int matrixSubTest();
int matrixMultTest();
int matrixInvTest();
int matrixDivTest();
int matrixArmTest();
bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
} // namespace math
template <unsigned int M, unsigned int N>
class __EXPORT Matrix;
// MxN matrix with float elements
template <unsigned int M, unsigned int N>
class __EXPORT MatrixBase
{
public:
/**
* matrix data[row][col]
*/
float data[M][N];
/**
* struct for using arm_math functions
*/
arm_matrix_instance_f32 arm_mat;
/**
* trivial ctor
* note that this ctor will not initialize elements
*/
MatrixBase() {
arm_mat = {M, N, &data[0][0]};
}
/**
* copyt ctor
*/
MatrixBase(const MatrixBase<M, N> &m) {
arm_mat = {M, N, &data[0][0]};
memcpy(data, m.data, sizeof(data));
}
MatrixBase(const float *d) {
arm_mat = {M, N, &data[0][0]};
memcpy(data, d, sizeof(data));
}
MatrixBase(const float d[M][N]) {
arm_mat = {M, N, &data[0][0]};
memcpy(data, d, sizeof(data));
}
/**
* set data
*/
void set(const float *d) {
memcpy(data, d, sizeof(data));
}
/**
* set data
*/
void set(const float d[M][N]) {
memcpy(data, d, sizeof(data));
}
/**
* access by index
*/
float &operator()(const unsigned int row, const unsigned int col) {
return data[row][col];
}
/**
* access by index
*/
float operator()(const unsigned int row, const unsigned int col) const {
return data[row][col];
}
/**
* get rows number
*/
unsigned int get_rows() const {
return M;
}
/**
* get columns number
*/
unsigned int get_cols() const {
return N;
}
/**
* test for equality
*/
bool operator ==(const Matrix<M, N> &m) const {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m.data[i][j])
return false;
return true;
}
/**
* test for inequality
*/
bool operator !=(const Matrix<M, N> &m) const {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
if (data[i][j] != m.data[i][j])
return true;
return false;
}
/**
* set to value
*/
const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
memcpy(data, m.data, sizeof(data));
return *static_cast<Matrix<M, N>*>(this);
}
/**
* negation
*/
Matrix<M, N> operator -(void) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
res.data[i][j] = -data[i][j];
return res;
}
/**
* addition
*/
Matrix<M, N> operator +(const Matrix<M, N> &m) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
res.data[i][j] = data[i][j] + m.data[i][j];
return res;
}
Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
data[i][j] += m.data[i][j];
return *static_cast<Matrix<M, N>*>(this);
}
/**
* subtraction
*/
Matrix<M, N> operator -(const Matrix<M, N> &m) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res.data[i][j] = data[i][j] - m.data[i][j];
return res;
}
Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
for (unsigned int i = 0; i < N; i++)
for (unsigned int j = 0; j < M; j++)
data[i][j] -= m.data[i][j];
return *static_cast<Matrix<M, N>*>(this);
}
/**
* uniform scaling
*/
Matrix<M, N> operator *(const float num) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res.data[i][j] = data[i][j] * num;
return res;
}
Matrix<M, N> &operator *=(const float num) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
data[i][j] *= num;
return *static_cast<Matrix<M, N>*>(this);
}
Matrix<M, N> operator /(const float num) const {
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
res[i][j] = data[i][j] / num;
return res;
}
Matrix<M, N> &operator /=(const float num) {
for (unsigned int i = 0; i < M; i++)
for (unsigned int j = 0; j < N; j++)
data[i][j] /= num;
return *static_cast<Matrix<M, N>*>(this);
}
/**
* multiplication by another matrix
*/
template <unsigned int P>
Matrix<M, P> operator *(const Matrix<N, P> &m) const {
Matrix<M, P> res;
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
return res;
}
/**
* transpose the matrix
*/
Matrix<N, M> transposed(void) const {
Matrix<N, M> res;
arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
return res;
}
/**
* invert the matrix
*/
Matrix<M, N> inversed(void) const {
Matrix<M, N> res;
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res;
}
/**
* set zero matrix
*/
void zero(void) {
memset(data, 0, sizeof(data));
}
/**
* set identity matrix
*/
void identity(void) {
memset(data, 0, sizeof(data));
unsigned int n = (M < N) ? M : N;
for (unsigned int i = 0; i < n; i++)
data[i][i] = 1;
}
void print(void) {
for (unsigned int i = 0; i < M; i++) {
printf("[ ");
for (unsigned int j = 0; j < N; j++)
printf("%.3f\t", data[i][j]);
printf(" ]\n");
}
}
};
template <unsigned int M, unsigned int N>
class __EXPORT Matrix : public MatrixBase<M, N>
{
public:
using MatrixBase<M, N>::operator *;
Matrix() : MatrixBase<M, N>() {}
Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {}
Matrix(const float *d) : MatrixBase<M, N>(d) {}
Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {}
/**
* set to value
*/
const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
memcpy(this->data, m.data, sizeof(this->data));
return *this;
}
/**
* multiplication by a vector
*/
Vector<M> operator *(const Vector<N> &v) const {
Vector<M> res;
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
return res;
}
};
template <>
class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
{
public:
using MatrixBase<3, 3>::operator *;
Matrix() : MatrixBase<3, 3>() {}
Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {}
Matrix(const float *d) : MatrixBase<3, 3>(d) {}
Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {}
/**
* set to value
*/
const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) {
memcpy(this->data, m.data, sizeof(this->data));
return *this;
}
/**
* multiplication by a vector
*/
Vector<3> operator *(const Vector<3> &v) const {
Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
return res;
}
/**
* create a rotation matrix from given euler angles
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
*/
void from_euler(float roll, float pitch, float yaw) {
float cp = cosf(pitch);
float sp = sinf(pitch);
float sr = sinf(roll);
float cr = cosf(roll);
float sy = sinf(yaw);
float cy = cosf(yaw);
data[0][0] = cp * cy;
data[0][1] = (sr * sp * cy) - (cr * sy);
data[0][2] = (cr * sp * cy) + (sr * sy);
data[1][0] = cp * sy;
data[1][1] = (sr * sp * sy) + (cr * cy);
data[1][2] = (cr * sp * sy) - (sr * cy);
data[2][0] = -sp;
data[2][1] = sr * cp;
data[2][2] = cr * cp;
}
/**
* get euler angles from rotation matrix
*/
Vector<3> to_euler(void) const {
Vector<3> euler;
euler.data[1] = asinf(-data[2][0]);
if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];
} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];
} else {
euler.data[0] = atan2f(data[2][1], data[2][2]);
euler.data[2] = atan2f(data[1][0], data[0][0]);
}
return euler;
}
};
}
#endif // MATRIX_HPP
src/lib/mathlib/math/Quaternion.cpp
-174
View File
@@ -1,174 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 Quaternion.cpp
*
* math vector
*/
#include "test/test.hpp"
#include "Quaternion.hpp"
#include "Dcm.hpp"
#include "EulerAngles.hpp"
namespace math
{
Quaternion::Quaternion() :
Vector(4)
{
setA(1.0f);
setB(0.0f);
setC(0.0f);
setD(0.0f);
}
Quaternion::Quaternion(float a, float b,
float c, float d) :
Vector(4)
{
setA(a);
setB(b);
setC(c);
setD(d);
}
Quaternion::Quaternion(const float *data) :
Vector(4, data)
{
}
Quaternion::Quaternion(const Vector &v) :
Vector(v)
{
}
Quaternion::Quaternion(const Dcm &dcm) :
Vector(4)
{
// avoiding singularities by not using
// division equations
setA(0.5 * sqrt(1.0 +
double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
setB(0.5 * sqrt(1.0 +
double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
setC(0.5 * sqrt(1.0 +
double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
setD(0.5 * sqrt(1.0 +
double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
}
Quaternion::Quaternion(const EulerAngles &euler) :
Vector(4)
{
double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2);
setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
cosPhi_2 * sinTheta_2 * sinPsi_2);
setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
sinPhi_2 * cosTheta_2 * sinPsi_2);
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
sinPhi_2 * sinTheta_2 * cosPsi_2);
}
Quaternion::Quaternion(const Quaternion &right) :
Vector(right)
{
}
Quaternion::~Quaternion()
{
}
Vector Quaternion::derivative(const Vector &w)
{
#ifdef QUATERNION_ASSERT
ASSERT(w.getRows() == 3);
#endif
float dataQ[] = {
getA(), -getB(), -getC(), -getD(),
getB(), getA(), -getD(), getC(),
getC(), getD(), getA(), -getB(),
getD(), -getC(), getB(), getA()
};
Vector v(4);
v(0) = 0.0f;
v(1) = w(0);
v(2) = w(1);
v(3) = w(2);
Matrix Q(4, 4, dataQ);
return Q * v * 0.5f;
}
int __EXPORT quaternionTest()
{
printf("Test Quaternion\t\t: ");
// test default ctor
Quaternion q;
ASSERT(equal(q.getA(), 1.0f));
ASSERT(equal(q.getB(), 0.0f));
ASSERT(equal(q.getC(), 0.0f));
ASSERT(equal(q.getD(), 0.0f));
// test float ctor
q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
ASSERT(equal(q.getA(), 0.1825742f));
ASSERT(equal(q.getB(), 0.3651484f));
ASSERT(equal(q.getC(), 0.5477226f));
ASSERT(equal(q.getD(), 0.7302967f));
// test euler ctor
q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
// test dcm ctor
q = Quaternion(Dcm());
ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
// TODO test derivative
// test accessors
q.setA(0.1f);
q.setB(0.2f);
q.setC(0.3f);
q.setD(0.4f);
ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
printf("PASS\n");
return 0;
}
} // namespace math
src/lib/mathlib/math/Quaternion.hpp
+94 -44
View File
@@ -1,6 +1,9 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,82 +37,129 @@
/**
* @file Quaternion.hpp
*
* math quaternion lib
* Quaternion class
*/
#pragma once
#ifndef QUATERNION_HPP
#define QUATERNION_HPP
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
#include "Vector.hpp"
#include "Matrix.hpp"
namespace math
{
class Dcm;
class EulerAngles;
class __EXPORT Quaternion : public Vector
class __EXPORT Quaternion : public Vector<4>
{
public:
/**
* trivial ctor
*/
Quaternion() : Vector<4>() {}
/**
* default ctor
* copy ctor
*/
Quaternion();
Quaternion(const Quaternion &q) : Vector<4>(q) {}
/**
* ctor from floats
* casting from vector
*/
Quaternion(float a, float b, float c, float d);
Quaternion(const Vector<4> &v) : Vector<4>(v) {}
/**
* ctor from data
* setting ctor
*/
Quaternion(const float *data);
Quaternion(const float d[4]) : Vector<4>(d) {}
/**
* ctor from Vector
* setting ctor
*/
Quaternion(const Vector &v);
Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {}
using Vector<4>::operator *;
/**
* ctor from EulerAngles
* multiplication
*/
Quaternion(const EulerAngles &euler);
/**
* ctor from Dcm
*/
Quaternion(const Dcm &dcm);
/**
* deep copy ctor
*/
Quaternion(const Quaternion &right);
/**
* dtor
*/
virtual ~Quaternion();
const Quaternion operator *(const Quaternion &q) const {
return Quaternion(
data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
}
/**
* derivative
*/
Vector derivative(const Vector &w);
const Quaternion derivative(const Vector<3> &w) {
float dataQ[] = {
data[0], -data[1], -data[2], -data[3],
data[1], data[0], -data[3], data[2],
data[2], data[3], data[0], -data[1],
data[3], -data[2], data[1], data[0]
};
Matrix<4, 4> Q(dataQ);
Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
return Q * v * 0.5f;
}
/**
* accessors
* imaginary part of quaternion
*/
void setA(float a) { (*this)(0) = a; }
void setB(float b) { (*this)(1) = b; }
void setC(float c) { (*this)(2) = c; }
void setD(float d) { (*this)(3) = d; }
const float &getA() const { return (*this)(0); }
const float &getB() const { return (*this)(1); }
const float &getC() const { return (*this)(2); }
const float &getD() const { return (*this)(3); }
Vector<3> imag(void) {
return Vector<3>(&data[1]);
}
/**
* set quaternion to rotation defined by euler angles
*/
void from_euler(float roll, float pitch, float yaw) {
double cosPhi_2 = cos(double(roll) / 2.0);
double sinPhi_2 = sin(double(roll) / 2.0);
double cosTheta_2 = cos(double(pitch) / 2.0);
double sinTheta_2 = sin(double(pitch) / 2.0);
double cosPsi_2 = cos(double(yaw) / 2.0);
double sinPsi_2 = sin(double(yaw) / 2.0);
data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2;
data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2;
data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2;
data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2;
}
void from_dcm(const Matrix<3, 3> &m) {
// avoiding singularities by not using division equations
data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]);
data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]);
data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]);
data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]);
}
/**
* create rotation matrix for the quaternion
*/
Matrix<3, 3> to_dcm(void) const {
Matrix<3, 3> R;
float aSq = data[0] * data[0];
float bSq = data[1] * data[1];
float cSq = data[2] * data[2];
float dSq = data[3] * data[3];
R.data[0][0] = aSq + bSq - cSq - dSq;
R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
R.data[1][1] = aSq - bSq + cSq - dSq;
R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
R.data[2][2] = aSq - bSq - cSq + dSq;
return R;
}
};
int __EXPORT quaternionTest();
} // math
}
#endif // QUATERNION_HPP
src/lib/mathlib/math/Vector.hpp
+460 -17
View File
@@ -1,6 +1,9 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,26 +35,466 @@
****************************************************************************/
/**
* @file Vector.h
* @file Vector.hpp
*
* math vector
* Vector class
*/
#pragma once
#ifndef VECTOR_HPP
#define VECTOR_HPP
#include <nuttx/config.h>
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
#include "arm/Vector.hpp"
#else
#include "generic/Vector.hpp"
#endif
#include <stdio.h>
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
namespace math
{
class Vector;
int __EXPORT vectorTest();
int __EXPORT vectorAddTest();
int __EXPORT vectorSubTest();
bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
} // math
template <unsigned int N>
class __EXPORT Vector;
template <unsigned int N>
class __EXPORT VectorBase
{
public:
/**
* vector data
*/
float data[N];
/**
* struct for using arm_math functions, represents column vector
*/
arm_matrix_instance_f32 arm_col;
/**
* trivial ctor
* note that this ctor will not initialize elements
*/
VectorBase() {
arm_col = {N, 1, &data[0]};
}
/**
* copy ctor
*/
VectorBase(const VectorBase<N> &v) {
arm_col = {N, 1, &data[0]};
memcpy(data, v.data, sizeof(data));
}
/**
* setting ctor
*/
VectorBase(const float d[N]) {
arm_col = {N, 1, &data[0]};
memcpy(data, d, sizeof(data));
}
/**
* set data
*/
void set(const float d[N]) {
memcpy(data, d, sizeof(data));
}
/**
* access to elements by index
*/
float &operator()(const unsigned int i) {
return data[i];
}
/**
* access to elements by index
*/
float operator()(const unsigned int i) const {
return data[i];
}
/**
* get vector size
*/
unsigned int get_size() const {
return N;
}
/**
* test for equality
*/
bool operator ==(const Vector<N> &v) const {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v.data[i])
return false;
return true;
}
/**
* test for inequality
*/
bool operator !=(const Vector<N> &v) const {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v.data[i])
return true;
return false;
}
/**
* set to value
*/
const Vector<N> &operator =(const Vector<N> &v) {
memcpy(data, v.data, sizeof(data));
return *static_cast<const Vector<N>*>(this);
}
/**
* negation
*/
const Vector<N> operator -(void) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = -data[i];
return res;
}
/**
* addition
*/
const Vector<N> operator +(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] + v.data[i];
return res;
}
/**
* subtraction
*/
const Vector<N> operator -(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] - v.data[i];
return res;
}
/**
* uniform scaling
*/
const Vector<N> operator *(const float num) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] * num;
return res;
}
/**
* uniform scaling
*/
const Vector<N> operator /(const float num) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] / num;
return res;
}
/**
* addition
*/
const Vector<N> &operator +=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
data[i] += v.data[i];
return *static_cast<const Vector<N>*>(this);
}
/**
* subtraction
*/
const Vector<N> &operator -=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
data[i] -= v.data[i];
return *static_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
const Vector<N> &operator *=(const float num) {
for (unsigned int i = 0; i < N; i++)
data[i] *= num;
return *static_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
const Vector<N> &operator /=(const float num) {
for (unsigned int i = 0; i < N; i++)
data[i] /= num;
return *static_cast<const Vector<N>*>(this);
}
/**
* dot product
*/
float operator *(const Vector<N> &v) const {
float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * v.data[i];
return res;
}
/**
* element by element multiplication
*/
const Vector<N> emult(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] * v.data[i];
return res;
}
/**
* element by element division
*/
const Vector<N> edivide(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] / v.data[i];
return res;
}
/**
* gets the length of this vector squared
*/
float length_squared() const {
float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * data[i];
return res;
}
/**
* gets the length of this vector
*/
float length() const {
float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * data[i];
return sqrtf(res);
}
/**
* normalizes this vector
*/
void normalize() {
*this /= length();
}
/**
* returns the normalized version of this vector
*/
Vector<N> normalized() const {
return *this / length();
}
/**
* set zero vector
*/
void zero(void) {
memset(data, 0, sizeof(data));
}
void print(void) {
printf("[ ");
for (unsigned int i = 0; i < N; i++)
printf("%.3f\t", data[i]);
printf("]\n");
}
};
template <unsigned int N>
class __EXPORT Vector : public VectorBase<N>
{
public:
Vector() : VectorBase<N>() {}
Vector(const Vector<N> &v) : VectorBase<N>(v) {}
Vector(const float d[N]) : VectorBase<N>(d) {}
/**
* set to value
*/
const Vector<N> &operator =(const Vector<N> &v) {
memcpy(this->data, v.data, sizeof(this->data));
return *this;
}
};
template <>
class __EXPORT Vector<2> : public VectorBase<2>
{
public:
Vector() : VectorBase<2>() {}
// simple copy is 1.6 times faster than memcpy
Vector(const Vector<2> &v) : VectorBase<2>() {
data[0] = v.data[0];
data[1] = v.data[1];
}
Vector(const float d[2]) : VectorBase<2>() {
data[0] = d[0];
data[1] = d[1];
}
Vector(const float x, const float y) : VectorBase<2>() {
data[0] = x;
data[1] = y;
}
/**
* set data
*/
void set(const float d[2]) {
data[0] = d[0];
data[1] = d[1];
}
/**
* set to value
*/
const Vector<2> &operator =(const Vector<2> &v) {
data[0] = v.data[0];
data[1] = v.data[1];
return *this;
}
float operator %(const Vector<2> &v) const {
return data[0] * v.data[1] - data[1] * v.data[0];
}
};
template <>
class __EXPORT Vector<3> : public VectorBase<3>
{
public:
Vector() : VectorBase<3>() {}
// simple copy is 1.6 times faster than memcpy
Vector(const Vector<3> &v) : VectorBase<3>() {
for (unsigned int i = 0; i < 3; i++)
data[i] = v.data[i];
}
Vector(const float d[3]) : VectorBase<3>() {
for (unsigned int i = 0; i < 3; i++)
data[i] = d[i];
}
Vector(const float x, const float y, const float z) : VectorBase<3>() {
data[0] = x;
data[1] = y;
data[2] = z;
}
/**
* set data
*/
void set(const float d[3]) {
for (unsigned int i = 0; i < 3; i++)
data[i] = d[i];
}
/**
* set to value
*/
const Vector<3> &operator =(const Vector<3> &v) {
for (unsigned int i = 0; i < 3; i++)
data[i] = v.data[i];
return *this;
}
Vector<3> operator %(const Vector<3> &v) const {
return Vector<3>(
data[1] * v.data[2] - data[2] * v.data[1],
data[2] * v.data[0] - data[0] * v.data[2],
data[0] * v.data[1] - data[1] * v.data[0]
);
}
};
template <>
class __EXPORT Vector<4> : public VectorBase<4>
{
public:
Vector() : VectorBase() {}
Vector(const Vector<4> &v) : VectorBase<4>() {
for (unsigned int i = 0; i < 4; i++)
data[i] = v.data[i];
}
Vector(const float d[4]) : VectorBase<4>() {
for (unsigned int i = 0; i < 4; i++)
data[i] = d[i];
}
Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() {
data[0] = x0;
data[1] = x1;
data[2] = x2;
data[3] = x3;
}
/**
* set data
*/
void set(const float d[4]) {
for (unsigned int i = 0; i < 4; i++)
data[i] = d[i];
}
/**
* set to value
*/
const Vector<4> &operator =(const Vector<4> &v) {
for (unsigned int i = 0; i < 4; i++)
data[i] = v.data[i];
return *this;
}
};
}
#endif // VECTOR_HPP
src/lib/mathlib/math/Vector3.cpp
-99
View File
@@ -1,99 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 Vector3.cpp
*
* math vector
*/
#include "test/test.hpp"
#include "Vector3.hpp"
namespace math
{
Vector3::Vector3() :
Vector(3)
{
}
Vector3::Vector3(const Vector &right) :
Vector(right)
{
#ifdef VECTOR_ASSERT
ASSERT(right.getRows() == 3);
#endif
}
Vector3::Vector3(float x, float y, float z) :
Vector(3)
{
setX(x);
setY(y);
setZ(z);
}
Vector3::Vector3(const float *data) :
Vector(3, data)
{
}
Vector3::~Vector3()
{
}
Vector3 Vector3::cross(const Vector3 &b) const
{
const Vector3 &a = *this;
Vector3 result;
result(0) = a(1) * b(2) - a(2) * b(1);
result(1) = a(2) * b(0) - a(0) * b(2);
result(2) = a(0) * b(1) - a(1) * b(0);
return result;
}
int __EXPORT vector3Test()
{
printf("Test Vector3\t\t: ");
// test float ctor
Vector3 v(1, 2, 3);
ASSERT(equal(v(0), 1));
ASSERT(equal(v(1), 2));
ASSERT(equal(v(2), 3));
printf("PASS\n");
return 0;
}
} // namespace math

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