forked from PX4/PX4-Autopilot
/
voxl-px4-start
executable file
·247 lines (213 loc) · 7.41 KB
/
voxl-px4-start
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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
#!/bin/sh
# PX4 commands need the 'px4-' prefix in bash.
# (px4-alias.sh is expected to be in the PATH)
. px4-alias.sh
echo -e "\n*************************"
echo "GPS: $GPS"
echo "RC: $RC"
echo "ESC: $ESC"
echo "POWER MANAGER: $POWER_MANAGER"
echo "DISTANCE SENSOR: $DISTANCE_SENSOR"
echo "OSD: $OSD"
echo "EXTRA STEPS:"
for i in "${EXTRA_STEPS[@]}"
do
echo -e "\t$i"
done
echo -e "*************************\n"
# In order to just exit after starting the uorb / muorb modules define
# the environment variable MINIMAL_PX4. (e.g. export MINIMAL_PX4=1)
# This is useful for testing / debug where you may want to start drivers
# and modules manually from the px4 command shell
if [ ! -z $MINIMAL_PX4 ]; then
/bin/echo "Running minimal script"
exit 0
fi
# Figure out what platform we are running on.
PLATFORM=`/usr/bin/voxl-platform 2> /dev/null`
RETURNCODE=$?
if [ $RETURNCODE -ne 0 ]; then
# If we couldn't get the platform from the voxl-platform utility then check
# /etc/version to see if there is an M0052 substring in the version string. If so,
# then we assume that we are on M0052.
VERSIONSTRING=$(</etc/version)
M0052SUBSTRING="M0052"
if [[ "$VERSIONSTRING" == *"$M0052SUBSTRING"* ]]; then
PLATFORM="M0052"
fi
fi
# We can only run on M0052, M0054, or M0104 so exit with error if that is not the case
if [ $PLATFORM = "M0052" ]; then
/bin/echo "Running on M0052"
elif [ $PLATFORM = "M0054" ]; then
/bin/echo "Running on M0054"
elif [ $PLATFORM = "M0104" ]; then
/bin/echo "Running on M0104"
else
/bin/echo "Error, cannot determine platform!"
exit 0
fi
# Sleep a little here. A lot happens when the uorb and muorb start
# and we need to make sure that it all completes successfully to avoid
# any possible race conditions.
/bin/sleep 1
param select /data/px4/param/parameters
# Load in all of the parameters that have been saved in the file
param load
# Start logging and use timestamps for log files when possible.
# Add the "-e" option to start logging immediately. Default is
# to log only when armed. Caution must be used with the "-e" option
# because if power is removed without stopping the logger gracefully then
# the log file may be corrupted.
logger start -t
# IMU (accelerometer / gyroscope)
if [ "$PLATFORM" == "M0104" ]; then
/bin/echo "Starting IMU driver with rotation 12"
qshell icm42688p start -s -R 12
else
/bin/echo "Starting IMU driver with no rotation"
qshell icm42688p start -s
fi
# Start Invensense ICP 101xx barometer built on to VOXL 2
qshell icp101xx start -I -b 5
# Auto detect the magnetometer. If one or both of these devices
# are not connected it will fail but not cause any harm.
/bin/echo "Looking for qmc5883l magnetometer"
qshell qmc5883l start -R 10 -X -b 1
/bin/echo "Looking for ist8310 magnetometer"
qshell ist8310 start -R 10 -X -b 1
# GPS and magnetometer
if [ "$GPS" != "NONE" ]; then
# On M0052 the GPS driver runs on the apps processor
if [ $PLATFORM = "M0052" ]; then
gps start -d /dev/ttyHS2
# On M0054 and M0104 the GPS driver runs on SLPI DSP
else
qshell gps start
fi
fi
# Auto detect an ncp5623c i2c RGB LED. If one isn't connected this will
# fail but not cause any harm.
/bin/echo "Looking for ncp5623c RGB LED"
qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56
# We do not change the value of SYS_AUTOCONFIG but if it does not
# show up as used then it is not reported to QGC and we get a
# missing parameter error.
param touch SYS_AUTOCONFIG
# ESC driver
if [ "$ESC" == "VOXL_ESC" ]; then
/bin/echo "Starting VOXL ESC driver"
qshell voxl_esc start
elif [ "$ESC" == "VOXL2_IO_PWM_ESC" ]; then
if [ "$RC" == "M0065_SBUS" ]; then
/bin/echo "Starting VOXL IO for PWM ESC with SBUS RC"
qshell voxl2_io start
else
/bin/echo "Starting VOXL IO for PWM ESC without SBUS RC"
qshell voxl2_io start -e
fi
else
/bin/echo "No ESC type specified, not starting an ESC driver"
fi
# RC driver
if [ "$RC" == "FAKE_RC_INPUT" ]; then
/bin/echo "Starting fake RC driver"
qshell rc_controller start
elif [ "$RC" == "CRSF_RAW" ]; then
/bin/echo "Starting CRSF RC driver"
qshell crsf_rc start -d 7
elif [ "$RC" == "CRSF_MAV" ]; then
/bin/echo "Starting TBS crossfire RC - MAV Mode"
qshell mavlink_rc_in start -m -p 7 -b 115200
elif [ "$RC" == "SPEKTRUM" ]; then
/bin/echo "Starting Spektrum RC"
# On M0052 the RC driver runs on the apps processor
if [ $PLATFORM = "M0052" ]; then
rc_input start -d /dev/ttyHS1
# On M0054 and M0104 the RC driver runs on SLPI DSP
else
qshell spektrum_rc start
fi
elif [ "$RC" == "GHST" ]; then
/bin/echo "Starting GHST RC driver"
qshell ghst_rc start -d 7
elif [ "$RC" == "M0065_SBUS" ]; then
if [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then
/bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW"
qshell dsp_sbus start
retVal=$?
if [ $retVal -ne 0 ]; then
/bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed"
/bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW"
qshell voxl2_io start -d -p 7
fi
else
/bin/echo "M0065 SBUS RC driver already started with PWM ESC start"
fi
fi
if [ "$DISTANCE_SENSOR" == "LIGHTWARE_SF000" ]; then
# Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor
qshell lightware_laser_serial start -d 7
fi
if [ "$POWER_MANAGER" == "VOXLPM" ]; then
# APM power monitor
qshell voxlpm start -X -b 2
fi
# Optional distance sensor on spare i2c
# qshell vl53l0x start -X -b 4
# qshell vl53l1x start -X -b 4
# Start all of the processing modules on DSP
qshell sensors start
qshell ekf2 start
qshell mc_pos_control start
qshell mc_att_control start
qshell mc_rate_control start
qshell mc_hover_thrust_estimator start
qshell mc_autotune_attitude_control start
qshell land_detector start multicopter
qshell manual_control start
qshell control_allocator start
qshell load_mon start
# Only start the rc_update module if an actual RC driver
# is publishing input_rc topics. Otherwise for external RC
# over Mavlink this isn't needed.
if [ "$RC" != "EXTERNAL" ]; then
qshell rc_update start
fi
qshell commander start
# This is needed for altitude and position hold modes
qshell flight_mode_manager start
# Start all of the processing modules on the applications processor
dataman start
navigator start
# This bridge allows raw data packets to be sent over UART to the ESC
modal_io_bridge start
# Start microdds_client for ros2 offboard messages from agent over localhost
microdds_client start -t udp -h 127.0.0.1 -p 8888
# On M0052 there is only one IMU. So, PX4 needs to
# publish IMU samples externally for VIO to use.
if [ $PLATFORM = "M0052" ]; then
imu_server start
fi
# start the onboard fast link to connect to voxl-mavlink-server
mavlink start -x -u 14556 -o 14557 -r 100000 -n lo -m onboard
# slow down some of the fastest streams
mavlink stream -u 14556 -s HIGHRES_IMU -r 10
mavlink stream -u 14556 -s ATTITUDE -r 10
mavlink stream -u 14556 -s ATTITUDE_QUATERNION -r 10
mavlink stream -u 14556 -s GLOBAL_POSITION_INT -r 30
mavlink stream -u 14556 -s SCALED_PRESSURE -r 10
# start the slow normal mode for voxl-mavlink-server to forward to GCS
mavlink start -x -u 14558 -o 14559 -r 100000 -n lo
mavlink boot_complete
# Optional MSP OSD driver for DJI goggles
# This is only supported on M0054 (with M0125 accessory board)
if [ "$OSD" == "ENABLE" ]; then
/bin/echo "Starting OSD driver"
msp_osd start -d /dev/ttyHS1
fi
# Start optional EXTRA_STEPS
for i in "${EXTRA_STEPS[@]}"
do
$i
done