Reboot vehicle only for ROVs if heartbeat lost

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()
 

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):

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: