diff --git a/src/libs/vehicle/ardupilot/arducopter.ts b/src/libs/vehicle/ardupilot/arducopter.ts
index d9a356e06..4f40f4a74 100644
--- a/src/libs/vehicle/ardupilot/arducopter.ts
+++ b/src/libs/vehicle/ardupilot/arducopter.ts
@@ -1,3 +1,7 @@
+import type { Package } from '@/libs/connection/m2r/messages/mavlink2rest'
+import { MAVLinkType, MavModeFlag } from '@/libs/connection/m2r/messages/mavlink2rest-enum'
+import type { Message } from '@/libs/connection/m2r/messages/mavlink2rest-message'
+
 import * as Vehicle from '../vehicle'
 import { ArduPilotVehicle } from './ardupilot'
 
@@ -96,4 +100,37 @@ export class ArduCopter extends ArduPilotVehicle<CustomMode> {
       })
     return modeMap
   }
+
+  /**
+   * Deal with MAVLink messages necessary for vehicles of type copter
+   * @param {Package} mavlink
+   */
+  onMAVLinkPackage(mavlink: Package): void {
+    const { system_id, component_id } = mavlink.header
+    if (system_id != 1 || component_id !== 1) {
+      return
+    }
+
+    switch (mavlink.message.type) {
+      case MAVLinkType.HEARTBEAT: {
+        const heartbeat = mavlink.message as Message.Heartbeat
+
+        // The special case where base_mode was not set by the vehicle
+        if ((heartbeat.base_mode.bits as number) === 0) {
+          this._mode = CustomMode.PRE_FLIGHT
+          this.onMode.emit()
+          return
+        }
+
+        // We only deal with the custom modes since this is how ArduPilot works
+        if (!(heartbeat.base_mode.bits & MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
+          console.log(`no custom: ${JSON.stringify(heartbeat.base_mode)}`)
+          return
+        }
+
+        this._mode = heartbeat.custom_mode as CustomMode
+        this.onMode.emit()
+      }
+    }
+  }
 }
diff --git a/src/libs/vehicle/ardupilot/arduplane.ts b/src/libs/vehicle/ardupilot/arduplane.ts
index 5a151bb9f..aed760859 100644
--- a/src/libs/vehicle/ardupilot/arduplane.ts
+++ b/src/libs/vehicle/ardupilot/arduplane.ts
@@ -1,3 +1,7 @@
+import type { Package } from '@/libs/connection/m2r/messages/mavlink2rest'
+import { MAVLinkType, MavModeFlag } from '@/libs/connection/m2r/messages/mavlink2rest-enum'
+import type { Message } from '@/libs/connection/m2r/messages/mavlink2rest-message'
+
 import * as Vehicle from '../vehicle'
 import { ArduPilotVehicle } from './ardupilot'
 
@@ -69,4 +73,37 @@ export class ArduPlane extends ArduPilotVehicle<CustomMode> {
       })
     return modeMap
   }
+
+  /**
+   * Deal with MAVLink messages necessary for vehicles of type plane
+   * @param {Package} mavlink
+   */
+  onMAVLinkPackage(mavlink: Package): void {
+    const { system_id, component_id } = mavlink.header
+    if (system_id != 1 || component_id !== 1) {
+      return
+    }
+
+    switch (mavlink.message.type) {
+      case MAVLinkType.HEARTBEAT: {
+        const heartbeat = mavlink.message as Message.Heartbeat
+
+        // The special case where base_mode was not set by the vehicle
+        if ((heartbeat.base_mode.bits as number) === 0) {
+          this._mode = CustomMode.PRE_FLIGHT
+          this.onMode.emit()
+          return
+        }
+
+        // We only deal with the custom modes since this is how ArduPilot works
+        if (!(heartbeat.base_mode.bits & MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
+          console.log(`no custom: ${JSON.stringify(heartbeat.base_mode)}`)
+          return
+        }
+
+        this._mode = heartbeat.custom_mode as CustomMode
+        this.onMode.emit()
+      }
+    }
+  }
 }
diff --git a/src/stores/mainVehicle.ts b/src/stores/mainVehicle.ts
index 315a2dac2..77061f8b7 100644
--- a/src/stores/mainVehicle.ts
+++ b/src/stores/mainVehicle.ts
@@ -254,21 +254,6 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => {
     return (vehicle as ArduPilot) || undefined
   }
 
-  /**
-   * Retrieves the first key from a Map where the associated value matches the specified value.
-   * @param {Map<string, number>} map
-   * @param {number} value
-   * @returns {string | undefined}
-   */
-  function getStringFromValue(map: Map<string, number>, value: number): string | undefined {
-    for (const [key, val] of map.entries()) {
-      if (val === value) {
-        return key
-      }
-    }
-    return undefined
-  }
-
   VehicleFactory.onVehicles.once((vehicles: WeakRef<Vehicle.Abstract>[]) => {
     mainVehicle.value = getAutoPilot(vehicles)
     modes.value = mainVehicle.value.modesAvailable()
@@ -318,9 +303,6 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => {
       firmwareType.value = heartbeat.autopilot.type
       vehicleType.value = heartbeat.mavtype.type
       lastHeartbeat.value = new Date()
-      if (modes.value !== undefined) {
-        mode.value = getStringFromValue(modes.value, heartbeat.custom_mode)
-      }
     })
     // eslint-disable-next-line @typescript-eslint/no-explicit-any
     getAutoPilot(vehicles).onMode.add((vehicleMode: any) => {