Skip to content

Commit 0e96371

Browse files
1Blademaster1Blademaster
authored
Wait for flight controller heartbeat in initial setup (#1148)
* Wait for flight controller heartbeat in initial setup * Address copilot review comments * Update workflow to check if container is ready * Fix tests with old target_component value
1 parent 712c503 commit 0e96371

9 files changed

Lines changed: 96 additions & 31 deletions

‎.github/workflows/python_tests_copter.yml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,11 @@ jobs:
2929
- 5760:5760
3030
options: >-
3131
--name ardupilot-copter
32+
--health-cmd "test -f /tmp/fgcs_done || exit 1"
33+
--health-interval 5s
34+
--health-timeout 3s
35+
--health-retries 240
36+
--health-start-period 15s
3237
steps:
3338
- uses: actions/checkout@v3
3439

‎.github/workflows/python_tests_plane.yml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,11 @@ jobs:
2929
- 5760:5760
3030
options: >-
3131
--name ardupilot-plane
32+
--health-cmd "test -f /tmp/fgcs_done || exit 1"
33+
--health-interval 5s
34+
--health-timeout 3s
35+
--health-retries 240
36+
--health-start-period 15s
3237
steps:
3338
- uses: actions/checkout@v3
3439

‎dockerfile‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,5 +38,7 @@ WORKDIR /sitl_setup
3838

3939
RUN chmod +x run.sh
4040

41+
HEALTHCHECK --interval=5s --timeout=3s --start-period=15s --retries=240 CMD test -f /tmp/fgcs_done || exit 1
42+
4143
# ENTRYPOINT python ./Tools/autotest/sim_vehicle.py -v ArduCopter --custom-location=${LAT},${LON},${ALT},${DIR} --no-mavproxy
4244
ENTRYPOINT [ "./run.sh" ]

‎radio/app/drone.py‎

Lines changed: 32 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -149,12 +149,39 @@ def __init__(
149149
return
150150

151151
try:
152-
initial_heartbeat = self.master.wait_heartbeat(timeout=5)
152+
initial_heartbeat = None
153+
heartbeat_timeout_secs = 5.0
154+
deadline = time.monotonic() + heartbeat_timeout_secs
155+
156+
while True:
157+
now = time.monotonic()
158+
if now >= deadline:
159+
break
160+
161+
remaining = deadline - now
162+
heartbeat = self.master.recv_match(
163+
type="HEARTBEAT", blocking=True, timeout=max(remaining, 0.0)
164+
)
165+
166+
if heartbeat is None:
167+
continue
168+
169+
# Ignore heartbeats from non-autopilot MAVLink components.
170+
if heartbeat.autopilot == mavutil.mavlink.MAV_AUTOPILOT_INVALID:
171+
continue
172+
173+
initial_heartbeat = heartbeat
174+
break
175+
153176
if initial_heartbeat is None:
154-
self.logger.error("Heartbeat timed out after 5 seconds")
177+
self.logger.error(
178+
f"No heartbeat received after {heartbeat_timeout_secs:.0f} seconds"
179+
)
155180
self.master.close()
156181
self.master = None
157-
self.connectionError = "Could not connect to the drone."
182+
self.connectionError = (
183+
f"No heartbeat received after {heartbeat_timeout_secs:.0f} seconds."
184+
)
158185
return
159186
except Exception as e:
160187
self.logger.error(
@@ -182,8 +209,8 @@ def __init__(
182209
self.logger.info(f"Connected to aircraft of type {self.aircraft_type}")
183210

184211
self.autopilot = initial_heartbeat.autopilot
185-
self.target_system = self.master.target_system
186-
self.target_component = self.master.target_component
212+
self.target_system = initial_heartbeat.get_srcSystem()
213+
self.target_component = initial_heartbeat.get_srcComponent()
187214

188215
self.logger.debug(
189216
f"Heartbeat received (system {self.target_system} component {self.target_component})"

‎radio/tests/mission_test_files/test_importMissionFromFile_fenceImportSuccess_result.json‎

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
"param3": 0.0,
1616
"param4": 0.0,
1717
"seq": 0,
18-
"target_component": 0,
18+
"target_component": 1,
1919
"target_system": 1,
2020
"x": 527814618,
2121
"y": -7105708,
@@ -33,7 +33,7 @@
3333
"param3": 0.0,
3434
"param4": 0.0,
3535
"seq": 1,
36-
"target_component": 0,
36+
"target_component": 1,
3737
"target_system": 1,
3838
"x": 527810530,
3939
"y": -7102918,
@@ -51,7 +51,7 @@
5151
"param3": 0.0,
5252
"param4": 0.0,
5353
"seq": 2,
54-
"target_component": 0,
54+
"target_component": 1,
5555
"target_system": 1,
5656
"x": 527800342,
5757
"y": -7108390,
@@ -69,7 +69,7 @@
6969
"param3": 0.0,
7070
"param4": 0.0,
7171
"seq": 3,
72-
"target_component": 0,
72+
"target_component": 1,
7373
"target_system": 1,
7474
"x": 527818512,
7575
"y": -7035434,
@@ -87,7 +87,7 @@
8787
"param3": 0.0,
8888
"param4": 0.0,
8989
"seq": 4,
90-
"target_component": 0,
90+
"target_component": 1,
9191
"target_system": 1,
9292
"x": 527830582,
9393
"y": -7060110,
@@ -105,7 +105,7 @@
105105
"param3": 0.0,
106106
"param4": 0.0,
107107
"seq": 5,
108-
"target_component": 0,
108+
"target_component": 1,
109109
"target_system": 1,
110110
"x": 527825456,
111111
"y": -7081568,
@@ -123,7 +123,7 @@
123123
"param3": 0.0,
124124
"param4": 0.0,
125125
"seq": 6,
126-
"target_component": 0,
126+
"target_component": 1,
127127
"target_system": 1,
128128
"x": 527796578,
129129
"y": -7092163,
@@ -141,7 +141,7 @@
141141
"param3": 0.0,
142142
"param4": 0.0,
143143
"seq": 7,
144-
"target_component": 0,
144+
"target_component": 1,
145145
"target_system": 1,
146146
"x": 527801900,
147147
"y": -7044017,
@@ -159,7 +159,7 @@
159159
"param3": 0.0,
160160
"param4": 0.0,
161161
"seq": 8,
162-
"target_component": 0,
162+
"target_component": 1,
163163
"target_system": 1,
164164
"x": 527828246,
165165
"y": -7061076,
@@ -177,7 +177,7 @@
177177
"param3": 0.0,
178178
"param4": 0.0,
179179
"seq": 9,
180-
"target_component": 0,
180+
"target_component": 1,
181181
"target_system": 1,
182182
"x": 527818707,
183183
"y": -7041549,
@@ -195,7 +195,7 @@
195195
"param3": 0.0,
196196
"param4": 0.0,
197197
"seq": 10,
198-
"target_component": 0,
198+
"target_component": 1,
199199
"target_system": 1,
200200
"x": 527814489,
201201
"y": -7058179,
@@ -213,7 +213,7 @@
213213
"param3": 0.0,
214214
"param4": 0.0,
215215
"seq": 11,
216-
"target_component": 0,
216+
"target_component": 1,
217217
"target_system": 1,
218218
"x": 527821887,
219219
"y": -7058072,
@@ -231,7 +231,7 @@
231231
"param3": 0.0,
232232
"param4": 0.0,
233233
"seq": 12,
234-
"target_component": 0,
234+
"target_component": 1,
235235
"target_system": 1,
236236
"x": 527826364,
237237
"y": -7068908,

‎radio/tests/mission_test_files/test_importMissionFromFile_missionImportSuccess_result.json‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
"param3": 0.0,
1616
"param4": 0.0,
1717
"seq": 0,
18-
"target_component": 0,
18+
"target_component": 1,
1919
"target_system": 1,
2020
"x": 527806539,
2121
"y": -7083070,
@@ -24,7 +24,7 @@
2424
{
2525
"mavpackettype": "MISSION_ITEM",
2626
"target_system": 1,
27-
"target_component": 0,
27+
"target_component": 1,
2828
"seq": 1,
2929
"frame": 3,
3030
"command": 22,
@@ -42,7 +42,7 @@
4242
{
4343
"mavpackettype": "MISSION_ITEM",
4444
"target_system": 1,
45-
"target_component": 0,
45+
"target_component": 1,
4646
"seq": 2,
4747
"frame": 3,
4848
"command": 16,
@@ -60,7 +60,7 @@
6060
{
6161
"mavpackettype": "MISSION_ITEM",
6262
"target_system": 1,
63-
"target_component": 0,
63+
"target_component": 1,
6464
"seq": 3,
6565
"frame": 3,
6666
"command": 16,
@@ -78,7 +78,7 @@
7878
{
7979
"mavpackettype": "MISSION_ITEM",
8080
"target_system": 1,
81-
"target_component": 0,
81+
"target_component": 1,
8282
"seq": 4,
8383
"frame": 3,
8484
"command": 16,
@@ -96,7 +96,7 @@
9696
{
9797
"mavpackettype": "MISSION_ITEM",
9898
"target_system": 1,
99-
"target_component": 0,
99+
"target_component": 1,
100100
"seq": 5,
101101
"frame": 3,
102102
"command": 16,
@@ -114,7 +114,7 @@
114114
{
115115
"mavpackettype": "MISSION_ITEM",
116116
"target_system": 1,
117-
"target_component": 0,
117+
"target_component": 1,
118118
"seq": 6,
119119
"frame": 3,
120120
"command": 16,
@@ -132,7 +132,7 @@
132132
{
133133
"mavpackettype": "MISSION_ITEM",
134134
"target_system": 1,
135-
"target_component": 0,
135+
"target_component": 1,
136136
"seq": 7,
137137
"frame": 3,
138138
"command": 21,

‎radio/tests/mission_test_files/test_importMissionFromFile_rallyImportSuccess_result.json‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
"param3": 0.0,
1616
"param4": 0.0,
1717
"seq": 0,
18-
"target_component": 0,
18+
"target_component": 1,
1919
"target_system": 1,
2020
"x": 527810011,
2121
"y": -7094872,
@@ -33,7 +33,7 @@
3333
"param3": 0.0,
3434
"param4": 0.0,
3535
"seq": 1,
36-
"target_component": 0,
36+
"target_component": 1,
3737
"target_system": 1,
3838
"x": 527821108,
3939
"y": -7056785,

‎radio/tests/test_connections.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ def test_getTargetInfo(socketio_client: SocketIOTestClient, droneStatus):
4949
assert len(socketio_result) == 1
5050
assert socketio_result[0]["name"] == "target_info"
5151
assert socketio_result[0]["args"][0] == {
52-
"target_component": 0,
52+
"target_component": 1,
5353
"target_system": 1,
5454
}
5555

‎sitl_setup/run.sh‎

Lines changed: 28 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ ALT=0.1
99
DIR=270
1010
VEHICLE="ArduCopter"
1111
FIRMWARE_VERSION="latest"
12+
SENTINEL_FILE="/tmp/fgcs_done"
1213

1314
REPO_URL="https://github.com/ArduPilot/ardupilot.git"
1415
DEFAULT_WORKTREE="/ardupilot"
@@ -293,5 +294,30 @@ if ! ARDUPILOT_DIR=$(ensure_firmware_repo); then
293294
fi
294295
ensure_vehicle_binary "$ARDUPILOT_DIR"
295296

296-
exec python /sitl_setup/mission_upload.py &
297-
exec python "$ARDUPILOT_DIR/Tools/autotest/sim_vehicle.py" -v $VEHICLE --custom-location=$LAT,$LON,$ALT,$DIR --no-mavproxy --add-param-file=$PARAM_PATH
297+
rm -f "$SENTINEL_FILE"
298+
299+
python "$ARDUPILOT_DIR/Tools/autotest/sim_vehicle.py" -v "$VEHICLE" --custom-location="$LAT,$LON,$ALT,$DIR" --no-mavproxy --add-param-file="$PARAM_PATH" &
300+
SIM_PID=$!
301+
302+
python /sitl_setup/mission_upload.py &
303+
UPLOAD_PID=$!
304+
305+
while kill -0 "$UPLOAD_PID" >/dev/null 2>&1; do
306+
if ! kill -0 "$SIM_PID" >/dev/null 2>&1; then
307+
echo "sim_vehicle exited before mission upload completed"
308+
wait "$UPLOAD_PID" || true
309+
exit 1
310+
fi
311+
sleep 1
312+
done
313+
314+
if ! wait "$UPLOAD_PID"; then
315+
echo "Mission upload failed or timed out"
316+
kill "$SIM_PID" >/dev/null 2>&1 || true
317+
wait "$SIM_PID" || true
318+
exit 1
319+
fi
320+
321+
touch "$SENTINEL_FILE"
322+
323+
wait "$SIM_PID"

0 commit comments

Comments
 (0)