diff --git a/core/libs/commonwealth/src/commonwealth/mavlink_comm/VehicleManager.py b/core/libs/commonwealth/src/commonwealth/mavlink_comm/VehicleManager.py index 4401783b7e..81f2d7044b 100644 --- a/core/libs/commonwealth/src/commonwealth/mavlink_comm/VehicleManager.py +++ b/core/libs/commonwealth/src/commonwealth/mavlink_comm/VehicleManager.py @@ -6,6 +6,7 @@ from commonwealth.mavlink_comm.typedefs import ( FirmwareInfo, FirmwareVersionType, + MavlinkFirmwareType, MavlinkMessageId, MavlinkVehicleType, ) @@ -92,7 +93,7 @@ async def get_vehicle_type(self) -> MavlinkVehicleType: heartbeat_message = await self.mavlink2rest.get_updated_mavlink_message("HEARTBEAT") return MavlinkVehicleType[heartbeat_message["message"]["mavtype"]["type"]] # type: ignore - async def get_firmware_vehicle_type(self) -> str: + async def get_firmware_vehicle_type(self) -> MavlinkFirmwareType: vehicle_type = await self.get_vehicle_type() return vehicle_type.mavlink_firmware_type() diff --git a/core/libs/commonwealth/src/commonwealth/mavlink_comm/typedefs.py b/core/libs/commonwealth/src/commonwealth/mavlink_comm/typedefs.py index f8645a8975..1f65eeec5f 100644 --- a/core/libs/commonwealth/src/commonwealth/mavlink_comm/typedefs.py +++ b/core/libs/commonwealth/src/commonwealth/mavlink_comm/typedefs.py @@ -23,6 +23,14 @@ def from_value(firmware_value: int) -> "FirmwareVersionType": return FirmwareVersionType.DEV +class MavlinkFirmwareType(str, Enum): + ArduPlane = "ArduPlane" + ArduCopter = "ArduCopter" + ArduRover = "ArduRover" + ArduSub = "ArduSub" + Unknown = "Unknown" + + class MavlinkVehicleType(str, Enum): MAV_TYPE_GENERIC = "Generic" MAV_TYPE_FIXED_WING = "Fixed Wing" @@ -68,7 +76,7 @@ class MavlinkVehicleType(str, Enum): MAV_TYPE_GPS = "Gps" MAV_TYPE_WINCH = "Winch" - def mavlink_firmware_type(self) -> str: + def mavlink_firmware_type(self) -> MavlinkFirmwareType: plane_options = [ self.MAV_TYPE_FIXED_WING, self.MAV_TYPE_VTOL_DUOROTOR, @@ -99,21 +107,21 @@ def mavlink_firmware_type(self) -> str: ] if self in plane_options: - return "ArduPlane" + return MavlinkFirmwareType.ArduPlane if self in copter_options: - return "ArduCopter" + return MavlinkFirmwareType.ArduCopter if self in rover_options: - return "ArduRover" + return MavlinkFirmwareType.ArduRover if self in sub_options: - return "ArduSub" + return MavlinkFirmwareType.ArduSub - return "Unknown" + return MavlinkFirmwareType.Unknown def is_actually_a_vehicle(self) -> bool: - return self.mavlink_firmware_type() in ["ArduPlane", "ArduCopter", "ArduRover", "ArduSub"] + return self.mavlink_firmware_type() != MavlinkFirmwareType.Unknown class FirmwareInfo(BaseModel): diff --git a/core/services/ardupilot_manager/autopilot_manager.py b/core/services/ardupilot_manager/autopilot_manager.py index 50ca2900b5..e9ee8c2fd8 100644 --- a/core/services/ardupilot_manager/autopilot_manager.py +++ b/core/services/ardupilot_manager/autopilot_manager.py @@ -7,6 +7,7 @@ from typing import Any, List, Optional, Set import psutil +from commonwealth.mavlink_comm.typedefs import MavlinkFirmwareType from commonwealth.mavlink_comm.VehicleManager import VehicleManager from commonwealth.utils.Singleton import Singleton from elftools.elf.elffile import ELFFile @@ -151,6 +152,7 @@ async def setup(self) -> None: self.vehicle_manager = VehicleManager() self._heartbeat_fail_count = 0 # Consecutive heartbeat failures self._max_heartbeat_failures = 10 # Threshold for restarting Ardupilot after consecutive heartbeat failures + self._firmware_vehicle_type: Optional[MavlinkFirmwareType] = None self.should_be_running = False self.remove_old_logs() @@ -188,6 +190,45 @@ def is_running(self) -> bool: # Serial or others that are not processes based return self.should_be_running + async def _detect_firmware_vehicle_type(self) -> None: + if self._firmware_vehicle_type is not None: + return + try: + vehicle_type = await self.vehicle_manager.get_vehicle_type() + self._firmware_vehicle_type = vehicle_type.mavlink_firmware_type() + except Exception: + pass + + async def _monitor_heartbeat(self) -> None: + """Monitor MAVLink heartbeat and restart only for ROVs (Sub).""" + if not self.should_be_running or not self.is_running(): + return + + try: + alive = await self.vehicle_manager.is_heart_beating() + if alive: + self._heartbeat_fail_count = 0 + else: + self._heartbeat_fail_count += 1 + logger.warning(f"Heartbeat check False ({self._heartbeat_fail_count}/{self._max_heartbeat_failures})") + except Exception as error: + self._heartbeat_fail_count += 1 + logger.warning( + f"heartbeat check failed ({self._heartbeat_fail_count}/{self._max_heartbeat_failures}): {error}" + ) + + if self._heartbeat_fail_count < self._max_heartbeat_failures: + return + + logger.warning("Consecutive heartbeat failures threshold reached — restarting Ardupilot.") + try: + await self._detect_firmware_vehicle_type() + if self._firmware_vehicle_type == MavlinkFirmwareType.ArduSub: + await self.restart_ardupilot() + except Exception as error: + logger.warning(f"Failed to restart Ardupilot after heartbeat failures: {error}") + self._heartbeat_fail_count = 0 + async def auto_restart_ardupilot(self) -> None: """Auto-restart Ardupilot when it's not running but was supposed to.""" while True: @@ -203,31 +244,7 @@ async def auto_restart_ardupilot(self) -> None: except Exception as error: logger.warning(f"Could not start Ardupilot: {error}") - # Monitor MAVLink heartbeat while autopilot is supposed to be running - if self.should_be_running and self.is_running(): - try: - alive = await self.vehicle_manager.is_heart_beating() - if alive: - self._heartbeat_fail_count = 0 - else: - self._heartbeat_fail_count += 1 - logger.warning( - f"Heartbeat check False ({self._heartbeat_fail_count}/{self._max_heartbeat_failures})" - ) - except Exception as error: - self._heartbeat_fail_count += 1 - logger.warning( - f"heartbeat check failed ({self._heartbeat_fail_count}/{self._max_heartbeat_failures}): {error}" - ) - - if self._heartbeat_fail_count >= self._max_heartbeat_failures: - logger.warning("Consecutive heartbeat failures threshold reached — restarting Ardupilot.") - try: - await self.restart_ardupilot() - except Exception as error: - logger.warning(f"Failed to restart Ardupilot after heartbeat failures: {error}") - self._heartbeat_fail_count = 0 - + await self._monitor_heartbeat() await asyncio.sleep(5.0) async def start_mavlink_manager_watchdog(self) -> None: