Add support for non-default vehicle ID

Add detection, storage, and handling of system_id

src/libs/vehicle/ardupilot/arducopter.ts

@@ -76,9 +76,10 @@ export class ArduCopter extends ArduPilotVehicle<CustomMode> {
 
   /**
    * Create ArduCopter vehicle
+   * @param {number} system_id
    */
-  constructor() {
-    super(Vehicle.Type.Copter)
+  constructor(system_id: number) {
+    super(Vehicle.Type.Copter, system_id)
   }
 
   /**
@@ -109,7 +110,9 @@ export class ArduCopter extends ArduPilotVehicle<CustomMode> {
    */
   onMAVLinkPackage(mavlink: Package): void {
     const { system_id, component_id } = mavlink.header
-    if (system_id != 1 || component_id !== 1) {
+    this.currentSystemId = system_id
+
+    if (component_id !== 1) {
       return
     }
 

src/libs/vehicle/ardupilot/ardupilot.ts

@@ -82,6 +82,8 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
   onMAVLinkMessage = new SignalTyped()
   _flying = false
 
+  protected currentSystemId: number | null = null // Store system_id
+
   /**
    * Function for subclass inheritance
    * Helps to deal with specialized vehicles that has particular or custom behaviour
@@ -96,9 +98,11 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
   /**
    * Construct a new generic ArduPilot type
    * @param {Vehicle.Type} type
+   * @param {number} system_id
    */
-  constructor(type: Vehicle.Type) {
+  constructor(type: Vehicle.Type, system_id: number) {
     super(Vehicle.Firmware.ArduPilot, type)
+    this.currentSystemId = system_id
   }
 
   /**
@@ -123,6 +127,8 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
     param6?: number,
     param7?: number
   ): void {
+    const effectiveSystemId = this.currentSystemId ?? 1
+
     const command: Message.CommandLong = {
       type: MAVLinkType.COMMAND_LONG,
       param1: param1 ?? 0,
@@ -135,7 +141,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
       command: {
         type: mav_command,
       },
-      target_system: 1,
+      target_system: effectiveSystemId,
       target_component: 1,
       confirmation: 0,
     }
@@ -178,9 +184,10 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
       return
     }
 
-    const { system_id, component_id } = mavlink_message.header
+    const { component_id } = mavlink_message.header
+    this.currentSystemId = mavlink_message.header.system_id
 
-    if (system_id != 1 || component_id != 1) {
+    if (component_id != 1) {
       return
     }
 
@@ -435,9 +442,11 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
    * @param {Coordinates} coordinates
    */
   goTo(hold: number, acceptanceRadius: number, passRadius: number, yaw: number, coordinates: Coordinates): void {
+    const effectiveSystemId = this.currentSystemId ?? 1
+
     const gotoMessage: Message.CommandInt = {
       type: MAVLinkType.COMMAND_INT,
-      target_system: 1,
+      target_system: effectiveSystemId,
       target_component: 1,
       seq: 0,
       frame: { type: MavFrame.MAV_FRAME_GLOBAL },
@@ -652,13 +661,14 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
    * @param { ArduPilotParameterSetData } settings Data used to set a parameter
    */
   setParameter(settings: ArduPilotParameterSetData): void {
+    const effectiveSystemId = this.currentSystemId ?? 1
     const param_name = [...settings.id]
     while (param_name.length < 16) {
       param_name.push('\0')
     }
     const paramSetMessage: Message.ParamSet = {
       type: MAVLinkType.PARAM_SET,
-      target_system: 0,
+      target_system: effectiveSystemId,
       target_component: 0,
       // @ts-ignore: The correct type is indeed a char array
       param_id: param_name,
@@ -675,9 +685,10 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
    * @param { MavMissionType } missionType Type of mission to be executed
    */
   sendMissionCount(itemsCount: number, missionType: MavMissionType): void {
+    const effectiveSystemId = this.currentSystemId ?? 1
     const message: Message.MissionCount = {
       type: MAVLinkType.MISSION_COUNT,
-      target_system: 1,
+      target_system: effectiveSystemId,
       target_component: 1,
       count: itemsCount,
       mission_type: { type: missionType },
@@ -691,9 +702,10 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
    * @param { MavMissionType } missionType Type of mission to be executed
    */
   requestMissionItemsList(missionType: MavMissionType): void {
+    const effectiveSystemId = this.currentSystemId ?? 1
     const message: Message.MissionRequestList = {
       type: MAVLinkType.MISSION_REQUEST_LIST,
-      target_system: 1,
+      target_system: effectiveSystemId,
       target_component: 1,
       mission_type: { type: missionType },
     }
@@ -766,9 +778,10 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
     param7: number,
     missionType: MavMissionType
   ): void {
+    const effectiveSystemId = this.currentSystemId ?? 1
     const message: Message.MissionItem = {
       type: MAVLinkType.MISSION_ITEM,
-      target_system: 1,
+      target_system: effectiveSystemId,
       target_component: 1,
       seq: waypointSeq,
       frame: { type: frame },
@@ -907,9 +920,10 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
     items: Waypoint[],
     loadingCallback: MissionLoadingCallback = defaultLoadingCallback
   ): Promise<void> {
+    const systemIdToUse = this.currentSystemId ?? 1
     // Convert from Cockpit waypoints to MAVLink waypoints
     this._currentCockpitMissionItemsOnPlanning = items
-    const mavlinkWaypoints = convertCockpitWaypointsToMavlink(items)
+    const mavlinkWaypoints = convertCockpitWaypointsToMavlink(items, systemIdToUse)
 
     // Only deal with regular mission items for now
     const missionType = MavMissionType.MAV_MISSION_TYPE_MISSION

src/libs/vehicle/ardupilot/arduplane.ts

@@ -49,9 +49,10 @@ export class ArduPlane extends ArduPilotVehicle<CustomMode> {
 
   /**
    * Create ArduPlane vehicle
+   * @param {number} system_id
    */
-  constructor() {
-    super(Vehicle.Type.Plane)
+  constructor(system_id: number) {
+    super(Vehicle.Type.Plane, system_id)
   }
 
   /**
@@ -82,7 +83,9 @@ export class ArduPlane extends ArduPilotVehicle<CustomMode> {
    */
   onMAVLinkPackage(mavlink: Package): void {
     const { system_id, component_id } = mavlink.header
-    if (system_id != 1 || component_id !== 1) {
+    this.currentSystemId = system_id
+
+    if (component_id !== 1) {
       return
     }
 

src/libs/vehicle/ardupilot/ardurover.ts

@@ -37,9 +37,10 @@ export class ArduRover extends ArduPilotVehicle<CustomMode> {
 
   /**
    * Create ArduRover vehicle
+   * @param {number} system_id
    */
-  constructor() {
-    super(Vehicle.Type.Rover)
+  constructor(system_id: number) {
+    super(Vehicle.Type.Rover, system_id)
   }
 
   /**
@@ -70,7 +71,9 @@ export class ArduRover extends ArduPilotVehicle<CustomMode> {
    */
   onMAVLinkPackage(mavlink: Package): void {
     const { system_id, component_id } = mavlink.header
-    if (system_id != 1 || component_id !== 1) {
+    this.currentSystemId = system_id
+
+    if (component_id !== 1) {
       return
     }
 

src/libs/vehicle/ardupilot/ardusub.ts

@@ -43,9 +43,10 @@ export class ArduSub extends ArduPilotVehicle<CustomMode> {
 
   /**
    * Create ArduSub vehicle
+   * @param {number} system_id
    */
-  constructor() {
-    super(Vehicle.Type.Sub)
+  constructor(system_id: number) {
+    super(Vehicle.Type.Sub, system_id)
   }
 
   /**
@@ -76,7 +77,9 @@ export class ArduSub extends ArduPilotVehicle<CustomMode> {
    */
   onMAVLinkPackage(mavlink: Package): void {
     const { system_id, component_id } = mavlink.header
-    if (system_id != 1 || component_id !== 1) {
+    this.currentSystemId = system_id
+
+    if (component_id !== 1) {
       return
     }
 

src/libs/vehicle/ardupilot/types.ts

@@ -31,10 +31,13 @@ const cockpitAltRefFromMavlinkFrame = (mavframe: MavFrame): AltitudeReferenceTyp
   return correspondency === undefined ? undefined : correspondency[1]
 }
 
-export const convertCockpitWaypointsToMavlink = (cockpitWaypoints: Waypoint[]): Message.MissionItemInt[] => {
+export const convertCockpitWaypointsToMavlink = (
+  cockpitWaypoints: Waypoint[],
+  system_id: number
+): Message.MissionItemInt[] => {
   return cockpitWaypoints.map((cockpitWaypoint, i) => {
     return {
-      target_system: 1,
+      target_system: system_id,
       target_component: 1,
       type: MAVLinkType.MISSION_ITEM_INT,
       seq: i,

src/libs/vehicle/vehicle-factory.ts

@@ -23,14 +23,19 @@ export class VehicleFactory {
    * Create vehicle based on the firmware and vehicle type
    * @param {Vehicle.Firmware} firmware
    * @param {Vehicle.Type} type
+   * @param {number} system_id - The system ID for the vehicle
    * @returns {Vehicle.Abstract | undefined}
    */
-  static createVehicle(firmware: Vehicle.Firmware, type: Vehicle.Type): Vehicle.Abstract | undefined {
+  static createVehicle(
+    firmware: Vehicle.Firmware,
+    type: Vehicle.Type,
+    system_id: number
+  ): Vehicle.Abstract | undefined {
     let vehicle: undefined | Vehicle.Abstract = undefined
 
     switch (firmware) {
       case Vehicle.Firmware.ArduPilot:
-        vehicle = VehicleFactory.createArduPilotVehicle(type)
+        vehicle = VehicleFactory.createArduPilotVehicle(type, system_id)
         break
     }
 
@@ -50,18 +55,19 @@ export class VehicleFactory {
   /**
    * Create ArduPilot vehicle based on the category
    * @param  {Vehicle.Type} type
+   * @param  {number} system_id
    * @returns {Vehicle.Abstract | undefined}
    */
-  static createArduPilotVehicle(type: Vehicle.Type): Vehicle.Abstract | undefined {
+  static createArduPilotVehicle(type: Vehicle.Type, system_id: number): Vehicle.Abstract | undefined {
     switch (type) {
       case Vehicle.Type.Copter:
-        return new ArduCopter()
+        return new ArduCopter(system_id)
       case Vehicle.Type.Plane:
-        return new ArduPlane()
+        return new ArduPlane(system_id)
       case Vehicle.Type.Rover:
-        return new ArduRover()
+        return new ArduRover(system_id)
       case Vehicle.Type.Sub:
-        return new ArduSub()
+        return new ArduSub(system_id)
       default:
         unimplemented('Firmware not supported')
     }
@@ -105,18 +111,18 @@ function createVehicleFromMessage(message: Uint8Array): void {
 
   switch (heartbeat.mavtype.type) {
     case MavType.MAV_TYPE_SUBMARINE:
-      VehicleFactory.createVehicle(Vehicle.Firmware.ArduPilot, Vehicle.Type.Sub)
+      VehicleFactory.createVehicle(Vehicle.Firmware.ArduPilot, Vehicle.Type.Sub, system_id)
       break
     case MavType.MAV_TYPE_GROUND_ROVER:
     case MavType.MAV_TYPE_SURFACE_BOAT:
-      VehicleFactory.createVehicle(Vehicle.Firmware.ArduPilot, Vehicle.Type.Rover)
+      VehicleFactory.createVehicle(Vehicle.Firmware.ArduPilot, Vehicle.Type.Rover, system_id)
       break
     case MavType.MAV_TYPE_FLAPPING_WING:
     case MavType.MAV_TYPE_VTOL_TILTROTOR:
     case MavType.MAV_TYPE_VTOL_QUADROTOR:
     case MavType.MAV_TYPE_VTOL_DUOROTOR:
     case MavType.MAV_TYPE_FIXED_WING:
-      VehicleFactory.createVehicle(Vehicle.Firmware.ArduPilot, Vehicle.Type.Plane)
+      VehicleFactory.createVehicle(Vehicle.Firmware.ArduPilot, Vehicle.Type.Plane, system_id)
       break
     case MavType.MAV_TYPE_TRICOPTER:
     case MavType.MAV_TYPE_COAXIAL:
@@ -125,7 +131,7 @@ function createVehicleFromMessage(message: Uint8Array): void {
     case MavType.MAV_TYPE_OCTOROTOR:
     case MavType.MAV_TYPE_DODECAROTOR:
     case MavType.MAV_TYPE_QUADROTOR:
-      VehicleFactory.createVehicle(Vehicle.Firmware.ArduPilot, Vehicle.Type.Copter)
+      VehicleFactory.createVehicle(Vehicle.Firmware.ArduPilot, Vehicle.Type.Copter, system_id)
       break
     default:
       console.warn(`Vehicle type not supported: ${system_id}/${component_id}: ${heartbeat.mavtype.type}`)

src/stores/mainVehicle.ts

@@ -363,7 +363,7 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => {
       Object.assign(genericVariables, newGenericVariablesState)
     })
     mainVehicle.value.onMAVLinkMessage.add(MAVLinkType.HEARTBEAT, (pack: Package) => {
-      if (pack.header.system_id != 1 || pack.header.component_id != 1) {
+      if (pack.header.component_id != 1) {
         return
       }