Skip to content

Commit a25cbc9

Browse files
committed
Fixes linting issues
1 parent da74d86 commit a25cbc9

File tree

3 files changed

+27
-8
lines changed

3 files changed

+27
-8
lines changed

modules/hitl/hitl_base.py

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -131,17 +131,25 @@ def __del__(self) -> None:
131131
pass
132132

133133
def run_position(self) -> None:
134+
"""
135+
Runs the position emulator periodic function in a loop.
136+
"""
134137
assert self._stop_event is not None
135138
while not self._stop_event.is_set():
136139
try:
137140
self.position_emulator.periodic()
138141
except Exception as exc: # pylint: disable=broad-except
139142
print(f"HITL position thread error: {exc}")
143+
time.sleep(0.1)
140144

141145
def run_camera(self) -> None:
146+
"""
147+
Runs the camera emulator periodic function in a loop.
148+
"""
142149
assert self._stop_event is not None
143150
while not self._stop_event.is_set():
144151
try:
145152
self.camera_emulator.periodic()
146153
except Exception as exc: # pylint: disable=broad-except
147154
print(f"HITL camera thread error: {exc}")
155+
time.sleep(0.1)

modules/hitl/position_emulator.py

Lines changed: 17 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -55,16 +55,26 @@ def get_target_position(self) -> tuple[float, float, float]:
5555
Target position as (latitude, longitude, altitude).
5656
"""
5757
# pylint: disable=protected-access
58-
position_target = self.drone._master.recv_match(...)
58+
position_target = None
59+
try:
60+
# Poll non-blocking for the latest global position target from the FCU
61+
position_target = self.drone._master.recv_match(
62+
type="POSITION_TARGET_GLOBAL_INT", blocking=False
63+
)
64+
except Exception as exc: # pylint: disable=broad-except
65+
print(f"HITL get_target_position recv_match error: {exc}")
66+
position_target = None
5967
# pylint: enable=protected-access
68+
6069
if position_target:
6170
latitude = position_target.lat_int / 1e7
6271
longitude = position_target.lon_int / 1e7
6372
altitude = position_target.alt
64-
else:
65-
print("No POSITION_TARGET_GLOBAL_INT message received.")
73+
return (latitude, longitude, altitude)
74+
75+
print("No POSITION_TARGET_GLOBAL_INT message received.")
6676

67-
return (latitude, longitude, altitude) if position_target else self.target_position
77+
return self.target_position
6878

6979
def periodic(self) -> None:
7080
"""
@@ -73,9 +83,9 @@ def periodic(self) -> None:
7383

7484
self.target_position = self.get_target_position()
7585

76-
self.inject_position(self.target_position[0], self.target_position[1], self.target_position[2])
77-
78-
time.sleep(0.1) # 10 Hz
86+
self.inject_position(
87+
self.target_position[0], self.target_position[1], self.target_position[2]
88+
)
7989

8090
def inject_position(
8191
self,

tests/integration/hitl/test_threading.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,14 +8,15 @@
88
import time
99
from modules.mavlink import flight_controller
1010

11+
1112
def main() -> int:
1213
"""
1314
Main function.
1415
"""
1516
images_folder_path = os.path.join("tests", "integration", "camera_emulator", "images")
1617

1718
result, controller = flight_controller.FlightController.create(
18-
PIXHAWK_ADDRESS, 57600, True, True # True, images_folder_path
19+
PIXHAWK_ADDRESS, 57600, True, True # True, images_folder_path
1920
)
2021
if not result:
2122
print("Failed to create flight controller")

0 commit comments

Comments
 (0)