From a21edac84bdedd27d1f392bcb0f911a964d5d8ed Mon Sep 17 00:00:00 2001
From: Eric Johnson <eric.emj.johnson@gmail.com>
Date: Sun, 19 Nov 2023 22:46:01 -0600
Subject: [PATCH] Update Mode state for Copter and Plane vehicle types

---
 src/libs/vehicle/ardupilot/arducopter.ts | 37 ++++++++++++++++++++++++
 src/libs/vehicle/ardupilot/arduplane.ts  | 37 ++++++++++++++++++++++++
 2 files changed, 74 insertions(+)

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