Skip to content

Commit

Permalink
dont return on non 1 vid WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
@goasChris @flaeri
goasChris authored and flaeri committed Feb 16, 2024
1 parent 1a81705 commit c179ed2
Show file tree
Hide file tree
Showing 4 changed files with 78 additions and 35 deletions.
83 changes: 60 additions & 23 deletions src/libs/vehicle/ardupilot/ardupilot.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,17 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
onMAVLinkMessage = new SignalTyped()
_flying = false

protected currentSystemId: number | null = null // Now accessible in subclasses // Store system_id
private onSystemIdUpdated: ((systemId: number) => void) | null = null;

/**
* Set a callback to be invoked when the system_id is updated.
* @param callback A function to call when the system_id is updated.
*/
setSystemIdUpdateCallback(callback: (systemId: number) => void): void {
this.onSystemIdUpdated = callback;
}

/**
* Function for subclass inheritance
* Helps to deal with specialized vehicles that has particular or custom behaviour
Expand All @@ -97,14 +108,16 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
* Construct a new generic ArduPilot type
* @param {Vehicle.Type} type
*/
constructor(type: Vehicle.Type) {
super(Vehicle.Firmware.ArduPilot, type)
constructor(type: Vehicle.Type, system_id: number) {
super(Vehicle.Firmware.ArduPilot, type);
this.currentSystemId = system_id;
}

/**
* Helper to send long mavlink commands
* Each parameter depends of the value specified by the protocol
* @param {MavCmd} mav_command
* @param {number} [system_id] - The target system ID. If not provided, defaults to 1 or the last known system_id.
* @param {number} param1
* @param {number} param2
* @param {number} param3
Expand All @@ -115,6 +128,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
*/
sendCommandLong(
mav_command: MavCmd,
system_id: number = this.currentSystemId || 1,
param1?: number,
param2?: number,
param3?: number,
Expand All @@ -123,6 +137,8 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
param6?: number,
param7?: number
): void {
console.log(`Sending commandLong with system_id: ${system_id}`, "currentSystemId was: ", this.currentSystemId);

const command: Message.CommandLong = {
type: MAVLinkType.COMMAND_LONG,
param1: param1 ?? 0,
Expand All @@ -135,7 +151,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
command: {
type: mav_command,
},
target_system: 1,
target_system: system_id,
target_component: 1,
confirmation: 0,
}
Expand Down Expand Up @@ -179,8 +195,14 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
}

const { system_id, component_id } = mavlink_message.header
this.currentSystemId = mavlink_message.header.system_id

if (system_id != 1 || component_id != 1) {
// If a callback has been set, call it with the updated system_id
if (this.onSystemIdUpdated) {
this.onSystemIdUpdated(this.currentSystemId);
}

if (component_id != 1) {
return
}

Expand Down Expand Up @@ -370,11 +392,13 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
* Helper function to arm/disarm the vehicle
* @param {boolean} arm
* @param {boolean} force
* @param {number} system_id - The system ID to use for the command.
*/
_arm(arm: boolean, force?: boolean): void {
_arm(arm: boolean, force: boolean = false): void {
this.sendCommandLong(
MavCmd.MAV_CMD_COMPONENT_ARM_DISARM,
arm ? 1 : 0, // 0: Disarm, 1: ARM
undefined, //sys_id
arm ? 1 : 0, // 0: Disarm, 1: Arm
force ? 21196 : 0 // 21196: force arming/disarming (e.g. override preflight checks and disarming in flight)
)
}
Expand All @@ -384,7 +408,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
* @returns {boolean}
*/
arm(): boolean {
this._arm(true)
this._arm(true, false)
return true
}

Expand All @@ -393,7 +417,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
* @param {number} altitude (in meters)
*/
_takeoff(altitude: number): void {
this.sendCommandLong(MavCmd.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, altitude)
this.sendCommandLong(MavCmd.MAV_CMD_NAV_TAKEOFF, undefined, 0, 0, 0, 0, 0, 0, altitude)
}

/**
Expand Down Expand Up @@ -433,11 +457,12 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
* @param {number} passRadius 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.
* @param {number} yaw Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).
* @param {Coordinates} coordinates
*/
goTo(hold: number, acceptanceRadius: number, passRadius: number, yaw: number, coordinates: Coordinates): void {
* @param {number} [system_id=this.currentSystemId || 1] The system ID to target, defaults to the current system ID or 1 if unset.
*/
goTo(hold: number, acceptanceRadius: number, passRadius: number, yaw: number, coordinates: Coordinates, system_id: number = this.currentSystemId || 1): void {
const gotoMessage: Message.CommandInt = {
type: MAVLinkType.COMMAND_INT,
target_system: 1,
target_system: system_id,
target_component: 1,
seq: 0,
frame: { type: MavFrame.MAV_FRAME_GLOBAL },
Expand Down Expand Up @@ -501,7 +526,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
* @returns {boolean}
*/
disarm(): boolean {
this._arm(false)
this._arm(false, false)
return true
}

Expand Down Expand Up @@ -630,6 +655,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
setMode(mode: Modes): void {
this.sendCommandLong(
MavCmd.MAV_CMD_DO_SET_MODE,
undefined, //sys_id
MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
Number(mode) // Custom mode, please refer to the individual autopilot specifications for details
)
Expand All @@ -650,15 +676,16 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
/**
* Request parameters list from vehicle
* @param { ArduPilotParameterSetData } settings Data used to set a parameter
* @param {number} [system_id=this.currentSystemId || 1] The system ID to target, defaults to the current system ID or 1 if unset.
*/
setParameter(settings: ArduPilotParameterSetData): void {
setParameter(settings: ArduPilotParameterSetData, system_id: number = this.currentSystemId || 1): void {
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: system_id,
target_component: 0,
// @ts-ignore: The correct type is indeed a char array
param_id: param_name,
Expand All @@ -673,11 +700,12 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
* Send number of mission items that will be uploaded next
* @param { number } itemsCount Number of mission items that will be sent
* @param { MavMissionType } missionType Type of mission to be executed
* @param {number} [system_id=this.currentSystemId || 1] The system ID to target, defaults to the current system ID or 1 if unset.
*/
sendMissionCount(itemsCount: number, missionType: MavMissionType): void {
sendMissionCount(itemsCount: number, missionType: MavMissionType, system_id: number): void {
const message: Message.MissionCount = {
type: MAVLinkType.MISSION_COUNT,
target_system: 1,
target_system: system_id,
target_component: 1,
count: itemsCount,
mission_type: { type: missionType },
Expand All @@ -689,11 +717,12 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
/**
* Request the list of mission items from the vehicle
* @param { MavMissionType } missionType Type of mission to be executed
* @param {number} [system_id=this.currentSystemId || 1] The system ID to target, defaults to the current system ID or 1 if unset.
*/
requestMissionItemsList(missionType: MavMissionType): void {
requestMissionItemsList(missionType: MavMissionType, system_id: number): void {
const message: Message.MissionRequestList = {
type: MAVLinkType.MISSION_REQUEST_LIST,
target_system: 1,
target_system: system_id,
target_component: 1,
mission_type: { type: missionType },
}
Expand Down Expand Up @@ -750,6 +779,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
* @param { number } param6
* @param { number } param7
* @param { MavMissionType } missionType Type of mission to be executed
* @param {number} [system_id=this.currentSystemId || 1] The system ID to target, defaults to the current system ID or 1 if unset.
*/
sendMissionItem(
waypointSeq: number,
Expand All @@ -764,11 +794,12 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
param5: number,
param6: number,
param7: number,
missionType: MavMissionType
missionType: MavMissionType,
system_id: number = this.currentSystemId || 1
): void {
const message: Message.MissionItem = {
type: MAVLinkType.MISSION_ITEM,
target_system: 1,
target_system: system_id,
target_component: 1,
seq: waypointSeq,
frame: { type: frame },
Expand Down Expand Up @@ -797,11 +828,14 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
// Only deal with regular mission items for now
const missionType = MavMissionType.MAV_MISSION_TYPE_MISSION

// Use the current system_id or default to 1
const system_id = this.currentSystemId || 1;

// Get number of mission items to be downloaded
const initTimeCount = new Date().getTime()
let timeoutReachedCount = false
let itemsCount: number | undefined = undefined
this.requestMissionItemsList(missionType)
this.requestMissionItemsList(missionType, system_id)
while (itemsCount === undefined && !timeoutReachedCount) {
const lastMissionCountMessage = this._messages.get(MAVLinkType.MISSION_COUNT)
if (lastMissionCountMessage !== undefined && lastMissionCountMessage.epoch > initTimeCount) {
Expand Down Expand Up @@ -895,7 +929,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
throw Error('Could not arm the vehicle. Please arm it manually.')
}

this.sendCommandLong(MavCmd.MAV_CMD_MISSION_START, 0, 0)
this.sendCommandLong(MavCmd.MAV_CMD_MISSION_START, undefined, 0, 0)
}

/**
Expand All @@ -914,8 +948,11 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
// Only deal with regular mission items for now
const missionType = MavMissionType.MAV_MISSION_TYPE_MISSION

// Use the current system_id or default to 1
const system_id = this.currentSystemId || 1;

// Say to the vehicle how many mission items we are going to send
this.sendMissionCount(mavlinkWaypoints.length, missionType)
this.sendMissionCount(mavlinkWaypoints.length, missionType, system_id)

// Send the mission items one by one, only sending the next when explicitly requested by the vehicle
let missionAck: MavMissionResult | undefined = undefined
Expand Down
9 changes: 6 additions & 3 deletions src/libs/vehicle/ardupilot/ardurover.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,8 @@ export class ArduRover extends ArduPilotVehicle<CustomMode> {
/**
* Create ArduRover vehicle
*/
constructor() {
super(Vehicle.Type.Rover)
constructor(system_id: number) {
super(Vehicle.Type.Rover, system_id)
}

/**
Expand Down Expand Up @@ -70,7 +70,10 @@ 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
}

Expand Down
19 changes: 11 additions & 8 deletions src/libs/vehicle/vehicle-factory.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,14 +23,15 @@ 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
}

Expand All @@ -52,14 +53,15 @@ export class VehicleFactory {
* @param {Vehicle.Type} type
* @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()
case Vehicle.Type.Plane:
return new ArduPlane()
case Vehicle.Type.Rover:
return new ArduRover()
console.log("spawning rover type, with id:", system_id)
return new ArduRover(system_id);
case Vehicle.Type.Sub:
return new ArduSub()
default:
Expand Down Expand Up @@ -102,21 +104,22 @@ function createVehicleFromMessage(message: Uint8Array): void {
if (heartbeat.autopilot.type !== MavAutopilot.MAV_AUTOPILOT_ARDUPILOTMEGA) {
console.warn(`Vehicle not supported: ${system_id}/${component_id}: ${heartbeat.autopilot.type}`)
}
console.info('vid: ', system_id)

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:
Expand All @@ -125,7 +128,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}`)
Expand Down
2 changes: 1 addition & 1 deletion src/stores/mainVehicle.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}

Expand Down

0 comments on commit c179ed2

Please sign in to comment.