@@ -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