Skip to content

Commit dfc8dea

Browse files
core: services: ardupilot_manager: Make pylint happy
Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
1 parent f86371c commit dfc8dea

1 file changed

Lines changed: 42 additions & 32 deletions

File tree

core/services/ardupilot_manager/autopilot_manager.py

Lines changed: 42 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -190,6 +190,47 @@ def is_running(self) -> bool:
190190
# Serial or others that are not processes based
191191
return self.should_be_running
192192

193+
async def _detect_firmware_vehicle_type(self) -> None:
194+
if self._firmware_vehicle_type is not None:
195+
return
196+
try:
197+
vehicle_type = await self.vehicle_manager.get_vehicle_type()
198+
self._firmware_vehicle_type = vehicle_type.mavlink_firmware_type()
199+
except Exception:
200+
pass
201+
202+
async def _monitor_heartbeat(self) -> None:
203+
"""Monitor MAVLink heartbeat and restart only for ROVs (Sub)."""
204+
if not self.should_be_running or not self.is_running():
205+
return
206+
207+
try:
208+
alive = await self.vehicle_manager.is_heart_beating()
209+
if alive:
210+
self._heartbeat_fail_count = 0
211+
else:
212+
self._heartbeat_fail_count += 1
213+
logger.warning(
214+
f"Heartbeat check False ({self._heartbeat_fail_count}/{self._max_heartbeat_failures})"
215+
)
216+
except Exception as error:
217+
self._heartbeat_fail_count += 1
218+
logger.warning(
219+
f"heartbeat check failed ({self._heartbeat_fail_count}/{self._max_heartbeat_failures}): {error}"
220+
)
221+
222+
if self._heartbeat_fail_count < self._max_heartbeat_failures:
223+
return
224+
225+
logger.warning("Consecutive heartbeat failures threshold reached — restarting Ardupilot.")
226+
try:
227+
await self._detect_firmware_vehicle_type()
228+
if self._firmware_vehicle_type == MavlinkFirmwareType.ArduSub:
229+
await self.restart_ardupilot()
230+
except Exception as error:
231+
logger.warning(f"Failed to restart Ardupilot after heartbeat failures: {error}")
232+
self._heartbeat_fail_count = 0
233+
193234
async def auto_restart_ardupilot(self) -> None:
194235
"""Auto-restart Ardupilot when it's not running but was supposed to."""
195236
while True:
@@ -205,38 +246,7 @@ async def auto_restart_ardupilot(self) -> None:
205246
except Exception as error:
206247
logger.warning(f"Could not start Ardupilot: {error}")
207248

208-
# Monitor MAVLink heartbeat and restart only for ROVs (Sub)
209-
if self.should_be_running and self.is_running():
210-
try:
211-
alive = await self.vehicle_manager.is_heart_beating()
212-
if alive:
213-
self._heartbeat_fail_count = 0
214-
else:
215-
self._heartbeat_fail_count += 1
216-
logger.warning(
217-
f"Heartbeat check False ({self._heartbeat_fail_count}/{self._max_heartbeat_failures})"
218-
)
219-
except Exception as error:
220-
self._heartbeat_fail_count += 1
221-
logger.warning(
222-
f"heartbeat check failed ({self._heartbeat_fail_count}/{self._max_heartbeat_failures}): {error}"
223-
)
224-
225-
if self._heartbeat_fail_count >= self._max_heartbeat_failures:
226-
logger.warning("Consecutive heartbeat failures threshold reached — restarting Ardupilot.")
227-
try:
228-
if self._firmware_vehicle_type is None:
229-
try:
230-
vehicle_type = await self.vehicle_manager.get_vehicle_type()
231-
self._firmware_vehicle_type = vehicle_type.mavlink_firmware_type()
232-
except Exception:
233-
pass
234-
if self._firmware_vehicle_type == MavlinkFirmwareType.ArduSub:
235-
await self.restart_ardupilot()
236-
except Exception as error:
237-
logger.warning(f"Failed to restart Ardupilot after heartbeat failures: {error}")
238-
self._heartbeat_fail_count = 0
239-
249+
await self._monitor_heartbeat()
240250
await asyncio.sleep(5.0)
241251

242252
async def start_mavlink_manager_watchdog(self) -> None:

0 commit comments

Comments
 (0)