diff --git a/src/libs/vehicle/ardupilot/arducopter.ts b/src/libs/vehicle/ardupilot/arducopter.ts
index 9e97d9327..764632f44 100644
--- a/src/libs/vehicle/ardupilot/arducopter.ts
+++ b/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)
   }
 
   /**
@@ -108,11 +109,6 @@ export class ArduCopter extends ArduPilotVehicle<CustomMode> {
    * @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
diff --git a/src/libs/vehicle/ardupilot/ardupilot.ts b/src/libs/vehicle/ardupilot/ardupilot.ts
index fb2b340c2..53bef2910 100644
--- a/src/libs/vehicle/ardupilot/ardupilot.ts
+++ b/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 = 1
+
   /**
    * 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
   }
 
   /**
@@ -135,7 +139,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
       command: {
         type: mav_command,
       },
-      target_system: 1,
+      target_system: this.currentSystemId,
       target_component: 1,
       confirmation: 0,
     }
@@ -180,7 +184,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
 
     const { system_id, component_id } = mavlink_message.header
 
-    if (system_id != 1 || component_id != 1) {
+    if (system_id !== this.currentSystemId || component_id !== 1) {
       return
     }
 
@@ -437,7 +441,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
   goTo(hold: number, acceptanceRadius: number, passRadius: number, yaw: number, coordinates: Coordinates): void {
     const gotoMessage: Message.CommandInt = {
       type: MAVLinkType.COMMAND_INT,
-      target_system: 1,
+      target_system: this.currentSystemId,
       target_component: 1,
       seq: 0,
       frame: { type: MavFrame.MAV_FRAME_GLOBAL },
@@ -658,7 +662,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
     }
     const paramSetMessage: Message.ParamSet = {
       type: MAVLinkType.PARAM_SET,
-      target_system: 0,
+      target_system: this.currentSystemId,
       target_component: 0,
       // @ts-ignore: The correct type is indeed a char array
       param_id: param_name,
@@ -677,7 +681,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
   sendMissionCount(itemsCount: number, missionType: MavMissionType): void {
     const message: Message.MissionCount = {
       type: MAVLinkType.MISSION_COUNT,
-      target_system: 1,
+      target_system: this.currentSystemId,
       target_component: 1,
       count: itemsCount,
       mission_type: { type: missionType },
@@ -693,7 +697,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
   requestMissionItemsList(missionType: MavMissionType): void {
     const message: Message.MissionRequestList = {
       type: MAVLinkType.MISSION_REQUEST_LIST,
-      target_system: 1,
+      target_system: this.currentSystemId,
       target_component: 1,
       mission_type: { type: missionType },
     }
@@ -768,7 +772,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
   ): void {
     const message: Message.MissionItem = {
       type: MAVLinkType.MISSION_ITEM,
-      target_system: 1,
+      target_system: this.currentSystemId,
       target_component: 1,
       seq: waypointSeq,
       frame: { type: frame },
@@ -909,7 +913,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
   ): Promise<void> {
     // Convert from Cockpit waypoints to MAVLink waypoints
     this._currentCockpitMissionItemsOnPlanning = items
-    const mavlinkWaypoints = convertCockpitWaypointsToMavlink(items)
+    const mavlinkWaypoints = convertCockpitWaypointsToMavlink(items, this.currentSystemId)
 
     // Only deal with regular mission items for now
     const missionType = MavMissionType.MAV_MISSION_TYPE_MISSION
diff --git a/src/libs/vehicle/ardupilot/arduplane.ts b/src/libs/vehicle/ardupilot/arduplane.ts
index a74563e7c..57ae39f0a 100644
--- a/src/libs/vehicle/ardupilot/arduplane.ts
+++ b/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)
   }
 
   /**
@@ -81,11 +82,6 @@ export class ArduPlane extends ArduPilotVehicle<CustomMode> {
    * @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
diff --git a/src/libs/vehicle/ardupilot/ardurover.ts b/src/libs/vehicle/ardupilot/ardurover.ts
index 69d4fc787..d40e629c0 100644
--- a/src/libs/vehicle/ardupilot/ardurover.ts
+++ b/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)
   }
 
   /**
@@ -69,11 +70,6 @@ export class ArduRover extends ArduPilotVehicle<CustomMode> {
    * @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
diff --git a/src/libs/vehicle/ardupilot/ardusub.ts b/src/libs/vehicle/ardupilot/ardusub.ts
index 88e48cafd..3e677c14d 100644
--- a/src/libs/vehicle/ardupilot/ardusub.ts
+++ b/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)
   }
 
   /**
@@ -75,11 +76,6 @@ export class ArduSub extends ArduPilotVehicle<CustomMode> {
    * @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
diff --git a/src/libs/vehicle/ardupilot/types.ts b/src/libs/vehicle/ardupilot/types.ts
index aba764fd4..33a55874d 100644
--- a/src/libs/vehicle/ardupilot/types.ts
+++ b/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,
diff --git a/src/libs/vehicle/vehicle-factory.ts b/src/libs/vehicle/vehicle-factory.ts
index 264f38bff..37b3427f4 100644
--- a/src/libs/vehicle/vehicle-factory.ts
+++ b/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}`)
diff --git a/src/stores/mainVehicle.ts b/src/stores/mainVehicle.ts
index 71065d295..f21d59201 100644
--- a/src/stores/mainVehicle.ts
+++ b/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
       }