Update Mode state for Copter and Plane vehicle types

bluerobotics · Nov 20, 2023 · a21edac · a21edac
1 parent f9a899a
commit a21edac
Show file tree

Hide file tree

Showing 2 changed files with 74 additions and 0 deletions.
diff --git 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
@@ -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()
+      }
+    }
+  }
 }