forked from PX4/PX4-Autopilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
4020_holybro_px4vision_v1_5
134 lines (116 loc) · 3.73 KB
/
4020_holybro_px4vision_v1_5
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#!/bin/sh
#
# @name PX4 Vision Dev Kit v1.5
#
# @type Quadrotor x
# @class Copter
#
# @board px4_fmu-v2 exclude
# @board bitcraze_crazyflie exclude
#
. ${R}etc/init.d/rc.mc_defaults
# System parameters
# use FMU motor outputs for less delay in the rate control loop
param set-default SYS_USE_IO 0
# Commander Parameters
param set-default COM_OBS_AVOID 1
param set-default COM_DISARM_LAND 0.5
# EKF2 parameters
param set-default EKF2_AID_MASK 35
param set-default EKF2_IMU_POS_X 0.02
param set-default EKF2_GPS_POS_X 0.055
param set-default EKF2_GPS_POS_Z -0.15
param set-default EKF2_MIN_RNG 0.03
param set-default EKF2_OF_POS_X 0.055
param set-default EKF2_OF_POS_Y 0.02
param set-default EKF2_OF_POS_Z 0.065
param set-default EKF2_REQ_HDRIFT 0.3
param set-default EKF2_REQ_SACC 1
param set-default EKF2_REQ_VDRIFT 0.3
param set-default EKF2_RNG_A_HMAX 8
param set-default EKF2_RNG_A_VMAX 2
param set-default EKF2_RNG_POS_X 0.055
param set-default EKF2_RNG_POS_Y -0.01
param set-default EKF2_RNG_POS_Z 0.065
param set-default EKF2_PCOEF_XP -0.25
param set-default EKF2_PCOEF_YN -0.55
param set-default EKF2_PCOEF_YP -0.55
# MAVLink parameters
param set-default MAV_0_CONFIG 101
param set-default MAV_1_CONFIG 102
param set-default MAV_1_RATE 80000
param set-default MAV_1_MODE 9
param set-default SER_TEL1_BAUD 921600
# Vehicle attitude PID tuning
param set-default MC_ACRO_EXPO 0
param set-default MC_ACRO_EXPO_Y 0
param set-default MC_ACRO_P_MAX 200
param set-default MC_ACRO_R_MAX 200
param set-default MC_ACRO_SUPEXPO 0
param set-default MC_ACRO_SUPEXPOY 0
param set-default MC_ACRO_Y_MAX 150
param set-default MC_PITCHRATE_D 0.0015
param set-default MC_ROLLRATE_D 0.0015
param set-default MC_YAWRATE_P 0.3
# Position Control Tuning
param set-default CP_DIST 6
param set-default MPC_ACC_DOWN_MAX 5
param set-default MPC_ACC_HOR_MAX 10
param set-default MPC_ACC_UP_MAX 4
param set-default MPC_MANTHR_MIN 0
param set-default MPC_MAN_Y_MAX 120
param set-default MPC_TILTMAX_AIR 45
param set-default MPC_THR_HOVER 0.3
param set-default MPC_TKO_SPEED 1
param set-default MPC_VEL_MANUAL 5
param set-default MPC_XY_CRUISE 5
param set-default MPC_XY_VEL_MAX 5
param set-default MPC_XY_VEL_P_ACC 1.58
param set-default MPC_XY_TRAJ_P 0.3
param set-default MPC_Z_TRAJ_P 0.3
param set-default MPC_Z_VEL_P_ACC 5
param set-default MPC_Z_VEL_I_ACC 3
param set-default MPC_LAND_ALT1 3
param set-default MPC_LAND_ALT2 1
param set-default MPC_POS_MODE 3
param set-default CP_GO_NO_DATA 1
# Navigator Parameters
param set-default NAV_ACC_RAD 2
# use oneshot motor output protocol
param set-default PWM_MAIN_RATE 0
# RTL Parameters
param set-default RTL_DESCEND_ALT 5
param set-default RTL_RETURN_ALT 5
# Logging Parameters
param set-default SDLOG_PROFILE 131
# Sensors Parameters
param set-default SENS_CM8JL65_CFG 202
param set-default SENS_FLOW_MAXHGT 25
param set-default SENS_FLOW_MINHGT 0.5
param set-default IMU_GYRO_CUTOFF 100
param set-default SENS_TFLOW_CFG 103
# Power Parameters
param set-default BAT1_N_CELLS 4
param set-default BAT1_A_PER_V 36.364
param set-default BAT1_V_DIV 18.182
# Circuit breakers
param set-default CBRK_IO_SAFETY 22027
param set-default THR_MDL_FAC 0.3
param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR0_PX 0.15
param set-default CA_ROTOR0_PY 0.15
param set-default CA_ROTOR0_KM 0.05
param set-default CA_ROTOR1_PX -0.15
param set-default CA_ROTOR1_PY -0.15
param set-default CA_ROTOR1_KM 0.05
param set-default CA_ROTOR2_PX 0.15
param set-default CA_ROTOR2_PY -0.15
param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.15
param set-default CA_ROTOR3_PY 0.15
param set-default CA_ROTOR3_KM -0.05
# Outputs
param set-default PWM_AUX_FUNC1 101
param set-default PWM_AUX_FUNC2 102
param set-default PWM_AUX_FUNC3 103
param set-default PWM_AUX_FUNC4 104