mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-12 22:50:05 +08:00
Compare commits
31 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Daniel Agar
|
622c833768 | ||
|
ShiauweiZhao
|
931f602995 | ||
|
Daniel Agar
|
2be701f902 | ||
|
Eric Katzfey
|
a4aa76f0ac | ||
|
Julian Oes
|
b1fc0ca0d0 | ||
|
Julian Oes
|
0446292c75 | ||
|
Julian Oes
|
7b8cf4913e | ||
|
Daniel Agar
|
47215bb625 | ||
|
Daniel Agar
|
efe1d43550 | ||
|
Beat Küng
|
0687fd2689 | ||
|
Hamish Willee
|
f25abbc80a | ||
|
Daniel Agar
|
8da993c30e | ||
|
PX4 BuildBot
|
88038717dc | ||
|
Roman Bapst
|
4646762f9d | ||
|
Silvan Fuhrer
|
0c735dea2e | ||
|
Silvan Fuhrer
|
d32f400851 | ||
|
Silvan Fuhrer
|
a6d14796e4 | ||
|
Silvan Fuhrer
|
15335b194a | ||
|
PX4 BuildBot
|
21ddb04856 | ||
|
Silvan Fuhrer
|
48f2b42e12 | ||
|
Eric Katzfey
|
e862fde084 | ||
|
Silvan Fuhrer
|
d53d200aa5 | ||
|
Silvan Fuhrer
|
88ec117e59 | ||
|
Silvan Fuhrer
|
06f4195663 | ||
|
Silvan Fuhrer
|
fc1c5da92c | ||
|
Silvan Fuhrer
|
5099a91f87 | ||
|
Silvan Fuhrer
|
e16f98c2b6 | ||
|
Eric Katzfey
|
aae431e4d5 | ||
|
PX4 BuildBot
|
d6d41af9a4 | ||
|
PX4 BuildBot
|
cc5509381a | ||
|
PX4 BuildBot
|
03f681ebf8 |
@@ -73,6 +73,13 @@ menu "Toolchain"
|
||||
help
|
||||
relative path to the ROMFS root directory
|
||||
|
||||
config BOARD_ROOTFSDIR
|
||||
string "Root directory"
|
||||
depends on PLATFORM_POSIX
|
||||
default "."
|
||||
help
|
||||
Configure the root directory in the file system for PX4 files
|
||||
|
||||
config BOARD_IO
|
||||
string "IO board name"
|
||||
default "px4_io-v2_default"
|
||||
|
||||
@@ -79,4 +79,3 @@ param set-default SIM_GZ_SV_FUNC3 203
|
||||
param set-default SIM_GZ_SV_FUNC4 204
|
||||
param set-default SIM_GZ_SV_FUNC5 205
|
||||
param set-default SIM_GZ_SV_FUNC6 206
|
||||
param set-default FD_ESCS_EN 0
|
||||
|
||||
@@ -1,110 +0,0 @@
|
||||
#!/bin/sh
|
||||
#
|
||||
# @name Standard VTOL
|
||||
#
|
||||
# @type Standard VTOL
|
||||
#
|
||||
|
||||
. ${R}etc/init.d/rc.vtol_defaults
|
||||
|
||||
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
|
||||
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
|
||||
PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
|
||||
|
||||
# TODO: Enable motor failure detection when the
|
||||
# VTOL no longer reports 0A for all ESCs in SITL
|
||||
param set-default FD_ACT_EN 0
|
||||
param set-default FD_ACT_MOT_TOUT 500
|
||||
|
||||
param set-default CA_AIRFRAME 2
|
||||
|
||||
param set-default CA_ROTOR_COUNT 5
|
||||
param set-default CA_ROTOR0_PX 0.1515
|
||||
param set-default CA_ROTOR0_PY 0.245
|
||||
param set-default CA_ROTOR0_KM 0.05
|
||||
param set-default CA_ROTOR1_PX -0.1515
|
||||
param set-default CA_ROTOR1_PY -0.1875
|
||||
param set-default CA_ROTOR1_KM 0.05
|
||||
param set-default CA_ROTOR2_PX 0.1515
|
||||
param set-default CA_ROTOR2_PY -0.245
|
||||
param set-default CA_ROTOR2_KM -0.05
|
||||
param set-default CA_ROTOR3_PX -0.1515
|
||||
param set-default CA_ROTOR3_PY 0.1875
|
||||
param set-default CA_ROTOR3_KM -0.05
|
||||
param set-default CA_ROTOR4_AX 1.0
|
||||
param set-default CA_ROTOR4_AZ 0.0
|
||||
param set-default CA_ROTOR4_PX 0.2
|
||||
|
||||
param set-default SIM_GZ_EC_FUNC1 101
|
||||
param set-default SIM_GZ_EC_MIN1 10
|
||||
param set-default SIM_GZ_EC_MAX1 1500
|
||||
param set-default SIM_GZ_EC_FUNC2 102
|
||||
param set-default SIM_GZ_EC_MIN2 10
|
||||
param set-default SIM_GZ_EC_MAX2 1500
|
||||
param set-default SIM_GZ_EC_FUNC3 103
|
||||
param set-default SIM_GZ_EC_MIN3 10
|
||||
param set-default SIM_GZ_EC_MAX3 1500
|
||||
param set-default SIM_GZ_EC_FUNC4 104
|
||||
param set-default SIM_GZ_EC_MIN4 10
|
||||
param set-default SIM_GZ_EC_MAX4 1500
|
||||
|
||||
param set-default SIM_GZ_EC_FUNC5 105
|
||||
param set-default SIM_GZ_EC_MIN5 0
|
||||
param set-default SIM_GZ_EC_MAX5 3500
|
||||
|
||||
param set-default SIM_GZ_SV_FUNC1 201
|
||||
param set-default SIM_GZ_SV_FUNC2 202
|
||||
param set-default SIM_GZ_SV_FUNC3 203
|
||||
|
||||
param set-default COM_RC_IN_MODE 1
|
||||
param set-default ASPD_PRIMARY 1
|
||||
|
||||
param set-default CA_SV_CS_COUNT 3
|
||||
param set-default CA_SV_CS0_TYPE 1
|
||||
param set-default CA_SV_CS0_TRQ_R -0.5
|
||||
param set-default CA_SV_CS1_TYPE 2
|
||||
param set-default CA_SV_CS1_TRQ_R 0.5
|
||||
param set-default CA_SV_CS2_TYPE 3
|
||||
param set-default CA_SV_CS2_TRQ_P 1.0
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
param set-default PWM_MAIN_FUNC3 103
|
||||
param set-default PWM_MAIN_FUNC4 104
|
||||
param set-default PWM_MAIN_FUNC5 105
|
||||
param set-default PWM_MAIN_FUNC6 201
|
||||
param set-default PWM_MAIN_FUNC7 202
|
||||
param set-default PWM_MAIN_FUNC8 203
|
||||
|
||||
param set-default FW_L1_PERIOD 12
|
||||
param set-default FW_PR_FF 0.2
|
||||
param set-default FW_PR_P 0.9
|
||||
param set-default FW_PSP_OFF 2
|
||||
param set-default FW_P_LIM_MIN -15
|
||||
param set-default FW_RR_FF 0.1
|
||||
param set-default FW_RR_P 0.3
|
||||
param set-default FW_THR_TRIM 0.25
|
||||
param set-default FW_THR_MAX 0.6
|
||||
param set-default FW_THR_MIN 0.05
|
||||
param set-default FW_T_CLMB_MAX 8
|
||||
param set-default FW_T_SINK_MAX 2.7
|
||||
param set-default FW_T_SINK_MIN 2.2
|
||||
|
||||
param set-default MC_AIRMODE 1
|
||||
param set-default MC_ROLLRATE_P 0.3
|
||||
param set-default MC_YAW_P 1.6
|
||||
|
||||
param set-default MIS_TAKEOFF_ALT 10
|
||||
|
||||
param set-default MPC_ACC_HOR_MAX 2
|
||||
param set-default MPC_XY_P 0.8
|
||||
param set-default MPC_XY_VEL_P_ACC 3
|
||||
param set-default MPC_XY_VEL_I_ACC 4
|
||||
param set-default MPC_XY_VEL_D_ACC 0.1
|
||||
|
||||
param set-default NAV_ACC_RAD 5
|
||||
|
||||
param set-default VT_FWD_THRUST_EN 4
|
||||
param set-default VT_F_TRANS_THR 0.75
|
||||
param set-default VT_B_TRANS_DUR 8
|
||||
param set-default VT_TYPE 2
|
||||
param set-default FD_ESCS_EN 0
|
||||
@@ -1,110 +0,0 @@
|
||||
#!/bin/sh
|
||||
#
|
||||
# @name Standard VTOL
|
||||
#
|
||||
# @type Standard VTOL
|
||||
#
|
||||
|
||||
. ${R}etc/init.d/rc.vtol_defaults
|
||||
|
||||
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
|
||||
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
|
||||
PX4_SIM_MODEL=${PX4_SIM_MODEL:=foxtech_loong_2160}
|
||||
|
||||
# TODO: Enable motor failure detection when the
|
||||
# VTOL no longer reports 0A for all ESCs in SITL
|
||||
param set-default FD_ACT_EN 0
|
||||
param set-default FD_ACT_MOT_TOUT 500
|
||||
|
||||
param set-default CA_AIRFRAME 2
|
||||
|
||||
param set-default CA_ROTOR_COUNT 5
|
||||
param set-default CA_ROTOR0_PX 0.1515
|
||||
param set-default CA_ROTOR0_PY 0.245
|
||||
param set-default CA_ROTOR0_KM 0.05
|
||||
param set-default CA_ROTOR1_PX -0.1515
|
||||
param set-default CA_ROTOR1_PY -0.1875
|
||||
param set-default CA_ROTOR1_KM 0.05
|
||||
param set-default CA_ROTOR2_PX 0.1515
|
||||
param set-default CA_ROTOR2_PY -0.245
|
||||
param set-default CA_ROTOR2_KM -0.05
|
||||
param set-default CA_ROTOR3_PX -0.1515
|
||||
param set-default CA_ROTOR3_PY 0.1875
|
||||
param set-default CA_ROTOR3_KM -0.05
|
||||
param set-default CA_ROTOR4_AX 1.0
|
||||
param set-default CA_ROTOR4_AZ 0.0
|
||||
param set-default CA_ROTOR4_PX 0.2
|
||||
|
||||
param set-default SIM_GZ_EC_FUNC1 101
|
||||
param set-default SIM_GZ_EC_MIN1 0
|
||||
param set-default SIM_GZ_EC_MAX1 1500
|
||||
param set-default SIM_GZ_EC_FUNC2 102
|
||||
param set-default SIM_GZ_EC_MIN2 0
|
||||
param set-default SIM_GZ_EC_MAX2 1500
|
||||
param set-default SIM_GZ_EC_FUNC3 103
|
||||
param set-default SIM_GZ_EC_MIN3 0
|
||||
param set-default SIM_GZ_EC_MAX3 1500
|
||||
param set-default SIM_GZ_EC_FUNC4 104
|
||||
param set-default SIM_GZ_EC_MIN4 0
|
||||
param set-default SIM_GZ_EC_MAX4 1500
|
||||
|
||||
param set-default SIM_GZ_EC_FUNC5 105
|
||||
param set-default SIM_GZ_EC_MIN5 0
|
||||
param set-default SIM_GZ_EC_MAX5 3500
|
||||
|
||||
param set-default SIM_GZ_SV_FUNC1 201
|
||||
param set-default SIM_GZ_SV_FUNC2 202
|
||||
param set-default SIM_GZ_SV_FUNC3 203
|
||||
|
||||
param set-default COM_RC_IN_MODE 1
|
||||
param set-default ASPD_PRIMARY 1
|
||||
|
||||
param set-default CA_SV_CS_COUNT 3
|
||||
param set-default CA_SV_CS0_TYPE 1
|
||||
param set-default CA_SV_CS0_TRQ_R -0.5
|
||||
param set-default CA_SV_CS1_TYPE 2
|
||||
param set-default CA_SV_CS1_TRQ_R 0.5
|
||||
param set-default CA_SV_CS2_TYPE 3
|
||||
param set-default CA_SV_CS2_TRQ_P 1.0
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
param set-default PWM_MAIN_FUNC3 103
|
||||
param set-default PWM_MAIN_FUNC4 104
|
||||
param set-default PWM_MAIN_FUNC5 105
|
||||
param set-default PWM_MAIN_FUNC6 201
|
||||
param set-default PWM_MAIN_FUNC7 202
|
||||
param set-default PWM_MAIN_FUNC8 203
|
||||
|
||||
param set-default FW_L1_PERIOD 12
|
||||
param set-default FW_PR_FF 0.2
|
||||
param set-default FW_PR_P 0.9
|
||||
param set-default FW_PSP_OFF 2
|
||||
param set-default FW_P_LIM_MIN -15
|
||||
param set-default FW_RR_FF 0.1
|
||||
param set-default FW_RR_P 0.3
|
||||
param set-default FW_THR_TRIM 0.25
|
||||
param set-default FW_THR_MAX 0.6
|
||||
param set-default FW_THR_MIN 0.05
|
||||
param set-default FW_T_CLMB_MAX 8
|
||||
param set-default FW_T_SINK_MAX 2.7
|
||||
param set-default FW_T_SINK_MIN 2.2
|
||||
|
||||
param set-default MC_AIRMODE 1
|
||||
param set-default MC_ROLLRATE_P 0.3
|
||||
param set-default MC_YAW_P 1.6
|
||||
|
||||
param set-default MIS_TAKEOFF_ALT 10
|
||||
|
||||
param set-default MPC_ACC_HOR_MAX 2
|
||||
param set-default MPC_XY_P 0.8
|
||||
param set-default MPC_XY_VEL_P_ACC 3
|
||||
param set-default MPC_XY_VEL_I_ACC 4
|
||||
param set-default MPC_XY_VEL_D_ACC 0.1
|
||||
|
||||
param set-default NAV_ACC_RAD 5
|
||||
|
||||
param set-default VT_FWD_THRUST_EN 4
|
||||
param set-default VT_F_TRANS_THR 0.75
|
||||
param set-default VT_B_TRANS_DUR 8
|
||||
param set-default VT_TYPE 2
|
||||
param set-default FD_ESCS_EN 0
|
||||
@@ -73,8 +73,6 @@ px4_add_romfs_files(
|
||||
4001_gz_x500
|
||||
4002_gz_x500_depth
|
||||
4003_gz_rc_cessna
|
||||
4004_gz_standard_vtol
|
||||
4005_gz_foxtech_loong_2160
|
||||
|
||||
6011_gazebo-classic_typhoon_h480
|
||||
6011_gazebo-classic_typhoon_h480.post
|
||||
|
||||
@@ -49,7 +49,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
|
||||
|
||||
echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
|
||||
|
||||
${gz_command} ${gz_sub_command} --verbose=4 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
|
||||
${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
|
||||
|
||||
if [ -z "${HEADLESS}" ]; then
|
||||
# HEADLESS not set, starting gui
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -1,262 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
|
||||
<asset>
|
||||
<contributor>
|
||||
<author/>
|
||||
<authoring_tool>FBX COLLADA exporter</authoring_tool>
|
||||
<comments/>
|
||||
</contributor>
|
||||
<created>2023-01-04T01:05:20Z</created>
|
||||
<keywords/>
|
||||
<modified>2023-01-04T01:05:20Z</modified>
|
||||
<revision/>
|
||||
<subject/>
|
||||
<title/>
|
||||
<unit meter="0.010000" name="centimeter"/>
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_materials>
|
||||
<material id="FBXASC0481FBXASC032-FBXASC032Default" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<instance_effect url="#FBXASC0481FBXASC032-FBXASC032Default-fx"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_effects>
|
||||
<effect id="FBXASC0481FBXASC032-FBXASC032Default-fx" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<profile_COMMON>
|
||||
<technique sid="standard">
|
||||
<phong>
|
||||
<emission>
|
||||
<color sid="emission">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</emission>
|
||||
<ambient>
|
||||
<color sid="ambient">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</ambient>
|
||||
<diffuse>
|
||||
<color sid="diffuse">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</diffuse>
|
||||
<specular>
|
||||
<color sid="specular">0.486000 0.486000 0.486000 1.000000</color>
|
||||
</specular>
|
||||
<shininess>
|
||||
<float sid="shininess">2.000000</float>
|
||||
</shininess>
|
||||
<reflective>
|
||||
<color sid="reflective">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</reflective>
|
||||
<reflectivity>
|
||||
<float sid="reflectivity">1.000000</float>
|
||||
</reflectivity>
|
||||
<transparent opaque="RGB_ZERO">
|
||||
<color sid="transparent">1.000000 1.000000 1.000000 1.000000</color>
|
||||
</transparent>
|
||||
<transparency>
|
||||
<float sid="transparency">0.000000</float>
|
||||
</transparency>
|
||||
</phong>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_geometries>
|
||||
<geometry id="aileron_left-lib" name="aileron_leftMesh">
|
||||
<mesh>
|
||||
<source id="aileron_left-POSITION">
|
||||
<float_array id="aileron_left-POSITION-array" count="48">
|
||||
-8.508881 -8.085777 -0.718638
|
||||
-8.508881 -8.085783 -0.353647
|
||||
4.939667 -6.844479 -0.718638
|
||||
4.939667 -6.844481 -0.353647
|
||||
-17.070042 -8.085783 -0.353647
|
||||
-17.070042 -8.085777 -0.718638
|
||||
17.070045 -4.887245 -0.718638
|
||||
17.070038 -4.887245 -0.353647
|
||||
-8.508881 0.511266 -0.718638
|
||||
-8.508881 0.511267 0.718638
|
||||
4.939667 0.511266 -0.718638
|
||||
4.939667 0.511266 0.642922
|
||||
-17.070042 0.511266 -0.718638
|
||||
-17.070042 0.511266 0.718212
|
||||
17.070045 0.511266 -0.718638
|
||||
17.070030 0.511267 0.494327
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#aileron_left-POSITION-array" count="16" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="aileron_left-Normal0">
|
||||
<float_array id="aileron_left-Normal0-array" count="252">
|
||||
0.004350 -0.126175 0.991998
|
||||
0.012806 -0.135273 0.990726
|
||||
0.013164 -0.135857 0.990641
|
||||
0.012806 -0.135273 0.990726
|
||||
0.004350 -0.126175 0.991998
|
||||
0.004220 -0.126103 0.992008
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.013164 -0.135857 0.990641
|
||||
0.017606 -0.143110 0.989550
|
||||
0.017606 -0.143110 0.989550
|
||||
0.017606 -0.143110 0.989550
|
||||
0.013164 -0.135857 0.990641
|
||||
0.012806 -0.135273 0.990726
|
||||
0.159290 -0.987232 -0.000001
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.159290 -0.987232 -0.000001
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.159290 -0.987232 -0.000001
|
||||
0.125672 -0.992072 -0.000006
|
||||
-0.000025 -0.123744 0.992314
|
||||
0.004220 -0.126103 0.992008
|
||||
0.004350 -0.126175 0.991998
|
||||
0.004220 -0.126103 0.992008
|
||||
-0.000025 -0.123744 0.992314
|
||||
-0.000025 -0.123744 0.992314
|
||||
0.000000 -1.000000 -0.000016
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.000000 -1.000000 -0.000016
|
||||
0.000000 -1.000000 -0.000016
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#aileron_left-Normal0-array" count="84" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="aileron_left-UV0">
|
||||
<float_array id="aileron_left-UV0-array" count="48">
|
||||
0.250000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 1.000000
|
||||
0.125000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 1.000000
|
||||
0.715060 0.000000
|
||||
0.715060 1.000000
|
||||
0.284940 0.000000
|
||||
0.715060 0.000000
|
||||
0.076671 0.000000
|
||||
0.923329 0.000000
|
||||
0.923329 0.000000
|
||||
0.923329 1.000000
|
||||
0.250000 0.290830
|
||||
0.750000 0.284605
|
||||
0.125000 0.265730
|
||||
0.875000 0.259691
|
||||
0.284940 0.290830
|
||||
0.715060 0.284605
|
||||
0.078795 0.225632
|
||||
0.921382 0.233592
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#aileron_left-UV0-array" count="24" stride="2">
|
||||
<param name="S" type="float"/>
|
||||
<param name="T" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="aileron_left-VERTEX">
|
||||
<input semantic="POSITION" source="#aileron_left-POSITION"/>
|
||||
</vertices>
|
||||
<triangles count="28" material="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<input semantic="VERTEX" offset="0" source="#aileron_left-VERTEX"/>
|
||||
<input semantic="NORMAL" offset="1" source="#aileron_left-Normal0"/>
|
||||
<input semantic="TEXCOORD" offset="2" set="0" source="#aileron_left-UV0"/>
|
||||
<p> 9 0 17 3 1 6 11 2 19 3 3 6 9 4 17 1 5 2 1 6 3 2 7 5 3 8 7 2 9 5 1 10 3 0 11 1 11 12 19 7 13 14 15 14 23 7 15 14 11 16 19 3 17 6 7 18 15 2 19 5 6 20 13 2 21 5 7 22 15 3 23 7 13 24 21 1 25 2 9 26 17 1 27 2 13 28 21 4 29 8 4 30 9 0 31 1 1 32 3 0 33 1 4 34 9 5 35 11 2 36 4 0 37 0 8 38 16 2 39 4 8 40 16 10 41 18 6 42 12 2 43 4 10 44 18 6 45 12 10 46 18 14 47 22 5 48 10 12 49 20 0 50 0 0 51 0 12 52 20 8 53 16 4 54 4 13 55 13 12 56 12 4 57 4 12 58 12 5 59 5 13 60 13 8 61 8 12 62 12 8 63 8 13 64 13 9 65 9 11 66 11 8 67 8 9 68 9 8 69 8 11 70 11 10 71 10 15 72 15 10 73 10 11 74 11 10 75 10 15 76 15 14 77 14 7 78 7 14 79 14 15 80 15 14 81 14 7 82 7 6 83 6</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="aileron_left" name="aileron_left">
|
||||
<node name="aileron_left" id="aileron_left" sid="aileron_left">
|
||||
<matrix sid="matrix">-1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 -0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix>
|
||||
<instance_geometry url="#aileron_left-lib">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="FBXASC0481FBXASC032-FBXASC032Default" target="#FBXASC0481FBXASC032-FBXASC032Default"/>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
<extra>
|
||||
<technique profile="FCOLLADA">
|
||||
<visibility>1.000000</visibility>
|
||||
</technique>
|
||||
</extra>
|
||||
</node>
|
||||
<extra>
|
||||
<technique profile="MAX3D">
|
||||
<frame_rate>30.000000</frame_rate>
|
||||
</technique>
|
||||
<technique profile="FCOLLADA">
|
||||
<start_time>0.000000</start_time>
|
||||
<end_time>3.333333</end_time>
|
||||
</technique>
|
||||
</extra>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#aileron_left"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
@@ -1,262 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
|
||||
<asset>
|
||||
<contributor>
|
||||
<author/>
|
||||
<authoring_tool>FBX COLLADA exporter</authoring_tool>
|
||||
<comments/>
|
||||
</contributor>
|
||||
<created>2023-01-04T01:05:21Z</created>
|
||||
<keywords/>
|
||||
<modified>2023-01-04T01:05:21Z</modified>
|
||||
<revision/>
|
||||
<subject/>
|
||||
<title/>
|
||||
<unit meter="0.010000" name="centimeter"/>
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_materials>
|
||||
<material id="FBXASC0481FBXASC032-FBXASC032Default" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<instance_effect url="#FBXASC0481FBXASC032-FBXASC032Default-fx"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_effects>
|
||||
<effect id="FBXASC0481FBXASC032-FBXASC032Default-fx" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<profile_COMMON>
|
||||
<technique sid="standard">
|
||||
<phong>
|
||||
<emission>
|
||||
<color sid="emission">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</emission>
|
||||
<ambient>
|
||||
<color sid="ambient">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</ambient>
|
||||
<diffuse>
|
||||
<color sid="diffuse">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</diffuse>
|
||||
<specular>
|
||||
<color sid="specular">0.486000 0.486000 0.486000 1.000000</color>
|
||||
</specular>
|
||||
<shininess>
|
||||
<float sid="shininess">2.000000</float>
|
||||
</shininess>
|
||||
<reflective>
|
||||
<color sid="reflective">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</reflective>
|
||||
<reflectivity>
|
||||
<float sid="reflectivity">1.000000</float>
|
||||
</reflectivity>
|
||||
<transparent opaque="RGB_ZERO">
|
||||
<color sid="transparent">1.000000 1.000000 1.000000 1.000000</color>
|
||||
</transparent>
|
||||
<transparency>
|
||||
<float sid="transparency">0.000000</float>
|
||||
</transparency>
|
||||
</phong>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_geometries>
|
||||
<geometry id="aileron_right-lib" name="aileron_rightMesh">
|
||||
<mesh>
|
||||
<source id="aileron_right-POSITION">
|
||||
<float_array id="aileron_right-POSITION-array" count="48">
|
||||
-8.508881 -8.085777 -0.718638
|
||||
-8.508881 -8.085783 -0.353647
|
||||
4.939667 -6.844479 -0.718638
|
||||
4.939667 -6.844481 -0.353647
|
||||
-17.070042 -8.085783 -0.353647
|
||||
-17.070042 -8.085777 -0.718638
|
||||
17.070045 -4.887245 -0.718638
|
||||
17.070038 -4.887245 -0.353647
|
||||
-8.508881 0.511266 -0.718638
|
||||
-8.508881 0.511267 0.718638
|
||||
4.939667 0.511266 -0.718638
|
||||
4.939667 0.511266 0.642922
|
||||
-17.070042 0.511266 -0.718638
|
||||
-17.070042 0.511266 0.718212
|
||||
17.070045 0.511266 -0.718638
|
||||
17.070030 0.511267 0.494327
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#aileron_right-POSITION-array" count="16" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="aileron_right-Normal0">
|
||||
<float_array id="aileron_right-Normal0-array" count="252">
|
||||
0.004350 -0.126175 0.991998
|
||||
0.012806 -0.135273 0.990726
|
||||
0.013164 -0.135857 0.990641
|
||||
0.012806 -0.135273 0.990726
|
||||
0.004350 -0.126175 0.991998
|
||||
0.004220 -0.126103 0.992008
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.013164 -0.135857 0.990641
|
||||
0.017606 -0.143110 0.989550
|
||||
0.017606 -0.143110 0.989550
|
||||
0.017606 -0.143110 0.989550
|
||||
0.013164 -0.135857 0.990641
|
||||
0.012806 -0.135273 0.990726
|
||||
0.159290 -0.987232 -0.000001
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.159290 -0.987232 -0.000001
|
||||
0.125672 -0.992072 -0.000006
|
||||
0.159290 -0.987232 -0.000001
|
||||
0.125672 -0.992072 -0.000006
|
||||
-0.000025 -0.123744 0.992314
|
||||
0.004220 -0.126103 0.992008
|
||||
0.004350 -0.126175 0.991998
|
||||
0.004220 -0.126103 0.992008
|
||||
-0.000025 -0.123744 0.992314
|
||||
-0.000025 -0.123744 0.992314
|
||||
0.000000 -1.000000 -0.000016
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.046003 -0.998941 -0.000013
|
||||
0.000000 -1.000000 -0.000016
|
||||
0.000000 -1.000000 -0.000016
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
-0.000000 1.000000 -0.000000
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
1.000000 -0.000000 0.000015
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#aileron_right-Normal0-array" count="84" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="aileron_right-UV0">
|
||||
<float_array id="aileron_right-UV0-array" count="48">
|
||||
0.250000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 1.000000
|
||||
0.125000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 1.000000
|
||||
0.715060 0.000000
|
||||
0.715060 1.000000
|
||||
0.284940 0.000000
|
||||
0.715060 0.000000
|
||||
0.076671 0.000000
|
||||
0.923329 0.000000
|
||||
0.923329 0.000000
|
||||
0.923329 1.000000
|
||||
0.250000 0.290830
|
||||
0.750000 0.284605
|
||||
0.125000 0.265730
|
||||
0.875000 0.259691
|
||||
0.284940 0.290830
|
||||
0.715060 0.284605
|
||||
0.078795 0.225632
|
||||
0.921382 0.233592
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#aileron_right-UV0-array" count="24" stride="2">
|
||||
<param name="S" type="float"/>
|
||||
<param name="T" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="aileron_right-VERTEX">
|
||||
<input semantic="POSITION" source="#aileron_right-POSITION"/>
|
||||
</vertices>
|
||||
<triangles count="28" material="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<input semantic="VERTEX" offset="0" source="#aileron_right-VERTEX"/>
|
||||
<input semantic="NORMAL" offset="1" source="#aileron_right-Normal0"/>
|
||||
<input semantic="TEXCOORD" offset="2" set="0" source="#aileron_right-UV0"/>
|
||||
<p> 9 0 17 3 1 6 11 2 19 3 3 6 9 4 17 1 5 2 1 6 3 2 7 5 3 8 7 2 9 5 1 10 3 0 11 1 11 12 19 7 13 14 15 14 23 7 15 14 11 16 19 3 17 6 7 18 15 2 19 5 6 20 13 2 21 5 7 22 15 3 23 7 13 24 21 1 25 2 9 26 17 1 27 2 13 28 21 4 29 8 4 30 9 0 31 1 1 32 3 0 33 1 4 34 9 5 35 11 2 36 4 0 37 0 8 38 16 2 39 4 8 40 16 10 41 18 6 42 12 2 43 4 10 44 18 6 45 12 10 46 18 14 47 22 5 48 10 12 49 20 0 50 0 0 51 0 12 52 20 8 53 16 4 54 4 13 55 13 12 56 12 4 57 4 12 58 12 5 59 5 13 60 13 8 61 8 12 62 12 8 63 8 13 64 13 9 65 9 11 66 11 8 67 8 9 68 9 8 69 8 11 70 11 10 71 10 15 72 15 10 73 10 11 74 11 10 75 10 15 76 15 14 77 14 7 78 7 14 79 14 15 80 15 14 81 14 7 82 7 6 83 6</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="aileron_right" name="aileron_right">
|
||||
<node name="aileron_right" id="aileron_right" sid="aileron_right">
|
||||
<matrix sid="matrix">1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix>
|
||||
<instance_geometry url="#aileron_right-lib">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="FBXASC0481FBXASC032-FBXASC032Default" target="#FBXASC0481FBXASC032-FBXASC032Default"/>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
<extra>
|
||||
<technique profile="FCOLLADA">
|
||||
<visibility>1.000000</visibility>
|
||||
</technique>
|
||||
</extra>
|
||||
</node>
|
||||
<extra>
|
||||
<technique profile="MAX3D">
|
||||
<frame_rate>30.000000</frame_rate>
|
||||
</technique>
|
||||
<technique profile="FCOLLADA">
|
||||
<start_time>0.000000</start_time>
|
||||
<end_time>3.333333</end_time>
|
||||
</technique>
|
||||
</extra>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#aileron_right"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
@@ -1,262 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
|
||||
<asset>
|
||||
<contributor>
|
||||
<author/>
|
||||
<authoring_tool>FBX COLLADA exporter</authoring_tool>
|
||||
<comments/>
|
||||
</contributor>
|
||||
<created>2023-01-04T01:05:23Z</created>
|
||||
<keywords/>
|
||||
<modified>2023-01-04T01:05:23Z</modified>
|
||||
<revision/>
|
||||
<subject/>
|
||||
<title/>
|
||||
<unit meter="0.010000" name="centimeter"/>
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_materials>
|
||||
<material id="FBXASC0481FBXASC032-FBXASC032Default" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<instance_effect url="#FBXASC0481FBXASC032-FBXASC032Default-fx"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_effects>
|
||||
<effect id="FBXASC0481FBXASC032-FBXASC032Default-fx" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<profile_COMMON>
|
||||
<technique sid="standard">
|
||||
<phong>
|
||||
<emission>
|
||||
<color sid="emission">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</emission>
|
||||
<ambient>
|
||||
<color sid="ambient">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</ambient>
|
||||
<diffuse>
|
||||
<color sid="diffuse">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</diffuse>
|
||||
<specular>
|
||||
<color sid="specular">0.486000 0.486000 0.486000 1.000000</color>
|
||||
</specular>
|
||||
<shininess>
|
||||
<float sid="shininess">2.000000</float>
|
||||
</shininess>
|
||||
<reflective>
|
||||
<color sid="reflective">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</reflective>
|
||||
<reflectivity>
|
||||
<float sid="reflectivity">1.000000</float>
|
||||
</reflectivity>
|
||||
<transparent opaque="RGB_ZERO">
|
||||
<color sid="transparent">1.000000 1.000000 1.000000 1.000000</color>
|
||||
</transparent>
|
||||
<transparency>
|
||||
<float sid="transparency">0.000000</float>
|
||||
</transparency>
|
||||
</phong>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_geometries>
|
||||
<geometry id="elevator_left-lib" name="elevator_leftMesh">
|
||||
<mesh>
|
||||
<source id="elevator_left-POSITION">
|
||||
<float_array id="elevator_left-POSITION-array" count="48">
|
||||
18.025543 -15.576103 -0.524075
|
||||
18.025543 -15.576107 -0.159084
|
||||
31.474091 -14.334805 -0.524075
|
||||
31.474091 -14.334805 -0.159084
|
||||
16.329094 0.045925 -0.524075
|
||||
15.810318 0.045925 0.524076
|
||||
29.862923 0.045921 -0.524075
|
||||
29.375877 0.045925 0.487995
|
||||
-35.404789 -15.576107 -0.159084
|
||||
-35.404789 -15.576103 -0.524075
|
||||
-35.404789 0.045925 -0.524075
|
||||
-35.404789 0.045925 0.523587
|
||||
35.406570 -13.420116 -0.524075
|
||||
35.406570 -13.394806 -0.159084
|
||||
35.406570 0.045921 -0.524075
|
||||
35.406570 0.045925 0.445975
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#elevator_left-POSITION-array" count="16" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="elevator_left-Normal0">
|
||||
<float_array id="elevator_left-Normal0-array" count="252">
|
||||
0.005639 -0.044047 0.999014
|
||||
0.001521 -0.043737 0.999042
|
||||
0.001776 -0.043748 0.999041
|
||||
0.005639 -0.044047 0.999014
|
||||
0.001776 -0.043748 0.999041
|
||||
0.005840 -0.044067 0.999012
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.161065 -0.986799 0.016905
|
||||
0.161065 -0.986799 0.016905
|
||||
0.161065 -0.986799 0.016905
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.005639 -0.044047 0.999014
|
||||
0.008395 -0.044327 0.998982
|
||||
0.008395 -0.044327 0.998982
|
||||
0.008395 -0.044327 0.998982
|
||||
0.005639 -0.044047 0.999014
|
||||
0.005840 -0.044067 0.999012
|
||||
0.229390 -0.972750 0.033728
|
||||
0.161065 -0.986799 0.016905
|
||||
0.229390 -0.972750 0.033728
|
||||
0.161065 -0.986799 0.016905
|
||||
0.229390 -0.972750 0.033728
|
||||
0.161065 -0.986799 0.016905
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.001521 -0.043737 0.999042
|
||||
-0.000005 -0.043674 0.999046
|
||||
-0.000005 -0.043674 0.999046
|
||||
0.001521 -0.043737 0.999042
|
||||
-0.000005 -0.043674 0.999046
|
||||
0.001776 -0.043748 0.999041
|
||||
0.000000 -1.000000 -0.000010
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.000000 -1.000000 -0.000010
|
||||
0.000000 -1.000000 -0.000010
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
-0.000000 1.000000 -0.000004
|
||||
0.000000 1.000000 -0.000003
|
||||
-0.000000 1.000000 -0.000004
|
||||
0.000000 1.000000 -0.000003
|
||||
-0.000000 1.000000 -0.000004
|
||||
0.000000 1.000000 -0.000003
|
||||
0.000000 1.000000 -0.000003
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 -0.000003
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 -0.000003
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 0.000000
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 0.000000
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#elevator_left-Normal0-array" count="84" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="elevator_left-UV0">
|
||||
<float_array id="elevator_left-UV0-array" count="48">
|
||||
0.250000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 1.000000
|
||||
0.125000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 1.000000
|
||||
0.250000 0.247131
|
||||
0.750000 0.172769
|
||||
0.125000 0.234707
|
||||
0.875000 0.160663
|
||||
0.556720 0.000000
|
||||
0.556720 1.000000
|
||||
0.443280 0.000000
|
||||
0.556720 0.000000
|
||||
0.441864 0.247131
|
||||
0.558584 0.173020
|
||||
0.897586 0.000000
|
||||
0.102414 0.000000
|
||||
0.898211 0.000000
|
||||
0.898211 1.000000
|
||||
0.095974 0.232439
|
||||
0.905697 0.154750
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#elevator_left-UV0-array" count="24" stride="2">
|
||||
<param name="S" type="float"/>
|
||||
<param name="T" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="elevator_left-VERTEX">
|
||||
<input semantic="POSITION" source="#elevator_left-POSITION"/>
|
||||
</vertices>
|
||||
<triangles count="28" material="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<input semantic="VERTEX" offset="0" source="#elevator_left-VERTEX"/>
|
||||
<input semantic="NORMAL" offset="1" source="#elevator_left-Normal0"/>
|
||||
<input semantic="TEXCOORD" offset="2" set="0" source="#elevator_left-UV0"/>
|
||||
<p> 7 0 11 5 1 9 1 2 2 7 3 11 1 4 2 3 5 6 1 6 3 2 7 5 3 8 7 2 9 5 1 10 3 0 11 1 7 12 11 13 13 20 15 14 23 13 15 20 7 16 11 3 17 6 13 18 21 2 19 5 12 20 18 2 21 5 13 22 21 3 23 7 9 24 14 4 25 8 0 26 0 4 27 8 9 28 14 10 29 16 0 30 0 6 31 10 2 32 4 6 33 10 0 34 0 4 35 8 5 36 9 11 37 17 8 38 12 5 39 9 8 40 12 1 41 2 8 42 13 0 43 1 1 44 3 0 45 1 8 46 13 9 47 15 2 48 4 6 49 10 12 50 19 12 51 19 6 52 10 14 53 22 12 54 12 14 55 14 13 56 13 15 57 15 13 58 13 14 59 14 14 60 14 6 61 6 15 62 15 7 63 7 15 64 15 6 65 6 6 66 6 4 67 4 7 68 7 5 69 5 7 70 7 4 71 4 4 72 4 10 73 10 5 74 5 11 75 11 5 76 5 10 77 10 10 78 10 8 79 8 11 80 11 8 81 8 10 82 10 9 83 9</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="elevator_left" name="elevator_left">
|
||||
<node name="elevator_left" id="elevator_left" sid="elevator_left">
|
||||
<matrix sid="matrix">-0.280290 0.000000 0.000000 0.000000 0.000000 0.280290 -0.000000 0.000000 0.000000 0.000000 0.477438 0.000000 0.000000 0.000000 0.000000 1.000000</matrix>
|
||||
<instance_geometry url="#elevator_left-lib">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="FBXASC0481FBXASC032-FBXASC032Default" target="#FBXASC0481FBXASC032-FBXASC032Default"/>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
<extra>
|
||||
<technique profile="FCOLLADA">
|
||||
<visibility>1.000000</visibility>
|
||||
</technique>
|
||||
</extra>
|
||||
</node>
|
||||
<extra>
|
||||
<technique profile="MAX3D">
|
||||
<frame_rate>30.000000</frame_rate>
|
||||
</technique>
|
||||
<technique profile="FCOLLADA">
|
||||
<start_time>0.000000</start_time>
|
||||
<end_time>3.333333</end_time>
|
||||
</technique>
|
||||
</extra>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#elevator_left"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
@@ -1,262 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
|
||||
<asset>
|
||||
<contributor>
|
||||
<author/>
|
||||
<authoring_tool>FBX COLLADA exporter</authoring_tool>
|
||||
<comments/>
|
||||
</contributor>
|
||||
<created>2023-01-04T01:05:24Z</created>
|
||||
<keywords/>
|
||||
<modified>2023-01-04T01:05:24Z</modified>
|
||||
<revision/>
|
||||
<subject/>
|
||||
<title/>
|
||||
<unit meter="0.010000" name="centimeter"/>
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_materials>
|
||||
<material id="FBXASC0481FBXASC032-FBXASC032Default" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<instance_effect url="#FBXASC0481FBXASC032-FBXASC032Default-fx"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_effects>
|
||||
<effect id="FBXASC0481FBXASC032-FBXASC032Default-fx" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<profile_COMMON>
|
||||
<technique sid="standard">
|
||||
<phong>
|
||||
<emission>
|
||||
<color sid="emission">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</emission>
|
||||
<ambient>
|
||||
<color sid="ambient">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</ambient>
|
||||
<diffuse>
|
||||
<color sid="diffuse">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</diffuse>
|
||||
<specular>
|
||||
<color sid="specular">0.486000 0.486000 0.486000 1.000000</color>
|
||||
</specular>
|
||||
<shininess>
|
||||
<float sid="shininess">2.000000</float>
|
||||
</shininess>
|
||||
<reflective>
|
||||
<color sid="reflective">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</reflective>
|
||||
<reflectivity>
|
||||
<float sid="reflectivity">1.000000</float>
|
||||
</reflectivity>
|
||||
<transparent opaque="RGB_ZERO">
|
||||
<color sid="transparent">1.000000 1.000000 1.000000 1.000000</color>
|
||||
</transparent>
|
||||
<transparency>
|
||||
<float sid="transparency">0.000000</float>
|
||||
</transparency>
|
||||
</phong>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_geometries>
|
||||
<geometry id="elevator_right-lib" name="elevator_rightMesh">
|
||||
<mesh>
|
||||
<source id="elevator_right-POSITION">
|
||||
<float_array id="elevator_right-POSITION-array" count="48">
|
||||
18.025543 -15.576103 -0.524075
|
||||
18.025543 -15.576107 -0.159084
|
||||
31.474091 -14.334805 -0.524075
|
||||
31.474091 -14.334805 -0.159084
|
||||
16.329094 0.045925 -0.524075
|
||||
15.810318 0.045925 0.524076
|
||||
29.862923 0.045921 -0.524075
|
||||
29.375877 0.045925 0.487995
|
||||
-35.404789 -15.576107 -0.159084
|
||||
-35.404789 -15.576103 -0.524075
|
||||
-35.404789 0.045925 -0.524075
|
||||
-35.404789 0.045925 0.523587
|
||||
35.406570 -13.420116 -0.524075
|
||||
35.406570 -13.394806 -0.159084
|
||||
35.406570 0.045921 -0.524075
|
||||
35.406570 0.045925 0.445975
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#elevator_right-POSITION-array" count="16" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="elevator_right-Normal0">
|
||||
<float_array id="elevator_right-Normal0-array" count="252">
|
||||
0.005639 -0.044047 0.999014
|
||||
0.001521 -0.043737 0.999042
|
||||
0.001776 -0.043748 0.999041
|
||||
0.005639 -0.044047 0.999014
|
||||
0.001776 -0.043748 0.999041
|
||||
0.005840 -0.044067 0.999012
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.161065 -0.986799 0.016905
|
||||
0.161065 -0.986799 0.016905
|
||||
0.161065 -0.986799 0.016905
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.005639 -0.044047 0.999014
|
||||
0.008395 -0.044327 0.998982
|
||||
0.008395 -0.044327 0.998982
|
||||
0.008395 -0.044327 0.998982
|
||||
0.005639 -0.044047 0.999014
|
||||
0.005840 -0.044067 0.999012
|
||||
0.229390 -0.972750 0.033728
|
||||
0.161065 -0.986799 0.016905
|
||||
0.229390 -0.972750 0.033728
|
||||
0.161065 -0.986799 0.016905
|
||||
0.229390 -0.972750 0.033728
|
||||
0.161065 -0.986799 0.016905
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.001521 -0.043737 0.999042
|
||||
-0.000005 -0.043674 0.999046
|
||||
-0.000005 -0.043674 0.999046
|
||||
0.001521 -0.043737 0.999042
|
||||
-0.000005 -0.043674 0.999046
|
||||
0.001776 -0.043748 0.999041
|
||||
0.000000 -1.000000 -0.000010
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.046003 -0.998941 -0.000008
|
||||
0.000000 -1.000000 -0.000010
|
||||
0.000000 -1.000000 -0.000010
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
0.000000 0.000000 -1.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
1.000000 0.000000 0.000000
|
||||
-0.000000 1.000000 -0.000004
|
||||
0.000000 1.000000 -0.000003
|
||||
-0.000000 1.000000 -0.000004
|
||||
0.000000 1.000000 -0.000003
|
||||
-0.000000 1.000000 -0.000004
|
||||
0.000000 1.000000 -0.000003
|
||||
0.000000 1.000000 -0.000003
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 -0.000003
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 -0.000003
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 0.000000
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 0.000000
|
||||
0.000000 1.000000 -0.000001
|
||||
0.000000 1.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
-1.000000 0.000000 0.000000
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#elevator_right-Normal0-array" count="84" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="elevator_right-UV0">
|
||||
<float_array id="elevator_right-UV0-array" count="48">
|
||||
0.250000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 1.000000
|
||||
0.125000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 1.000000
|
||||
0.250000 0.247131
|
||||
0.750000 0.172769
|
||||
0.125000 0.234707
|
||||
0.875000 0.160663
|
||||
0.556720 0.000000
|
||||
0.556720 1.000000
|
||||
0.443280 0.000000
|
||||
0.556720 0.000000
|
||||
0.441864 0.247131
|
||||
0.558584 0.173020
|
||||
0.897586 0.000000
|
||||
0.102414 0.000000
|
||||
0.898211 0.000000
|
||||
0.898211 1.000000
|
||||
0.095974 0.232439
|
||||
0.905697 0.154750
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#elevator_right-UV0-array" count="24" stride="2">
|
||||
<param name="S" type="float"/>
|
||||
<param name="T" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="elevator_right-VERTEX">
|
||||
<input semantic="POSITION" source="#elevator_right-POSITION"/>
|
||||
</vertices>
|
||||
<triangles count="28" material="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<input semantic="VERTEX" offset="0" source="#elevator_right-VERTEX"/>
|
||||
<input semantic="NORMAL" offset="1" source="#elevator_right-Normal0"/>
|
||||
<input semantic="TEXCOORD" offset="2" set="0" source="#elevator_right-UV0"/>
|
||||
<p> 7 0 11 5 1 9 1 2 2 7 3 11 1 4 2 3 5 6 1 6 3 2 7 5 3 8 7 2 9 5 1 10 3 0 11 1 7 12 11 13 13 20 15 14 23 13 15 20 7 16 11 3 17 6 13 18 21 2 19 5 12 20 18 2 21 5 13 22 21 3 23 7 9 24 14 4 25 8 0 26 0 4 27 8 9 28 14 10 29 16 0 30 0 6 31 10 2 32 4 6 33 10 0 34 0 4 35 8 5 36 9 11 37 17 8 38 12 5 39 9 8 40 12 1 41 2 8 42 13 0 43 1 1 44 3 0 45 1 8 46 13 9 47 15 2 48 4 6 49 10 12 50 19 12 51 19 6 52 10 14 53 22 12 54 12 14 55 14 13 56 13 15 57 15 13 58 13 14 59 14 14 60 14 6 61 6 15 62 15 7 63 7 15 64 15 6 65 6 6 66 6 4 67 4 7 68 7 5 69 5 7 70 7 4 71 4 4 72 4 10 73 10 5 74 5 11 75 11 5 76 5 10 77 10 10 78 10 8 79 8 11 80 11 8 81 8 10 82 10 9 83 9</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="elevator_right" name="elevator_right">
|
||||
<node name="elevator_right" id="elevator_right" sid="elevator_right">
|
||||
<matrix sid="matrix">0.280290 0.000000 0.000000 0.000000 0.000000 0.280290 0.000000 0.000000 0.000000 0.000000 0.477438 0.000000 0.000000 0.000000 0.000000 1.000000</matrix>
|
||||
<instance_geometry url="#elevator_right-lib">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="FBXASC0481FBXASC032-FBXASC032Default" target="#FBXASC0481FBXASC032-FBXASC032Default"/>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
<extra>
|
||||
<technique profile="FCOLLADA">
|
||||
<visibility>1.000000</visibility>
|
||||
</technique>
|
||||
</extra>
|
||||
</node>
|
||||
<extra>
|
||||
<technique profile="MAX3D">
|
||||
<frame_rate>30.000000</frame_rate>
|
||||
</technique>
|
||||
<technique profile="FCOLLADA">
|
||||
<start_time>0.000000</start_time>
|
||||
<end_time>3.333333</end_time>
|
||||
</technique>
|
||||
</extra>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#elevator_right"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -1,308 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
|
||||
<asset>
|
||||
<contributor>
|
||||
<author/>
|
||||
<authoring_tool>FBX COLLADA exporter</authoring_tool>
|
||||
<comments/>
|
||||
</contributor>
|
||||
<created>2023-01-04T01:05:22Z</created>
|
||||
<keywords/>
|
||||
<modified>2023-01-04T01:05:22Z</modified>
|
||||
<revision/>
|
||||
<subject/>
|
||||
<title/>
|
||||
<unit meter="0.010000" name="centimeter"/>
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_materials>
|
||||
<material id="FBXASC0481FBXASC032-FBXASC032Default" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<instance_effect url="#FBXASC0481FBXASC032-FBXASC032Default-fx"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_effects>
|
||||
<effect id="FBXASC0481FBXASC032-FBXASC032Default-fx" name="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<profile_COMMON>
|
||||
<technique sid="standard">
|
||||
<phong>
|
||||
<emission>
|
||||
<color sid="emission">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</emission>
|
||||
<ambient>
|
||||
<color sid="ambient">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</ambient>
|
||||
<diffuse>
|
||||
<color sid="diffuse">0.588235 0.588235 0.588235 1.000000</color>
|
||||
</diffuse>
|
||||
<specular>
|
||||
<color sid="specular">0.486000 0.486000 0.486000 1.000000</color>
|
||||
</specular>
|
||||
<shininess>
|
||||
<float sid="shininess">2.000000</float>
|
||||
</shininess>
|
||||
<reflective>
|
||||
<color sid="reflective">0.000000 0.000000 0.000000 1.000000</color>
|
||||
</reflective>
|
||||
<reflectivity>
|
||||
<float sid="reflectivity">1.000000</float>
|
||||
</reflectivity>
|
||||
<transparent opaque="RGB_ZERO">
|
||||
<color sid="transparent">1.000000 1.000000 1.000000 1.000000</color>
|
||||
</transparent>
|
||||
<transparency>
|
||||
<float sid="transparency">0.000000</float>
|
||||
</transparency>
|
||||
</phong>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_geometries>
|
||||
<geometry id="Object004-lib" name="Object004Mesh">
|
||||
<mesh>
|
||||
<source id="Object004-POSITION">
|
||||
<float_array id="Object004-POSITION-array" count="66">
|
||||
17.816429 -14.408081 0.000001
|
||||
17.816437 -14.408085 0.364992
|
||||
31.264969 -14.233356 0.000001
|
||||
31.264977 -14.233356 0.364992
|
||||
31.264977 -14.233334 0.364993
|
||||
17.816437 -14.408085 -0.364990
|
||||
31.264977 -14.233356 -0.364990
|
||||
31.264977 -14.233334 -0.364991
|
||||
35.158424 -13.976414 0.000001
|
||||
35.158417 -13.975285 0.364992
|
||||
35.158424 -13.975285 -0.364990
|
||||
-35.158421 -14.408077 0.364992
|
||||
-35.158421 -14.408077 0.000001
|
||||
-35.158419 -14.408077 -0.364990
|
||||
14.887794 1.107790 1.268170
|
||||
28.354027 1.107794 1.262715
|
||||
14.887794 1.107794 -1.268169
|
||||
28.354027 1.107794 -1.262714
|
||||
35.158417 1.107790 1.254354
|
||||
35.158440 1.107790 -1.254353
|
||||
-35.158417 1.107792 1.267323
|
||||
-35.158417 1.107792 -1.267322
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#Object004-POSITION-array" count="22" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="Object004-Normal0">
|
||||
<float_array id="Object004-Normal0-array" count="342">
|
||||
0.000320 -0.058124 0.998309
|
||||
0.001385 -0.058292 0.998299
|
||||
0.001289 -0.058275 0.998300
|
||||
0.001385 -0.058292 0.998299
|
||||
0.000320 -0.058124 0.998309
|
||||
0.001364 -0.058288 0.998299
|
||||
0.001289 -0.058275 0.998300
|
||||
0.000251 -0.058116 0.998310
|
||||
0.000320 -0.058124 0.998309
|
||||
0.006496 -0.999979 -0.000005
|
||||
0.039506 -0.999219 0.000000
|
||||
0.039507 -0.999219 0.000769
|
||||
0.039506 -0.999219 0.000000
|
||||
0.006496 -0.999979 -0.000005
|
||||
0.006496 -0.999979 -0.000000
|
||||
0.065994 -0.997819 0.001543
|
||||
0.039506 -0.999219 0.000000
|
||||
0.065994 -0.997820 0.000000
|
||||
0.039506 -0.999219 0.000000
|
||||
0.065994 -0.997819 0.001543
|
||||
0.039507 -0.999219 0.000769
|
||||
0.001385 -0.058292 -0.998299
|
||||
0.000320 -0.058124 -0.998309
|
||||
0.001289 -0.058275 -0.998300
|
||||
0.000320 -0.058124 -0.998309
|
||||
0.000251 -0.058116 -0.998310
|
||||
0.001289 -0.058275 -0.998300
|
||||
0.001364 -0.058288 -0.998299
|
||||
0.000320 -0.058124 -0.998309
|
||||
0.001385 -0.058292 -0.998299
|
||||
0.039506 -0.999219 0.000000
|
||||
0.006496 -0.999979 -0.000000
|
||||
0.006496 -0.999979 0.000005
|
||||
0.039506 -0.999219 0.000000
|
||||
0.006496 -0.999979 0.000005
|
||||
0.039507 -0.999219 -0.000769
|
||||
0.065994 -0.997820 0.000000
|
||||
0.039506 -0.999219 0.000000
|
||||
0.039507 -0.999219 -0.000769
|
||||
0.065994 -0.997820 0.000000
|
||||
0.039507 -0.999219 -0.000769
|
||||
0.065994 -0.997819 -0.001543
|
||||
0.001385 -0.058292 -0.998299
|
||||
0.002190 -0.058429 -0.998289
|
||||
0.002190 -0.058429 -0.998289
|
||||
0.002190 -0.058429 -0.998289
|
||||
0.001364 -0.058288 -0.998299
|
||||
0.001385 -0.058292 -0.998299
|
||||
0.002190 -0.058429 -0.998289
|
||||
0.001385 -0.058292 -0.998299
|
||||
0.001289 -0.058275 -0.998300
|
||||
0.000251 -0.058116 0.998310
|
||||
-0.000008 -0.058085 0.998312
|
||||
-0.000008 -0.058085 0.998312
|
||||
0.000251 -0.058116 0.998310
|
||||
-0.000008 -0.058085 0.998312
|
||||
0.000320 -0.058124 0.998309
|
||||
-0.000000 -1.000000 -0.000005
|
||||
0.006496 -0.999979 -0.000000
|
||||
0.006496 -0.999979 -0.000005
|
||||
0.006496 -0.999979 -0.000000
|
||||
-0.000000 -1.000000 -0.000005
|
||||
-0.000000 -1.000000 -0.000000
|
||||
0.006496 -0.999979 -0.000000
|
||||
-0.000000 -1.000000 0.000005
|
||||
0.006496 -0.999979 0.000005
|
||||
-0.000000 -1.000000 0.000005
|
||||
0.006496 -0.999979 -0.000000
|
||||
-0.000000 -1.000000 -0.000000
|
||||
0.002190 -0.058429 0.998289
|
||||
0.001385 -0.058292 0.998299
|
||||
0.002190 -0.058429 0.998289
|
||||
0.001364 -0.058288 0.998299
|
||||
0.002190 -0.058429 0.998289
|
||||
0.001385 -0.058292 0.998299
|
||||
0.001385 -0.058292 0.998299
|
||||
0.002190 -0.058429 0.998289
|
||||
0.001289 -0.058275 0.998300
|
||||
-0.000008 -0.058085 -0.998312
|
||||
0.000251 -0.058116 -0.998310
|
||||
0.000320 -0.058124 -0.998309
|
||||
0.000251 -0.058116 -0.998310
|
||||
-0.000008 -0.058085 -0.998312
|
||||
-0.000008 -0.058085 -0.998312
|
||||
1.000000 -0.000001 0.000009
|
||||
1.000000 -0.000001 0.000009
|
||||
1.000000 -0.000001 0.000009
|
||||
1.000000 -0.000001 0.000009
|
||||
1.000000 -0.000001 0.000009
|
||||
1.000000 -0.000001 0.000009
|
||||
0.000001 1.000000 0.000000
|
||||
0.000000 1.000000 0.000000
|
||||
0.000001 1.000000 0.000000
|
||||
0.000000 1.000000 0.000000
|
||||
0.000001 1.000000 0.000000
|
||||
0.000000 1.000000 0.000000
|
||||
0.000000 1.000000 0.000000
|
||||
-0.000000 1.000000 0.000001
|
||||
0.000000 1.000000 0.000000
|
||||
-0.000000 1.000000 0.000001
|
||||
0.000000 1.000000 0.000000
|
||||
-0.000000 1.000000 0.000001
|
||||
-0.000000 1.000000 0.000001
|
||||
0.000000 1.000000 0.000001
|
||||
-0.000000 1.000000 0.000001
|
||||
0.000000 1.000000 0.000001
|
||||
-0.000000 1.000000 0.000001
|
||||
0.000000 1.000000 0.000001
|
||||
-1.000000 0.000000 -0.000001
|
||||
-1.000000 0.000000 -0.000001
|
||||
-1.000000 0.000000 -0.000001
|
||||
-1.000000 0.000000 -0.000001
|
||||
-1.000000 0.000000 -0.000001
|
||||
-1.000000 0.000000 -0.000001
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#Object004-Normal0-array" count="114" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="Object004-UV0">
|
||||
<float_array id="Object004-UV0-array" count="68">
|
||||
0.750000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 1.000000
|
||||
0.875000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 1.000000
|
||||
0.875000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 0.000000
|
||||
0.750000 1.000000
|
||||
0.875000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 0.000000
|
||||
0.875000 1.000000
|
||||
0.897733 0.000000
|
||||
0.897733 0.000000
|
||||
0.898362 1.000000
|
||||
0.898362 0.000000
|
||||
0.898362 1.000000
|
||||
0.898362 0.000000
|
||||
0.557666 0.000000
|
||||
0.557666 1.000000
|
||||
0.557666 0.000000
|
||||
0.557666 0.000000
|
||||
0.557666 0.000000
|
||||
0.557666 1.000000
|
||||
0.750000 0.228410
|
||||
0.875000 0.222896
|
||||
0.750000 0.228410
|
||||
0.875000 0.222896
|
||||
0.901391 0.229645
|
||||
0.901391 0.229645
|
||||
0.560354 0.228679
|
||||
0.560354 0.228679
|
||||
</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#Object004-UV0-array" count="34" stride="2">
|
||||
<param name="S" type="float"/>
|
||||
<param name="T" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="Object004-VERTEX">
|
||||
<input semantic="POSITION" source="#Object004-POSITION"/>
|
||||
</vertices>
|
||||
<triangles count="38" material="FBXASC0481FBXASC032-FBXASC032Default">
|
||||
<input semantic="VERTEX" offset="0" source="#Object004-VERTEX"/>
|
||||
<input semantic="NORMAL" offset="1" source="#Object004-Normal0"/>
|
||||
<input semantic="TEXCOORD" offset="2" set="0" source="#Object004-UV0"/>
|
||||
<p> 1 0 1 4 1 6 15 2 27 4 3 6 1 4 1 3 5 4 15 6 27 14 7 26 1 8 1 1 9 2 2 10 3 3 11 5 2 12 3 1 13 2 0 14 0 9 15 16 2 16 3 8 17 14 2 18 3 9 19 16 3 20 5 7 21 11 5 22 7 17 23 29 5 24 7 16 25 28 17 26 29 6 27 10 5 28 7 7 29 11 2 30 12 0 31 8 5 32 9 2 33 12 5 34 9 6 35 13 8 36 15 2 37 12 6 38 13 8 39 15 6 40 13 10 41 18 7 42 11 19 43 31 10 44 19 10 45 19 6 46 10 7 47 11 19 48 31 7 49 11 17 50 29 14 51 26 20 52 32 11 53 20 14 54 26 11 55 20 1 56 1 11 57 21 0 58 0 1 59 2 0 60 0 11 61 21 12 62 22 0 63 8 13 64 25 5 65 9 13 66 25 0 67 8 12 68 23 18 69 30 4 70 6 9 71 17 3 72 4 9 73 17 4 74 6 4 75 6 18 76 30 15 77 27 13 78 24 16 79 28 5 80 7 16 81 28 13 82 24 21 83 33 10 84 10 19 85 19 9 86 9 18 87 18 9 88 9 19 89 19 19 90 19 17 91 17 18 92 18 15 93 15 18 94 18 17 95 17 17 96 17 14 97 14 15 98 15 14 99 14 17 100 17 16 101 16 16 102 16 21 103 21 14 104 14 20 105 20 14 106 14 21 107 21 21 108 21 11 109 11 20 110 20 11 111 11 21 112 21 13 113 13</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="Object004" name="Object004">
|
||||
<node name="Object004" id="Object004" sid="Object004">
|
||||
<matrix sid="matrix">-0.000000 0.000000 0.287929 0.000000 0.000000 0.280290 -0.000000 0.000000 0.280290 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix>
|
||||
<instance_geometry url="#Object004-lib">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="FBXASC0481FBXASC032-FBXASC032Default" target="#FBXASC0481FBXASC032-FBXASC032Default"/>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
<extra>
|
||||
<technique profile="FCOLLADA">
|
||||
<visibility>1.000000</visibility>
|
||||
</technique>
|
||||
</extra>
|
||||
</node>
|
||||
<extra>
|
||||
<technique profile="MAX3D">
|
||||
<frame_rate>30.000000</frame_rate>
|
||||
</technique>
|
||||
<technique profile="FCOLLADA">
|
||||
<start_time>0.000000</start_time>
|
||||
<end_time>3.333333</end_time>
|
||||
</technique>
|
||||
</extra>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#Object004"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
@@ -1,15 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>foxtech_loong_2160</name>
|
||||
<version>1.0</version>
|
||||
<sdf version='1.10'>model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Alejandro Hernandez Cordero</name>
|
||||
<email>alejandro@OpenRobotics.org</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
This is a model of a VTOL.
|
||||
</description>
|
||||
</model>
|
||||
@@ -1,763 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.10">
|
||||
<model name="foxtech_loong_2160">
|
||||
<pose>0 0 0.5 0 0 0</pose>
|
||||
<link name="base_link">
|
||||
<visual name="visual_base_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/VTOL.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="collision_base_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/VTOL.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<pose>-0.13220955077769075 -4.9142356489896525e-08 0.04102678265168308 1.5707963267948966 -1.5231416433972742 0</pose>
|
||||
<mass>3.584838964525901</mass>
|
||||
<inertia>
|
||||
<ixx>0.67738485351961117</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.40350427325261343</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.28925142846350795</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>250</update_rate>
|
||||
</sensor>
|
||||
</link>
|
||||
<link name="legs_link">
|
||||
<pose>0 0 0 0 0 -1.57</pose>
|
||||
<visual name="visual_legs_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/legs.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>1 1 1 1.0</ambient>
|
||||
<diffuse>1 1 1 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<pose>3.7153930608820144e-06 -0.17032952180315669 -0.067888027893133446 0.010363008330798451 -1.494594844601834 1.5602561510969915</pose>
|
||||
<mass>0.6675493726413797</mass>
|
||||
<inertia>
|
||||
<ixx>0.032153703643267112</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.031670484418576728</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.00069030434527976208</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name="collision_legs_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/legs.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name='legs_joint' type='fixed'>
|
||||
<child>legs_link</child>
|
||||
<parent>base_link</parent>
|
||||
</joint>
|
||||
<link name="airspeed_link">
|
||||
<inertial>
|
||||
<mass>0.015</mass>
|
||||
<inertia>
|
||||
<ixx>1e-05</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>1e-05</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>1e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.01</radius>
|
||||
<length>0.1</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 0 1.0</ambient>
|
||||
<diffuse>0 0 0 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<sensor name="air_speed" type="air_speed">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>30</update_rate>
|
||||
<air_speed>
|
||||
<pressure>
|
||||
<noise type="gaussian">
|
||||
<mean>0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</pressure>
|
||||
</air_speed>
|
||||
</sensor>
|
||||
</link>
|
||||
<joint name='airspeed_joint' type='fixed'>
|
||||
<child>airspeed_link</child>
|
||||
<parent>base_link</parent>
|
||||
</joint>
|
||||
<link name="aileron_left_link">
|
||||
<pose>-0.122 0.713 0.062 0 0 -1.57</pose>
|
||||
<visual name="visual_aileron_left_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/aileron_left.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="collision_aileron_left_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/aileron_left.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<pose>0.016428520956860652 -0.026006344294799606 -0.0023574007803185606 -1.5395303459623877 -1.5044452951949348 -1.6400967799666224</pose>
|
||||
<mass>0.48889497715279695</mass>
|
||||
<inertia>
|
||||
<ixx>0.0046714785213929121</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.0044549919806967505</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.00022471747503113157</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="servo_0" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>aileron_left_link</child>
|
||||
<axis>
|
||||
<xyz>-1 0 0</xyz>
|
||||
<limit>
|
||||
<lower>-0.53</lower>
|
||||
<upper>0.53</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>1.000</damping>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name="aileron_right_link">
|
||||
<pose>-0.122 -0.713 0.062 0 0 -1.57</pose>
|
||||
<visual name="visual_aileron_right_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/aileron_right.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="collision_aileron_right_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/aileron_right.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<pose>-0.016428520373255796 -0.026006344259039589 -0.0023573984798765397 -1.6020624043352432 -1.5044453783967096 -1.5014957739781367</pose>
|
||||
<mass>0.48889496879062661</mass>
|
||||
<inertia>
|
||||
<ixx>0.0046714784977765411</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.0044549919610427299</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.00022471747055809963</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="servo_1" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>aileron_right_link</child>
|
||||
<axis>
|
||||
<xyz>-1 0 0</xyz>
|
||||
<limit>
|
||||
<lower>-0.53</lower>
|
||||
<upper>0.53</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>1.000</damping>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name="elevator_link">
|
||||
<visual name="visual_elevator_right_link">
|
||||
<pose>-0.883 -0.145 0.058 0 0 -1.57</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/elevator_right.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="collision_elevator_right_link">
|
||||
<pose>-0.883 -0.145 0.058 0 0 -1.57</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/elevator_right.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual_elevator_left_link">
|
||||
<pose>-0.883 0.145 0.058 0 0 -1.57</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/elevator_left.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="collision_elevator_left_link">
|
||||
<pose>-0.883 0.145 0.058 0 0 -1.57</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/elevator_left.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<pose>-0.90103043712540809 -1.4356019912623529e-05 0.057304774890379953 -1.5708004219891478 -1.5333831628670098 -3.141493135229823</pose>
|
||||
<mass>0.12150608610187363</mass>
|
||||
<inertia>
|
||||
<ixx>0.0029053735939891879</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.0028879706692375028</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>1.7683703559870421e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="servo_2" type="revolute">
|
||||
<pose>-0.883 0 0.058 0 0 -1.57</pose>
|
||||
<parent>base_link</parent>
|
||||
<child>elevator_link</child>
|
||||
<axis>
|
||||
<xyz>-1 0 0</xyz>
|
||||
<limit>
|
||||
<lower>-0.53</lower>
|
||||
<upper>0.53</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>1.000</damping>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name="rudder_link">
|
||||
<pose>-0.887 0 0.201 0 0 -1.57</pose>
|
||||
<visual name="visual_rudder_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/rudder.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="collision_rudder_link">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/rudder.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>p
|
||||
<pose>3.0720176589210631e-09 -0.014597633583934367 -0.00023793299837082617 -0.00096325055376496349 0 0</pose>
|
||||
<mass>0.08686124232617691</mass>
|
||||
<inertia>
|
||||
<ixx>0.00029239473616759126</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.00028035840542611708</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>1.2453403617197103e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="servo_3" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>rudder_link</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-0.53</lower>
|
||||
<upper>0.53</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>1.000</damping>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name="rotor_3">
|
||||
<gravity>true</gravity>
|
||||
<self_collide>false</self_collide>
|
||||
<velocity_decay/>
|
||||
<pose>-0.403 -0.427 0.083 0 0 -1.57</pose>
|
||||
<inertial>
|
||||
<mass>0.016076923076923075</mass>
|
||||
<inertia>
|
||||
<ixx>3.8464910483993325e-07</ixx>
|
||||
<iyy>2.6115851691700804e-05</iyy>
|
||||
<izz>2.649858234714004e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="rotor_3_visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/propeller_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="rotor_3_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<min_depth>0.001</min_depth>
|
||||
<max_vel>0</max_vel>
|
||||
</ode>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="rotor_0_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>rotor_0</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name="rotor_2">
|
||||
<gravity>true</gravity>
|
||||
<self_collide>false</self_collide>
|
||||
<velocity_decay/>
|
||||
<pose>0.403 0.427 0.083 0 0 -1.57</pose>
|
||||
<inertial>
|
||||
<mass>0.016076923076923075</mass>
|
||||
<inertia>
|
||||
<ixx>3.8464910483993325e-07</ixx>
|
||||
<iyy>2.6115851691700804e-05</iyy>
|
||||
<izz>2.649858234714004e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="rotor_2_visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/propeller_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="rotor_2_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<min_depth>0.001</min_depth>
|
||||
<max_vel>0</max_vel>
|
||||
</ode>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="rotor_1_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>rotor_1</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name="rotor_0">
|
||||
<gravity>true</gravity>
|
||||
<self_collide>false</self_collide>
|
||||
<velocity_decay/>
|
||||
<pose>0.403 -0.427 0.083 0 0 -1.57</pose>
|
||||
<inertial>
|
||||
<mass>0.016076923076923075</mass>
|
||||
<inertia>
|
||||
<ixx>3.8464910483993325e-07</ixx>
|
||||
<iyy>2.6115851691700804e-05</iyy>
|
||||
<izz>2.649858234714004e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="rotor_0_visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/propeller_cw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="rotor_0_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<min_depth>0.001</min_depth>
|
||||
<max_vel>0</max_vel>
|
||||
</ode>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="rotor_2_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>rotor_2</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name="rotor_1">
|
||||
<gravity>true</gravity>
|
||||
<self_collide>false</self_collide>
|
||||
<velocity_decay/>
|
||||
<pose>-0.403 0.427 0.083 0 0 -1.57</pose>
|
||||
<inertial>
|
||||
<mass>0.016076923076923075</mass>
|
||||
<inertia>
|
||||
<ixx>3.8464910483993325e-07</ixx>
|
||||
<iyy>2.6115851691700804e-05</iyy>
|
||||
<izz>2.649858234714004e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="rotor_1_visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/propeller_cw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="rotor_1_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<min_depth>0.001</min_depth>
|
||||
<max_vel>0</max_vel>
|
||||
</ode>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="rotor_3_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>rotor_3</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='rotor_puller'>
|
||||
<pose>0.379 0.0 0.0 -1.57 1.57 -1.57</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='rotor_puller_collision'>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='rotor_puller_visual'>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://foxtech_loong_2160/meshes/propeller_cw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
<self_collide>0</self_collide>
|
||||
</link>
|
||||
<joint name='rotor_puller_joint' type='revolute'>
|
||||
<pose>0.0 0 0.0 0 -1.57 0</pose>
|
||||
<child>rotor_puller</child>
|
||||
<parent>base_link</parent>
|
||||
<axis>
|
||||
<xyz>1 0 0</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_0_joint</jointName>
|
||||
<linkName>rotor_0</linkName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>1500</maxRotVelocity>
|
||||
<motorConstant>2e-05</motorConstant>
|
||||
<momentConstant>0.06</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>0</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<!-- <plugin name='back_left_motor_model' filename='libgazebo_motor_model.so'> -->
|
||||
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_1_joint</jointName>
|
||||
<linkName>rotor_1</linkName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>1500</maxRotVelocity>
|
||||
<motorConstant>2e-05</motorConstant>
|
||||
<momentConstant>0.06</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>1</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-multicopter-motor-model-system"
|
||||
name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_2_joint</jointName>
|
||||
<linkName>rotor_2</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>1500</maxRotVelocity>
|
||||
<motorConstant>2e-05</motorConstant>
|
||||
<momentConstant>0.06</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>2</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-multicopter-motor-model-system"
|
||||
name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_3_joint</jointName>
|
||||
<linkName>rotor_3</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>1500</maxRotVelocity>
|
||||
<motorConstant>2e-05</motorConstant>
|
||||
<momentConstant>0.06</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>3</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-multicopter-motor-model-system"
|
||||
name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_puller_joint</jointName>
|
||||
<linkName>rotor_puller</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>3500</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.01</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>4</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.05984281113</a0>
|
||||
<cla>4.752798721</cla>
|
||||
<cda>0.6417112299</cda>
|
||||
<cma>0.0</cma>
|
||||
<alpha_stall>0.3391428111</alpha_stall>
|
||||
<cla_stall>-3.85</cla_stall>
|
||||
<cda_stall>-0.9233984055</cda_stall>
|
||||
<cma_stall>0</cma_stall>
|
||||
<cp>-0.05 0.45 0.05</cp>
|
||||
<area>0.6</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<forward>1 0 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>base_link</link_name>
|
||||
<control_joint_name>servo_0</control_joint_name>
|
||||
<control_joint_rad_to_cl>-0.3</control_joint_rad_to_cl>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>servo_0</joint_name>
|
||||
<sub_topic>servo_0</sub_topic>
|
||||
<p_gain>10.0</p_gain>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.05984281113</a0>
|
||||
<cla>4.752798721</cla>
|
||||
<cda>0.6417112299</cda>
|
||||
<cma>0.0</cma>
|
||||
<alpha_stall>0.3391428111</alpha_stall>
|
||||
<cla_stall>-3.85</cla_stall>
|
||||
<cda_stall>-0.9233984055</cda_stall>
|
||||
<cma_stall>0</cma_stall>
|
||||
<cp>-0.05 -0.45 0.05</cp>
|
||||
<area>0.6</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<forward>1 0 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>base_link</link_name>
|
||||
<control_joint_name>servo_1</control_joint_name>
|
||||
<control_joint_rad_to_cl>-0.3</control_joint_rad_to_cl>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>servo_1</joint_name>
|
||||
<sub_topic>servo_1</sub_topic>
|
||||
<p_gain>10.0</p_gain>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>-0.2</a0>
|
||||
<cla>4.752798721</cla>
|
||||
<cda>0.6417112299</cda>
|
||||
<cma>0.0</cma>
|
||||
<alpha_stall>0.3391428111</alpha_stall>
|
||||
<cla_stall>-3.85</cla_stall>
|
||||
<cda_stall>-0.9233984055</cda_stall>
|
||||
<cma_stall>0</cma_stall>
|
||||
<cp>-0.5 0 0</cp>
|
||||
<area>0.01</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<forward>1 0 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>base_link</link_name>
|
||||
<control_joint_name>servo_2</control_joint_name>
|
||||
<control_joint_rad_to_cl>-4.0</control_joint_rad_to_cl>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>servo_2</joint_name>
|
||||
<sub_topic>servo_2</sub_topic>
|
||||
<p_gain>10.0</p_gain>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.0</a0>
|
||||
<cla>4.752798721</cla>
|
||||
<cda>0.6417112299</cda>
|
||||
<cma>0.0.</cma>
|
||||
<alpha_stall>0.3391428111</alpha_stall>
|
||||
<cla_stall>-3.85</cla_stall>
|
||||
<cda_stall>-0.9233984055</cda_stall>
|
||||
<cma_stall>0</cma_stall>
|
||||
<cp>-0.5 0 0.05</cp>
|
||||
<area>0.02</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<forward>1 0 0</forward>
|
||||
<upward>0 1 0</upward>
|
||||
<link_name>base_link</link_name>
|
||||
<control_joint_name>servo_3</control_joint_name>
|
||||
<control_joint_rad_to_cl>0.8</control_joint_rad_to_cl>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>servo_3</joint_name>
|
||||
<sub_topic>servo_3</sub_topic>
|
||||
<p_gain>10.0</p_gain>
|
||||
</plugin>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -2,7 +2,7 @@
|
||||
<model>
|
||||
<name>rc_cessna</name>
|
||||
<version>1.0</version>
|
||||
<sdf version='1.10'>model.sdf</sdf>
|
||||
<sdf version='1.9'>model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Benjamin Perseghetti</name>
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version='1.10'>
|
||||
<sdf version='1.9'>
|
||||
<model name='rc_cessna'>
|
||||
<pose>0 0 0.246 0 0 0</pose>
|
||||
<link name='base_link'>
|
||||
@@ -85,7 +85,7 @@
|
||||
<update_rate>250</update_rate>
|
||||
</sensor>
|
||||
</link>
|
||||
<link name="airspeed_link">
|
||||
<link name="airspeed">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
@@ -111,22 +111,22 @@
|
||||
<diffuse>0 0 0 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<sensor name="air_speed" type="air_speed">
|
||||
<!-- <sensor name="air_speed" type="air_speed">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>5.0</update_rate>
|
||||
<enable_metrics>false</enable_metrics>
|
||||
<air_speed>
|
||||
<pressure>
|
||||
<airspeed>
|
||||
<noise type="gaussian">
|
||||
<mean>0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</pressure>
|
||||
</airspeed>
|
||||
</air_speed>
|
||||
</sensor>
|
||||
</sensor> -->
|
||||
</link>
|
||||
<joint name='airspeed_joint' type='fixed'>
|
||||
<child>airspeed_link</child>
|
||||
<child>airspeed</child>
|
||||
<parent>base_link</parent>
|
||||
</joint>
|
||||
<link name='rotor_puller'>
|
||||
@@ -685,7 +685,6 @@
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>servo_0</joint_name>
|
||||
<sub_topic>servo_0</sub_topic>
|
||||
<p_gain>10.0</p_gain>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.05984281113</a0>
|
||||
@@ -709,7 +708,6 @@
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>servo_1</joint_name>
|
||||
<sub_topic>servo_1</sub_topic>
|
||||
<p_gain>10.0</p_gain>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.05984281113</a0>
|
||||
@@ -769,7 +767,6 @@
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>servo_2</joint_name>
|
||||
<sub_topic>servo_2</sub_topic>
|
||||
<p_gain>10.0</p_gain>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.0</a0>
|
||||
@@ -792,7 +789,6 @@
|
||||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>rudder_joint</joint_name>
|
||||
<p_gain>10.0</p_gain>
|
||||
</plugin>
|
||||
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_puller_joint</jointName>
|
||||
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -1,15 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>Standard VTOL</name>
|
||||
<version>1.0</version>
|
||||
<sdf version='1.10'>model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Roman Bapst</name>
|
||||
<email>roman@px4.io</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
This is a model of a standard VTOL quad plane.
|
||||
</description>
|
||||
</model>
|
||||
@@ -1,734 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- DO NOT EDIT: Generated from standard_vtol.sdf.jinja -->
|
||||
<sdf version='1.10'>
|
||||
<model name='standard_vtol'>
|
||||
<pose>0 0 0.246 0 0 0</pose>
|
||||
<link name='base_link'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>5</mass>
|
||||
<inertia>
|
||||
<ixx>0.477708333333</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.341666666667</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.811041666667</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='base_link_collision'>
|
||||
<pose>0 0 -0.07 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.55 2.144 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>100000</kp>
|
||||
<kd>1.0</kd>
|
||||
<max_vel>0.1</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='base_link_visual'>
|
||||
<pose>0.53 -1.072 -0.1 1.5707963268 0 3.1415926536</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
<uri>model://standard_vtol/meshes/x8_wing.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>.175 .175 .175 1.0</ambient>
|
||||
<diffuse>.175 .175 .175 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name='left_motor_column'>
|
||||
<pose>0 0.35 0.01 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.74 0.03 0.03</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>.175 .175 .175 1.0</ambient>
|
||||
<diffuse>.175 .175 .175 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name='right_motor_column'>
|
||||
<pose>0 -0.35 0.01 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.74 0.03 0.03</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>.175 .175 .175 1.0</ambient>
|
||||
<diffuse>.175 .175 .175 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name='m0'>
|
||||
<pose>-0.35 0.35 0.045 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.035</length>
|
||||
<radius>0.02</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>.175 .175 .175 1.0</ambient>
|
||||
<diffuse>.175 .175 .175 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name='m1'>
|
||||
<pose>-0.35 -0.35 0.045 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.035</length>
|
||||
<radius>0.02</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>.175 .175 .175 1.0</ambient>
|
||||
<diffuse>.175 .175 .175 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name='m2'>
|
||||
<pose>0.35 -0.35 0.045 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.035</length>
|
||||
<radius>0.02</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>.175 .175 .175 1.0</ambient>
|
||||
<diffuse>.175 .175 .175 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name='m3'>
|
||||
<pose>0.35 0.35 0.045 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.035</length>
|
||||
<radius>0.02</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>.175 .175 .175 1.0</ambient>
|
||||
<diffuse>.175 .175 .175 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
<self_collide>0</self_collide>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>250</update_rate>
|
||||
</sensor>
|
||||
</link>
|
||||
<link name="airspeed_link">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>0.015</mass>
|
||||
<inertia>
|
||||
<ixx>1e-05</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>1e-05</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>1e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.01</radius>
|
||||
<length>0.1</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 0 1.0</ambient>
|
||||
<diffuse>0 0 0 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<sensor name="air_speed" type="air_speed">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>30</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<enable_metrics>true</enable_metrics>
|
||||
<air_speed>
|
||||
<pressure>
|
||||
<noise type="gaussian">
|
||||
<mean>0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</pressure>
|
||||
</air_speed>
|
||||
</sensor>
|
||||
</link>
|
||||
<joint name='airspeed_joint' type='fixed'>
|
||||
<child>airspeed_link</child>
|
||||
<parent>base_link</parent>
|
||||
</joint>
|
||||
<link name='rotor_0'>
|
||||
<pose>0.35 -0.35 0.07 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='rotor_0_collision'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.1</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='rotor_0_visual'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 1 1.0</ambient>
|
||||
<diffuse>0 0 1 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
<self_collide>0</self_collide>
|
||||
</link>
|
||||
<joint name='rotor_0_joint' type='revolute'>
|
||||
<child>rotor_0</child>
|
||||
<parent>base_link</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='rotor_1'>
|
||||
<pose>-0.35 0.35 0.07 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='rotor_1_collision'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.1</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='rotor_1_visual'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 1 1.0</ambient>
|
||||
<diffuse>0 0 1 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
<self_collide>0</self_collide>
|
||||
</link>
|
||||
<joint name='rotor_1_joint' type='revolute'>
|
||||
<child>rotor_1</child>
|
||||
<parent>base_link</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='rotor_2'>
|
||||
<pose>0.35 0.35 0.07 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='rotor_2_collision'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.1</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='rotor_2_visual'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 1 1.0</ambient>
|
||||
<diffuse>0 0 1 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
<self_collide>0</self_collide>
|
||||
</link>
|
||||
<joint name='rotor_2_joint' type='revolute'>
|
||||
<child>rotor_2</child>
|
||||
<parent>base_link</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='rotor_3'>
|
||||
<pose>-0.35 -0.35 0.07 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='rotor_3_collision'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.1</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='rotor_3_visual'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 1 1.0</ambient>
|
||||
<diffuse>0 0 1 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
<self_collide>0</self_collide>
|
||||
</link>
|
||||
<joint name='rotor_3_joint' type='revolute'>
|
||||
<child>rotor_3</child>
|
||||
<parent>base_link</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<link name='rotor_puller'>
|
||||
<pose>-0.22 0 0.0 0 1.57 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='rotor_puller_collision'>
|
||||
<pose>0.0 0 -0.04 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='rotor_puller_visual'>
|
||||
<pose>0 0 -0.04 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.8 0.8 0.8</scale>
|
||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 1 1.0</ambient>
|
||||
<diffuse>0 0 1 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
<self_collide>0</self_collide>
|
||||
</link>
|
||||
<joint name='rotor_puller_joint' type='revolute'>
|
||||
<pose>0.0 0 0.0 0 -1.57 0</pose>
|
||||
<child>rotor_puller</child>
|
||||
<parent>base_link</parent>
|
||||
<axis>
|
||||
<xyz>1 0 0</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<link name="left_elevon">
|
||||
<inertial>
|
||||
<mass>0.00000001</mass>
|
||||
<inertia>
|
||||
<ixx>0.000001</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<iyy>0.000001</iyy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.000001</izz>
|
||||
</inertia>
|
||||
<pose>0 0.3 0 0.00 0 0.0</pose>
|
||||
</inertial>
|
||||
<visual name='left_elevon_visual'>
|
||||
<pose>-0.105 0.004 -0.034 1.5707963268 0 3.1415926536</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
<uri>model://standard_vtol/meshes/x8_elevon_left.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 1 1.0</ambient>
|
||||
<diffuse>0 0 1 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="right_elevon">
|
||||
<inertial>
|
||||
<mass>0.00000001</mass>
|
||||
<inertia>
|
||||
<ixx>0.000001</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<iyy>0.000001</iyy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.000001</izz>
|
||||
</inertia>
|
||||
<pose>0 -0.6 0 0.00 0 0.0</pose>
|
||||
</inertial>
|
||||
<visual name='right_elevon_visual'>
|
||||
<pose>0.281 -1.032 -0.034 1.5707963268 0 3.1415926536</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
<uri>model://standard_vtol/meshes/x8_elevon_right.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 1 1.0</ambient>
|
||||
<diffuse>0 0 1 1.0</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='servo_0' type='revolute'>
|
||||
<parent>base_link</parent>
|
||||
<child>left_elevon</child>
|
||||
<pose>-0.18 0.6 -0.005 0 0 0.265</pose>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-0.53</lower>
|
||||
<upper>0.53</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>1.000</damping>
|
||||
</dynamics>
|
||||
</axis>
|
||||
<physics>
|
||||
<ode>
|
||||
<implicit_spring_damper>1</implicit_spring_damper>
|
||||
</ode>
|
||||
</physics>
|
||||
</joint>
|
||||
<joint name='servo_1' type='revolute'>
|
||||
<parent>base_link</parent>
|
||||
<child>right_elevon</child>
|
||||
<pose>-0.18 -0.6 -0.005 0 0 -0.265</pose>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-0.53</lower>
|
||||
<upper>0.53</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<damping>1.000</damping>
|
||||
</dynamics>
|
||||
</axis>
|
||||
<physics>
|
||||
<ode>
|
||||
<implicit_spring_damper>1</implicit_spring_damper>
|
||||
</ode>
|
||||
</physics>
|
||||
</joint>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.05984281113</a0>
|
||||
<cla>4.752798721</cla>
|
||||
<cda>0.6417112299</cda>
|
||||
<cma>0.0</cma>
|
||||
<alpha_stall>0.3391428111</alpha_stall>
|
||||
<cla_stall>-3.85</cla_stall>
|
||||
<cda_stall>-0.9233984055</cda_stall>
|
||||
<cma_stall>0</cma_stall>
|
||||
<cp>-0.05 0.3 0.05</cp>
|
||||
<area>0.50</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<forward>1 0 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>base_link</link_name>
|
||||
<control_joint_name>servo_0</control_joint_name>
|
||||
<control_joint_rad_to_cl>-1.0</control_joint_rad_to_cl>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>servo_0</joint_name>
|
||||
<sub_topic>servo_0</sub_topic>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.05984281113</a0>
|
||||
<cla>4.752798721</cla>
|
||||
<cda>0.6417112299</cda>
|
||||
<cma>0.0</cma>
|
||||
<alpha_stall>0.3391428111</alpha_stall>
|
||||
<cla_stall>-3.85</cla_stall>
|
||||
<cda_stall>-0.9233984055</cda_stall>
|
||||
<cma_stall>0</cma_stall>
|
||||
<cp>-0.05 -0.3 0.05</cp>
|
||||
<area>0.50</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<forward>1 0 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>base_link</link_name>
|
||||
<control_joint_name>servo_1</control_joint_name>
|
||||
<control_joint_rad_to_cl>-1.0</control_joint_rad_to_cl>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>servo_1</joint_name>
|
||||
<sub_topic>servo_1</sub_topic>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_0_joint</jointName>
|
||||
<linkName>rotor_0</linkName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>1500</maxRotVelocity>
|
||||
<motorConstant>2e-05</motorConstant>
|
||||
<momentConstant>0.06</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>0</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_1_joint</jointName>
|
||||
<linkName>rotor_1</linkName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>1500</maxRotVelocity>
|
||||
<motorConstant>2e-05</motorConstant>
|
||||
<momentConstant>0.06</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>1</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-multicopter-motor-model-system"
|
||||
name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_2_joint</jointName>
|
||||
<linkName>rotor_2</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>1500</maxRotVelocity>
|
||||
<motorConstant>2e-05</motorConstant>
|
||||
<momentConstant>0.06</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>2</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-multicopter-motor-model-system"
|
||||
name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_3_joint</jointName>
|
||||
<linkName>rotor_3</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>1500</maxRotVelocity>
|
||||
<motorConstant>2e-05</motorConstant>
|
||||
<momentConstant>0.06</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>3</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-multicopter-motor-model-system"
|
||||
name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_puller_joint</jointName>
|
||||
<linkName>rotor_puller</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>3500</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.6</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>4</motorNumber>
|
||||
<rotorDragCoefficient>0.000106428</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<static>0</static>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -5,14 +5,12 @@
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
<real_time_update_rate>250</real_time_update_rate>
|
||||
</physics>
|
||||
<plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
|
||||
<plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
|
||||
<plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
|
||||
<plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
|
||||
<plugin name="gz::sim::systems::AirSpeed" filename="gz-sim-air-speed-system">
|
||||
</plugin>
|
||||
<plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
|
||||
<plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
|
||||
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
|
||||
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
|
||||
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
|
||||
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
|
||||
<plugin name='ignition::gazebo::systems::Imu' filename='ignition-gazebo-imu-system'/>
|
||||
<plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
|
||||
<render_engine>ogre2</render_engine>
|
||||
</plugin>
|
||||
<gui fullscreen='false'>
|
||||
|
||||
@@ -13,9 +13,9 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
|
||||
@@ -13,19 +13,15 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=n
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
CONFIG_DRIVERS_OSD=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_DRIVERS_RPM=y
|
||||
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
||||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
@@ -64,7 +60,7 @@ CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
|
||||
CONFIG_MODULES_SIMULATION_SIH=y
|
||||
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
|
||||
@@ -76,12 +72,9 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
|
||||
CONFIG_SYSTEMCMDS_I2CDETECT=y
|
||||
CONFIG_SYSTEMCMDS_LED_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_MFT=y
|
||||
CONFIG_SYSTEMCMDS_MIXER=y
|
||||
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
|
||||
CONFIG_SYSTEMCMDS_NSHTERM=y
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_PWM=y
|
||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||
CONFIG_SYSTEMCMDS_SD_BENCH=y
|
||||
CONFIG_SYSTEMCMDS_SD_STRESS=y
|
||||
|
||||
@@ -7,7 +7,6 @@
|
||||
#
|
||||
# CONFIG_DEV_CONSOLE is not set
|
||||
# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
|
||||
# CONFIG_DISABLE_PTHREAD is not set
|
||||
# CONFIG_SPI_EXCHANGE is not set
|
||||
# CONFIG_STM32H7_SYSCFG is not set
|
||||
CONFIG_ARCH="arm"
|
||||
@@ -23,14 +22,13 @@ CONFIG_ARMV7M_BASEPRI_WAR=y
|
||||
CONFIG_ARMV7M_ICACHE=y
|
||||
CONFIG_ARMV7M_MEMCPY=y
|
||||
CONFIG_ARMV7M_USEBASEPRI=y
|
||||
CONFIG_BOARDCTL_RESET=y
|
||||
CONFIG_BOARDCTL=y
|
||||
CONFIG_BOARDCTL_RESET=y
|
||||
CONFIG_BOARD_ASSERT_RESET_VALUE=0
|
||||
CONFIG_BOARD_INITTHREAD_PRIORITY=254
|
||||
CONFIG_BOARD_LATE_INITIALIZE=y
|
||||
CONFIG_BOARD_LOOPSPERMSEC=22114
|
||||
CONFIG_BOARD_RESET_ON_ASSERT=2
|
||||
CONFIG_BOARD_ASSERT_RESET_VALUE=0
|
||||
CONFIG_C99_BOOL8=y
|
||||
CONFIG_CDCACM=y
|
||||
CONFIG_CDCACM_IFLOWCONTROL=y
|
||||
CONFIG_CDCACM_PRODUCTID=0x0050
|
||||
@@ -39,37 +37,17 @@ CONFIG_CDCACM_RXBUFSIZE=600
|
||||
CONFIG_CDCACM_TXBUFSIZE=12000
|
||||
CONFIG_CDCACM_VENDORID=0x3162
|
||||
CONFIG_CDCACM_VENDORSTR="Holybro"
|
||||
CONFIG_CLOCK_MONOTONIC=y
|
||||
CONFIG_DEBUG_FULLOPT=y
|
||||
CONFIG_DEBUG_SYMBOLS=y
|
||||
CONFIG_DEFAULT_SMALL=y
|
||||
# CONFIG_NSH_DISABLE_ECHO is not set
|
||||
# CONFIG_NSH_DISABLE_ENV is not set
|
||||
# CONFIG_NSH_DISABLE_FREE is not set
|
||||
# CONFIG_NSH_DISABLE_HELP is not set
|
||||
# CONFIG_NSH_DISABLE_KILL is not set
|
||||
# CONFIG_NSH_DISABLE_LS is not set
|
||||
# CONFIG_NSH_DISABLE_MKDIR is not set
|
||||
# CONFIG_NSH_DISABLE_MODCMDS is not set
|
||||
# CONFIG_NSH_DISABLE_MOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_MV is not set
|
||||
# CONFIG_NSH_DISABLE_PRINTF is not set
|
||||
# CONFIG_NSH_DISABLE_PSSTACKUSAGE
|
||||
# CONFIG_NSH_DISABLE_PWD is not set
|
||||
# CONFIG_NSH_DISABLE_SOURCE is not set
|
||||
# CONFIG_NSH_DISABLE_SLEEP is not set
|
||||
# CONFIG_NSH_DISABLE_TEST is not set
|
||||
# CONFIG_NSH_DISABLE_UMOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_UNSET is not set
|
||||
# CONFIG_NSH_DISABLE_USLEEP is not set
|
||||
CONFIG_DISABLE_MQUEUE=y
|
||||
CONFIG_DISABLE_PTHREAD=y
|
||||
CONFIG_EXPERIMENTAL=y
|
||||
CONFIG_FDCLONE_DISABLE=y
|
||||
CONFIG_FDCLONE_STDIO=y
|
||||
CONFIG_HAVE_CXX=y
|
||||
CONFIG_HAVE_CXXINITIALIZE=y
|
||||
CONFIG_IDLETHREAD_STACKSIZE=750
|
||||
CONFIG_INIT_ENTRYPOINT="bootloader_main"
|
||||
CONFIG_INIT_STACKSIZE=2944
|
||||
CONFIG_LIBC_FLOATINGPOINT=y
|
||||
CONFIG_LIBC_LONG_LONG=y
|
||||
CONFIG_LIBC_STRERROR=y
|
||||
@@ -80,7 +58,6 @@ CONFIG_PTHREAD_STACK_MIN=512
|
||||
CONFIG_RAM_SIZE=245760
|
||||
CONFIG_RAM_START=0x20010000
|
||||
CONFIG_RAW_BINARY=y
|
||||
CONFIG_SDCLONE_DISABLE=y
|
||||
CONFIG_SERIAL_TERMIOS=y
|
||||
CONFIG_SIG_DEFAULT=y
|
||||
CONFIG_SIG_SIGALRM_ACTION=y
|
||||
@@ -110,5 +87,3 @@ CONFIG_USBDEV=y
|
||||
CONFIG_USBDEV_BUSPOWERED=y
|
||||
CONFIG_USBDEV_MAXPOWER=500
|
||||
CONFIG_USEC_PER_TICK=1000
|
||||
CONFIG_INIT_STACKSIZE=2944
|
||||
CONFIG_INIT_ENTRYPOINT="bootloader_main"
|
||||
|
||||
@@ -34,6 +34,7 @@
|
||||
# CONFIG_NSH_DISABLE_MKFATFS is not set
|
||||
# CONFIG_NSH_DISABLE_MOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_MV is not set
|
||||
# CONFIG_NSH_DISABLE_PRINTF is not set
|
||||
# CONFIG_NSH_DISABLE_PS is not set
|
||||
# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
|
||||
# CONFIG_NSH_DISABLE_PWD is not set
|
||||
@@ -67,13 +68,11 @@ CONFIG_ARMV7M_MEMCPY=y
|
||||
CONFIG_ARMV7M_USEBASEPRI=y
|
||||
CONFIG_ARM_MPU_EARLY_RESET=y
|
||||
CONFIG_BOARDCTL_RESET=y
|
||||
CONFIG_BOARDCTL=y
|
||||
CONFIG_BOARD_ASSERT_RESET_VALUE=0
|
||||
CONFIG_BOARD_CRASHDUMP=y
|
||||
CONFIG_BOARD_LOOPSPERMSEC=95150
|
||||
CONFIG_BOARD_RESET_ON_ASSERT=2
|
||||
CONFIG_BOARD_ASSERT_RESET_VALUE=0
|
||||
CONFIG_BUILTIN=y
|
||||
CONFIG_C99_BOOL8=y
|
||||
CONFIG_CDCACM=y
|
||||
CONFIG_CDCACM_IFLOWCONTROL=y
|
||||
CONFIG_CDCACM_PRODUCTID=0x0050
|
||||
@@ -82,31 +81,11 @@ CONFIG_CDCACM_RXBUFSIZE=600
|
||||
CONFIG_CDCACM_TXBUFSIZE=12000
|
||||
CONFIG_CDCACM_VENDORID=0x3162
|
||||
CONFIG_CDCACM_VENDORSTR="Holybro"
|
||||
CONFIG_CLOCK_MONOTONIC=y
|
||||
CONFIG_DEBUG_FULLOPT=y
|
||||
CONFIG_DEBUG_HARDFAULT_ALERT=y
|
||||
CONFIG_DEBUG_MEMFAULT=y
|
||||
CONFIG_DEBUG_SYMBOLS=y
|
||||
CONFIG_DEFAULT_SMALL=y
|
||||
# CONFIG_NSH_DISABLE_ECHO is not set
|
||||
# CONFIG_NSH_DISABLE_ENV is not set
|
||||
# CONFIG_NSH_DISABLE_FREE is not set
|
||||
# CONFIG_NSH_DISABLE_HELP is not set
|
||||
# CONFIG_NSH_DISABLE_KILL is not set
|
||||
# CONFIG_NSH_DISABLE_LS is not set
|
||||
# CONFIG_NSH_DISABLE_MKDIR is not set
|
||||
# CONFIG_NSH_DISABLE_MODCMDS is not set
|
||||
# CONFIG_NSH_DISABLE_MOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_MV is not set
|
||||
# CONFIG_NSH_DISABLE_PRINTF is not set
|
||||
# CONFIG_NSH_DISABLE_PSSTACKUSAGE
|
||||
# CONFIG_NSH_DISABLE_PWD is not set
|
||||
# CONFIG_NSH_DISABLE_SOURCE is not set
|
||||
# CONFIG_NSH_DISABLE_SLEEP is not set
|
||||
# CONFIG_NSH_DISABLE_TEST is not set
|
||||
# CONFIG_NSH_DISABLE_UMOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_UNSET is not set
|
||||
# CONFIG_NSH_DISABLE_USLEEP is not set
|
||||
CONFIG_DEV_FIFO_SIZE=0
|
||||
CONFIG_DEV_PIPE_MAXSIZE=1024
|
||||
CONFIG_DEV_PIPE_SIZE=70
|
||||
@@ -132,8 +111,11 @@ CONFIG_HAVE_CXXINITIALIZE=y
|
||||
CONFIG_I2C=y
|
||||
CONFIG_I2C_RESET=y
|
||||
CONFIG_IDLETHREAD_STACKSIZE=750
|
||||
CONFIG_INIT_ENTRYPOINT="nsh_main"
|
||||
CONFIG_INIT_STACKSIZE=2944
|
||||
CONFIG_LIBC_FLOATINGPOINT=y
|
||||
CONFIG_LIBC_LONG_LONG=y
|
||||
CONFIG_LIBC_MAX_EXITFUNS=1
|
||||
CONFIG_LIBC_STRERROR=y
|
||||
CONFIG_MEMSET_64BIT=y
|
||||
CONFIG_MEMSET_OPTSPEED=y
|
||||
@@ -151,9 +133,6 @@ CONFIG_NSH_ARGCAT=y
|
||||
CONFIG_NSH_BUILTIN_APPS=y
|
||||
CONFIG_NSH_CMDPARMS=y
|
||||
CONFIG_NSH_CROMFSETC=y
|
||||
CONFIG_NSH_DISABLE_IFCONFIG=y
|
||||
CONFIG_NSH_DISABLE_IFUPDOWN=y
|
||||
CONFIG_NSH_DISABLE_TELNETD=y
|
||||
CONFIG_NSH_LINELEN=128
|
||||
CONFIG_NSH_MAXARGUMENTS=15
|
||||
CONFIG_NSH_MMCSDSPIPORTNO=1
|
||||
@@ -176,7 +155,6 @@ CONFIG_RAW_BINARY=y
|
||||
CONFIG_READLINE_CMD_HISTORY=y
|
||||
CONFIG_READLINE_TABCOMPLETION=y
|
||||
CONFIG_RTC_DATETIME=y
|
||||
CONFIG_LIBC_MAX_EXITFUNS=1
|
||||
CONFIG_SCHED_HPWORK=y
|
||||
CONFIG_SCHED_HPWORKPRIORITY=249
|
||||
CONFIG_SCHED_HPWORKSTACKSIZE=1280
|
||||
@@ -187,7 +165,6 @@ CONFIG_SCHED_LPWORK=y
|
||||
CONFIG_SCHED_LPWORKPRIORITY=50
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=1632
|
||||
CONFIG_SCHED_WAITPID=y
|
||||
CONFIG_SDCLONE_DISABLE=y
|
||||
CONFIG_SEM_PREALLOCHOLDERS=32
|
||||
CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
|
||||
CONFIG_SERIAL_TERMIOS=y
|
||||
@@ -227,7 +204,6 @@ CONFIG_STM32H7_SPI2=y
|
||||
CONFIG_STM32H7_SPI4=y
|
||||
CONFIG_STM32H7_SPI4_DMA=y
|
||||
CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
|
||||
CONFIG_STM32H7_SPI_DMA=y
|
||||
CONFIG_STM32H7_UART4=y
|
||||
CONFIG_STM32H7_UART7=y
|
||||
CONFIG_STM32H7_USART1=y
|
||||
@@ -267,6 +243,4 @@ CONFIG_USBDEV=y
|
||||
CONFIG_USBDEV_BUSPOWERED=y
|
||||
CONFIG_USBDEV_MAXPOWER=500
|
||||
CONFIG_USEC_PER_TICK=1000
|
||||
CONFIG_INIT_STACKSIZE=2944
|
||||
CONFIG_INIT_ENTRYPOINT="nsh_main"
|
||||
CONFIG_WATCHDOG=y
|
||||
|
||||
@@ -13,18 +13,16 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
CONFIG_DRIVERS_OSD=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_DRIVERS_RPM=y
|
||||
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
||||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
@@ -64,10 +62,6 @@ CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
|
||||
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
|
||||
CONFIG_MODULES_SIMULATION_SENSOR_BARO_SIM=y
|
||||
CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM=y
|
||||
CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||
|
||||
@@ -7,7 +7,6 @@
|
||||
#
|
||||
# CONFIG_DEV_CONSOLE is not set
|
||||
# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
|
||||
# CONFIG_DISABLE_PTHREAD is not set
|
||||
# CONFIG_SPI_EXCHANGE is not set
|
||||
# CONFIG_STM32H7_SYSCFG is not set
|
||||
CONFIG_ARCH="arm"
|
||||
@@ -23,14 +22,13 @@ CONFIG_ARMV7M_BASEPRI_WAR=y
|
||||
CONFIG_ARMV7M_ICACHE=y
|
||||
CONFIG_ARMV7M_MEMCPY=y
|
||||
CONFIG_ARMV7M_USEBASEPRI=y
|
||||
CONFIG_BOARDCTL_RESET=y
|
||||
CONFIG_BOARDCTL=y
|
||||
CONFIG_BOARDCTL_RESET=y
|
||||
CONFIG_BOARD_ASSERT_RESET_VALUE=0
|
||||
CONFIG_BOARD_INITTHREAD_PRIORITY=254
|
||||
CONFIG_BOARD_LATE_INITIALIZE=y
|
||||
CONFIG_BOARD_LOOPSPERMSEC=22114
|
||||
CONFIG_BOARD_RESET_ON_ASSERT=2
|
||||
CONFIG_BOARD_ASSERT_RESET_VALUE=0
|
||||
CONFIG_C99_BOOL8=y
|
||||
CONFIG_CDCACM=y
|
||||
CONFIG_CDCACM_IFLOWCONTROL=y
|
||||
CONFIG_CDCACM_PRODUCTID=0x0050
|
||||
@@ -39,37 +37,17 @@ CONFIG_CDCACM_RXBUFSIZE=600
|
||||
CONFIG_CDCACM_TXBUFSIZE=12000
|
||||
CONFIG_CDCACM_VENDORID=0x3162
|
||||
CONFIG_CDCACM_VENDORSTR="Holybro"
|
||||
CONFIG_CLOCK_MONOTONIC=y
|
||||
CONFIG_DEBUG_FULLOPT=y
|
||||
CONFIG_DEBUG_SYMBOLS=y
|
||||
CONFIG_DEFAULT_SMALL=y
|
||||
# CONFIG_NSH_DISABLE_ECHO is not set
|
||||
# CONFIG_NSH_DISABLE_ENV is not set
|
||||
# CONFIG_NSH_DISABLE_FREE is not set
|
||||
# CONFIG_NSH_DISABLE_HELP is not set
|
||||
# CONFIG_NSH_DISABLE_KILL is not set
|
||||
# CONFIG_NSH_DISABLE_LS is not set
|
||||
# CONFIG_NSH_DISABLE_MKDIR is not set
|
||||
# CONFIG_NSH_DISABLE_MODCMDS is not set
|
||||
# CONFIG_NSH_DISABLE_MOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_MV is not set
|
||||
# CONFIG_NSH_DISABLE_PRINTF is not set
|
||||
# CONFIG_NSH_DISABLE_PSSTACKUSAGE
|
||||
# CONFIG_NSH_DISABLE_PWD is not set
|
||||
# CONFIG_NSH_DISABLE_SOURCE is not set
|
||||
# CONFIG_NSH_DISABLE_SLEEP is not set
|
||||
# CONFIG_NSH_DISABLE_TEST is not set
|
||||
# CONFIG_NSH_DISABLE_UMOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_UNSET is not set
|
||||
# CONFIG_NSH_DISABLE_USLEEP is not set
|
||||
CONFIG_DISABLE_MQUEUE=y
|
||||
CONFIG_DISABLE_PTHREAD=y
|
||||
CONFIG_EXPERIMENTAL=y
|
||||
CONFIG_FDCLONE_DISABLE=y
|
||||
CONFIG_FDCLONE_STDIO=y
|
||||
CONFIG_HAVE_CXX=y
|
||||
CONFIG_HAVE_CXXINITIALIZE=y
|
||||
CONFIG_IDLETHREAD_STACKSIZE=750
|
||||
CONFIG_INIT_ENTRYPOINT="bootloader_main"
|
||||
CONFIG_INIT_STACKSIZE=2944
|
||||
CONFIG_LIBC_FLOATINGPOINT=y
|
||||
CONFIG_LIBC_LONG_LONG=y
|
||||
CONFIG_LIBC_STRERROR=y
|
||||
@@ -80,7 +58,6 @@ CONFIG_PTHREAD_STACK_MIN=512
|
||||
CONFIG_RAM_SIZE=245760
|
||||
CONFIG_RAM_START=0x20010000
|
||||
CONFIG_RAW_BINARY=y
|
||||
CONFIG_SDCLONE_DISABLE=y
|
||||
CONFIG_SERIAL_TERMIOS=y
|
||||
CONFIG_SIG_DEFAULT=y
|
||||
CONFIG_SIG_SIGALRM_ACTION=y
|
||||
@@ -110,5 +87,3 @@ CONFIG_USBDEV=y
|
||||
CONFIG_USBDEV_BUSPOWERED=y
|
||||
CONFIG_USBDEV_MAXPOWER=500
|
||||
CONFIG_USEC_PER_TICK=1000
|
||||
CONFIG_INIT_STACKSIZE=2944
|
||||
CONFIG_INIT_ENTRYPOINT="bootloader_main"
|
||||
|
||||
@@ -34,6 +34,7 @@
|
||||
# CONFIG_NSH_DISABLE_MKFATFS is not set
|
||||
# CONFIG_NSH_DISABLE_MOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_MV is not set
|
||||
# CONFIG_NSH_DISABLE_PRINTF is not set
|
||||
# CONFIG_NSH_DISABLE_PS is not set
|
||||
# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
|
||||
# CONFIG_NSH_DISABLE_PWD is not set
|
||||
@@ -67,13 +68,11 @@ CONFIG_ARMV7M_MEMCPY=y
|
||||
CONFIG_ARMV7M_USEBASEPRI=y
|
||||
CONFIG_ARM_MPU_EARLY_RESET=y
|
||||
CONFIG_BOARDCTL_RESET=y
|
||||
CONFIG_BOARDCTL=y
|
||||
CONFIG_BOARD_ASSERT_RESET_VALUE=0
|
||||
CONFIG_BOARD_CRASHDUMP=y
|
||||
CONFIG_BOARD_LOOPSPERMSEC=95150
|
||||
CONFIG_BOARD_RESET_ON_ASSERT=2
|
||||
CONFIG_BOARD_ASSERT_RESET_VALUE=0
|
||||
CONFIG_BUILTIN=y
|
||||
CONFIG_C99_BOOL8=y
|
||||
CONFIG_CDCACM=y
|
||||
CONFIG_CDCACM_IFLOWCONTROL=y
|
||||
CONFIG_CDCACM_PRODUCTID=0x0050
|
||||
@@ -82,31 +81,11 @@ CONFIG_CDCACM_RXBUFSIZE=600
|
||||
CONFIG_CDCACM_TXBUFSIZE=12000
|
||||
CONFIG_CDCACM_VENDORID=0x3162
|
||||
CONFIG_CDCACM_VENDORSTR="Holybro"
|
||||
CONFIG_CLOCK_MONOTONIC=y
|
||||
CONFIG_DEBUG_FULLOPT=y
|
||||
CONFIG_DEBUG_HARDFAULT_ALERT=y
|
||||
CONFIG_DEBUG_MEMFAULT=y
|
||||
CONFIG_DEBUG_SYMBOLS=y
|
||||
CONFIG_DEFAULT_SMALL=y
|
||||
# CONFIG_NSH_DISABLE_ECHO is not set
|
||||
# CONFIG_NSH_DISABLE_ENV is not set
|
||||
# CONFIG_NSH_DISABLE_FREE is not set
|
||||
# CONFIG_NSH_DISABLE_HELP is not set
|
||||
# CONFIG_NSH_DISABLE_KILL is not set
|
||||
# CONFIG_NSH_DISABLE_LS is not set
|
||||
# CONFIG_NSH_DISABLE_MKDIR is not set
|
||||
# CONFIG_NSH_DISABLE_MODCMDS is not set
|
||||
# CONFIG_NSH_DISABLE_MOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_MV is not set
|
||||
# CONFIG_NSH_DISABLE_PRINTF is not set
|
||||
# CONFIG_NSH_DISABLE_PSSTACKUSAGE
|
||||
# CONFIG_NSH_DISABLE_PWD is not set
|
||||
# CONFIG_NSH_DISABLE_SOURCE is not set
|
||||
# CONFIG_NSH_DISABLE_SLEEP is not set
|
||||
# CONFIG_NSH_DISABLE_TEST is not set
|
||||
# CONFIG_NSH_DISABLE_UMOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_UNSET is not set
|
||||
# CONFIG_NSH_DISABLE_USLEEP is not set
|
||||
CONFIG_DEV_FIFO_SIZE=0
|
||||
CONFIG_DEV_PIPE_MAXSIZE=1024
|
||||
CONFIG_DEV_PIPE_SIZE=70
|
||||
@@ -132,8 +111,11 @@ CONFIG_HAVE_CXXINITIALIZE=y
|
||||
CONFIG_I2C=y
|
||||
CONFIG_I2C_RESET=y
|
||||
CONFIG_IDLETHREAD_STACKSIZE=750
|
||||
CONFIG_INIT_ENTRYPOINT="nsh_main"
|
||||
CONFIG_INIT_STACKSIZE=2944
|
||||
CONFIG_LIBC_FLOATINGPOINT=y
|
||||
CONFIG_LIBC_LONG_LONG=y
|
||||
CONFIG_LIBC_MAX_EXITFUNS=1
|
||||
CONFIG_LIBC_STRERROR=y
|
||||
CONFIG_MEMSET_64BIT=y
|
||||
CONFIG_MEMSET_OPTSPEED=y
|
||||
@@ -151,9 +133,6 @@ CONFIG_NSH_ARGCAT=y
|
||||
CONFIG_NSH_BUILTIN_APPS=y
|
||||
CONFIG_NSH_CMDPARMS=y
|
||||
CONFIG_NSH_CROMFSETC=y
|
||||
CONFIG_NSH_DISABLE_IFCONFIG=y
|
||||
CONFIG_NSH_DISABLE_IFUPDOWN=y
|
||||
CONFIG_NSH_DISABLE_TELNETD=y
|
||||
CONFIG_NSH_LINELEN=128
|
||||
CONFIG_NSH_MAXARGUMENTS=15
|
||||
CONFIG_NSH_MMCSDSPIPORTNO=1
|
||||
@@ -176,7 +155,6 @@ CONFIG_RAW_BINARY=y
|
||||
CONFIG_READLINE_CMD_HISTORY=y
|
||||
CONFIG_READLINE_TABCOMPLETION=y
|
||||
CONFIG_RTC_DATETIME=y
|
||||
CONFIG_LIBC_MAX_EXITFUNS=1
|
||||
CONFIG_SCHED_HPWORK=y
|
||||
CONFIG_SCHED_HPWORKPRIORITY=249
|
||||
CONFIG_SCHED_HPWORKSTACKSIZE=1280
|
||||
@@ -187,7 +165,6 @@ CONFIG_SCHED_LPWORK=y
|
||||
CONFIG_SCHED_LPWORKPRIORITY=50
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=1632
|
||||
CONFIG_SCHED_WAITPID=y
|
||||
CONFIG_SDCLONE_DISABLE=y
|
||||
CONFIG_SEM_PREALLOCHOLDERS=32
|
||||
CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
|
||||
CONFIG_SERIAL_TERMIOS=y
|
||||
@@ -227,7 +204,6 @@ CONFIG_STM32H7_SPI2=y
|
||||
CONFIG_STM32H7_SPI4=y
|
||||
CONFIG_STM32H7_SPI4_DMA=y
|
||||
CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
|
||||
CONFIG_STM32H7_SPI_DMA=y
|
||||
CONFIG_STM32H7_UART4=y
|
||||
CONFIG_STM32H7_UART7=y
|
||||
CONFIG_STM32H7_USART1=y
|
||||
@@ -267,6 +243,4 @@ CONFIG_USBDEV=y
|
||||
CONFIG_USBDEV_BUSPOWERED=y
|
||||
CONFIG_USBDEV_MAXPOWER=500
|
||||
CONFIG_USEC_PER_TICK=1000
|
||||
CONFIG_INIT_STACKSIZE=2944
|
||||
CONFIG_INIT_ENTRYPOINT="nsh_main"
|
||||
CONFIG_WATCHDOG=y
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
CONFIG_PLATFORM_QURT=y
|
||||
CONFIG_BOARD_TOOLCHAIN="qurt"
|
||||
CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
|
||||
CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
|
||||
CONFIG_DRIVERS_QSHELL_QURT=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
|
||||
@@ -414,7 +414,7 @@ bool ICM42688P::Configure()
|
||||
return success;
|
||||
}
|
||||
|
||||
static bool interrupt_debug = true;
|
||||
static bool interrupt_debug = false;
|
||||
static uint32_t interrupt_debug_count = 0;
|
||||
static const uint32_t interrupt_debug_trigger = 800;
|
||||
static hrt_abstime last_interrupt_time = 0;
|
||||
|
||||
@@ -15,4 +15,6 @@ CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
||||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_ORB_COMMUNICATOR=y
|
||||
CONFIG_BOARD_ROOTFSDIR="/data/px4"
|
||||
|
||||
@@ -19,5 +19,3 @@ fi
|
||||
muorb start
|
||||
|
||||
qshell icm42688p start -s
|
||||
|
||||
qshell modal_io start
|
||||
|
||||
@@ -31,7 +31,7 @@ bool height_sensor_timeout # 4 - true when the height sensor has n
|
||||
bool stopping_navigation # 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
|
||||
bool invalid_accel_bias_cov_reset # 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements
|
||||
bool bad_yaw_using_gps_course # 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
|
||||
bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
|
||||
bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
|
||||
bool vision_data_stopped # 9 - true when the vision system data has stopped for a significant time period
|
||||
bool emergency_yaw_reset_mag_stopped # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
|
||||
bool emergency_yaw_reset_mag_stopped # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
|
||||
bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
|
||||
|
||||
@@ -35,7 +35,7 @@ uint8 CS_EV_HGT = 14 # 14 - true when height data from external vision measurem
|
||||
uint8 CS_BETA = 15 # 15 - true when synthetic sideslip measurements are being fused
|
||||
uint8 CS_MAG_FIELD = 16 # 16 - true when only the magnetic field states are updated by the magnetometer
|
||||
uint8 CS_FIXED_WING = 17 # 17 - true when thought to be operating as a fixed wing vehicle with constrained sideslip
|
||||
uint8 CS_MAG_FAULT = 18 # 18 - true when the magnetomer has been declared faulty and is no longer being used
|
||||
uint8 CS_MAG_FAULT = 18 # 18 - true when the magnetometer has been declared faulty and is no longer being used
|
||||
uint8 CS_ASPD = 19 # 19 - true when airspeed measurements are being fused
|
||||
uint8 CS_GND_EFFECT = 20 # 20 - true when when protection from ground effect induced static pressure rise is active
|
||||
uint8 CS_RNG_STUCK = 21 # 21 - true when a stuck range finder sensor has been detected
|
||||
|
||||
@@ -1,18 +1,12 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
float32 nav_roll
|
||||
float32 nav_pitch
|
||||
float32 nav_bearing # set to NAN if not valid
|
||||
|
||||
float32 target_bearing # set to NAN if not valid
|
||||
float32 xtrack_error # set to NAN if not valid
|
||||
|
||||
float32 wp_dist
|
||||
|
||||
float32 acceptance_radius # the optimal distance to a waypoint to switch to the next
|
||||
|
||||
float32 yaw_acceptance # NaN if not set
|
||||
|
||||
float32 altitude_acceptance # the optimal vertical distance to a waypoint to switch to the next
|
||||
|
||||
uint8 type
|
||||
float32 nav_roll # Roll setpoint [rad]
|
||||
float32 nav_pitch # Pitch setpoint [rad]
|
||||
float32 nav_bearing # Bearing angle[rad]
|
||||
float32 target_bearing # Bearing angle from aircraft to current target [rad]
|
||||
float32 xtrack_error # Signed track error [m]
|
||||
float32 wp_dist # Distance to active (next) waypoint [m]
|
||||
float32 acceptance_radius # Current horizontal acceptance radius [m]
|
||||
float32 yaw_acceptance # Yaw acceptance error[rad]
|
||||
float32 altitude_acceptance # Current vertical acceptance error [m]
|
||||
uint8 type # Current (applied) position setpoint type (see PositionSetpoint.msg)
|
||||
|
||||
+27
-27
@@ -1,4 +1,31 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
float32 altitude_sp # Altitude setpoint AMSL [m]
|
||||
float32 altitude_sp_filtered # Altitude setpoint filtered AMSL [m]
|
||||
float32 height_rate_setpoint # Height rate setpoint [m/s]
|
||||
float32 height_rate # Height rate [m/s]
|
||||
float32 equivalent_airspeed_sp # Equivalent airspeed setpoint [m/s]
|
||||
float32 true_airspeed_sp # True airspeed setpoint [m/s]
|
||||
float32 true_airspeed_filtered # True airspeed filtered [m/s]
|
||||
float32 true_airspeed_derivative_sp # True airspeed derivative setpoint [m/s^2]
|
||||
float32 true_airspeed_derivative # True airspeed derivative [m/s^2]
|
||||
float32 true_airspeed_derivative_raw # True airspeed derivative raw [m/s^2]
|
||||
|
||||
float32 total_energy_rate_sp # Total energy rate setpoint [m^2/s^3]
|
||||
float32 total_energy_rate # Total energy rate estimate [m^2/s^3]
|
||||
|
||||
float32 total_energy_balance_rate_sp # Energy balance rate setpoint [m^2/s^3]
|
||||
float32 total_energy_balance_rate # Energy balance rate estimate [m^2/s^3]
|
||||
|
||||
float32 throttle_integ # Throttle integrator value [-]
|
||||
float32 pitch_integ # Pitch integrator value [rad]
|
||||
|
||||
float32 throttle_sp # Current throttle setpoint [-]
|
||||
float32 pitch_sp_rad # Current pitch setpoint [rad]
|
||||
float32 throttle_trim # estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions
|
||||
|
||||
# TECS mode
|
||||
uint8 mode
|
||||
uint8 TECS_MODE_NORMAL = 0
|
||||
uint8 TECS_MODE_UNDERSPEED = 1
|
||||
uint8 TECS_MODE_TAKEOFF = 2
|
||||
@@ -6,30 +33,3 @@ uint8 TECS_MODE_LAND = 3
|
||||
uint8 TECS_MODE_LAND_THROTTLELIM = 4
|
||||
uint8 TECS_MODE_BAD_DESCENT = 5
|
||||
uint8 TECS_MODE_CLIMBOUT = 6
|
||||
|
||||
|
||||
float32 altitude_sp
|
||||
float32 altitude_filtered
|
||||
float32 height_rate_setpoint
|
||||
float32 height_rate
|
||||
float32 equivalent_airspeed_sp
|
||||
float32 true_airspeed_sp
|
||||
float32 true_airspeed_filtered
|
||||
float32 true_airspeed_derivative_sp
|
||||
float32 true_airspeed_derivative
|
||||
float32 true_airspeed_derivative_raw
|
||||
|
||||
float32 total_energy_rate
|
||||
float32 total_energy_balance_rate
|
||||
|
||||
float32 total_energy_rate_sp
|
||||
float32 total_energy_balance_rate_sp
|
||||
|
||||
float32 throttle_integ
|
||||
float32 pitch_integ
|
||||
|
||||
float32 throttle_sp
|
||||
float32 pitch_sp_rad
|
||||
float32 throttle_trim # estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions
|
||||
|
||||
uint8 mode
|
||||
|
||||
@@ -40,6 +40,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <px4_boardconfig.h>
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Defines for all platforms.
|
||||
@@ -95,7 +97,7 @@ __BEGIN_DECLS
|
||||
extern long PX4_TICKS_PER_SEC;
|
||||
__END_DECLS
|
||||
|
||||
#define PX4_ROOTFSDIR "."
|
||||
#define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR
|
||||
|
||||
#define PX4_STORAGEDIR PX4_ROOTFSDIR
|
||||
|
||||
|
||||
@@ -470,7 +470,7 @@ if(NOT NUTTX_DIR MATCHES "external")
|
||||
)
|
||||
|
||||
# JLINK_RTOS_PATH used by launch.json.in
|
||||
set(JLINK_RTOS_PATH ${NUTTX_DIR}/tools/jlink-nuttx)
|
||||
set(JLINK_RTOS_PATH ${NUTTX_DIR}/tools/jlink-nuttx.so)
|
||||
else()
|
||||
set(JLINK_RTOS_PATH "")
|
||||
endif()
|
||||
|
||||
@@ -3,6 +3,7 @@
|
||||
"configurations": [
|
||||
{
|
||||
"name": "jlink (@PX4_BOARD@)",
|
||||
"gdbPath": "@CMAKE_GDB@",
|
||||
"device": "@DEBUG_DEVICE@",
|
||||
"svdFile": "@DEBUG_SVD_FILE_PATH@",
|
||||
"rtos": "@JLINK_RTOS_PATH@",
|
||||
@@ -24,6 +25,7 @@
|
||||
},
|
||||
{
|
||||
"name": "stlink (@PX4_BOARD@)",
|
||||
"gdbPath": "@CMAKE_GDB@",
|
||||
"device": "@DEBUG_DEVICE@",
|
||||
"svdFile": "@DEBUG_SVD_FILE_PATH@",
|
||||
"executable": "${command:cmake.launchTargetPath}",
|
||||
@@ -42,6 +44,7 @@
|
||||
},
|
||||
{
|
||||
"name": "blackmagic (@PX4_BOARD@)",
|
||||
"gdbPath": "@CMAKE_GDB@",
|
||||
"device": "@DEBUG_DEVICE@",
|
||||
"svdFile": "@DEBUG_SVD_FILE_PATH@",
|
||||
"executable": "${command:cmake.launchTargetPath}",
|
||||
|
||||
Submodule platforms/nuttx/NuttX/nuttx updated: f80785664f...35997053c5
@@ -53,7 +53,7 @@
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
#include <lockstep_scheduler/lockstep_scheduler.h>
|
||||
static LockstepScheduler lockstep_scheduler {};
|
||||
static LockstepScheduler lockstep_scheduler {true};
|
||||
#endif
|
||||
|
||||
// Intervals in usec
|
||||
|
||||
+2
-1
@@ -46,7 +46,7 @@
|
||||
class LockstepComponents
|
||||
{
|
||||
public:
|
||||
LockstepComponents();
|
||||
LockstepComponents(bool no_cleanup_on_destroy = false);
|
||||
~LockstepComponents();
|
||||
|
||||
/**
|
||||
@@ -69,6 +69,7 @@ public:
|
||||
void wait_for_components();
|
||||
|
||||
private:
|
||||
const bool _no_cleanup_on_destroy;
|
||||
|
||||
px4_sem_t _components_sem;
|
||||
|
||||
|
||||
+1
@@ -46,6 +46,7 @@
|
||||
class LockstepScheduler
|
||||
{
|
||||
public:
|
||||
LockstepScheduler(bool no_cleanup_on_destroy = false) : _components(no_cleanup_on_destroy) {}
|
||||
~LockstepScheduler();
|
||||
|
||||
void set_absolute_time(uint64_t time_us);
|
||||
|
||||
@@ -42,14 +42,19 @@
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <limits.h>
|
||||
|
||||
LockstepComponents::LockstepComponents()
|
||||
LockstepComponents::LockstepComponents(bool no_cleanup_on_destroy)
|
||||
: _no_cleanup_on_destroy(no_cleanup_on_destroy)
|
||||
{
|
||||
px4_sem_init(&_components_sem, 0, 0);
|
||||
}
|
||||
|
||||
LockstepComponents::~LockstepComponents()
|
||||
{
|
||||
px4_sem_destroy(&_components_sem);
|
||||
// Trying to destroy a condition variable with threads currently blocked on it results in undefined behavior.
|
||||
// Therefore we allow the caller not to cleanup and let the OS take care of that.
|
||||
if (!_no_cleanup_on_destroy) {
|
||||
px4_sem_destroy(&_components_sem);
|
||||
}
|
||||
}
|
||||
|
||||
int LockstepComponents::register_component()
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 ModalAI, Inc. All rights reserved.
|
||||
* Copyright (C) 2022-2023 ModalAI, Inc. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -39,7 +39,9 @@
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
//Defining hap_debug
|
||||
extern void qurt_log_to_apps(int level, const char *message);
|
||||
|
||||
// Defining hap_debug
|
||||
void HAP_debug(const char *msg, int level, const char *filename, int line);
|
||||
|
||||
static __inline void qurt_log(int level, const char *file, int line,
|
||||
@@ -51,6 +53,8 @@ static __inline void qurt_log(int level, const char *file, int line,
|
||||
vsnprintf(buf, sizeof(buf), format, args);
|
||||
va_end(args);
|
||||
HAP_debug(buf, level, file, line);
|
||||
|
||||
qurt_log_to_apps(level, buf);
|
||||
}
|
||||
|
||||
__END_DECLS
|
||||
|
||||
@@ -37,6 +37,7 @@ set(QURT_LAYER_SRCS
|
||||
tasks.cpp
|
||||
px4_qurt_impl.cpp
|
||||
main.cpp
|
||||
qurt_log.cpp
|
||||
)
|
||||
|
||||
add_library(px4_layer
|
||||
|
||||
@@ -0,0 +1,48 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 ModalAI, Inc. 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <uORB/uORBManager.hpp>
|
||||
|
||||
// This function will send a debug or error message up to the apps proc
|
||||
// so that it can be displayed and logged. Otherwise the messages are only
|
||||
// available with the mini-dm tool that requires adb (i.e. USB cable attached)
|
||||
extern "C" void qurt_log_to_apps(int level, const char *message)
|
||||
{
|
||||
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
|
||||
|
||||
if (ch != nullptr) {
|
||||
if (level >= _PX4_LOG_LEVEL_ERROR) { ch->send_message("slpi_error", strlen(message) + 1, (uint8_t *) message); }
|
||||
|
||||
else { ch->send_message("slpi_debug", strlen(message) + 1, (uint8_t *) message); }
|
||||
}
|
||||
}
|
||||
@@ -80,7 +80,7 @@
|
||||
#define DRV_IMU_DEVTYPE_ICM42605 0x29
|
||||
#define DRV_IMU_DEVTYPE_ICM42670P 0x2A
|
||||
#define DRV_IMU_DEVTYPE_IIM42652 0x2B
|
||||
|
||||
#define DRV_IMU_DEVTYPE_IAM20680HP 0x2C
|
||||
#define DRV_RNG_DEVTYPE_MB12XX 0x31
|
||||
#define DRV_RNG_DEVTYPE_LL40LS 0x32
|
||||
#define DRV_ACC_DEVTYPE_MPU6050 0x33
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__imu__invensense__iam20680hp
|
||||
MAIN iam20680hp
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
IAM20680HP.cpp
|
||||
IAM20680HP.hpp
|
||||
iam20680hp_main.cpp
|
||||
InvenSense_IAM20680HP_registers.hpp
|
||||
DEPENDS
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
px4_work_queue
|
||||
)
|
||||
@@ -0,0 +1,641 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#include "IAM20680HP.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
{
|
||||
return (msb << 8u) | lsb;
|
||||
}
|
||||
|
||||
IAM20680HP::IAM20680HP(const I2CSPIDriverConfig &config) :
|
||||
SPI(config),
|
||||
I2CSPIDriver(config),
|
||||
_drdy_gpio(config.drdy_gpio),
|
||||
_px4_accel(get_device_id(), config.rotation),
|
||||
_px4_gyro(get_device_id(), config.rotation)
|
||||
{
|
||||
if (_drdy_gpio != 0) {
|
||||
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
|
||||
}
|
||||
|
||||
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
|
||||
}
|
||||
|
||||
IAM20680HP::~IAM20680HP()
|
||||
{
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
perf_free(_fifo_empty_perf);
|
||||
perf_free(_fifo_overflow_perf);
|
||||
perf_free(_fifo_reset_perf);
|
||||
perf_free(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
int IAM20680HP::init()
|
||||
{
|
||||
int ret = SPI::init();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
DEVICE_DEBUG("SPI::init failed (%i)", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
return Reset() ? 0 : -1;
|
||||
}
|
||||
|
||||
bool IAM20680HP::Reset()
|
||||
{
|
||||
_state = STATE::RESET;
|
||||
DataReadyInterruptDisable();
|
||||
ScheduleClear();
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
void IAM20680HP::exit_and_cleanup()
|
||||
{
|
||||
DataReadyInterruptDisable();
|
||||
I2CSPIDriverBase::exit_and_cleanup();
|
||||
}
|
||||
|
||||
void IAM20680HP::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
|
||||
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
|
||||
|
||||
perf_print_counter(_bad_register_perf);
|
||||
perf_print_counter(_bad_transfer_perf);
|
||||
perf_print_counter(_fifo_empty_perf);
|
||||
perf_print_counter(_fifo_overflow_perf);
|
||||
perf_print_counter(_fifo_reset_perf);
|
||||
perf_print_counter(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
bool IAM20680HP::StoreCheckedRegisterValue(Register reg)
|
||||
{
|
||||
// 3 retries
|
||||
for (int i = 0; i < 3; i++) {
|
||||
uint8_t read1 = RegisterRead(reg);
|
||||
uint8_t read2 = RegisterRead(reg);
|
||||
|
||||
if (read1 == read2) {
|
||||
for (auto &r : _register_cfg) {
|
||||
if (r.reg == reg) {
|
||||
r.set_bits = read1;
|
||||
r.clear_bits = ~read1;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast<uint8_t>(reg), read1, read2);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int IAM20680HP::probe()
|
||||
{
|
||||
const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
|
||||
|
||||
if (whoami != WHOAMI) {
|
||||
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void IAM20680HP::RunImpl()
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
switch (_state) {
|
||||
case STATE::RESET:
|
||||
// PWR_MGMT_1: Device Reset
|
||||
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
|
||||
_reset_timestamp = now;
|
||||
_failure_count = 0;
|
||||
_state = STATE::WAIT_FOR_RESET;
|
||||
ScheduleDelayed(100_ms);
|
||||
break;
|
||||
|
||||
case STATE::WAIT_FOR_RESET:
|
||||
|
||||
// The reset value is 0x00 for all registers other than the registers below
|
||||
// Document Number: DS-000114 Page Page 35 of 53
|
||||
if (RegisterRead(Register::WHO_AM_I) == WHOAMI) {
|
||||
|
||||
// offset registers (factory calibration) should not change during normal operation
|
||||
StoreCheckedRegisterValue(Register::XA_OFFSET_H);
|
||||
StoreCheckedRegisterValue(Register::XA_OFFSET_L);
|
||||
StoreCheckedRegisterValue(Register::YA_OFFSET_H);
|
||||
StoreCheckedRegisterValue(Register::YA_OFFSET_L);
|
||||
StoreCheckedRegisterValue(Register::ZA_OFFSET_H);
|
||||
StoreCheckedRegisterValue(Register::ZA_OFFSET_L);
|
||||
|
||||
// Wakeup and reset digital signal path
|
||||
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
|
||||
RegisterWrite(Register::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST);
|
||||
RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST | USER_CTRL_BIT::I2C_IF_DIS);
|
||||
|
||||
// if reset succeeded then configure
|
||||
_state = STATE::CONFIGURE;
|
||||
ScheduleDelayed(35_ms); // max 35 ms start-up time from sleep
|
||||
|
||||
} else {
|
||||
// RESET not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
|
||||
PX4_DEBUG("Reset failed, retrying");
|
||||
_state = STATE::RESET;
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("Reset not complete, check again in 10 ms");
|
||||
ScheduleDelayed(10_ms);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::CONFIGURE:
|
||||
if (Configure()) {
|
||||
// if configure succeeded then start reading from FIFO
|
||||
_state = STATE::FIFO_READ;
|
||||
|
||||
if (DataReadyInterruptConfigure()) {
|
||||
_data_ready_interrupt_enabled = true;
|
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
} else {
|
||||
_data_ready_interrupt_enabled = false;
|
||||
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
|
||||
}
|
||||
|
||||
FIFOReset();
|
||||
|
||||
} else {
|
||||
// CONFIGURE not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
|
||||
PX4_DEBUG("Configure failed, resetting");
|
||||
_state = STATE::RESET;
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("Configure failed, retrying");
|
||||
}
|
||||
|
||||
ScheduleDelayed(100_ms);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::FIFO_READ: {
|
||||
hrt_abstime timestamp_sample = now;
|
||||
|
||||
if (_data_ready_interrupt_enabled) {
|
||||
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
|
||||
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
|
||||
|
||||
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
|
||||
timestamp_sample = drdy_timestamp_sample;
|
||||
|
||||
} else {
|
||||
perf_count(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
// push backup schedule back
|
||||
ScheduleDelayed(_fifo_empty_interval_us * 2);
|
||||
}
|
||||
|
||||
// always check current FIFO count
|
||||
bool success = false;
|
||||
const uint16_t fifo_count = FIFOReadCount();
|
||||
|
||||
if (fifo_count >= FIFO::SIZE) {
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
|
||||
} else if (fifo_count == 0) {
|
||||
perf_count(_fifo_empty_perf);
|
||||
|
||||
} else {
|
||||
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
|
||||
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
|
||||
|
||||
// tolerate minor jitter, leave sample to next iteration if behind by only 1
|
||||
if (samples == _fifo_gyro_samples + 1) {
|
||||
timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
|
||||
samples--;
|
||||
}
|
||||
|
||||
if (samples > FIFO_MAX_SAMPLES) {
|
||||
// not technically an overflow, but more samples than we expected or can publish
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
|
||||
} else if (samples >= SAMPLES_PER_TRANSFER) {
|
||||
if (FIFORead(timestamp_sample, samples)) {
|
||||
success = true;
|
||||
|
||||
if (_failure_count > 0) {
|
||||
_failure_count--;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!success) {
|
||||
_failure_count++;
|
||||
|
||||
// full reset if things are failing consistently
|
||||
if (_failure_count > 10) {
|
||||
Reset();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
|
||||
// check configuration registers periodically or immediately following any failure
|
||||
if (RegisterCheck(_register_cfg[_checked_register])) {
|
||||
_last_config_check_timestamp = now;
|
||||
_checked_register = (_checked_register + 1) % size_register_cfg;
|
||||
|
||||
} else {
|
||||
// register check failed, force reset
|
||||
perf_count(_bad_register_perf);
|
||||
Reset();
|
||||
}
|
||||
|
||||
} else {
|
||||
// periodically update temperature (~1 Hz)
|
||||
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
|
||||
UpdateTemperature();
|
||||
_temperature_update_timestamp = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void IAM20680HP::ConfigureAccel()
|
||||
{
|
||||
const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
|
||||
|
||||
switch (ACCEL_FS_SEL) {
|
||||
case ACCEL_FS_SEL_2G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
|
||||
_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_4G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
|
||||
_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_8G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
|
||||
_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_16G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
|
||||
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void IAM20680HP::ConfigureGyro()
|
||||
{
|
||||
const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
|
||||
|
||||
float range_dps = 0.f;
|
||||
|
||||
switch (FS_SEL) {
|
||||
case FS_SEL_250_DPS:
|
||||
range_dps = 250.f;
|
||||
break;
|
||||
|
||||
case FS_SEL_500_DPS:
|
||||
range_dps = 500.f;
|
||||
break;
|
||||
|
||||
case FS_SEL_1000_DPS:
|
||||
range_dps = 1000.f;
|
||||
break;
|
||||
|
||||
case FS_SEL_2000_DPS:
|
||||
range_dps = 2000.f;
|
||||
break;
|
||||
}
|
||||
|
||||
_px4_gyro.set_scale(math::radians(range_dps / 32768.f)); //灵敏度为16.4 LSB/dps
|
||||
_px4_gyro.set_range(math::radians(range_dps)); //陀螺仪量程
|
||||
}
|
||||
|
||||
void IAM20680HP::ConfigureSampleRate(int sample_rate)
|
||||
{
|
||||
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
|
||||
const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
|
||||
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
|
||||
|
||||
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
|
||||
|
||||
// recompute FIFO empty interval (us) with actual gyro sample limit
|
||||
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
|
||||
}
|
||||
|
||||
bool IAM20680HP::Configure()
|
||||
{
|
||||
// first set and clear all configured register bits
|
||||
for (const auto ®_cfg : _register_cfg) {
|
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
|
||||
}
|
||||
|
||||
// now check that all are configured
|
||||
bool success = true;
|
||||
|
||||
for (const auto ®_cfg : _register_cfg) {
|
||||
if (!RegisterCheck(reg_cfg)) {
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
|
||||
ConfigureAccel();
|
||||
ConfigureGyro();
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
int IAM20680HP::DataReadyInterruptCallback(int irq, void *context, void *arg)
|
||||
{
|
||||
static_cast<IAM20680HP *>(arg)->DataReady();
|
||||
return 0;
|
||||
}
|
||||
|
||||
void IAM20680HP::DataReady()
|
||||
{
|
||||
// at least the required number of samples in the FIFO
|
||||
if (++_drdy_count >= _fifo_gyro_samples) {
|
||||
_drdy_timestamp_sample.store(hrt_absolute_time());
|
||||
_drdy_count -= _fifo_gyro_samples;
|
||||
ScheduleNow();
|
||||
}
|
||||
}
|
||||
|
||||
bool IAM20680HP::DataReadyInterruptConfigure()
|
||||
{
|
||||
if (_drdy_gpio == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Setup data ready on falling edge
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
|
||||
}
|
||||
|
||||
bool IAM20680HP::DataReadyInterruptDisable()
|
||||
{
|
||||
if (_drdy_gpio == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
|
||||
}
|
||||
|
||||
bool IAM20680HP::RegisterCheck(const register_config_t ®_cfg)
|
||||
{
|
||||
bool success = true;
|
||||
|
||||
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
|
||||
|
||||
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
|
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
|
||||
success = false;
|
||||
}
|
||||
|
||||
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
|
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
|
||||
success = false;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
uint8_t IAM20680HP::RegisterRead(Register reg)
|
||||
{
|
||||
uint8_t cmd[2] {};
|
||||
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
|
||||
transfer(cmd, cmd, sizeof(cmd));
|
||||
return cmd[1];
|
||||
}
|
||||
|
||||
void IAM20680HP::RegisterWrite(Register reg, uint8_t value)
|
||||
{
|
||||
uint8_t cmd[2] { (uint8_t)reg, value };
|
||||
transfer(cmd, cmd, sizeof(cmd));
|
||||
}
|
||||
|
||||
void IAM20680HP::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
|
||||
{
|
||||
const uint8_t orig_val = RegisterRead(reg);
|
||||
|
||||
uint8_t val = (orig_val & ~clearbits) | setbits;
|
||||
|
||||
if (orig_val != val) {
|
||||
RegisterWrite(reg, val);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t IAM20680HP::FIFOReadCount()
|
||||
{
|
||||
// read FIFO count
|
||||
uint8_t fifo_count_buf[3] {};
|
||||
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
|
||||
|
||||
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return 0;
|
||||
}
|
||||
|
||||
return combine(fifo_count_buf[1], fifo_count_buf[2]);
|
||||
}
|
||||
|
||||
bool IAM20680HP::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
||||
{
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
ProcessGyro(timestamp_sample, buffer.f, samples);
|
||||
return ProcessAccel(timestamp_sample, buffer.f, samples);
|
||||
}
|
||||
|
||||
void IAM20680HP::FIFOReset()
|
||||
{
|
||||
perf_count(_fifo_reset_perf);
|
||||
|
||||
// FIFO_EN: disable FIFO
|
||||
RegisterWrite(Register::FIFO_EN, 0);
|
||||
|
||||
// USER_CTRL: reset FIFO
|
||||
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
|
||||
|
||||
// reset while FIFO is disabled
|
||||
_drdy_count = 0;
|
||||
_drdy_timestamp_sample.store(0);
|
||||
|
||||
// FIFO_EN: enable both gyro and accel
|
||||
// USER_CTRL: re-enable FIFO
|
||||
for (const auto &r : _register_cfg) {
|
||||
if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) {
|
||||
RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
|
||||
{
|
||||
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
|
||||
}
|
||||
|
||||
bool IAM20680HP::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_accel_fifo_s accel{};
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.samples = 0;
|
||||
accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
|
||||
|
||||
bool bad_data = false;
|
||||
|
||||
// accel data is doubled in FIFO, but might be shifted
|
||||
int accel_first_sample = 1;
|
||||
|
||||
if (samples >= 4) {
|
||||
if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0==A1, A2==A3
|
||||
accel_first_sample = 1;
|
||||
|
||||
} else if (fifo_accel_equal(fifo[1], fifo[2])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0, A1==A2, A3
|
||||
accel_first_sample = 0;
|
||||
|
||||
} else {
|
||||
// no matching accel samples is an error
|
||||
bad_data = true;
|
||||
perf_count(_bad_transfer_perf);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
|
||||
int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
|
||||
int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
|
||||
int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
|
||||
|
||||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
if (accel.samples > 0) {
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
|
||||
return !bad_data;
|
||||
}
|
||||
|
||||
void IAM20680HP::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_gyro_fifo_s gyro{};
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = FIFO_SAMPLE_DT;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
|
||||
const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
|
||||
const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
|
||||
|
||||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
}
|
||||
|
||||
void IAM20680HP::UpdateTemperature()
|
||||
{
|
||||
// read current temperature
|
||||
uint8_t temperature_buf[3] {};
|
||||
temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
|
||||
|
||||
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
|
||||
const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
|
||||
|
||||
if (PX4_ISFINITE(TEMP_degC)) {
|
||||
_px4_accel.set_temperature(TEMP_degC);
|
||||
_px4_gyro.set_temperature(TEMP_degC);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "InvenSense_IAM20680HP_registers.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/device/spi.h>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/atomic.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
|
||||
using namespace InvenSense_IAM20680HP;
|
||||
|
||||
class IAM20680HP : public device::SPI, public I2CSPIDriver<IAM20680HP>
|
||||
{
|
||||
public:
|
||||
IAM20680HP(const I2CSPIDriverConfig &config);
|
||||
~IAM20680HP() override;
|
||||
|
||||
static void print_usage();
|
||||
|
||||
void RunImpl();
|
||||
|
||||
int init() override;
|
||||
void print_status() override;
|
||||
|
||||
private:
|
||||
void exit_and_cleanup() override;
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
|
||||
static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
|
||||
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro
|
||||
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
|
||||
|
||||
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
|
||||
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
|
||||
|
||||
// Transfer data
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
int probe() override;
|
||||
|
||||
bool Reset();
|
||||
|
||||
bool Configure();
|
||||
void ConfigureAccel();
|
||||
void ConfigureGyro();
|
||||
void ConfigureSampleRate(int sample_rate);
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
void DataReady();
|
||||
bool DataReadyInterruptConfigure();
|
||||
bool DataReadyInterruptDisable();
|
||||
|
||||
bool RegisterCheck(const register_config_t ®_cfg);
|
||||
bool StoreCheckedRegisterValue(Register reg);
|
||||
|
||||
uint8_t RegisterRead(Register reg);
|
||||
void RegisterWrite(Register reg, uint8_t value);
|
||||
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
|
||||
|
||||
uint16_t FIFOReadCount();
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
|
||||
void UpdateTemperature();
|
||||
|
||||
const spi_drdy_gpio_t _drdy_gpio;
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
|
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
|
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
|
||||
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
|
||||
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
|
||||
perf_counter_t _drdy_missed_perf{nullptr};
|
||||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
hrt_abstime _temperature_update_timestamp{0};
|
||||
int _failure_count{0};
|
||||
|
||||
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
|
||||
int32_t _drdy_count{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
FIFO_READ,
|
||||
} _state{STATE::RESET};
|
||||
|
||||
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
|
||||
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
|
||||
|
||||
uint8_t _checked_register{0};
|
||||
static constexpr uint8_t size_register_cfg{16};
|
||||
register_config_t _register_cfg[size_register_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 },
|
||||
{ Register::SMPLRT_DIV, SMPLRT_DIV_BIT::SPEED, 0 },//输出速率100hz
|
||||
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },//陀螺仪±2000dps量程
|
||||
{ Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 },//加速度计±16G量程
|
||||
{ Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B, ACCEL_CONFIG2_BIT::FIFO_SIZE },//加速度计低通滤波BW=21.2Hz
|
||||
{ Register::FIFO_EN, FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN },
|
||||
{ Register::INT_PIN_CFG, INT_PIN_CFG_BIT::INT_LEVEL, 0 },
|
||||
{ Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 },
|
||||
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
|
||||
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP },
|
||||
{ Register::XA_OFFSET_H, 0, 0 },
|
||||
{ Register::XA_OFFSET_L, 0, 0 },
|
||||
{ Register::YA_OFFSET_H, 0, 0 },
|
||||
{ Register::YA_OFFSET_L, 0, 0 },
|
||||
{ Register::ZA_OFFSET_H, 0, 0 },
|
||||
{ Register::ZA_OFFSET_L, 0, 0 },
|
||||
};
|
||||
};
|
||||
@@ -0,0 +1,193 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
// TODO: move to a central header
|
||||
static constexpr uint8_t Bit0 = (1 << 0);
|
||||
static constexpr uint8_t Bit1 = (1 << 1);
|
||||
static constexpr uint8_t Bit2 = (1 << 2);
|
||||
static constexpr uint8_t Bit3 = (1 << 3);
|
||||
static constexpr uint8_t Bit4 = (1 << 4);
|
||||
static constexpr uint8_t Bit5 = (1 << 5);
|
||||
static constexpr uint8_t Bit6 = (1 << 6);
|
||||
static constexpr uint8_t Bit7 = (1 << 7);
|
||||
|
||||
namespace InvenSense_IAM20680HP
|
||||
{
|
||||
static constexpr uint32_t SPI_SPEED = 8 * 1000 * 1000; // 8MHz SPI serial interface
|
||||
static constexpr uint8_t DIR_READ = 0x80;
|
||||
|
||||
static constexpr uint8_t WHOAMI = 0xF8; //whoami默认值
|
||||
|
||||
static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C 灵敏度
|
||||
static constexpr float TEMPERATURE_OFFSET = 25.f; // C 温度偏移
|
||||
//寄存器地址
|
||||
enum class Register : uint8_t {
|
||||
SMPLRT_DIV = 0X19,
|
||||
CONFIG = 0x1A,
|
||||
GYRO_CONFIG = 0x1B,
|
||||
ACCEL_CONFIG = 0x1C,
|
||||
ACCEL_CONFIG2 = 0x1D,
|
||||
|
||||
FIFO_EN = 0x23,
|
||||
|
||||
INT_PIN_CFG = 0x37,
|
||||
INT_ENABLE = 0x38,
|
||||
|
||||
TEMP_OUT_H = 0x41,
|
||||
TEMP_OUT_L = 0x42,
|
||||
|
||||
SIGNAL_PATH_RESET = 0x68,
|
||||
|
||||
USER_CTRL = 0x6A,
|
||||
PWR_MGMT_1 = 0x6B,
|
||||
|
||||
FIFO_COUNTH = 0x72,
|
||||
FIFO_COUNTL = 0x73,
|
||||
FIFO_R_W = 0x74,
|
||||
WHO_AM_I = 0x75,
|
||||
|
||||
XA_OFFSET_H = 0x77,
|
||||
XA_OFFSET_L = 0x78,
|
||||
|
||||
YA_OFFSET_H = 0x7A,
|
||||
YA_OFFSET_L = 0x7B,
|
||||
|
||||
ZA_OFFSET_H = 0x7D,
|
||||
ZA_OFFSET_L = 0x7E,
|
||||
};
|
||||
|
||||
// CONFIG
|
||||
enum CONFIG_BIT : uint8_t {
|
||||
FIFO_MODE = Bit6, // when the FIFO is full, additional writes will not be written to FIFO
|
||||
|
||||
DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz [2:0]
|
||||
};
|
||||
|
||||
// GYRO_CONFIG
|
||||
enum GYRO_CONFIG_BIT : uint8_t {
|
||||
// FS_SEL [4:3]
|
||||
FS_SEL_250_DPS = 0, // 0b00000
|
||||
FS_SEL_500_DPS = Bit3, // 0b01000
|
||||
FS_SEL_1000_DPS = Bit4, // 0b10000
|
||||
FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000
|
||||
|
||||
// FCHOICE_B [1:0]
|
||||
FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG
|
||||
enum ACCEL_CONFIG_BIT : uint8_t {
|
||||
// ACCEL_FS_SEL [4:3]
|
||||
ACCEL_FS_SEL_2G = 0, // 0b00000
|
||||
ACCEL_FS_SEL_4G = Bit3, // 0b01000
|
||||
ACCEL_FS_SEL_8G = Bit4, // 0b10000
|
||||
ACCEL_FS_SEL_16G = Bit4 | Bit3, // 0b11000
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG2
|
||||
enum ACCEL_CONFIG2_BIT : uint8_t {
|
||||
FIFO_SIZE = Bit7 | Bit6, // 0=512bytes,
|
||||
ACCEL_FCHOICE_B = Bit3, // Used to bypass DLPF (DS-000114 Page 40 of 53)
|
||||
};
|
||||
|
||||
// FIFO_EN
|
||||
enum FIFO_EN_BIT : uint8_t {
|
||||
TEMP_FIFO_EN = Bit7,
|
||||
XG_FIFO_EN = Bit6,
|
||||
YG_FIFO_EN = Bit5,
|
||||
ZG_FIFO_EN = Bit4,
|
||||
ACCEL_FIFO_EN = Bit3,
|
||||
};
|
||||
|
||||
// INT_PIN_CFG
|
||||
enum INT_PIN_CFG_BIT : uint8_t {
|
||||
INT_LEVEL = Bit7,
|
||||
};
|
||||
|
||||
// INT_ENABLE
|
||||
enum INT_ENABLE_BIT : uint8_t {
|
||||
DATA_RDY_INT_EN = Bit0,
|
||||
};
|
||||
|
||||
// SIGNAL_PATH_RESET
|
||||
enum SIGNAL_PATH_RESET_BIT : uint8_t {
|
||||
ACCEL_RST = Bit1,
|
||||
TEMP_RST = Bit0,
|
||||
};
|
||||
|
||||
// USER_CTRL
|
||||
enum USER_CTRL_BIT : uint8_t {
|
||||
FIFO_EN = Bit6,
|
||||
I2C_IF_DIS = Bit4,
|
||||
FIFO_RST = Bit2,
|
||||
SIG_COND_RST = Bit0,
|
||||
};
|
||||
|
||||
// PWR_MGMT_1
|
||||
enum PWR_MGMT_1_BIT : uint8_t {
|
||||
DEVICE_RESET = Bit7,
|
||||
SLEEP = Bit6,
|
||||
|
||||
// CLKSEL[2:0]
|
||||
CLKSEL_0 = Bit0, // It is required that CLKSEL[2:0] be set to 001 to achieve full gyroscope performance.
|
||||
};
|
||||
enum SMPLRT_DIV_BIT : uint8_t {
|
||||
SPEED = Bit3 | Bit0, // It is required that CLKSEL[2:0] be set to 001 to achieve full gyroscope performance.
|
||||
};
|
||||
namespace FIFO
|
||||
{
|
||||
static constexpr size_t SIZE = 512; // max is 4 KB, but limited in software to 512 bytes via ACCEL_CONFIG2
|
||||
|
||||
// FIFO_DATA layout when FIFO_EN has both {X, Y, Z}G_FIFO_EN and ACCEL_FIFO_EN set
|
||||
struct DATA {
|
||||
uint8_t ACCEL_XOUT_H;
|
||||
uint8_t ACCEL_XOUT_L;
|
||||
uint8_t ACCEL_YOUT_H;
|
||||
uint8_t ACCEL_YOUT_L;
|
||||
uint8_t ACCEL_ZOUT_H;
|
||||
uint8_t ACCEL_ZOUT_L;
|
||||
uint8_t GYRO_XOUT_H;
|
||||
uint8_t GYRO_XOUT_L;
|
||||
uint8_t GYRO_YOUT_H;
|
||||
uint8_t GYRO_YOUT_L;
|
||||
uint8_t GYRO_ZOUT_H;
|
||||
uint8_t GYRO_ZOUT_L;
|
||||
};
|
||||
}
|
||||
|
||||
} // namespace InvenSense_IAM20680HP
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_IMU_INVENSENSE_IAM20680HP
|
||||
bool "iam20680hp"
|
||||
default n
|
||||
---help---
|
||||
Enable support for iam20680hp
|
||||
@@ -0,0 +1,88 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#include "IAM20680HP.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
void IAM20680HP::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("iam20680hp", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
extern "C" int iam20680hp_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
using ThisDriver = IAM20680HP;
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'R':
|
||||
cli.rotation = (enum Rotation)atoi(cli.optArg());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
const char *verb = cli.optArg();
|
||||
|
||||
if (!verb) {
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_IAM20680HP);
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return ThisDriver::module_start(cli, iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
return ThisDriver::module_stop(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
return ThisDriver::module_status(iterator);
|
||||
}
|
||||
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
@@ -87,8 +87,6 @@ private:
|
||||
uint8_t _g{0};
|
||||
uint8_t _b{0};
|
||||
volatile bool _running{false};
|
||||
volatile bool _should_run{true};
|
||||
bool _leds_enabled{true};
|
||||
|
||||
LedController _led_controller;
|
||||
|
||||
|
||||
@@ -1254,9 +1254,11 @@ UavcanNode::get_next_dirty_node_id(uint8_t base)
|
||||
*/
|
||||
static void print_usage()
|
||||
{
|
||||
PX4_INFO("usage: \n"
|
||||
"\tuavcan {start|status|stop|shrink|update}\n"
|
||||
"\t param [set|get|list|save] <node-id> <name> <value>|reset <node-id>");
|
||||
PX4_INFO_RAW("usage: \n"
|
||||
"\tuavcan {start|status|stop|shrink|update}\n"
|
||||
"\t param [set|get|list|save] <node-id> <name> <value>|reset <node-id>"
|
||||
"\t reset <node-id>"
|
||||
);
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
|
||||
@@ -1317,6 +1319,17 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
|
||||
::exit(0);
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "reset")) {
|
||||
int node_arg = 2;
|
||||
|
||||
if (argc < node_arg + 1) {
|
||||
errx(1, "Node id required");
|
||||
}
|
||||
|
||||
int nodeid = atoi(argv[node_arg]);
|
||||
return inst->reset_node(nodeid);
|
||||
}
|
||||
|
||||
/*
|
||||
* Parameter setting commands
|
||||
*
|
||||
@@ -1326,9 +1339,9 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
|
||||
* uavcan param set <node> <name> <value>
|
||||
*
|
||||
*/
|
||||
int node_arg = !std::strcmp(argv[1], "reset") ? 2 : 3;
|
||||
if (!std::strcmp(argv[1], "param")) {
|
||||
int node_arg = 3;
|
||||
|
||||
if (!std::strcmp(argv[1], "param") || node_arg == 2) {
|
||||
if (argc < node_arg + 1) {
|
||||
errx(1, "Node id required");
|
||||
}
|
||||
@@ -1339,11 +1352,7 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
|
||||
errx(1, "Invalid Node id");
|
||||
}
|
||||
|
||||
if (node_arg == 2) {
|
||||
|
||||
return inst->reset_node(nodeid);
|
||||
|
||||
} else if (!std::strcmp(argv[2], "list")) {
|
||||
if (!std::strcmp(argv[2], "list")) {
|
||||
|
||||
return inst->list_params(nodeid);
|
||||
|
||||
|
||||
+18
-36
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2017-2023 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
|
||||
@@ -54,8 +54,6 @@ static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt
|
||||
|
||||
void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
|
||||
{
|
||||
|
||||
|
||||
_airspeed_state.speed = equivalent_airspeed;
|
||||
_airspeed_state.speed_rate = 0.0f;
|
||||
}
|
||||
@@ -373,10 +371,7 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
|
||||
// Calculate the weight applied to control of specific kinetic energy error
|
||||
float pitch_speed_weight = constrain(param.pitch_speed_weight, 0.0f, 2.0f);
|
||||
|
||||
if (flag.climbout_mode_active && flag.airspeed_enabled) {
|
||||
pitch_speed_weight = 2.0f;
|
||||
|
||||
} else if (_ratio_undersped > FLT_EPSILON && flag.airspeed_enabled) {
|
||||
if (_ratio_undersped > FLT_EPSILON && flag.airspeed_enabled) {
|
||||
pitch_speed_weight = 2.0f * _ratio_undersped + (1.0f - _ratio_undersped) * pitch_speed_weight;
|
||||
|
||||
} else if (!flag.airspeed_enabled) {
|
||||
@@ -399,7 +394,7 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific
|
||||
const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
|
||||
ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
|
||||
|
||||
_calcPitchControlUpdate(dt, seb_rate, param);
|
||||
_calcPitchControlUpdate(dt, input, seb_rate, param);
|
||||
const float pitch_setpoint{_calcPitchControlOutput(input, seb_rate, param, flag)};
|
||||
|
||||
// Comply with the specified vertical acceleration limit by applying a pitch rate limit
|
||||
@@ -438,11 +433,16 @@ TECSControl::ControlValues TECSControl::_calcPitchControlSebRate(const SpecificE
|
||||
return seb_rate;
|
||||
}
|
||||
|
||||
void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param ¶m)
|
||||
void TECSControl::_calcPitchControlUpdate(float dt, const Input &input, const ControlValues &seb_rate,
|
||||
const Param ¶m)
|
||||
{
|
||||
if (param.integrator_gain_pitch > FLT_EPSILON) {
|
||||
|
||||
// Calculate derivative from change in climb angle to rate of change of specific energy balance
|
||||
const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G;
|
||||
|
||||
// Calculate pitch integrator input term
|
||||
float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch;
|
||||
float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch / climb_angle_to_SEB_rate;
|
||||
|
||||
// Prevent the integrator changing in a direction that will increase pitch demand saturation
|
||||
if (_pitch_setpoint >= param.pitch_max) {
|
||||
@@ -463,27 +463,19 @@ void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rat
|
||||
float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param ¶m,
|
||||
const Flag &flag) const
|
||||
{
|
||||
// Pitch setpoint
|
||||
// Calculate a specific energy correction that doesn't include the integrator contribution
|
||||
float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain + _pitch_integ_state +
|
||||
param.seb_rate_ff *
|
||||
seb_rate.setpoint;
|
||||
|
||||
// Calculate derivative from change in climb angle to rate of change of specific energy balance
|
||||
const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G;
|
||||
|
||||
// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
|
||||
// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
|
||||
// having to catch up before the nose can be raised to reduce excess speed during climbout.
|
||||
if (flag.climbout_mode_active) {
|
||||
SEB_rate_correction += param.pitch_min * climb_angle_to_SEB_rate;
|
||||
}
|
||||
// Calculate a specific energy correction that doesn't include the integrator contribution
|
||||
float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain +
|
||||
param.seb_rate_ff *
|
||||
seb_rate.setpoint;
|
||||
|
||||
// Convert the specific energy balance rate correction to a target pitch angle. This calculation assumes:
|
||||
// a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop.
|
||||
// b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
|
||||
// pitch transients due to control action or turbulence.
|
||||
const float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate;
|
||||
const float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate + _pitch_integ_state;
|
||||
|
||||
return constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max);
|
||||
}
|
||||
@@ -562,12 +554,6 @@ void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit
|
||||
// This will be added to the total throttle demand to compensate for steady state errors
|
||||
_throttle_integ_state = _throttle_integ_state + throttle_integ_input;
|
||||
|
||||
if (flag.climbout_mode_active) {
|
||||
// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
|
||||
// at end of climbout when we transition to closed loop throttle control
|
||||
_throttle_integ_state = param.throttle_max - _throttle_setpoint;
|
||||
}
|
||||
|
||||
} else {
|
||||
_throttle_integ_state = 0.0f;
|
||||
}
|
||||
@@ -720,7 +706,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
|
||||
_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
|
||||
_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
|
||||
const TECSReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()};
|
||||
_debug_status.altitude_sp = ref_alt.alt;
|
||||
_debug_status.altitude_sp_ref = ref_alt.alt;
|
||||
_debug_status.altitude_rate_alt_ref = ref_alt.alt_rate;
|
||||
_debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference();
|
||||
|
||||
@@ -728,7 +714,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
|
||||
}
|
||||
|
||||
void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
|
||||
float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max,
|
||||
float eas_to_tas, float throttle_min, float throttle_setpoint_max,
|
||||
float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
|
||||
const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
|
||||
{
|
||||
@@ -748,7 +734,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
|
||||
_control_param.throttle_trim = throttle_trim;
|
||||
_control_param.throttle_max = throttle_setpoint_max;
|
||||
_control_param.throttle_min = throttle_min;
|
||||
_control_flag.climbout_mode_active = climb_out_setpoint;
|
||||
|
||||
if (dt < DT_MIN) {
|
||||
// Update intervall too small, do not update. Assume constant states/output in this case.
|
||||
@@ -804,9 +789,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
|
||||
} else if (_uncommanded_descent_recovery) {
|
||||
_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
|
||||
|
||||
} else if (climb_out_setpoint) {
|
||||
_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
|
||||
|
||||
} else {
|
||||
// This is the default operation mode
|
||||
_tecs_mode = ECL_TECS_MODE_NORMAL;
|
||||
@@ -816,7 +798,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
|
||||
_debug_status.control = _control.getDebugOutput();
|
||||
_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
|
||||
_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
|
||||
_debug_status.altitude_sp = control_setpoint.altitude_reference.alt;
|
||||
_debug_status.altitude_sp_ref = control_setpoint.altitude_reference.alt;
|
||||
_debug_status.altitude_rate_alt_ref = control_setpoint.altitude_reference.alt_rate;
|
||||
_debug_status.altitude_rate_feedforward = control_setpoint.altitude_rate_setpoint;
|
||||
}
|
||||
|
||||
+10
-13
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2017-2023 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
|
||||
@@ -257,7 +257,7 @@ public:
|
||||
*
|
||||
*/
|
||||
struct Input {
|
||||
float altitude; ///< Current altitude of the UAS [m].
|
||||
float altitude; ///< Current altitude amsl of the UAS [m].
|
||||
float altitude_rate; ///< Current altitude rate of the UAS [m/s].
|
||||
float tas; ///< Current true airspeed of the UAS [m/s].
|
||||
float tas_rate; ///< Current true airspeed rate of the UAS [m/s²].
|
||||
@@ -269,7 +269,6 @@ public:
|
||||
*/
|
||||
struct Flag {
|
||||
bool airspeed_enabled; ///< Flag if the airspeed sensor is enabled.
|
||||
bool climbout_mode_active; ///< Flag if climbout mode is activated.
|
||||
bool detect_underspeed_enabled; ///< Flag if underspeed detection is enabled.
|
||||
};
|
||||
public:
|
||||
@@ -457,13 +456,14 @@ private:
|
||||
|
||||
/**
|
||||
* @brief Calculate the pitch control update function.
|
||||
* Update the states of the pitch control
|
||||
* Update the states of the pitch control (pitch integrator).
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param input is the current input measurement of the UAS.
|
||||
* @param seb_rate is the specific energy balance rate in [m²/s³].
|
||||
* @param param is the control parameters.
|
||||
*/
|
||||
void _calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param ¶m);
|
||||
void _calcPitchControlUpdate(float dt, const Input &input, const ControlValues &seb_rate, const Param ¶m);
|
||||
|
||||
/**
|
||||
* @brief Calculate the pitch control output function.
|
||||
@@ -500,7 +500,7 @@ private:
|
||||
|
||||
/**
|
||||
* @brief Calculate the throttle control update function.
|
||||
* Update the throttle control states.
|
||||
* Update the throttle control states (throttle integrator).
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param limit is the specific total energy rate limits in [m²/s³].
|
||||
@@ -527,8 +527,7 @@ private:
|
||||
// State
|
||||
AlphaFilter<float> _ste_rate_estimate_filter; ///< Low pass filter for the specific total energy rate.
|
||||
float _pitch_integ_state{0.0f}; ///< Pitch integrator state [rad].
|
||||
float _throttle_integ_state{0.0f}; ///< Throttle integrator state.
|
||||
|
||||
float _throttle_integ_state{0.0f}; ///< Throttle integrator state [-].
|
||||
|
||||
// Output
|
||||
DebugOutput _debug_output; ///< Debug output.
|
||||
@@ -552,7 +551,7 @@ public:
|
||||
TECSControl::DebugOutput control;
|
||||
float true_airspeed_filtered;
|
||||
float true_airspeed_derivative;
|
||||
float altitude_sp;
|
||||
float altitude_sp_ref;
|
||||
float altitude_rate_alt_ref;
|
||||
float altitude_rate_feedforward;
|
||||
enum ECL_TECS_MODE tecs_mode;
|
||||
@@ -586,7 +585,7 @@ public:
|
||||
*
|
||||
*/
|
||||
void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
|
||||
float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max,
|
||||
float eas_to_tas, float throttle_min, float throttle_setpoint_max,
|
||||
float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
|
||||
float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
|
||||
|
||||
@@ -603,7 +602,7 @@ public:
|
||||
|
||||
void set_detect_underspeed_enabled(bool enabled) { _control_flag.detect_underspeed_enabled = enabled; };
|
||||
|
||||
// // setters for parameters
|
||||
// setters for parameters
|
||||
void set_airspeed_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_measurement_std_dev = std_dev;};
|
||||
void set_airspeed_rate_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_measurement_std_dev = std_dev;};
|
||||
void set_airspeed_filter_process_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_noise_std_dev = std_dev;};
|
||||
@@ -655,7 +654,6 @@ public:
|
||||
float get_pitch_setpoint() {return _control.getPitchSetpoint();}
|
||||
float get_throttle_setpoint() {return _control.getThrottleSetpoint();}
|
||||
|
||||
// // TECS status
|
||||
uint64_t timestamp() { return _update_timestamp; }
|
||||
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
|
||||
|
||||
@@ -726,7 +724,6 @@ private:
|
||||
|
||||
TECSControl::Flag _control_flag{
|
||||
.airspeed_enabled = false,
|
||||
.climbout_mode_active = false,
|
||||
.detect_underspeed_enabled = false,
|
||||
};
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2023 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
|
||||
@@ -91,13 +91,13 @@ WindEstimator::update(uint64_t time_now)
|
||||
return;
|
||||
}
|
||||
|
||||
float dt = (float)(time_now - _time_last_update) * 1e-6f;
|
||||
const float dt = (float)(time_now - _time_last_update) * 1e-6f;
|
||||
_time_last_update = time_now;
|
||||
|
||||
matrix::Matrix3f Qk;
|
||||
Qk(0, 0) = _wind_psd * dt;
|
||||
Qk(1, 1) = Qk(0, 0);
|
||||
Qk(2, 2) = _tas_scale_psd * dt;
|
||||
Qk(INDEX_W_N, INDEX_W_N) = _wind_psd * dt;
|
||||
Qk(INDEX_W_E, INDEX_W_E) = Qk(INDEX_W_N, INDEX_W_N);
|
||||
Qk(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = _tas_scale_psd * dt;
|
||||
_P += Qk;
|
||||
}
|
||||
|
||||
@@ -127,7 +127,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
|
||||
|
||||
const bool meas_is_rejected = check_if_meas_is_rejected(_tas_innov, _tas_innov_var, _tas_gate);
|
||||
|
||||
if (_tas_innov_var < 0.0f) {
|
||||
if (_tas_innov_var < FLT_EPSILON) {
|
||||
// re init filter in case of a negative variance, and trigger early return to not fuse measurement
|
||||
_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
|
||||
return;
|
||||
@@ -137,9 +137,9 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
|
||||
}
|
||||
|
||||
// apply correction to state
|
||||
_state(INDEX_W_N) += _tas_innov * K(0, 0);
|
||||
_state(INDEX_W_E) += _tas_innov * K(1, 0);
|
||||
_state(INDEX_TAS_SCALE) += _tas_innov * K(2, 0);
|
||||
_state(INDEX_W_N) += _tas_innov * K(INDEX_W_N, 0);
|
||||
_state(INDEX_W_E) += _tas_innov * K(INDEX_W_E, 0);
|
||||
_state(INDEX_TAS_SCALE) += _tas_innov * K(INDEX_TAS_SCALE, 0);
|
||||
|
||||
// update covariance matrix
|
||||
_P = _P - K * H_tas * _P;
|
||||
@@ -172,7 +172,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
|
||||
|
||||
const bool meas_is_rejected = check_if_meas_is_rejected(_beta_innov, _beta_innov_var, _beta_gate);
|
||||
|
||||
if (_beta_innov_var < 0.0f) {
|
||||
if (_beta_innov_var < FLT_EPSILON) {
|
||||
// re init filter in case of a negative variance, and trigger early return to not fuse measurement
|
||||
_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
|
||||
return;
|
||||
@@ -182,9 +182,9 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
|
||||
}
|
||||
|
||||
// apply correction to state
|
||||
_state(INDEX_W_N) += _beta_innov * K(0, 0);
|
||||
_state(INDEX_W_E) += _beta_innov * K(1, 0);
|
||||
_state(INDEX_TAS_SCALE) += _beta_innov * K(2, 0);
|
||||
_state(INDEX_W_N) += _beta_innov * K(INDEX_W_N, 0);
|
||||
_state(INDEX_W_E) += _beta_innov * K(INDEX_W_E, 0);
|
||||
_state(INDEX_TAS_SCALE) += _beta_innov * K(INDEX_TAS_SCALE, 0);
|
||||
|
||||
// update covariance matrix
|
||||
_P = _P - K * H_beta * _P;
|
||||
@@ -211,7 +211,7 @@ WindEstimator::run_sanity_checks()
|
||||
}
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_state(INDEX_W_N)) || !PX4_ISFINITE(_state(INDEX_W_E)) || !PX4_ISFINITE(_state(INDEX_TAS_SCALE))) {
|
||||
if (!_state.isAllFinite()) {
|
||||
_initialised = false;
|
||||
return;
|
||||
}
|
||||
@@ -219,7 +219,7 @@ WindEstimator::run_sanity_checks()
|
||||
// attain symmetry
|
||||
for (unsigned row = 0; row < 3; row++) {
|
||||
for (unsigned column = 0; column < row; column++) {
|
||||
float tmp = (_P(row, column) + _P(column, row)) / 2;
|
||||
const float tmp = (_P(row, column) + _P(column, row)) * 0.5f;
|
||||
_P(row, column) = tmp;
|
||||
_P(column, row) = tmp;
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -76,12 +76,12 @@ public:
|
||||
|
||||
// invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10]
|
||||
float get_tas_scale() { return 1.f / math::constrain(_state(INDEX_TAS_SCALE), 0.1f, 10.0f); }
|
||||
float get_tas_scale_var() { return _P(2, 2); }
|
||||
float get_tas_scale_var() { return _P(INDEX_TAS_SCALE, INDEX_TAS_SCALE); }
|
||||
float get_tas_innov() { return _tas_innov; }
|
||||
float get_tas_innov_var() { return _tas_innov_var; }
|
||||
float get_beta_innov() { return _beta_innov; }
|
||||
float get_beta_innov_var() { return _beta_innov_var; }
|
||||
matrix::Vector2f get_wind_var() { return matrix::Vector2f{_P(0, 0), _P(1, 1)}; }
|
||||
matrix::Vector2f get_wind_var() { return matrix::Vector2f{_P(INDEX_W_N, INDEX_W_N), _P(INDEX_W_E, INDEX_W_E)}; }
|
||||
bool get_wind_estimator_reset() { return _wind_estimator_reset; }
|
||||
|
||||
// unaided, the state uncertainty (diagonal of sqrt(P)) grows by the process noise spectral density every second
|
||||
|
||||
Regular → Executable
@@ -38,6 +38,8 @@ from symforce import symbolic as sm
|
||||
from symforce import geo
|
||||
from symforce import typing as T
|
||||
|
||||
import re
|
||||
|
||||
# q: quaternion describing rotation from frame 1 to frame 2
|
||||
# returns a rotation matrix derived form q which describes the same
|
||||
# rotation
|
||||
@@ -87,6 +89,10 @@ def generate_px4_function(function_name, output_names):
|
||||
line = line.replace("std::min", "math::min")
|
||||
line = line.replace("Eigen", "matrix")
|
||||
line = line.replace("matrix/Dense", "matrix/math.hpp")
|
||||
|
||||
# don't allow underscore + uppercase identifier naming (always reserved for any use)
|
||||
line = re.sub(r'_([A-Z])', lambda x: '_' + x.group(1).lower(), line)
|
||||
|
||||
print(line, end='')
|
||||
|
||||
def generate_python_function(function_name, output_names):
|
||||
|
||||
@@ -58,19 +58,19 @@ void FuseAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local,
|
||||
|
||||
// Output terms (4)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 1, 3>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 1, 3>& _h = (*H);
|
||||
|
||||
_H(0, 0) = -_tmp4;
|
||||
_H(0, 1) = -_tmp5;
|
||||
_H(0, 2) = _tmp2;
|
||||
_h(0, 0) = -_tmp4;
|
||||
_h(0, 1) = -_tmp5;
|
||||
_h(0, 2) = _tmp2;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _K = (*K);
|
||||
matrix::Matrix<Scalar, 3, 1>& _k = (*K);
|
||||
|
||||
_K(0, 0) = _tmp10 * (-P(0, 1) * _tmp5 + P(0, 2) * _tmp2 + _tmp6);
|
||||
_K(1, 0) = _tmp10 * (-P(1, 0) * _tmp4 + P(1, 2) * _tmp2 + _tmp7);
|
||||
_K(2, 0) = _tmp10 * (-P(2, 0) * _tmp4 - P(2, 1) * _tmp5 + _tmp8);
|
||||
_k(0, 0) = _tmp10 * (-P(0, 1) * _tmp5 + P(0, 2) * _tmp2 + _tmp6);
|
||||
_k(1, 0) = _tmp10 * (-P(1, 0) * _tmp4 + P(1, 2) * _tmp2 + _tmp7);
|
||||
_k(2, 0) = _tmp10 * (-P(2, 0) * _tmp4 - P(2, 1) * _tmp5 + _tmp8);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
|
||||
@@ -43,7 +43,7 @@ def fuse_airspeed(v_local, state, P, airspeed, R, epsilon):
|
||||
# Intermediate terms (11)
|
||||
_tmp0 = -state[0] + v_local[0]
|
||||
_tmp1 = -state[1] + v_local[1]
|
||||
_tmp2 = math.sqrt(_tmp0**2 + _tmp1**2 + epsilon + v_local[2] ** 2)
|
||||
_tmp2 = math.sqrt(_tmp0 ** 2 + _tmp1 ** 2 + epsilon + v_local[2] ** 2)
|
||||
_tmp3 = state[2] / _tmp2
|
||||
_tmp4 = _tmp0 * _tmp3
|
||||
_tmp5 = _tmp1 * _tmp3
|
||||
|
||||
@@ -70,19 +70,19 @@ void FuseBeta(const matrix::Matrix<Scalar, 3, 1>& v_local, const matrix::Matrix<
|
||||
|
||||
// Output terms (4)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 1, 3>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 1, 3>& _h = (*H);
|
||||
|
||||
_H(0, 0) = _tmp17;
|
||||
_H(0, 1) = _tmp18;
|
||||
_H(0, 2) = 0;
|
||||
_h(0, 0) = _tmp17;
|
||||
_h(0, 1) = _tmp18;
|
||||
_h(0, 2) = 0;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _K = (*K);
|
||||
matrix::Matrix<Scalar, 3, 1>& _k = (*K);
|
||||
|
||||
_K(0, 0) = _tmp22 * (P(0, 1) * _tmp18 + _tmp19);
|
||||
_K(1, 0) = _tmp22 * (P(1, 0) * _tmp17 + _tmp20);
|
||||
_K(2, 0) = _tmp22 * (P(2, 0) * _tmp17 + P(2, 1) * _tmp18);
|
||||
_k(0, 0) = _tmp22 * (P(0, 1) * _tmp18 + _tmp19);
|
||||
_k(1, 0) = _tmp22 * (P(1, 0) * _tmp17 + _tmp20);
|
||||
_k(2, 0) = _tmp22 * (P(2, 0) * _tmp17 + P(2, 1) * _tmp18);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
|
||||
@@ -56,12 +56,12 @@ void InitWindUsingAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local, const Sc
|
||||
}
|
||||
|
||||
if (P != nullptr) {
|
||||
matrix::Matrix<Scalar, 2, 2>& _P = (*P);
|
||||
matrix::Matrix<Scalar, 2, 2>& _p = (*P);
|
||||
|
||||
_P(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var;
|
||||
_P(1, 0) = _tmp6;
|
||||
_P(0, 1) = _tmp6;
|
||||
_P(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var;
|
||||
_p(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var;
|
||||
_p(1, 0) = _tmp6;
|
||||
_p(0, 1) = _tmp6;
|
||||
_p(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
@@ -47,80 +47,80 @@ static constexpr int LON_DIM = 37;
|
||||
// Magnetic declination data in radians * 10^-4
|
||||
// Model: WMM-2020,
|
||||
// Version: 0.5.1.11,
|
||||
// Date: 2022.937,
|
||||
// Date: 2023.074,
|
||||
static constexpr const int16_t declination_table[19][37] {
|
||||
// LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180,
|
||||
/* LAT: -90 */ { 25977, 24232, 22486, 20741, 18996, 17250, 15505, 13760, 12014, 10269, 8524, 6778, 5033, 3288, 1543, -203, -1948, -3693, -5438, -7184, -8929,-10674,-12420,-14165,-15910,-17656,-19401,-21146,-22892,-24637,-26383,-28128,-29873, 31213, 29468, 27722, 25977, },
|
||||
/* LAT: -80 */ { 22542, 20412, 18473, 16699, 15059, 13521, 12057, 10646, 9271, 7921, 6590, 5271, 3962, 2657, 1350, 30, -1311, -2684, -4094, -5547, -7042, -8579,-10157,-11777,-13442,-15160,-16946,-18819,-20802,-22921,-25196,-27630,-30199, 29994, 27377, 24872, 22542, },
|
||||
/* LAT: -70 */ { 14979, 13580, 12453, 11491, 10621, 9789, 8947, 8059, 7106, 6087, 5017, 3924, 2839, 1785, 763, -247, -1284, -2391, -3593, -4894, -6276, -7707, -9156,-10599,-12025,-13442,-14871,-16358,-17983,-19899,-22424,-26229, 30683, 24132, 19622, 16850, 14979, },
|
||||
/* LAT: -60 */ { 8427, 8180, 7898, 7622, 7368, 7113, 6804, 6371, 5755, 4935, 3934, 2822, 1703, 677, -203, -959, -1688, -2511, -3517, -4718, -6051, -7423, -8745, -9956,-11020,-11919,-12634,-13128,-13300,-12851,-10748, -3476, 4938, 7673, 8439, 8563, 8427, },
|
||||
/* LAT: -50 */ { 5490, 5526, 5469, 5379, 5304, 5266, 5231, 5103, 4759, 4095, 3084, 1811, 471, -707, -1574, -2134, -2528, -2969, -3659, -4679, -5928, -7208, -8350, -9251, -9847,-10084, -9887, -9126, -7615, -5251, -2345, 403, 2513, 3938, 4812, 5286, 5490, },
|
||||
/* LAT: -40 */ { 3958, 4051, 4058, 4012, 3951, 3917, 3921, 3910, 3736, 3200, 2175, 732, -831, -2142, -3000, -3446, -3625, -3683, -3855, -4435, -5421, -6502, -7387, -7923, -8025, -7643, -6748, -5362, -3652, -1953, -495, 726, 1770, 2637, 3295, 3726, 3958, },
|
||||
/* LAT: -30 */ { 2986, 3072, 3102, 3086, 3027, 2948, 2887, 2853, 2726, 2247, 1209, -313, -1920, -3169, -3904, -4244, -4318, -4103, -3668, -3456, -3834, -4588, -5284, -5629, -5498, -4906, -3943, -2738, -1534, -593, 82, 681, 1306, 1911, 2423, 2786, 2986, },
|
||||
/* LAT: -20 */ { 2344, 2389, 2406, 2407, 2363, 2268, 2160, 2083, 1938, 1441, 381, -1110, -2588, -3645, -4171, -4282, -4074, -3510, -2647, -1856, -1591, -1954, -2605, -3070, -3101, -2738, -2101, -1286, -514, -30, 230, 531, 973, 1454, 1880, 2189, 2344, },
|
||||
/* LAT: -10 */ { 1950, 1944, 1921, 1917, 1888, 1802, 1691, 1599, 1415, 863, -202, -1581, -2852, -3676, -3936, -3702, -3123, -2340, -1495, -743, -284, -323, -797, -1302, -1511, -1419, -1104, -607, -113, 124, 170, 328, 701, 1142, 1539, 1829, 1950, },
|
||||
/* LAT: 0 */ { 1737, 1703, 1646, 1636, 1624, 1554, 1447, 1332, 1078, 457, -585, -1812, -2863, -3444, -3437, -2932, -2163, -1382, -731, -197, 214, 322, 40, -377, -630, -685, -587, -326, -39, 47, -17, 70, 413, 859, 1283, 1606, 1737, },
|
||||
/* LAT: 10 */ { 1600, 1608, 1564, 1580, 1604, 1554, 1431, 1243, 867, 150, -871, -1946, -2770, -3104, -2895, -2286, -1510, -799, -286, 94, 418, 565, 401, 78, -155, -263, -288, -209, -107, -151, -294, -269, 35, 493, 978, 1388, 1600, },
|
||||
/* LAT: 20 */ { 1414, 1562, 1622, 1714, 1800, 1778, 1622, 1319, 775, -81, -1122, -2071, -2665, -2771, -2440, -1837, -1126, -478, -19, 290, 548, 693, 600, 357, 158, 39, -55, -122, -205, -397, -636, -695, -459, -9, 535, 1055, 1414, },
|
||||
/* LAT: 30 */ { 1109, 1475, 1735, 1959, 2119, 2129, 1941, 1518, 791, -236, -1351, -2225, -2636, -2572, -2172, -1593, -937, -320, 141, 446, 677, 822, 801, 651, 502, 377, 217, 2, -284, -660, -1030, -1189, -1028, -600, -22, 588, 1109, },
|
||||
/* LAT: 40 */ { 747, 1333, 1828, 2221, 2471, 2515, 2300, 1767, 857, -367, -1601, -2464, -2783, -2630, -2184, -1592, -938, -309, 200, 564, 834, 1031, 1124, 1112, 1036, 887, 615, 199, -343, -951, -1473, -1715, -1598, -1180, -582, 89, 747, },
|
||||
/* LAT: 50 */ { 453, 1199, 1880, 2439, 2808, 2916, 2688, 2036, 898, -593, -2010, -2922, -3215, -3024, -2534, -1888, -1178, -481, 135, 640, 1058, 1412, 1691, 1860, 1878, 1689, 1239, 525, -373, -1277, -1954, -2238, -2112, -1671, -1038, -309, 453, },
|
||||
/* LAT: 60 */ { 250, 1101, 1907, 2605, 3112, 3328, 3119, 2314, 808, -1141, -2851, -3829, -4083, -3824, -3253, -2509, -1683, -843, -35, 716, 1407, 2037, 2579, 2973, 3130, 2940, 2294, 1170, -249, -1569, -2434, -2740, -2574, -2082, -1393, -596, 250, },
|
||||
/* LAT: 70 */ { 2, 935, 1832, 2633, 3251, 3550, 3307, 2192, -14, -2700, -4648, -5480, -5497, -5019, -4253, -3321, -2299, -1234, -160, 899, 1924, 2890, 3757, 4463, 4908, 4935, 4318, 2832, 628, -1482, -2777, -3218, -3060, -2531, -1786, -921, 2, },
|
||||
/* LAT: 80 */ { -747, 173, 1025, 1716, 2104, 1936, 785, -1722, -4840, -6956, -7735, -7633, -7024, -6120, -5038, -3846, -2584, -1282, 41, 1367, 2681, 3963, 5190, 6330, 7327, 8089, 8433, 7983, 6027, 2185, -1455, -3170, -3527, -3194, -2517, -1668, -747, },
|
||||
/* LAT: 90 */ { -29691,-27945,-26200,-24454,-22709,-20964,-19218,-17473,-15728,-13983,-12237,-10492, -8747, -7002, -5257, -3512, -1766, -21, 1724, 3469, 5214, 6960, 8705, 10450, 12196, 13941, 15686, 17432, 19177, 20923, 22668, 24414, 26159, 27905, 29650, 31396,-29691, },
|
||||
/* LAT: -90 */ { 25974, 24228, 22483, 20738, 18992, 17247, 15502, 13756, 12011, 10266, 8520, 6775, 5030, 3285, 1539, -206, -1951, -3697, -5442, -7187, -8932,-10678,-12423,-14168,-15914,-17659,-19404,-21150,-22895,-24641,-26386,-28131,-29877, 31210, 29464, 27719, 25974, },
|
||||
/* LAT: -80 */ { 22539, 20409, 18470, 16697, 15057, 13519, 12055, 10644, 9269, 7919, 6588, 5269, 3960, 2655, 1348, 28, -1314, -2686, -4097, -5550, -7045, -8583,-10161,-11781,-13446,-15165,-16951,-18824,-20807,-22927,-25201,-27636,-30204, 29989, 27372, 24868, 22539, },
|
||||
/* LAT: -70 */ { 14980, 13581, 12454, 11491, 10621, 9788, 8946, 8058, 7105, 6086, 5016, 3923, 2838, 1784, 762, -247, -1285, -2392, -3595, -4897, -6279, -7711, -9160,-10603,-12030,-13447,-14876,-16363,-17989,-19907,-22434,-26242, 30671, 24126, 19621, 16851, 14980, },
|
||||
/* LAT: -60 */ { 8431, 8183, 7900, 7624, 7369, 7114, 6804, 6370, 5754, 4934, 3932, 2821, 1702, 676, -203, -958, -1687, -2511, -3518, -4720, -6054, -7427, -8749, -9960,-11024,-11923,-12637,-13132,-13304,-12855,-10750, -3465, 4953, 7682, 8446, 8568, 8431, },
|
||||
/* LAT: -50 */ { 5493, 5528, 5471, 5380, 5305, 5267, 5231, 5103, 4758, 4093, 3081, 1808, 468, -708, -1573, -2132, -2525, -2967, -3658, -4680, -5931, -7213, -8355, -9255, -9851,-10086, -9887, -9126, -7614, -5249, -2342, 406, 2516, 3941, 4815, 5289, 5493, },
|
||||
/* LAT: -40 */ { 3960, 4053, 4059, 4013, 3952, 3917, 3921, 3909, 3735, 3198, 2172, 728, -835, -2144, -3000, -3444, -3621, -3678, -3852, -4435, -5425, -6507, -7392, -7926, -8027, -7642, -6746, -5359, -3650, -1951, -494, 727, 1771, 2639, 3297, 3728, 3960, },
|
||||
/* LAT: -30 */ { 2988, 3074, 3103, 3087, 3027, 2948, 2887, 2852, 2725, 2245, 1205, -318, -1925, -3172, -3904, -4243, -4315, -4097, -3662, -3454, -3836, -4592, -5288, -5631, -5498, -4904, -3940, -2735, -1532, -593, 81, 681, 1306, 1912, 2424, 2788, 2988, },
|
||||
/* LAT: -20 */ { 2346, 2391, 2407, 2407, 2363, 2267, 2158, 2082, 1936, 1438, 377, -1116, -2593, -3648, -4172, -4280, -4070, -3504, -2640, -1851, -1591, -1957, -2607, -3071, -3101, -2736, -2098, -1284, -514, -30, 229, 529, 972, 1454, 1880, 2191, 2346, },
|
||||
/* LAT: -10 */ { 1952, 1946, 1923, 1917, 1888, 1801, 1689, 1597, 1412, 860, -206, -1586, -2856, -3678, -3935, -3699, -3119, -2335, -1489, -739, -282, -324, -798, -1303, -1511, -1418, -1103, -605, -113, 123, 168, 326, 699, 1142, 1540, 1831, 1952, },
|
||||
/* LAT: 0 */ { 1739, 1705, 1647, 1636, 1624, 1553, 1445, 1330, 1075, 454, -589, -1816, -2866, -3445, -3435, -2928, -2158, -1377, -727, -193, 217, 323, 39, -378, -630, -684, -586, -326, -39, 46, -19, 67, 411, 859, 1283, 1607, 1739, },
|
||||
/* LAT: 10 */ { 1601, 1609, 1565, 1580, 1604, 1553, 1429, 1240, 864, 146, -875, -1949, -2771, -3104, -2893, -2282, -1505, -796, -283, 96, 421, 566, 401, 78, -155, -262, -288, -209, -108, -153, -296, -271, 33, 493, 978, 1389, 1601, },
|
||||
/* LAT: 20 */ { 1414, 1563, 1623, 1714, 1799, 1777, 1620, 1316, 772, -84, -1125, -2073, -2665, -2769, -2437, -1833, -1122, -474, -16, 292, 550, 694, 601, 357, 159, 40, -55, -123, -206, -398, -638, -697, -461, -10, 535, 1056, 1414, },
|
||||
/* LAT: 30 */ { 1108, 1475, 1735, 1959, 2118, 2128, 1939, 1516, 789, -238, -1352, -2225, -2635, -2570, -2169, -1589, -933, -316, 144, 449, 679, 824, 802, 652, 504, 378, 217, 1, -285, -662, -1032, -1191, -1030, -601, -23, 587, 1108, },
|
||||
/* LAT: 40 */ { 746, 1331, 1826, 2219, 2469, 2513, 2298, 1765, 855, -368, -1602, -2463, -2780, -2626, -2180, -1587, -934, -305, 204, 568, 837, 1034, 1125, 1114, 1038, 888, 615, 198, -346, -954, -1475, -1718, -1599, -1181, -583, 88, 746, },
|
||||
/* LAT: 50 */ { 449, 1195, 1877, 2436, 2805, 2913, 2686, 2034, 897, -593, -2008, -2918, -3211, -3019, -2529, -1883, -1173, -476, 139, 644, 1062, 1415, 1693, 1862, 1880, 1689, 1238, 523, -377, -1280, -1957, -2240, -2113, -1673, -1040, -312, 449, },
|
||||
/* LAT: 60 */ { 244, 1095, 1901, 2599, 3107, 3324, 3116, 2312, 809, -1137, -2844, -3822, -4075, -3817, -3246, -2502, -1676, -837, -30, 721, 1412, 2042, 2583, 2976, 3133, 2940, 2293, 1167, -254, -1573, -2437, -2742, -2576, -2084, -1396, -601, 244, },
|
||||
/* LAT: 70 */ { -8, 925, 1822, 2622, 3241, 3541, 3301, 2190, -8, -2686, -4631, -5465, -5483, -5007, -4243, -3312, -2290, -1226, -152, 907, 1931, 2896, 3763, 4468, 4912, 4938, 4317, 2828, 621, -1489, -2783, -3223, -3065, -2537, -1794, -930, -8, },
|
||||
/* LAT: 80 */ { -771, 149, 1000, 1691, 2080, 1915, 776, -1705, -4800, -6913, -7700, -7604, -7000, -6100, -5021, -3830, -2570, -1269, 54, 1379, 2692, 3974, 5202, 6341, 7339, 8100, 8444, 7991, 6025, 2161, -1488, -3200, -3553, -3219, -2541, -1692, -771, },
|
||||
/* LAT: 90 */ { -29638,-27893,-26147,-24402,-22657,-20911,-19166,-17421,-15675,-13930,-12185,-10440, -8695, -6949, -5204, -3459, -1714, 31, 1776, 3521, 5267, 7012, 8757, 10503, 12248, 13993, 15739, 17484, 19229, 20975, 22720, 24466, 26211, 27957, 29702,-31384,-29638, },
|
||||
};
|
||||
|
||||
// Magnetic inclination data in radians * 10^-4
|
||||
// Model: WMM-2020,
|
||||
// Version: 0.5.1.11,
|
||||
// Date: 2022.937,
|
||||
// Date: 2023.074,
|
||||
static constexpr const int16_t inclination_table[19][37] {
|
||||
// LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180,
|
||||
/* LAT: -90 */ { -12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569,-12569, },
|
||||
/* LAT: -80 */ { -13653,-13519,-13359,-13178,-12985,-12783,-12579,-12379,-12188,-12012,-11856,-11721,-11611,-11525,-11461,-11420,-11400,-11403,-11430,-11484,-11567,-11682,-11827,-12001,-12199,-12416,-12645,-12878,-13106,-13319,-13505,-13655,-13759,-13810,-13806,-13752,-13653, },
|
||||
/* LAT: -70 */ { -14101,-13783,-13463,-13140,-12809,-12465,-12110,-11753,-11410,-11102,-10850,-10666,-10554,-10502,-10488,-10491,-10497,-10506,-10529,-10584,-10693,-10868,-11114,-11429,-11803,-12221,-12669,-13133,-13599,-14052,-14469,-14813,-15001,-14946,-14715,-14417,-14101, },
|
||||
/* LAT: -60 */ { -13516,-13162,-12824,-12491,-12148,-11775,-11360,-10906,-10439,-10009, -9680, -9505, -9503, -9640, -9844,-10035,-10159,-10200,-10186,-10171,-10218,-10378,-10668,-11078,-11578,-12134,-12721,-13317,-13906,-14467,-14964,-15253,-15075,-14690,-14283,-13889,-13516, },
|
||||
/* LAT: -50 */ { -12495,-12152,-11821,-11499,-11175,-10829,-10430, -9959, -9429, -8909, -8521, -8397, -8600, -9062, -9625,-10135,-10491,-10650,-10617,-10463,-10321,-10328,-10552,-10971,-11514,-12107,-12694,-13232,-13677,-13975,-14083,-14009,-13806,-13522,-13193,-12845,-12495, },
|
||||
/* LAT: -40 */ { -11239,-10890,-10543,-10197, -9858, -9520, -9159, -8734, -8214, -7650, -7226, -7188, -7652, -8477, -9398,-10225,-10881,-11312,-11446,-11274,-10930,-10659,-10656,-10941,-11405,-11910,-12355,-12675,-12833,-12838,-12748,-12612,-12433,-12201,-11915,-11586,-11239, },
|
||||
/* LAT: -30 */ { -9602, -9222, -8842, -8451, -8059, -7684, -7328, -6938, -6426, -5815, -5366, -5456, -6232, -7437, -8688, -9780,-10691,-11396,-11789,-11773,-11397,-10889,-10559,-10563,-10817,-11138,-11393,-11504,-11440,-11265,-11092,-10959,-10814,-10608,-10327, -9981, -9602, },
|
||||
/* LAT: -20 */ { -7372, -6929, -6510, -6082, -5637, -5208, -4818, -4407, -3842, -3158, -2714, -2984, -4105, -5722, -7349, -8713, -9773,-10543,-10975,-11003,-10633,-10017, -9461, -9220, -9272, -9434, -9574, -9589, -9416, -9144, -8953, -8869, -8768, -8562, -8246, -7832, -7372, },
|
||||
/* LAT: -10 */ { -4416, -3877, -3421, -2983, -2526, -2076, -1663, -1212, -588, 114, 470, 31, -1310, -3233, -5206, -6806, -7893, -8520, -8779, -8710, -8295, -7606, -6943, -6601, -6564, -6657, -6773, -6795, -6606, -6311, -6160, -6175, -6140, -5926, -5542, -5013, -4416, },
|
||||
/* LAT: 0 */ { -908, -281, 187, 591, 1009, 1426, 1814, 2254, 2835, 3409, 3605, 3099, 1792, -125, -2167, -3807, -4805, -5225, -5278, -5103, -4657, -3936, -3231, -2863, -2804, -2872, -2996, -3068, -2938, -2704, -2654, -2804, -2874, -2691, -2266, -1634, -908, },
|
||||
/* LAT: 10 */ { 2559, 3189, 3626, 3968, 4323, 4690, 5040, 5424, 5871, 6238, 6269, 5773, 4682, 3111, 1422, 55, -740, -979, -882, -643, -227, 415, 1048, 1383, 1446, 1403, 1304, 1213, 1255, 1357, 1277, 1003, 805, 874, 1226, 1831, 2559, },
|
||||
/* LAT: 20 */ { 5415, 5945, 6326, 6622, 6936, 7279, 7621, 7965, 8296, 8493, 8402, 7934, 7094, 5995, 4871, 3968, 3446, 3333, 3487, 3733, 4067, 4534, 4995, 5249, 5306, 5291, 5244, 5186, 5174, 5154, 4977, 4639, 4337, 4245, 4417, 4842, 5415, },
|
||||
/* LAT: 30 */ { 7568, 7942, 8260, 8543, 8852, 9199, 9556, 9895, 10170, 10281, 10133, 9709, 9078, 8367, 7708, 7200, 6912, 6874, 7019, 7232, 7481, 7781, 8070, 8244, 8301, 8316, 8320, 8310, 8286, 8202, 7978, 7622, 7265, 7043, 7027, 7223, 7568, },
|
||||
/* LAT: 40 */ { 9266, 9487, 9743, 10028, 10354, 10715, 11082, 11420, 11670, 11751, 11600, 11239, 10765, 10293, 9898, 9617, 9470, 9468, 9581, 9743, 9920, 10105, 10277, 10402, 10480, 10542, 10598, 10631, 10614, 10502, 10258, 9907, 9538, 9253, 9110, 9123, 9266, },
|
||||
/* LAT: 50 */ { 10802, 10923, 11124, 11393, 11716, 12069, 12423, 12739, 12959, 13018, 12878, 12580, 12216, 11872, 11597, 11412, 11321, 11319, 11386, 11490, 11605, 11721, 11838, 11954, 12072, 12196, 12310, 12382, 12372, 12246, 11998, 11670, 11330, 11045, 10856, 10775, 10802, },
|
||||
/* LAT: 60 */ { 12319, 12391, 12541, 12759, 13028, 13328, 13630, 13895, 14071, 14098, 13963, 13715, 13426, 13156, 12936, 12780, 12690, 12660, 12676, 12724, 12793, 12879, 12985, 13117, 13277, 13453, 13618, 13726, 13731, 13610, 13385, 13106, 12827, 12591, 12421, 12329, 12319, },
|
||||
/* LAT: 70 */ { 13758, 13800, 13894, 14035, 14214, 14417, 14624, 14805, 14910, 14891, 14756, 14555, 14338, 14135, 13962, 13827, 13733, 13679, 13660, 13673, 13716, 13788, 13892, 14029, 14196, 14383, 14567, 14707, 14753, 14680, 14518, 14319, 14125, 13960, 13840, 13771, 13758, },
|
||||
/* LAT: 80 */ { 14996, 15008, 15044, 15103, 15179, 15263, 15341, 15387, 15371, 15294, 15183, 15060, 14938, 14824, 14725, 14643, 14581, 14540, 14522, 14526, 14552, 14602, 14674, 14766, 14878, 15005, 15141, 15273, 15380, 15424, 15384, 15297, 15201, 15116, 15051, 15010, 14996, },
|
||||
/* LAT: -90 */ { -12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568, },
|
||||
/* LAT: -80 */ { -13652,-13518,-13358,-13177,-12983,-12782,-12578,-12378,-12187,-12011,-11855,-11721,-11610,-11524,-11461,-11419,-11399,-11402,-11429,-11483,-11567,-11681,-11826,-12000,-12198,-12415,-12645,-12878,-13106,-13318,-13504,-13654,-13758,-13809,-13805,-13751,-13652, },
|
||||
/* LAT: -70 */ { -14100,-13781,-13462,-13139,-12807,-12464,-12109,-11752,-11409,-11101,-10849,-10666,-10554,-10502,-10488,-10491,-10497,-10505,-10528,-10583,-10692,-10867,-11114,-11429,-11803,-12221,-12669,-13133,-13599,-14052,-14469,-14813,-15000,-14945,-14714,-14416,-14100, },
|
||||
/* LAT: -60 */ { -13515,-13161,-12823,-12490,-12147,-11774,-11360,-10905,-10438,-10009, -9680, -9506, -9504, -9641, -9845,-10036,-10159,-10199,-10185,-10169,-10216,-10376,-10668,-11078,-11578,-12135,-12722,-13318,-13906,-14468,-14965,-15254,-15075,-14689,-14282,-13889,-13515, },
|
||||
/* LAT: -50 */ { -12494,-12152,-11820,-11498,-11175,-10828,-10429, -9958, -9429, -8909, -8521, -8399, -8603, -9065, -9628,-10137,-10492,-10650,-10616,-10461,-10319,-10327,-10551,-10971,-11515,-12108,-12695,-13233,-13678,-13975,-14083,-14010,-13806,-13522,-13193,-12845,-12494, },
|
||||
/* LAT: -40 */ { -11239,-10890,-10542,-10196, -9857, -9519, -9159, -8734, -8214, -7650, -7227, -7191, -7656, -8482, -9403,-10229,-10884,-11313,-11446,-11272,-10927,-10657,-10655,-10942,-11406,-11912,-12356,-12676,-12833,-12838,-12748,-12612,-12433,-12202,-11915,-11587,-11239, },
|
||||
/* LAT: -30 */ { -9602, -9222, -8841, -8450, -8058, -7683, -7328, -6938, -6426, -5815, -5368, -5459, -6238, -7445, -8695, -9786,-10696,-11400,-11791,-11772,-11394,-10886,-10557,-10563,-10818,-11139,-11394,-11504,-11440,-11264,-11091,-10959,-10815,-10609,-10328, -9981, -9602, },
|
||||
/* LAT: -20 */ { -7372, -6929, -6509, -6080, -5636, -5206, -4817, -4407, -3842, -3158, -2716, -2989, -4113, -5732, -7358, -8721, -9779,-10547,-10977,-11002,-10632,-10014, -9458, -9219, -9271, -9434, -9573, -9588, -9414, -9143, -8952, -8869, -8769, -8563, -8247, -7833, -7372, },
|
||||
/* LAT: -10 */ { -4417, -3876, -3419, -2981, -2523, -2073, -1661, -1211, -588, 113, 467, 25, -1319, -3245, -5217, -6815, -7899, -8523, -8780, -8710, -8293, -7603, -6940, -6599, -6563, -6656, -6771, -6793, -6604, -6309, -6158, -6175, -6141, -5928, -5544, -5014, -4417, },
|
||||
/* LAT: 0 */ { -909, -280, 189, 593, 1012, 1428, 1816, 2255, 2834, 3407, 3602, 3092, 1782, -137, -2178, -3815, -4810, -5227, -5279, -5102, -4655, -3933, -3228, -2861, -2801, -2870, -2993, -3065, -2935, -2701, -2652, -2804, -2876, -2693, -2269, -1636, -909, },
|
||||
/* LAT: 10 */ { 2559, 3190, 3628, 3970, 4326, 4692, 5042, 5424, 5871, 6236, 6266, 5767, 4674, 3101, 1413, 48, -744, -981, -883, -643, -226, 418, 1051, 1385, 1448, 1406, 1307, 1216, 1258, 1360, 1279, 1003, 803, 871, 1224, 1829, 2559, },
|
||||
/* LAT: 20 */ { 5415, 5946, 6328, 6624, 6938, 7281, 7622, 7966, 8295, 8491, 8399, 7930, 7088, 5988, 4865, 3963, 3443, 3332, 3487, 3734, 4068, 4536, 4997, 5251, 5309, 5294, 5247, 5189, 5176, 5156, 4978, 4639, 4335, 4243, 4415, 4840, 5415, },
|
||||
/* LAT: 30 */ { 7568, 7943, 8261, 8544, 8853, 9200, 9556, 9895, 10169, 10279, 10131, 9706, 9074, 8362, 7704, 7196, 6910, 6873, 7019, 7233, 7482, 7783, 8072, 8245, 8303, 8318, 8322, 8312, 8288, 8204, 7979, 7623, 7264, 7043, 7027, 7222, 7568, },
|
||||
/* LAT: 40 */ { 9266, 9487, 9743, 10028, 10355, 10715, 11082, 11419, 11669, 11749, 11597, 11236, 10762, 10290, 9896, 9615, 9469, 9468, 9581, 9744, 9921, 10106, 10278, 10403, 10482, 10544, 10600, 10633, 10615, 10503, 10259, 9907, 9538, 9253, 9110, 9123, 9266, },
|
||||
/* LAT: 50 */ { 10802, 10923, 11124, 11393, 11716, 12069, 12422, 12737, 12958, 13016, 12876, 12578, 12214, 11870, 11596, 11411, 11320, 11318, 11386, 11491, 11606, 11722, 11839, 11955, 12074, 12197, 12311, 12384, 12373, 12247, 11999, 11670, 11330, 11045, 10856, 10776, 10802, },
|
||||
/* LAT: 60 */ { 12319, 12391, 12541, 12758, 13027, 13327, 13628, 13894, 14069, 14096, 13961, 13713, 13425, 13155, 12935, 12779, 12690, 12660, 12676, 12725, 12794, 12879, 12986, 13119, 13278, 13455, 13619, 13728, 13732, 13611, 13385, 13106, 12828, 12591, 12421, 12329, 12319, },
|
||||
/* LAT: 70 */ { 13758, 13799, 13893, 14034, 14212, 14415, 14622, 14803, 14908, 14890, 14755, 14554, 14338, 14134, 13961, 13827, 13733, 13679, 13660, 13674, 13716, 13789, 13893, 14030, 14197, 14384, 14568, 14708, 14753, 14680, 14518, 14320, 14125, 13961, 13840, 13771, 13758, },
|
||||
/* LAT: 80 */ { 14995, 15007, 15043, 15102, 15177, 15261, 15339, 15385, 15369, 15293, 15183, 15060, 14938, 14824, 14725, 14643, 14581, 14541, 14522, 14526, 14553, 14603, 14674, 14767, 14879, 15006, 15142, 15274, 15381, 15425, 15385, 15297, 15201, 15116, 15050, 15010, 14995, },
|
||||
/* LAT: 90 */ { 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, },
|
||||
};
|
||||
|
||||
// Magnetic strength data in milli-Gauss * 10
|
||||
// Model: WMM-2020,
|
||||
// Version: 0.5.1.11,
|
||||
// Date: 2022.937,
|
||||
// Date: 2023.074,
|
||||
static constexpr const int16_t strength_table[19][37] {
|
||||
// LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180,
|
||||
/* LAT: -90 */ { 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, 5449, },
|
||||
/* LAT: -80 */ { 6056, 5992, 5913, 5821, 5717, 5606, 5488, 5366, 5243, 5124, 5009, 4904, 4811, 4731, 4668, 4623, 4599, 4597, 4619, 4665, 4735, 4829, 4944, 5076, 5220, 5370, 5520, 5663, 5794, 5908, 6000, 6069, 6113, 6132, 6128, 6102, 6056, },
|
||||
/* LAT: -70 */ { 6300, 6167, 6016, 5851, 5672, 5480, 5274, 5060, 4841, 4626, 4423, 4241, 4083, 3952, 3848, 3772, 3725, 3713, 3741, 3818, 3946, 4127, 4355, 4624, 4919, 5226, 5530, 5814, 6063, 6267, 6418, 6513, 6555, 6547, 6497, 6412, 6300, },
|
||||
/* LAT: -60 */ { 6185, 5993, 5791, 5582, 5363, 5128, 4871, 4593, 4301, 4011, 3745, 3518, 3338, 3204, 3105, 3032, 2981, 2961, 2987, 3078, 3247, 3499, 3827, 4213, 4636, 5070, 5491, 5875, 6200, 6448, 6611, 6690, 6691, 6628, 6514, 6362, 6185, },
|
||||
/* LAT: -50 */ { 5843, 5612, 5380, 5149, 4916, 4671, 4401, 4097, 3769, 3437, 3135, 2896, 2735, 2644, 2595, 2561, 2527, 2500, 2505, 2578, 2753, 3044, 3441, 3913, 4420, 4926, 5401, 5819, 6158, 6399, 6537, 6578, 6534, 6424, 6262, 6063, 5843, },
|
||||
/* LAT: -40 */ { 5392, 5146, 4901, 4661, 4426, 4189, 3934, 3650, 3336, 3010, 2712, 2490, 2373, 2348, 2367, 2388, 2392, 2379, 2366, 2395, 2527, 2804, 3223, 3738, 4285, 4809, 5275, 5663, 5955, 6142, 6230, 6233, 6163, 6031, 5850, 5632, 5392, },
|
||||
/* LAT: -30 */ { 4878, 4637, 4397, 4163, 3936, 3717, 3498, 3267, 3011, 2736, 2478, 2296, 2227, 2252, 2319, 2390, 2455, 2504, 2526, 2538, 2606, 2806, 3171, 3660, 4192, 4688, 5106, 5422, 5624, 5724, 5750, 5723, 5643, 5511, 5331, 5115, 4878, },
|
||||
/* LAT: -20 */ { 4321, 4108, 3899, 3694, 3498, 3315, 3146, 2983, 2805, 2607, 2416, 2282, 2242, 2286, 2376, 2486, 2614, 2741, 2829, 2864, 2890, 2987, 3229, 3614, 4065, 4491, 4839, 5073, 5177, 5185, 5155, 5108, 5025, 4898, 4731, 4534, 4321, },
|
||||
/* LAT: -10 */ { 3790, 3629, 3476, 3329, 3193, 3073, 2970, 2878, 2780, 2665, 2543, 2444, 2399, 2424, 2511, 2640, 2796, 2954, 3077, 3139, 3153, 3180, 3305, 3558, 3885, 4207, 4471, 4633, 4668, 4615, 4547, 4484, 4396, 4271, 4122, 3958, 3790, },
|
||||
/* LAT: 0 */ { 3412, 3319, 3235, 3162, 3106, 3068, 3042, 3023, 2998, 2950, 2871, 2776, 2696, 2666, 2709, 2812, 2944, 3080, 3194, 3269, 3300, 3322, 3397, 3556, 3765, 3979, 4158, 4264, 4269, 4202, 4114, 4021, 3910, 3778, 3644, 3520, 3412, },
|
||||
/* LAT: 10 */ { 3283, 3251, 3231, 3227, 3251, 3298, 3354, 3407, 3441, 3431, 3362, 3248, 3121, 3027, 3002, 3043, 3125, 3223, 3323, 3408, 3472, 3534, 3623, 3742, 3877, 4016, 4135, 4206, 4207, 4145, 4036, 3892, 3730, 3571, 3435, 3339, 3283, },
|
||||
/* LAT: 20 */ { 3399, 3402, 3428, 3481, 3573, 3694, 3822, 3939, 4020, 4032, 3958, 3817, 3650, 3511, 3436, 3424, 3460, 3533, 3629, 3727, 3818, 3916, 4027, 4139, 4248, 4360, 4462, 4528, 4539, 4480, 4342, 4140, 3912, 3703, 3540, 3438, 3399, },
|
||||
/* LAT: 30 */ { 3722, 3729, 3783, 3882, 4024, 4195, 4370, 4526, 4634, 4661, 4588, 4433, 4244, 4081, 3976, 3930, 3934, 3986, 4073, 4171, 4269, 4375, 4491, 4609, 4729, 4856, 4977, 5063, 5089, 5031, 4874, 4634, 4360, 4106, 3906, 3778, 3722, },
|
||||
/* LAT: 40 */ { 4222, 4219, 4284, 4407, 4573, 4760, 4943, 5100, 5205, 5230, 5162, 5014, 4828, 4654, 4527, 4452, 4426, 4449, 4510, 4589, 4676, 4773, 4888, 5022, 5173, 5335, 5487, 5596, 5635, 5580, 5424, 5186, 4912, 4653, 4442, 4297, 4222, },
|
||||
/* LAT: 50 */ { 4832, 4823, 4878, 4988, 5133, 5292, 5442, 5563, 5637, 5647, 5584, 5457, 5295, 5132, 4995, 4898, 4844, 4832, 4857, 4908, 4977, 5068, 5187, 5337, 5513, 5697, 5864, 5981, 6025, 5980, 5849, 5653, 5429, 5213, 5033, 4904, 4832, },
|
||||
/* LAT: 60 */ { 5392, 5379, 5406, 5468, 5553, 5647, 5735, 5801, 5835, 5826, 5773, 5679, 5559, 5431, 5312, 5216, 5150, 5116, 5115, 5143, 5199, 5286, 5404, 5551, 5718, 5886, 6033, 6136, 6180, 6157, 6075, 5949, 5803, 5660, 5538, 5447, 5392, },
|
||||
/* LAT: 70 */ { 5726, 5705, 5702, 5713, 5735, 5761, 5785, 5800, 5800, 5781, 5742, 5686, 5616, 5540, 5466, 5402, 5355, 5327, 5322, 5342, 5387, 5457, 5549, 5657, 5774, 5888, 5987, 6060, 6099, 6102, 6073, 6020, 5953, 5883, 5817, 5764, 5726, },
|
||||
/* LAT: 80 */ { 5790, 5772, 5757, 5745, 5735, 5725, 5715, 5704, 5689, 5671, 5649, 5624, 5597, 5570, 5546, 5526, 5512, 5507, 5512, 5527, 5553, 5588, 5631, 5679, 5729, 5777, 5819, 5854, 5878, 5891, 5893, 5886, 5871, 5852, 5831, 5810, 5790, },
|
||||
/* LAT: -90 */ { 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, },
|
||||
/* LAT: -80 */ { 6055, 5991, 5912, 5820, 5716, 5605, 5486, 5365, 5242, 5122, 5008, 4903, 4809, 4730, 4667, 4622, 4598, 4596, 4618, 4664, 4735, 4829, 4944, 5076, 5220, 5370, 5519, 5663, 5794, 5907, 6000, 6069, 6113, 6132, 6127, 6101, 6055, },
|
||||
/* LAT: -70 */ { 6299, 6166, 6015, 5850, 5671, 5478, 5273, 5058, 4839, 4624, 4422, 4240, 4082, 3951, 3847, 3771, 3724, 3712, 3741, 3817, 3946, 4126, 4355, 4624, 4919, 5226, 5530, 5814, 6063, 6267, 6418, 6513, 6554, 6547, 6496, 6412, 6299, },
|
||||
/* LAT: -60 */ { 6185, 5992, 5790, 5581, 5361, 5127, 4870, 4592, 4299, 4010, 3743, 3516, 3337, 3203, 3104, 3031, 2980, 2960, 2987, 3078, 3247, 3499, 3827, 4214, 4636, 5071, 5492, 5876, 6200, 6449, 6612, 6690, 6691, 6627, 6513, 6362, 6185, },
|
||||
/* LAT: -50 */ { 5842, 5612, 5379, 5148, 4915, 4670, 4399, 4096, 3767, 3436, 3134, 2895, 2734, 2643, 2595, 2560, 2526, 2499, 2505, 2578, 2752, 3044, 3441, 3914, 4421, 4927, 5402, 5820, 6159, 6400, 6537, 6578, 6535, 6424, 6261, 6063, 5842, },
|
||||
/* LAT: -40 */ { 5392, 5146, 4900, 4660, 4426, 4188, 3933, 3649, 3335, 3009, 2711, 2489, 2373, 2347, 2366, 2387, 2391, 2378, 2365, 2394, 2526, 2804, 3224, 3739, 4286, 4810, 5276, 5664, 5955, 6143, 6230, 6233, 6163, 6031, 5850, 5631, 5392, },
|
||||
/* LAT: -30 */ { 4878, 4636, 4397, 4162, 3936, 3717, 3498, 3266, 3010, 2735, 2477, 2296, 2227, 2252, 2319, 2390, 2454, 2504, 2524, 2536, 2605, 2807, 3171, 3661, 4193, 4690, 5107, 5422, 5625, 5724, 5750, 5723, 5644, 5511, 5331, 5114, 4878, },
|
||||
/* LAT: -20 */ { 4321, 4107, 3898, 3693, 3497, 3314, 3146, 2982, 2804, 2606, 2415, 2282, 2242, 2286, 2376, 2486, 2614, 2741, 2828, 2863, 2889, 2987, 3229, 3615, 4066, 4493, 4840, 5073, 5178, 5185, 5155, 5108, 5026, 4898, 4731, 4534, 4321, },
|
||||
/* LAT: -10 */ { 3790, 3629, 3475, 3329, 3193, 3073, 2969, 2877, 2779, 2664, 2542, 2443, 2399, 2424, 2512, 2641, 2796, 2954, 3077, 3138, 3152, 3180, 3305, 3559, 3886, 4208, 4472, 4633, 4668, 4615, 4547, 4484, 4396, 4271, 4122, 3958, 3790, },
|
||||
/* LAT: 0 */ { 3412, 3319, 3234, 3161, 3106, 3068, 3041, 3022, 2997, 2948, 2870, 2775, 2696, 2666, 2709, 2812, 2945, 3080, 3194, 3269, 3299, 3322, 3398, 3556, 3766, 3980, 4159, 4264, 4270, 4202, 4114, 4021, 3910, 3779, 3644, 3520, 3412, },
|
||||
/* LAT: 10 */ { 3283, 3251, 3230, 3227, 3251, 3298, 3353, 3406, 3440, 3430, 3361, 3247, 3120, 3027, 3002, 3044, 3125, 3223, 3323, 3408, 3472, 3534, 3623, 3742, 3878, 4017, 4136, 4206, 4208, 4146, 4036, 3892, 3731, 3571, 3436, 3339, 3283, },
|
||||
/* LAT: 20 */ { 3399, 3402, 3427, 3481, 3572, 3693, 3821, 3938, 4018, 4030, 3957, 3815, 3649, 3510, 3435, 3424, 3460, 3533, 3630, 3727, 3818, 3917, 4028, 4140, 4249, 4361, 4464, 4529, 4539, 4480, 4343, 4140, 3913, 3703, 3540, 3439, 3399, },
|
||||
/* LAT: 30 */ { 3722, 3728, 3782, 3881, 4023, 4194, 4369, 4525, 4633, 4659, 4586, 4431, 4243, 4080, 3975, 3930, 3934, 3986, 4073, 4172, 4270, 4376, 4492, 4610, 4730, 4857, 4978, 5065, 5090, 5032, 4875, 4635, 4361, 4106, 3907, 3778, 3722, },
|
||||
/* LAT: 40 */ { 4222, 4219, 4283, 4406, 4572, 4759, 4942, 5098, 5203, 5229, 5161, 5013, 4827, 4654, 4526, 4452, 4426, 4449, 4510, 4590, 4677, 4774, 4889, 5023, 5174, 5336, 5488, 5598, 5636, 5581, 5425, 5187, 4913, 4653, 4443, 4297, 4222, },
|
||||
/* LAT: 50 */ { 4832, 4823, 4878, 4987, 5132, 5291, 5441, 5562, 5636, 5646, 5583, 5456, 5294, 5131, 4995, 4898, 4844, 4833, 4858, 4908, 4978, 5069, 5188, 5339, 5514, 5699, 5865, 5982, 6025, 5980, 5849, 5653, 5429, 5214, 5034, 4904, 4832, },
|
||||
/* LAT: 60 */ { 5392, 5378, 5405, 5467, 5552, 5646, 5733, 5800, 5834, 5826, 5772, 5679, 5559, 5431, 5312, 5216, 5150, 5117, 5115, 5143, 5200, 5287, 5405, 5552, 5719, 5887, 6034, 6137, 6180, 6158, 6075, 5949, 5804, 5661, 5539, 5447, 5392, },
|
||||
/* LAT: 70 */ { 5726, 5705, 5701, 5712, 5734, 5760, 5784, 5799, 5799, 5781, 5742, 5685, 5616, 5540, 5466, 5403, 5355, 5327, 5323, 5343, 5388, 5458, 5549, 5658, 5775, 5889, 5988, 6060, 6099, 6103, 6074, 6021, 5954, 5883, 5818, 5764, 5726, },
|
||||
/* LAT: 80 */ { 5790, 5772, 5757, 5745, 5734, 5725, 5715, 5703, 5689, 5671, 5649, 5624, 5597, 5571, 5546, 5526, 5513, 5508, 5513, 5528, 5554, 5589, 5632, 5680, 5729, 5777, 5820, 5854, 5878, 5891, 5893, 5886, 5872, 5853, 5831, 5810, 5790, },
|
||||
/* LAT: 90 */ { 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, },
|
||||
};
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -25,7 +25,7 @@ PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-2f);
|
||||
* @decimal 5
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 0.0001f);
|
||||
PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 1.e-4f);
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Wind estimator true airspeed measurement noise
|
||||
|
||||
@@ -548,7 +548,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
|
||||
_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
|
||||
}
|
||||
|
||||
// Check for a magnetomer fault and notify the user
|
||||
// Check for a magnetometer fault and notify the user
|
||||
if (estimator_status_flags.cs_mag_fault) {
|
||||
/* EVENT
|
||||
* @description
|
||||
|
||||
@@ -623,9 +623,9 @@ union warning_event_status_u {
|
||||
bool stopping_navigation : 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
|
||||
bool invalid_accel_bias_cov_reset : 1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements
|
||||
bool bad_yaw_using_gps_course : 1; ///< 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
|
||||
bool stopping_mag_use : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
|
||||
bool stopping_mag_use : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
|
||||
bool vision_data_stopped : 1; ///< 9 - true when the vision system data has stopped for a significant time period
|
||||
bool emergency_yaw_reset_mag_stopped : 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
|
||||
bool emergency_yaw_reset_mag_stopped : 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
|
||||
bool emergency_yaw_reset_gps_yaw_stopped: 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
|
||||
} flags;
|
||||
uint32_t value;
|
||||
|
||||
@@ -416,7 +416,7 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
|
||||
if (!_mag_decl_cov_reset) {
|
||||
// After any magnetic field covariance reset event the earth field state
|
||||
// covariances need to be corrected to incorporate knowledge of the declination
|
||||
// before fusing magnetomer data to prevent rapid rotation of the earth field
|
||||
// before fusing magnetometer data to prevent rapid rotation of the earth field
|
||||
// states for the first few observations.
|
||||
fuseDeclination(0.02f);
|
||||
_mag_decl_cov_reset = true;
|
||||
|
||||
@@ -436,7 +436,7 @@ float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const
|
||||
// of the earth magnetic field vector at the current location
|
||||
const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f));
|
||||
|
||||
// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetomer Z component
|
||||
// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetometer Z component
|
||||
const float mag_z_body_pred = mag_earth_predicted.dot(_R_to_earth.col(2));
|
||||
|
||||
return (mag_z_body_pred < 0) ? -mag_z_abs : mag_z_abs;
|
||||
|
||||
Regular → Executable
Regular → Executable
@@ -36,6 +36,8 @@ Description:
|
||||
|
||||
import symforce.symbolic as sf
|
||||
|
||||
import re
|
||||
|
||||
# q: quaternion describing rotation from frame 1 to frame 2
|
||||
# returns a rotation matrix derived form q which describes the same
|
||||
# rotation
|
||||
@@ -107,6 +109,10 @@ def generate_px4_function(function_name, output_names):
|
||||
line = line.replace("std::min", "math::min")
|
||||
line = line.replace("Eigen", "matrix")
|
||||
line = line.replace("matrix/Dense", "matrix/math.hpp")
|
||||
|
||||
# don't allow underscore + uppercase identifier naming (always reserved for any use)
|
||||
line = re.sub(r'_([A-Z])', lambda x: '_' + x.group(1).lower(), line)
|
||||
|
||||
print(line, end='')
|
||||
|
||||
def generate_python_function(function_name, output_names):
|
||||
|
||||
@@ -47,67 +47,67 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(4, 0) = _tmp3;
|
||||
_H(5, 0) = _tmp4;
|
||||
_H(6, 0) = _tmp5;
|
||||
_H(22, 0) = -_tmp3;
|
||||
_H(23, 0) = -_tmp4;
|
||||
_h(4, 0) = _tmp3;
|
||||
_h(5, 0) = _tmp4;
|
||||
_h(6, 0) = _tmp5;
|
||||
_h(22, 0) = -_tmp3;
|
||||
_h(23, 0) = -_tmp4;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _K = (*K);
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_K(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 +
|
||||
_k(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 +
|
||||
P(0, 6) * _tmp5);
|
||||
_K(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 +
|
||||
_k(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 +
|
||||
P(1, 6) * _tmp5);
|
||||
_K(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 +
|
||||
_k(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 +
|
||||
P(2, 6) * _tmp5);
|
||||
_K(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 +
|
||||
_k(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 +
|
||||
P(3, 6) * _tmp5);
|
||||
_K(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 +
|
||||
_k(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 +
|
||||
P(4, 6) * _tmp5);
|
||||
_K(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 +
|
||||
_k(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 +
|
||||
P(5, 6) * _tmp5);
|
||||
_K(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 +
|
||||
_k(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 +
|
||||
P(6, 6) * _tmp5);
|
||||
_K(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 +
|
||||
_k(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 +
|
||||
P(7, 6) * _tmp5);
|
||||
_K(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 +
|
||||
_k(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 +
|
||||
P(8, 6) * _tmp5);
|
||||
_K(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 +
|
||||
_k(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 +
|
||||
P(9, 6) * _tmp5);
|
||||
_K(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 +
|
||||
_k(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 +
|
||||
P(10, 5) * _tmp4 + P(10, 6) * _tmp5);
|
||||
_K(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 +
|
||||
_k(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 +
|
||||
P(11, 5) * _tmp4 + P(11, 6) * _tmp5);
|
||||
_K(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 +
|
||||
_k(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 +
|
||||
P(12, 5) * _tmp4 + P(12, 6) * _tmp5);
|
||||
_K(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 +
|
||||
_k(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 +
|
||||
P(13, 5) * _tmp4 + P(13, 6) * _tmp5);
|
||||
_K(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 +
|
||||
_k(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 +
|
||||
P(14, 5) * _tmp4 + P(14, 6) * _tmp5);
|
||||
_K(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 +
|
||||
_k(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 +
|
||||
P(15, 5) * _tmp4 + P(15, 6) * _tmp5);
|
||||
_K(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 +
|
||||
_k(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 +
|
||||
P(16, 5) * _tmp4 + P(16, 6) * _tmp5);
|
||||
_K(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 +
|
||||
_k(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 +
|
||||
P(17, 5) * _tmp4 + P(17, 6) * _tmp5);
|
||||
_K(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 +
|
||||
_k(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 +
|
||||
P(18, 5) * _tmp4 + P(18, 6) * _tmp5);
|
||||
_K(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 +
|
||||
_k(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 +
|
||||
P(19, 5) * _tmp4 + P(19, 6) * _tmp5);
|
||||
_K(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 +
|
||||
_k(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 +
|
||||
P(20, 5) * _tmp4 + P(20, 6) * _tmp5);
|
||||
_K(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 +
|
||||
_k(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 +
|
||||
P(21, 5) * _tmp4 + P(21, 6) * _tmp5);
|
||||
_K(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 +
|
||||
_k(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 +
|
||||
P(22, 5) * _tmp4 + P(22, 6) * _tmp5);
|
||||
_K(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 +
|
||||
_k(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 +
|
||||
P(23, 5) * _tmp4 + P(23, 6) * _tmp5);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
+4
-4
@@ -167,14 +167,14 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _K = (*K);
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_K.setZero();
|
||||
_k.setZero();
|
||||
|
||||
_K(22, 0) = _tmp75 * (P(22, 0) * _tmp57 + P(22, 1) * _tmp67 + P(22, 2) * _tmp45 +
|
||||
_k(22, 0) = _tmp75 * (P(22, 0) * _tmp57 + P(22, 1) * _tmp67 + P(22, 2) * _tmp45 +
|
||||
P(22, 23) * _tmp66 + P(22, 3) * _tmp70 + P(22, 4) * _tmp63 +
|
||||
P(22, 5) * _tmp69 + P(22, 6) * _tmp51 + _tmp73);
|
||||
_K(23, 0) = _tmp75 * (P(23, 0) * _tmp57 + P(23, 1) * _tmp67 + P(23, 2) * _tmp45 +
|
||||
_k(23, 0) = _tmp75 * (P(23, 0) * _tmp57 + P(23, 1) * _tmp67 + P(23, 2) * _tmp45 +
|
||||
P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp63 +
|
||||
P(23, 5) * _tmp69 + P(23, 6) * _tmp51 + _tmp72);
|
||||
}
|
||||
|
||||
+4
-4
@@ -168,14 +168,14 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _K = (*K);
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_K.setZero();
|
||||
_k.setZero();
|
||||
|
||||
_K(22, 0) = _tmp75 * (P(22, 0) * _tmp50 + P(22, 1) * _tmp44 + P(22, 2) * _tmp51 +
|
||||
_k(22, 0) = _tmp75 * (P(22, 0) * _tmp50 + P(22, 1) * _tmp44 + P(22, 2) * _tmp51 +
|
||||
P(22, 23) * _tmp58 + P(22, 3) * _tmp53 + P(22, 4) * _tmp65 +
|
||||
P(22, 5) * _tmp71 + P(22, 6) * _tmp70 + _tmp72);
|
||||
_K(23, 0) = _tmp75 * (P(23, 0) * _tmp50 + P(23, 1) * _tmp44 + P(23, 2) * _tmp51 +
|
||||
_k(23, 0) = _tmp75 * (P(23, 0) * _tmp50 + P(23, 1) * _tmp44 + P(23, 2) * _tmp51 +
|
||||
P(23, 22) * _tmp66 + P(23, 3) * _tmp53 + P(23, 4) * _tmp65 +
|
||||
P(23, 5) * _tmp71 + P(23, 6) * _tmp70 + _tmp73);
|
||||
}
|
||||
|
||||
+9
-9
@@ -109,17 +109,17 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = _tmp17;
|
||||
_H(1, 0) = _tmp21;
|
||||
_H(2, 0) = _tmp22;
|
||||
_H(3, 0) = _tmp15;
|
||||
_H(4, 0) = _tmp9;
|
||||
_H(5, 0) = _tmp4;
|
||||
_H(6, 0) = _tmp23;
|
||||
_h(0, 0) = _tmp17;
|
||||
_h(1, 0) = _tmp21;
|
||||
_h(2, 0) = _tmp22;
|
||||
_h(3, 0) = _tmp15;
|
||||
_h(4, 0) = _tmp9;
|
||||
_h(5, 0) = _tmp4;
|
||||
_h(6, 0) = _tmp23;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+9
-9
@@ -77,17 +77,17 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = -_tmp10;
|
||||
_H(1, 0) = -_tmp11;
|
||||
_H(2, 0) = -_tmp12;
|
||||
_H(3, 0) = -_tmp8;
|
||||
_H(4, 0) = -_tmp1;
|
||||
_H(5, 0) = -_tmp4;
|
||||
_H(6, 0) = -_tmp5;
|
||||
_h(0, 0) = -_tmp10;
|
||||
_h(1, 0) = -_tmp11;
|
||||
_h(2, 0) = -_tmp12;
|
||||
_h(3, 0) = -_tmp8;
|
||||
_h(4, 0) = -_tmp1;
|
||||
_h(5, 0) = -_tmp4;
|
||||
_h(6, 0) = -_tmp5;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+6
-6
@@ -88,14 +88,14 @@ void ComputeGnssYawInnonInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = _tmp26;
|
||||
_H(1, 0) = _tmp25;
|
||||
_H(2, 0) = _tmp27;
|
||||
_H(3, 0) = _tmp19;
|
||||
_h(0, 0) = _tmp26;
|
||||
_h(1, 0) = _tmp25;
|
||||
_h(2, 0) = _tmp27;
|
||||
_h(3, 0) = _tmp19;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+4
-4
@@ -61,12 +61,12 @@ void ComputeMagDeclinationInnovInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>&
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(16, 0) = -_tmp2;
|
||||
_H(17, 0) = _tmp3;
|
||||
_h(16, 0) = -_tmp2;
|
||||
_h(17, 0) = _tmp3;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+10
-10
@@ -158,18 +158,18 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _Hx = (*Hx);
|
||||
matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);
|
||||
|
||||
_Hx.setZero();
|
||||
_hx.setZero();
|
||||
|
||||
_Hx(0, 0) = _tmp23;
|
||||
_Hx(1, 0) = _tmp24;
|
||||
_Hx(2, 0) = _tmp32;
|
||||
_Hx(3, 0) = _tmp28;
|
||||
_Hx(16, 0) = _tmp6;
|
||||
_Hx(17, 0) = _tmp35;
|
||||
_Hx(18, 0) = _tmp38;
|
||||
_Hx(19, 0) = 1;
|
||||
_hx(0, 0) = _tmp23;
|
||||
_hx(1, 0) = _tmp24;
|
||||
_hx(2, 0) = _tmp32;
|
||||
_hx(3, 0) = _tmp28;
|
||||
_hx(16, 0) = _tmp6;
|
||||
_hx(17, 0) = _tmp35;
|
||||
_hx(18, 0) = _tmp38;
|
||||
_hx(19, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+10
-10
@@ -72,18 +72,18 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = _tmp3;
|
||||
_H(1, 0) = _tmp5;
|
||||
_H(2, 0) = _tmp7;
|
||||
_H(3, 0) = _tmp6;
|
||||
_H(16, 0) = _tmp8;
|
||||
_H(17, 0) = _tmp10;
|
||||
_H(18, 0) = _tmp9;
|
||||
_H(20, 0) = 1;
|
||||
_h(0, 0) = _tmp3;
|
||||
_h(1, 0) = _tmp5;
|
||||
_h(2, 0) = _tmp7;
|
||||
_h(3, 0) = _tmp6;
|
||||
_h(16, 0) = _tmp8;
|
||||
_h(17, 0) = _tmp10;
|
||||
_h(18, 0) = _tmp9;
|
||||
_h(20, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+10
-10
@@ -72,18 +72,18 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = _tmp3;
|
||||
_H(1, 0) = _tmp5;
|
||||
_H(2, 0) = _tmp7;
|
||||
_H(3, 0) = _tmp6;
|
||||
_H(16, 0) = _tmp8;
|
||||
_H(17, 0) = _tmp9;
|
||||
_H(18, 0) = _tmp10;
|
||||
_H(21, 0) = 1;
|
||||
_h(0, 0) = _tmp3;
|
||||
_h(1, 0) = _tmp5;
|
||||
_h(2, 0) = _tmp7;
|
||||
_h(3, 0) = _tmp6;
|
||||
_h(16, 0) = _tmp8;
|
||||
_h(17, 0) = _tmp9;
|
||||
_h(18, 0) = _tmp10;
|
||||
_h(21, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
@@ -84,94 +84,94 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = _tmp26;
|
||||
_H(1, 0) = _tmp31;
|
||||
_H(2, 0) = _tmp32;
|
||||
_H(3, 0) = _tmp33;
|
||||
_H(4, 0) = _tmp37;
|
||||
_H(5, 0) = _tmp39;
|
||||
_H(6, 0) = _tmp40;
|
||||
_H(22, 0) = _tmp41;
|
||||
_H(23, 0) = _tmp42;
|
||||
_h(0, 0) = _tmp26;
|
||||
_h(1, 0) = _tmp31;
|
||||
_h(2, 0) = _tmp32;
|
||||
_h(3, 0) = _tmp33;
|
||||
_h(4, 0) = _tmp37;
|
||||
_h(5, 0) = _tmp39;
|
||||
_h(6, 0) = _tmp40;
|
||||
_h(22, 0) = _tmp41;
|
||||
_h(23, 0) = _tmp42;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _K = (*K);
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_K(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 +
|
||||
_k(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 +
|
||||
P(0, 22) * _tmp41 + P(0, 23) * _tmp42 + P(0, 3) * _tmp33 +
|
||||
P(0, 4) * _tmp37 + P(0, 5) * _tmp39 + P(0, 6) * _tmp40);
|
||||
_K(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 +
|
||||
_k(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 +
|
||||
P(1, 22) * _tmp41 + P(1, 23) * _tmp42 + P(1, 3) * _tmp33 +
|
||||
P(1, 4) * _tmp37 + P(1, 5) * _tmp39 + P(1, 6) * _tmp40);
|
||||
_K(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 +
|
||||
_k(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 +
|
||||
P(2, 22) * _tmp41 + P(2, 23) * _tmp42 + P(2, 3) * _tmp33 +
|
||||
P(2, 4) * _tmp37 + P(2, 5) * _tmp39 + P(2, 6) * _tmp40);
|
||||
_K(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 +
|
||||
_k(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 +
|
||||
P(3, 22) * _tmp41 + P(3, 23) * _tmp42 + P(3, 3) * _tmp33 +
|
||||
P(3, 4) * _tmp37 + P(3, 5) * _tmp39 + P(3, 6) * _tmp40);
|
||||
_K(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 +
|
||||
_k(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 +
|
||||
P(4, 22) * _tmp41 + P(4, 23) * _tmp42 + P(4, 3) * _tmp33 +
|
||||
P(4, 4) * _tmp37 + P(4, 5) * _tmp39 + P(4, 6) * _tmp40);
|
||||
_K(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 +
|
||||
_k(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 +
|
||||
P(5, 22) * _tmp41 + P(5, 23) * _tmp42 + P(5, 3) * _tmp33 +
|
||||
P(5, 4) * _tmp37 + P(5, 5) * _tmp39 + P(5, 6) * _tmp40);
|
||||
_K(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 +
|
||||
_k(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 +
|
||||
P(6, 22) * _tmp41 + P(6, 23) * _tmp42 + P(6, 3) * _tmp33 +
|
||||
P(6, 4) * _tmp37 + P(6, 5) * _tmp39 + P(6, 6) * _tmp40);
|
||||
_K(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 +
|
||||
_k(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 +
|
||||
P(7, 22) * _tmp41 + P(7, 23) * _tmp42 + P(7, 3) * _tmp33 +
|
||||
P(7, 4) * _tmp37 + P(7, 5) * _tmp39 + P(7, 6) * _tmp40);
|
||||
_K(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 +
|
||||
_k(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 +
|
||||
P(8, 22) * _tmp41 + P(8, 23) * _tmp42 + P(8, 3) * _tmp33 +
|
||||
P(8, 4) * _tmp37 + P(8, 5) * _tmp39 + P(8, 6) * _tmp40);
|
||||
_K(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 +
|
||||
_k(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 +
|
||||
P(9, 22) * _tmp41 + P(9, 23) * _tmp42 + P(9, 3) * _tmp33 +
|
||||
P(9, 4) * _tmp37 + P(9, 5) * _tmp39 + P(9, 6) * _tmp40);
|
||||
_K(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 +
|
||||
_k(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 +
|
||||
P(10, 22) * _tmp41 + P(10, 23) * _tmp42 + P(10, 3) * _tmp33 +
|
||||
P(10, 4) * _tmp37 + P(10, 5) * _tmp39 + P(10, 6) * _tmp40);
|
||||
_K(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 +
|
||||
_k(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 +
|
||||
P(11, 22) * _tmp41 + P(11, 23) * _tmp42 + P(11, 3) * _tmp33 +
|
||||
P(11, 4) * _tmp37 + P(11, 5) * _tmp39 + P(11, 6) * _tmp40);
|
||||
_K(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 +
|
||||
_k(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 +
|
||||
P(12, 22) * _tmp41 + P(12, 23) * _tmp42 + P(12, 3) * _tmp33 +
|
||||
P(12, 4) * _tmp37 + P(12, 5) * _tmp39 + P(12, 6) * _tmp40);
|
||||
_K(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 +
|
||||
_k(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 +
|
||||
P(13, 22) * _tmp41 + P(13, 23) * _tmp42 + P(13, 3) * _tmp33 +
|
||||
P(13, 4) * _tmp37 + P(13, 5) * _tmp39 + P(13, 6) * _tmp40);
|
||||
_K(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 +
|
||||
_k(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 +
|
||||
P(14, 22) * _tmp41 + P(14, 23) * _tmp42 + P(14, 3) * _tmp33 +
|
||||
P(14, 4) * _tmp37 + P(14, 5) * _tmp39 + P(14, 6) * _tmp40);
|
||||
_K(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 +
|
||||
_k(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 +
|
||||
P(15, 22) * _tmp41 + P(15, 23) * _tmp42 + P(15, 3) * _tmp33 +
|
||||
P(15, 4) * _tmp37 + P(15, 5) * _tmp39 + P(15, 6) * _tmp40);
|
||||
_K(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 +
|
||||
_k(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 +
|
||||
P(16, 22) * _tmp41 + P(16, 23) * _tmp42 + P(16, 3) * _tmp33 +
|
||||
P(16, 4) * _tmp37 + P(16, 5) * _tmp39 + P(16, 6) * _tmp40);
|
||||
_K(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 +
|
||||
_k(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 +
|
||||
P(17, 22) * _tmp41 + P(17, 23) * _tmp42 + P(17, 3) * _tmp33 +
|
||||
P(17, 4) * _tmp37 + P(17, 5) * _tmp39 + P(17, 6) * _tmp40);
|
||||
_K(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 +
|
||||
_k(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 +
|
||||
P(18, 22) * _tmp41 + P(18, 23) * _tmp42 + P(18, 3) * _tmp33 +
|
||||
P(18, 4) * _tmp37 + P(18, 5) * _tmp39 + P(18, 6) * _tmp40);
|
||||
_K(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 +
|
||||
_k(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 +
|
||||
P(19, 22) * _tmp41 + P(19, 23) * _tmp42 + P(19, 3) * _tmp33 +
|
||||
P(19, 4) * _tmp37 + P(19, 5) * _tmp39 + P(19, 6) * _tmp40);
|
||||
_K(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 +
|
||||
_k(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 +
|
||||
P(20, 22) * _tmp41 + P(20, 23) * _tmp42 + P(20, 3) * _tmp33 +
|
||||
P(20, 4) * _tmp37 + P(20, 5) * _tmp39 + P(20, 6) * _tmp40);
|
||||
_K(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 +
|
||||
_k(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 +
|
||||
P(21, 22) * _tmp41 + P(21, 23) * _tmp42 + P(21, 3) * _tmp33 +
|
||||
P(21, 4) * _tmp37 + P(21, 5) * _tmp39 + P(21, 6) * _tmp40);
|
||||
_K(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 +
|
||||
_k(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 +
|
||||
P(22, 22) * _tmp41 + P(22, 23) * _tmp42 + P(22, 3) * _tmp33 +
|
||||
P(22, 4) * _tmp37 + P(22, 5) * _tmp39 + P(22, 6) * _tmp40);
|
||||
_K(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 +
|
||||
_k(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 +
|
||||
P(23, 22) * _tmp41 + P(23, 23) * _tmp42 + P(23, 3) * _tmp33 +
|
||||
P(23, 4) * _tmp37 + P(23, 5) * _tmp39 + P(23, 6) * _tmp40);
|
||||
}
|
||||
|
||||
+6
-6
@@ -60,14 +60,14 @@ void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = _tmp10;
|
||||
_H(1, 0) = _tmp7;
|
||||
_H(2, 0) = _tmp8;
|
||||
_H(3, 0) = _tmp9;
|
||||
_h(0, 0) = _tmp10;
|
||||
_h(1, 0) = _tmp7;
|
||||
_h(2, 0) = _tmp8;
|
||||
_h(3, 0) = _tmp9;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+6
-6
@@ -60,14 +60,14 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = -_tmp10;
|
||||
_H(1, 0) = -_tmp8;
|
||||
_H(2, 0) = -_tmp7;
|
||||
_H(3, 0) = -_tmp9;
|
||||
_h(0, 0) = -_tmp10;
|
||||
_h(1, 0) = -_tmp8;
|
||||
_h(2, 0) = -_tmp7;
|
||||
_h(3, 0) = -_tmp9;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+6
-6
@@ -60,14 +60,14 @@ void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = _tmp9;
|
||||
_H(1, 0) = _tmp8;
|
||||
_H(2, 0) = _tmp7;
|
||||
_H(3, 0) = _tmp10;
|
||||
_h(0, 0) = _tmp9;
|
||||
_h(1, 0) = _tmp8;
|
||||
_h(2, 0) = _tmp7;
|
||||
_h(3, 0) = _tmp10;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+6
-6
@@ -60,14 +60,14 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_H.setZero();
|
||||
_h.setZero();
|
||||
|
||||
_H(0, 0) = -_tmp9;
|
||||
_H(1, 0) = -_tmp8;
|
||||
_H(2, 0) = -_tmp10;
|
||||
_H(3, 0) = -_tmp7;
|
||||
_h(0, 0) = -_tmp9;
|
||||
_h(1, 0) = -_tmp8;
|
||||
_h(2, 0) = -_tmp10;
|
||||
_h(3, 0) = -_tmp7;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
+2
-2
@@ -56,9 +56,9 @@ void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
Scalar& _H = (*H);
|
||||
Scalar& _h = (*H);
|
||||
|
||||
_H = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2));
|
||||
_h = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2));
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+2
-2
@@ -55,9 +55,9 @@ void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P,
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
Scalar& _H = (*H);
|
||||
Scalar& _h = (*H);
|
||||
|
||||
_H = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2));
|
||||
_h = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2));
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user