diff --git a/src/assets/joystick-profiles.ts b/src/assets/joystick-profiles.ts index 8bc5bc87f4..9b7e1baa84 100644 --- a/src/assets/joystick-profiles.ts +++ b/src/assets/joystick-profiles.ts @@ -1,11 +1,9 @@ import { MavType } from '@/libs/connection/m2r/messages/mavlink2rest-enum' import { JoystickModel } from '@/libs/joystick/manager' import { availableCockpitActions } from '@/libs/joystick/protocols/cockpit-actions' -import { - availableMavlinkManualControlButtonFunctions, - mavlinkManualControlAxes, -} from '@/libs/joystick/protocols/mavlink-manual-control' +import { availableMavlinkManualControlButtonFunctions } from '@/libs/joystick/protocols/mavlink-manual-control' import { modifierKeyActions, otherAvailableActions } from '@/libs/joystick/protocols/other' +import { joystickInputAxes } from '@/libs/joystick/protocols/predefined-resources' import { getVehicleModeAction } from '@/libs/vehicle/ardupilot/common' import { RoverMode } from '@/libs/vehicle/ardupilot/types/modes' import { Type as VehicleType } from '@/libs/vehicle/vehicle' @@ -33,10 +31,10 @@ export const cockpitStandardToProtocols: JoystickProtocolActionsMapping[] = [ name: 'ROV functions mapping', hash: defaultRovMappingHash, axesCorrespondencies: { - [JoystickAxis.A0]: { action: mavlinkManualControlAxes.axis_y, min: -1000, max: +1000 }, - [JoystickAxis.A1]: { action: mavlinkManualControlAxes.axis_x, min: +1000, max: -1000 }, - [JoystickAxis.A2]: { action: mavlinkManualControlAxes.axis_r, min: -1000, max: +1000 }, - [JoystickAxis.A3]: { action: mavlinkManualControlAxes.axis_z, min: +1000, max: 0 }, + [JoystickAxis.A0]: { action: joystickInputAxes.axis_y, min: -1000, max: +1000 }, + [JoystickAxis.A1]: { action: joystickInputAxes.axis_x, min: +1000, max: -1000 }, + [JoystickAxis.A2]: { action: joystickInputAxes.axis_r, min: -1000, max: +1000 }, + [JoystickAxis.A3]: { action: joystickInputAxes.axis_z, min: +1000, max: 0 }, }, buttonsCorrespondencies: { [CockpitModifierKeyOption.regular]: { @@ -85,10 +83,10 @@ export const cockpitStandardToProtocols: JoystickProtocolActionsMapping[] = [ name: 'Boat functions mapping', hash: defaultBoatMappingHash, axesCorrespondencies: { - [JoystickAxis.A0]: { action: mavlinkManualControlAxes.axis_y, min: -1000, max: +1000 }, - [JoystickAxis.A1]: { action: mavlinkManualControlAxes.axis_x, min: +1000, max: -1000 }, - [JoystickAxis.A2]: { action: mavlinkManualControlAxes.axis_r, min: -1000, max: +1000 }, - [JoystickAxis.A3]: { action: mavlinkManualControlAxes.axis_z, min: +1000, max: -1000 }, + [JoystickAxis.A0]: { action: joystickInputAxes.axis_y, min: -1000, max: +1000 }, + [JoystickAxis.A1]: { action: joystickInputAxes.axis_x, min: +1000, max: -1000 }, + [JoystickAxis.A2]: { action: joystickInputAxes.axis_r, min: -1000, max: +1000 }, + [JoystickAxis.A3]: { action: joystickInputAxes.axis_z, min: +1000, max: -1000 }, }, buttonsCorrespondencies: { [CockpitModifierKeyOption.regular]: { @@ -137,10 +135,10 @@ export const cockpitStandardToProtocols: JoystickProtocolActionsMapping[] = [ name: 'MAV functions mapping', hash: defaultMavMappingHash, axesCorrespondencies: { - [JoystickAxis.A0]: { action: mavlinkManualControlAxes.axis_r, min: -1000, max: +1000 }, - [JoystickAxis.A1]: { action: mavlinkManualControlAxes.axis_z, min: +1000, max: 0 }, - [JoystickAxis.A2]: { action: mavlinkManualControlAxes.axis_y, min: -1000, max: +1000 }, - [JoystickAxis.A3]: { action: mavlinkManualControlAxes.axis_x, min: +1000, max: -1000 }, + [JoystickAxis.A0]: { action: joystickInputAxes.axis_r, min: -1000, max: +1000 }, + [JoystickAxis.A1]: { action: joystickInputAxes.axis_z, min: +1000, max: 0 }, + [JoystickAxis.A2]: { action: joystickInputAxes.axis_y, min: -1000, max: +1000 }, + [JoystickAxis.A3]: { action: joystickInputAxes.axis_x, min: +1000, max: -1000 }, }, buttonsCorrespondencies: { [CockpitModifierKeyOption.regular]: { diff --git a/src/libs/joystick/protocols.ts b/src/libs/joystick/protocols.ts index 27910da6d4..fb3f61e897 100644 --- a/src/libs/joystick/protocols.ts +++ b/src/libs/joystick/protocols.ts @@ -4,17 +4,13 @@ import { availableCockpitActions } from './protocols/cockpit-actions' import { availableDataLakeActions } from './protocols/data-lake' import { availableMavlinkManualControlButtonFunctions, - mavlinkManualControlAxes, + migrateMavlinkManualControlAxes, migrateMavlinkManualControlButtons, } from './protocols/mavlink-manual-control' import { modifierKeyActions, otherAvailableActions } from './protocols/other' export const allAvailableAxes = (): ProtocolAction[] => { - return [ - ...Object.values(mavlinkManualControlAxes), - ...Object.values(availableDataLakeActions()), - otherAvailableActions.no_function, - ] + return [...Object.values(availableDataLakeActions()), otherAvailableActions.no_function] } export const allAvailableButtons = (): ProtocolAction[] => { @@ -30,5 +26,5 @@ export const allAvailableButtons = (): ProtocolAction[] => { export const performJoystickMappingMigrations = ( mappings: JoystickProtocolActionsMapping[] ): JoystickProtocolActionsMapping[] => { - return migrateMavlinkManualControlButtons(mappings) + return migrateMavlinkManualControlAxes(migrateMavlinkManualControlButtons(mappings)) } diff --git a/src/libs/joystick/protocols/cockpit-actions.ts b/src/libs/joystick/protocols/cockpit-actions.ts index 5cabbb0a6e..336c8f7343 100644 --- a/src/libs/joystick/protocols/cockpit-actions.ts +++ b/src/libs/joystick/protocols/cockpit-actions.ts @@ -20,6 +20,11 @@ export const CockpitActionsFunction = { toggle_recording_all_streams: 'toggle_recording_all_streams', take_snapshot: 'take_snapshot', hold_to_confirm: 'hold_to_confirm', + start_reversing: 'start_reversing', + stop_reversing: 'stop_reversing', + toggle_reversing: 'toggle_reversing', + increase_pilot_gain: 'increase_pilot_gain', + reduce_pilot_gain: 'reduce_pilot_gain', } as const /** @@ -61,6 +66,11 @@ export const predefinedCockpitActions: { [key in CockpitActionsFunction]: Cockpi [CockpitActionsFunction.toggle_recording_all_streams]: new CockpitAction(CockpitActionsFunction.toggle_recording_all_streams, 'Toggle recording all streams'), [CockpitActionsFunction.take_snapshot]: new CockpitAction(CockpitActionsFunction.take_snapshot, 'Take snapshot'), [CockpitActionsFunction.hold_to_confirm]: new CockpitAction(CockpitActionsFunction.hold_to_confirm, 'Hold to confirm'), + [CockpitActionsFunction.start_reversing]: new CockpitAction(CockpitActionsFunction.start_reversing, 'Start Reversing'), + [CockpitActionsFunction.stop_reversing]: new CockpitAction(CockpitActionsFunction.stop_reversing, 'Stop Reversing'), + [CockpitActionsFunction.toggle_reversing]: new CockpitAction(CockpitActionsFunction.toggle_reversing, 'Toggle Reversing'), + [CockpitActionsFunction.increase_pilot_gain]: new CockpitAction(CockpitActionsFunction.increase_pilot_gain, 'Increase Pilot Gain'), + [CockpitActionsFunction.reduce_pilot_gain]: new CockpitAction(CockpitActionsFunction.reduce_pilot_gain, 'Reduce Pilot Gain'), } export type CockpitActionCallback = () => void diff --git a/src/libs/joystick/protocols/mavlink-manual-control.ts b/src/libs/joystick/protocols/mavlink-manual-control.ts index c850b7aa8b..d71034d810 100644 --- a/src/libs/joystick/protocols/mavlink-manual-control.ts +++ b/src/libs/joystick/protocols/mavlink-manual-control.ts @@ -5,11 +5,12 @@ import { capitalize } from 'vue' import { useInteractionDialog } from '@/composables/interactionDialog' +import { getDataLakeVariableData } from '@/libs/actions/data-lake' import { sendManualControl } from '@/libs/communication/mavlink' import { modifierKeyActions, otherAvailableActions } from '@/libs/joystick/protocols/other' -import { round, scale } from '@/libs/utils' +import { round } from '@/libs/utils' import type { ArduPilot } from '@/libs/vehicle/ardupilot/ardupilot' -import { type JoystickProtocolActionsMapping, type JoystickState, type ProtocolAction, CockpitModifierKeyOption, JoystickAxis, JoystickButton, JoystickProtocol } from '@/types/joystick' +import { type JoystickProtocolActionsMapping, type JoystickState, type ProtocolAction, CockpitModifierKeyOption, JoystickButton, JoystickProtocol } from '@/types/joystick' /** * Possible axes in the MAVLink `MANUAL_CONTROL` message protocol @@ -500,21 +501,13 @@ export class MavlinkManualControlManager { } } - // Calculate axes values - const xCorrespondency = Object.entries(this.currentActionsMapping.axesCorrespondencies).find((entry) => entry[1].action.protocol === JoystickProtocol.MAVLinkManualControl && entry[1].action.id === mavlinkManualControlAxes.axis_x.id) - const yCorrespondency = Object.entries(this.currentActionsMapping.axesCorrespondencies).find((entry) => entry[1].action.protocol === JoystickProtocol.MAVLinkManualControl && entry[1].action.id === mavlinkManualControlAxes.axis_y.id) - const zCorrespondency = Object.entries(this.currentActionsMapping.axesCorrespondencies).find((entry) => entry[1].action.protocol === JoystickProtocol.MAVLinkManualControl && entry[1].action.id === mavlinkManualControlAxes.axis_z.id) - const rCorrespondency = Object.entries(this.currentActionsMapping.axesCorrespondencies).find((entry) => entry[1].action.protocol === JoystickProtocol.MAVLinkManualControl && entry[1].action.id === mavlinkManualControlAxes.axis_r.id) - const sCorrespondency = Object.entries(this.currentActionsMapping.axesCorrespondencies).find((entry) => entry[1].action.protocol === JoystickProtocol.MAVLinkManualControl && entry[1].action.id === mavlinkManualControlAxes.axis_s.id) - const tCorrespondency = Object.entries(this.currentActionsMapping.axesCorrespondencies).find((entry) => entry[1].action.protocol === JoystickProtocol.MAVLinkManualControl && entry[1].action.id === mavlinkManualControlAxes.axis_t.id) - - // Populate MAVLink Manual Control state of axes and buttons - this.manualControlState.x = xCorrespondency === undefined ? 0 : round(scale(this.joystickState.axes[xCorrespondency[0] as unknown as JoystickAxis] ?? 0, -1, 1, xCorrespondency[1].min, xCorrespondency[1].max), 0) - this.manualControlState.y = yCorrespondency === undefined ? 0 : round(scale(this.joystickState.axes[yCorrespondency[0] as unknown as JoystickAxis] ?? 0, -1, 1, yCorrespondency[1].min, yCorrespondency[1].max), 0) - this.manualControlState.z = zCorrespondency === undefined ? 0 : round(scale(this.joystickState.axes[zCorrespondency[0] as unknown as JoystickAxis] ?? 0, -1, 1, zCorrespondency[1].min, zCorrespondency[1].max), 0) - this.manualControlState.r = rCorrespondency === undefined ? 0 : round(scale(this.joystickState.axes[rCorrespondency[0] as unknown as JoystickAxis] ?? 0, -1, 1, rCorrespondency[1].min, rCorrespondency[1].max), 0) - this.manualControlState.s = sCorrespondency === undefined ? 0 : round(scale(this.joystickState.axes[sCorrespondency[0] as unknown as JoystickAxis] ?? 0, -1, 1, sCorrespondency[1].min, sCorrespondency[1].max), 0) - this.manualControlState.t = tCorrespondency === undefined ? 0 : round(scale(this.joystickState.axes[tCorrespondency[0] as unknown as JoystickAxis] ?? 0, -1, 1, tCorrespondency[1].min, tCorrespondency[1].max), 0) + // Read axis values from data lake output variables (scaling is handled by the data-lake protocol handler) + this.manualControlState.x = round(Number(getDataLakeVariableData('joystick/outputs/axis-x') ?? 0), 0) + this.manualControlState.y = round(Number(getDataLakeVariableData('joystick/outputs/axis-y') ?? 0), 0) + this.manualControlState.z = round(Number(getDataLakeVariableData('joystick/outputs/axis-z') ?? 0), 0) + this.manualControlState.r = round(Number(getDataLakeVariableData('joystick/outputs/axis-r') ?? 0), 0) + this.manualControlState.s = round(Number(getDataLakeVariableData('joystick/outputs/axis-s') ?? 0), 0) + this.manualControlState.t = round(Number(getDataLakeVariableData('joystick/outputs/axis-t') ?? 0), 0) this.manualControlState.buttons = buttons_int this.manualControlState.buttons2 = buttons2_int } @@ -703,3 +696,31 @@ const migrateServoSubButtonsToActuators = (mappings: JoystickProtocolActionsMapp export const migrateMavlinkManualControlButtons = (mappings: JoystickProtocolActionsMapping[]): JoystickProtocolActionsMapping[] => { return migrateServoSubButtonsToActuators(mappings) } + +const mavlinkAxisToDataLakeMap: Record = { + [MAVLinkAxisFunction.X]: { id: 'joystick/inputs/axis-x', name: 'Axis X' }, + [MAVLinkAxisFunction.Y]: { id: 'joystick/inputs/axis-y', name: 'Axis Y' }, + [MAVLinkAxisFunction.Z]: { id: 'joystick/inputs/axis-z', name: 'Axis Z' }, + [MAVLinkAxisFunction.R]: { id: 'joystick/inputs/axis-r', name: 'Axis R' }, + [MAVLinkAxisFunction.S]: { id: 'joystick/inputs/axis-s', name: 'Axis S' }, + [MAVLinkAxisFunction.T]: { id: 'joystick/inputs/axis-t', name: 'Axis T' }, +} + +export const migrateMavlinkManualControlAxes = (mappings: JoystickProtocolActionsMapping[]): JoystickProtocolActionsMapping[] => { + const migratedMappings: JoystickProtocolActionsMapping[] = JSON.parse(JSON.stringify(mappings)) + migratedMappings.forEach((mapping) => { + Object.entries(mapping.axesCorrespondencies).forEach(([axisIndex, axisConfig]) => { + if (axisConfig.action.protocol === JoystickProtocol.MAVLinkManualControl) { + const replacement = mavlinkAxisToDataLakeMap[axisConfig.action.id] + if (replacement) { + mapping.axesCorrespondencies[axisIndex as unknown as number].action = { + protocol: JoystickProtocol.DataLakeVariable, + id: replacement.id, + name: replacement.name, + } + } + } + }) + }) + return migratedMappings +} diff --git a/src/libs/joystick/protocols/predefined-resources.ts b/src/libs/joystick/protocols/predefined-resources.ts index 35935e46fd..faece2b875 100644 --- a/src/libs/joystick/protocols/predefined-resources.ts +++ b/src/libs/joystick/protocols/predefined-resources.ts @@ -1,5 +1,11 @@ import { getAllActionLinks, saveActionLink } from '@/libs/actions/action-links' -import { createDataLakeVariable, DataLakeVariableType } from '@/libs/actions/data-lake' +import { + createDataLakeVariable, + DataLakeVariableType, + getDataLakeVariableData, + getDataLakeVariableInfo, + setDataLakeVariableData, +} from '@/libs/actions/data-lake' import { createTransformingFunction, getAllTransformingFunctions } from '@/libs/actions/data-lake-transformations' import { getAllMavlinkMessageActionConfigs, @@ -10,9 +16,75 @@ import { MavCmd, MAVLinkType } from '@/libs/connection/m2r/messages/mavlink2rest import { getUnindentedString } from '@/libs/utils' import { customActionTypes } from '@/types/cockpit-actions' +import { availableCockpitActions, registerActionCallback } from './cockpit-actions' +import { DataLakeVariableAction } from './data-lake' + export let mavlinkCameraZoomActionId: string | undefined = undefined export let mavlinkCameraFocusActionId: string | undefined = undefined +const joystickAxisConfig = [ + { key: 'axis_x', reverseVehicleTypes: ['copter', 'sub'], translationVehicleTypes: ['sub'] }, + { key: 'axis_y', reverseVehicleTypes: ['copter', 'sub'], translationVehicleTypes: ['sub'] }, + { key: 'axis_z', reverseVehicleTypes: ['rover'], translationVehicleTypes: ['copter', 'sub', 'rover', 'plane'] }, + { key: 'axis_r', reverseVehicleTypes: [] as string[], translationVehicleTypes: [] as string[] }, + { key: 'axis_s', reverseVehicleTypes: ['sub'], translationVehicleTypes: [] as string[] }, + { key: 'axis_t', reverseVehicleTypes: ['sub'], translationVehicleTypes: [] as string[] }, +] as const + +const axisInputId = (key: string): string => `joystick/inputs/${key.replace('_', '-')}` +const axisName = (key: string): string => + key + .split('_') + .map((w) => w.charAt(0).toUpperCase() + w.slice(1)) + .join(' ') + +/** + * Generates a JavaScript expression that evaluates to -1 when the vehicle type matches + * one of the given types and reverse is active, or 1 otherwise + * @param {string[]} vehicleTypes - Vehicle types that should trigger reversal + * @returns {string} A data-lake expression string + */ +const getReverseExpression = (vehicleTypes: readonly string[]): string => { + if (vehicleTypes.length === 0) return '1' + if (vehicleTypes.length === 1) { + return `('{{cockpit/vehicle/type}}' === '${vehicleTypes[0]}' && {{joystick/inputs/reverse}}) ? -1 : 1` + } + return `([${vehicleTypes + .map((t) => `'${t}'`) + .join(', ')}].includes('{{cockpit/vehicle/type}}') && {{joystick/inputs/reverse}}) ? -1 : 1` +} + +/** + * Generates a JavaScript expression that evaluates to the pilot gain value when the + * vehicle type matches one of the given translation types, or 1 otherwise + * @param {string[]} vehicleTypes - Vehicle types for which this axis is a translation axis + * @returns {string} A data-lake expression string + */ +const getGainExpression = (vehicleTypes: readonly string[]): string => { + if (vehicleTypes.length === 0) return '1' + if (vehicleTypes.length === 1) { + return `('{{cockpit/vehicle/type}}' === '${vehicleTypes[0]}') ? {{joystick/inputs/gain}} : 1` + } + return `([${vehicleTypes + .map((t) => `'${t}'`) + .join(', ')}].includes('{{cockpit/vehicle/type}}')) ? {{joystick/inputs/gain}} : 1` +} + +/** + * Pre-built data lake variable actions for joystick axis inputs, used in joystick profile mappings + */ +export const joystickInputAxes: Record<(typeof joystickAxisConfig)[number]['key'], DataLakeVariableAction> = + Object.fromEntries( + joystickAxisConfig.map((axis) => [ + axis.key, + new DataLakeVariableAction({ + id: axisInputId(axis.key), + name: axisName(axis.key), + type: 'number' as DataLakeVariableType, + }), + ]) + ) as Record<(typeof joystickAxisConfig)[number]['key'], DataLakeVariableAction> + export const setupMavlinkCameraResources = (): void => { const commonVariableConfig = { type: 'number' as DataLakeVariableType, allowUserToChangeValue: true } // Initialize camera zoom variables @@ -126,6 +198,123 @@ export const setupMavlinkCameraResources = (): void => { } } +export const setupJoystickAxesResources = (): void => { + const commonVariableConfig = { type: 'number' as DataLakeVariableType, allowUserToChangeValue: true } + + for (const axis of joystickAxisConfig) { + const id = axisInputId(axis.key) + const name = axisName(axis.key) + const outputId = id.replace('/inputs/', '/outputs/') + const scaleId = `${outputId}-scale` + + createDataLakeVariable({ id, name, ...commonVariableConfig }, 0) + + try { + const existingScale = getAllTransformingFunctions().find((f) => f.id === scaleId) + if (!existingScale) { + createTransformingFunction( + scaleId, + `${name} Scale`, + 'number', + `(${getReverseExpression(axis.reverseVehicleTypes)}) * (${getGainExpression(axis.translationVehicleTypes)})`, + `Scale factor for ${name} combining reverse direction and pilot gain.` + ) + } + } catch (error) { + console.error(`Error creating scale transforming function for ${name}:`, error) + } + + try { + const existingOutput = getAllTransformingFunctions().find((f) => f.id === outputId) + if (!existingOutput) { + createTransformingFunction( + outputId, + `${name} Output`, + 'number', + `{{${scaleId}}} * {{${id}}}`, + `Output value for MANUAL_CONTROL ${name}.` + ) + } + } catch (error) { + console.error(`Error creating output transforming function for ${name}:`, error) + } + } +} + +const reverseVariableId = 'joystick/inputs/reverse' + +export const setupReverseResources = (): void => { + createDataLakeVariable( + { + id: reverseVariableId, + name: 'Reverse', + type: 'boolean' as DataLakeVariableType, + description: 'Trigger for user-specified reversing functionality', + allowUserToChangeValue: true, + }, + false + ) + + registerActionCallback(availableCockpitActions.start_reversing, () => { + setDataLakeVariableData(reverseVariableId, true) + }) + registerActionCallback(availableCockpitActions.stop_reversing, () => { + setDataLakeVariableData(reverseVariableId, false) + }) + registerActionCallback(availableCockpitActions.toggle_reversing, () => { + setDataLakeVariableData(reverseVariableId, !getDataLakeVariableData(reverseVariableId)) + }) +} + +const gainVariableId = 'joystick/inputs/gain' +const gainStepsVariableId = 'joystick/inputs/gain-steps' + +export const setupPilotGainResources = (): void => { + if (!getDataLakeVariableInfo(gainVariableId)) { + createDataLakeVariable( + { + id: gainVariableId, + name: 'Pilot Gain', + type: 'number' as DataLakeVariableType, + description: + 'Pilot gain multiplier applied to manual control axes (0 to 1). By default only applied to vehicle translation axes.', + allowUserToChangeValue: true, + persistent: true, + persistValue: true, + }, + 1 + ) + } + if (!getDataLakeVariableInfo(gainStepsVariableId)) { + createDataLakeVariable( + { + id: gainStepsVariableId, + name: 'Pilot Gain Steps', + type: 'number' as DataLakeVariableType, + description: 'Number of steps from minimum to maximum pilot gain', + allowUserToChangeValue: true, + persistent: true, + persistValue: true, + }, + 4 + ) + } + + registerActionCallback(availableCockpitActions.increase_pilot_gain, () => { + const gain = Number(getDataLakeVariableData(gainVariableId)) + const steps = Number(getDataLakeVariableData(gainStepsVariableId)) + setDataLakeVariableData(gainVariableId, Math.min(1, gain + 1 / steps)) + }) + registerActionCallback(availableCockpitActions.reduce_pilot_gain, () => { + const gain = Number(getDataLakeVariableData(gainVariableId)) + const steps = Number(getDataLakeVariableData(gainStepsVariableId)) + setDataLakeVariableData(gainVariableId, Math.max(0, gain - 1 / steps)) + }) +} + export const setupPredefinedLakeAndActionResources = (): void => { setupMavlinkCameraResources() + setupReverseResources() + setupPilotGainResources() + setupJoystickAxesResources() } diff --git a/src/stores/mainVehicle.ts b/src/stores/mainVehicle.ts index 3a2b708768..3fd48a361a 100644 --- a/src/stores/mainVehicle.ts +++ b/src/stores/mainVehicle.ts @@ -30,6 +30,7 @@ import { MavlinkManualControlManager } from '@/libs/joystick/protocols/mavlink-m import { canByPassCategory, EventCategory, slideToConfirm } from '@/libs/slide-to-confirm' import type { ArduPilot } from '@/libs/vehicle/ardupilot/ardupilot' import { CustomMode } from '@/libs/vehicle/ardupilot/ardurover' +import { getVehicleTypeFromMavType } from '@/libs/vehicle/ardupilot/common' import { defaultMessageIntervalsOptions } from '@/libs/vehicle/mavlink/defaults' import type { MAVLinkParameterSetData, MessageIntervalOptions } from '@/libs/vehicle/mavlink/types' import { MAVLINK_MESSAGE_INTERVALS_STORAGE_KEY } from '@/libs/vehicle/mavlink/vehicle' @@ -266,6 +267,33 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { }) } + if (!getDataLakeVariableInfo('cockpit/vehicle/type')) { + createDataLakeVariable({ + id: 'cockpit/vehicle/type', + name: 'Vehicle Type', + type: 'string', + description: 'The autopilot type of the vehicle currently being controlled (e.g. "copter", "sub").', + }) + } + + if (!getDataLakeVariableInfo('cockpit/vehicle/firmware_type')) { + createDataLakeVariable({ + id: 'cockpit/vehicle/firmware_type', + name: 'Firmware Type', + type: 'string', + description: 'The autopilot firmware family (e.g. "ArduPilot", "PX4", "Unknown").', + }) + } + + if (!getDataLakeVariableInfo('cockpit/vehicle/firmware_version')) { + createDataLakeVariable({ + id: 'cockpit/vehicle/firmware_version', + name: 'Firmware Version', + type: 'string', + description: 'The autopilot firmware version (major.minor.patch, e.g. "4.5.7").', + }) + } + // Update the variables with the new address setDataLakeVariableData(vehicleAddressVariableId, cleanedAddress) setDataLakeVariableData(vehicleMavlink2RestHttpEndpointVariableId, defaultMAVLink2RestHttpURI.value) @@ -643,14 +671,28 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { } const heartbeat = pack.message as Message.Heartbeat + const oldFirmwareType = firmwareType.value firmwareType.value = heartbeat.autopilot.type const oldVehicleType = vehicleType.value vehicleType.value = heartbeat.mavtype.type lastHeartbeat.value = new Date() + if (oldFirmwareType !== firmwareType.value && firmwareType.value !== undefined) { + const firmwareFamilyMap: Partial> = { + [MavAutopilot.MAV_AUTOPILOT_ARDUPILOTMEGA]: 'ArduPilot', + [MavAutopilot.MAV_AUTOPILOT_PX4]: 'PX4', + } + setDataLakeVariableData('cockpit/vehicle/firmware_type', firmwareFamilyMap[firmwareType.value] ?? 'Unknown') + } + if (oldVehicleType !== vehicleType.value && vehicleType.value !== undefined) { console.log('Vehicle type changed to', vehicleType.value) + const friendlyType = getVehicleTypeFromMavType(vehicleType.value) + if (friendlyType) { + setDataLakeVariableData('cockpit/vehicle/type', friendlyType) + } + try { controllerStore.loadDefaultProtocolMappingForVehicle(vehicleType.value) console.info(`Loaded default joystick protocol mapping for vehicle type ${vehicleType.value}.`) @@ -666,6 +708,13 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { } } }) + mainVehicle.value.onIncomingMAVLinkMessage.add(MAVLinkType.AUTOPILOT_VERSION, (pack: Package) => { + const version = (pack.message as Message.AutopilotVersion).flight_sw_version + const major = (version >> 24) & 0xff + const minor = (version >> 16) & 0xff + const patch = (version >> 8) & 0xff + setDataLakeVariableData('cockpit/vehicle/firmware_version', `${major}.${minor}.${patch}`) + }) // eslint-disable-next-line @typescript-eslint/no-explicit-any getAutoPilot(vehicles).onMode.add((vehicleMode: any) => { mode.value = [...(modes.value?.entries() ?? [])]