From 4533de85422c70779508a23cd7864ea03f44cff1 Mon Sep 17 00:00:00 2001 From: Arturo Manzoli Date: Thu, 7 May 2026 09:29:52 -0300 Subject: [PATCH 1/9] types: add geofence plan types Adds the runtime types backing the new geofencing flow: FenceLatLng, FencePolygon (with inclusion flag and vertex list), FenceCircle, BreachReturnPoint, and the Cockpit-level GeoFencePlan, plus the instanceOfGeoFencePlan guard that inspects only the outer discriminator + version so deeper validation stays at the call site that knows the schema. --- src/types/geofence.ts | 108 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 src/types/geofence.ts diff --git a/src/types/geofence.ts b/src/types/geofence.ts new file mode 100644 index 0000000000..9cbfed5671 --- /dev/null +++ b/src/types/geofence.ts @@ -0,0 +1,108 @@ +import type { WaypointCoordinates } from '@/types/mission' + +/** + * Geographical coordinates in the format [latitude, longitude]. + */ +export type FenceLatLng = WaypointCoordinates + +/** + * A polygon-shaped geofence. May be inclusion (vehicle must stay inside) + * or exclusion (vehicle must stay outside). + */ +export interface FencePolygon { + /** + * Unique identification for the polygon. + */ + id: string + /** + * If true, the vehicle is restricted to the inside of the polygon. + * If false, the vehicle is restricted to the outside of the polygon. + */ + inclusion: boolean + /** + * Ordered list of polygon vertices. Polygons are 2D — altitude limits are + * enforced via vehicle parameters (FENCE_ALT_MAX/MIN, GF_MAX_VER_DIST). + */ + vertices: FenceLatLng[] +} + +/** + * A circle-shaped geofence. May be inclusion (vehicle must stay inside) + * or exclusion (vehicle must stay outside). + */ +export interface FenceCircle { + /** + * Unique identification for the circle. + */ + id: string + /** + * If true, the vehicle is restricted to the inside of the circle. + * If false, the vehicle is restricted to the outside of the circle. + */ + inclusion: boolean + /** + * Geographical coordinates of the circle center. + */ + center: FenceLatLng + /** + * Radius of the circle in meters. + */ + radius: number +} + +/** + * Optional point the vehicle should return to when the geofence is breached. + * Stored separately from polygons/circles. On ArduPilot Plane this can be + * overridden by rally points when FENCE_RET_RALLY = 1. Not honored by PX4. + */ +export interface BreachReturnPoint { + /** + * Geographical coordinates of the breach return point. + */ + coordinates: FenceLatLng + /** + * Altitude (meters, relative to home) the vehicle should fly to. + */ + altitude: number +} + +/** + * Complete geofence plan as used at runtime in Cockpit. Schema mirrors the + * de-facto MAVLink-ecosystem `.plan` `geoFence` block (version 2) so files + * can round-trip with other ground stations. Polygons store vertices as + * `[lat, lon]`; circles store `{ center: [lat, lon], radius }`; + * `breachReturn` is the structured `BreachReturnPoint` (separate + * `coordinates` and `altitude`). + */ +export interface GeoFencePlan { + /** + * Schema version. Matches the `.plan` `geoFence.version` field. + */ + version: 2 + /** + * Inclusion and exclusion polygons. + */ + polygons: FencePolygon[] + /** + * Inclusion and exclusion circles. + */ + circles: FenceCircle[] + /** + * Optional breach return point. + */ + breachReturn?: BreachReturnPoint +} + +/** + * Validates that a parsed object conforms to the `GeoFencePlan` shape. + * @param { unknown } maybePlan The parsed JSON to inspect. + * @returns { boolean } True if the object is a valid `GeoFencePlan`. + */ +export const instanceOfGeoFencePlan = (maybePlan: unknown): maybePlan is GeoFencePlan => { + if (!maybePlan || typeof maybePlan !== 'object') return false + const p = maybePlan as Partial + if (p.version !== 2) return false + if (!Array.isArray(p.polygons)) return false + if (!Array.isArray(p.circles)) return false + return true +} From 2e9735df27bfb1ba1bd7d4c3695cea4014289757 Mon Sep 17 00:00:00 2001 From: Arturo Manzoli Date: Thu, 7 May 2026 09:31:42 -0300 Subject: [PATCH 2/9] lib: vehicle-mavlink: track autopilot capability bitmask On vehicle bring-up, send MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES and cache the bitmask received via the AUTOPILOT_VERSION message. Exposes a `capabilities()` accessor and a new `onCapabilities` signal so consumers can react when the bitmask first arrives. Failures are logged at warn level and gracefully fall back to "optimistic" capability reporting (consumers default to enabling features when no AUTOPILOT_VERSION has been seen yet). --- src/libs/vehicle/mavlink/vehicle.ts | 37 +++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/libs/vehicle/mavlink/vehicle.ts b/src/libs/vehicle/mavlink/vehicle.ts index c4a979ff85..956cbc94d4 100644 --- a/src/libs/vehicle/mavlink/vehicle.ts +++ b/src/libs/vehicle/mavlink/vehicle.ts @@ -86,10 +86,12 @@ export abstract class MAVLinkVehicle extends Vehicle.AbstractVehicle() + onCapabilities = new Signal() _flying = false shouldCreateDatalakeVariablesFromOtherSystems = false @@ -116,6 +118,15 @@ export abstract class MAVLinkVehicle extends Vehicle.AbstractVehicle { + console.warn( + 'Failed to request autopilot capabilities. Capability gating will fall back to optimistic mode.', + error + ) + }) + // Create data-lake variables for the vehicle this.createPredefinedDataLakeVariables() @@ -487,6 +498,14 @@ export abstract class MAVLinkVehicle extends Vehicle.AbstractVehicle extends Vehicle.AbstractVehicle } Resolves when the request command is acknowledged. + */ + async requestAutopilotCapabilities(): Promise { + await this.sendCommandLong(MavCmd.MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, 1) + } + + /** + * Get the cached autopilot capability bitmask from the last received + * `AUTOPILOT_VERSION` message. + * @returns { number | undefined } The capability bits, or `undefined` if not yet received. + */ + capabilities(): number | undefined { + return this._capabilities + } + /** * Request parameters list from vehicle * @param { MAVLinkParameterSetData } settings Data used to set a parameter From d76631389fc0a6ea90e7d593398d1f1affdbd1ca Mon Sep 17 00:00:00 2001 From: Arturo Manzoli Date: Thu, 7 May 2026 09:32:17 -0300 Subject: [PATCH 3/9] lib: vehicle-mavlink: add parameter request helpers Adds two helpers that turn the existing PARAM_REQUEST_READ / PARAM_VALUE round-trip into something callers can actually \`await\`: - \`requestParameter(name)\`: fires a single PARAM_REQUEST_READ for the given parameter id; the reply still bubbles up through \`onParameter\` for code that wants to keep listening passively. - \`requestParameterValue(name, timeoutMs)\`: subscribes a one-shot listener that resolves with the value as soon as the matching PARAM_VALUE arrives (or undefined on timeout). Both the listener and the timeout handle are cleared as soon as the promise settles to avoid stray callbacks. Signed-off-by: Arturo Manzoli --- src/libs/vehicle/mavlink/vehicle.ts | 54 +++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/src/libs/vehicle/mavlink/vehicle.ts b/src/libs/vehicle/mavlink/vehicle.ts index 956cbc94d4..10e9106174 100644 --- a/src/libs/vehicle/mavlink/vehicle.ts +++ b/src/libs/vehicle/mavlink/vehicle.ts @@ -872,6 +872,60 @@ export abstract class MAVLinkVehicle extends Vehicle.AbstractVehicle } The parameter value, or undefined on timeout. + */ + async requestParameterValue(paramName: string, timeoutMs = 1500): Promise { + return new Promise((resolve) => { + let resolved = false + // Slot lifetime is bounded by the timeout: either the matching PARAM_VALUE + // arrives and the slot removes itself, or `setTimeout` fires after + // `timeoutMs` and removes it. The promise always settles, so the slot is + // never left registered on `onParameter`. + const slot = ([param]: [Parameter, number | undefined]): void => { + if (resolved || param?.name !== paramName) return + resolved = true + this.onParameter.remove(slot) + clearTimeout(timeoutHandle) + resolve(param.value) + } + this.onParameter.add(slot) + this.requestParameter(paramName) + const timeoutHandle = setTimeout(() => { + if (resolved) return + resolved = true + this.onParameter.remove(slot) + resolve(undefined) + }, timeoutMs) + }) + } + /** * Request the vehicle to publish its `AUTOPILOT_VERSION` message so the * GCS can read the autopilot capability bitmask. From 00081637a3c5e2dff2d2c231fc8d470baf2dc0e9 Mon Sep 17 00:00:00 2001 From: Arturo Manzoli Date: Thu, 7 May 2026 09:32:54 -0300 Subject: [PATCH 4/9] lib: vehicle-mavlink: add fence upload, fetch and clear MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Wires geofence support into the existing MAVLink mission micro-service: - geofence-conversion.ts: pure helpers that translate between a Cockpit GeoFencePlan and a flat list of MissionItemInt[]. Polygons are emitted vertex-per-item with a shared param1=vertex_count; circles use param1 for the radius; the optional breach return point goes last as a MAV_FRAME_GLOBAL_RELATIVE_ALT item. The reverse path detects bad/incomplete polygon item streams from the autopilot and surfaces a descriptive error. - vehicle.ts: extracts _fetchMissionItems / _uploadMissionItems from the mission code so fence and mission paths share the handshake, then layers uploadFence / fetchFence / clearFence on top using MAV_MISSION_TYPE_FENCE. uploadFence also calls _ensureArduPilotPolygonFenceTypeBit to set the Polygon bit (4) of FENCE_TYPE on ArduPilot — without it the autopilot silently ignores the uploaded polygons/circles. Other FENCE_TYPE bits are preserved. Signed-off-by: Arturo Manzoli --- .../vehicle/mavlink/geofence-conversion.ts | 217 ++++++++++++++++++ src/libs/vehicle/mavlink/vehicle.ts | 153 +++++++++++- 2 files changed, 363 insertions(+), 7 deletions(-) create mode 100644 src/libs/vehicle/mavlink/geofence-conversion.ts diff --git a/src/libs/vehicle/mavlink/geofence-conversion.ts b/src/libs/vehicle/mavlink/geofence-conversion.ts new file mode 100644 index 0000000000..9e77743181 --- /dev/null +++ b/src/libs/vehicle/mavlink/geofence-conversion.ts @@ -0,0 +1,217 @@ +import { v4 as uuid } from 'uuid' + +import { MavCmd, MavFrame, MAVLinkType, MavMissionType } from '@/libs/connection/m2r/messages/mavlink2rest-enum' +import type { Message } from '@/libs/connection/m2r/messages/mavlink2rest-message' +import type { BreachReturnPoint, FenceCircle, FenceLatLng, FencePolygon, GeoFencePlan } from '@/types/geofence' + +const COORD_SCALE = 1e7 + +/** + * Builds a `MISSION_ITEM_INT` for a single fence vertex/circle/return point. + * @param { number } seq Sequence index of the item in the upload. + * @param { number } system_id Target system ID. + * @param { MavCmd } command MAV_CMD to use for this item. + * @param { MavFrame } frame Coordinate frame for this item. + * @param { number } param1 Item-specific parameter 1 (vertex count or radius). + * @param { number } lat Latitude in decimal degrees. + * @param { number } lon Longitude in decimal degrees. + * @param { number } alt Altitude in meters (used by breach return point only). + * @returns { Message.MissionItemInt } The encoded mission item. + */ +const buildFenceItem = ( + seq: number, + system_id: number, + command: MavCmd, + frame: MavFrame, + param1: number, + lat: number, + lon: number, + alt: number +): Message.MissionItemInt => ({ + target_system: system_id, + target_component: 1, + type: MAVLinkType.MISSION_ITEM_INT, + seq, + frame: { type: frame }, + command: { type: command }, + current: 0, + autocontinue: 1, + param1, + param2: 0, + param3: 0, + param4: 0, + x: Math.round(lat * COORD_SCALE), + y: Math.round(lon * COORD_SCALE), + z: alt, + mission_type: { type: MavMissionType.MAV_MISSION_TYPE_FENCE }, +}) + +/** + * Encodes a `GeoFencePlan` into a flat `MissionItemInt[]` ready to be uploaded + * via the MAVLink mission micro-service with `mission_type = MAV_MISSION_TYPE_FENCE`. + * + * Polygon vertices belonging to the same polygon are sent sequentially and + * carry the same `param1 = vertex_count`. The vehicle uses the change in + * `command` or `param1` to detect polygon boundaries. + * + * The optional breach return point is emitted last with + * `MAV_FRAME_GLOBAL_RELATIVE_ALT` so altitude is interpreted relative to home. + * @param { GeoFencePlan } plan The plan to encode. + * @param { number } system_id Target system ID for the items. + * @returns { Message.MissionItemInt[] } The flat sequence of mission items. + */ +export const convertGeoFencePlanToMavlink = (plan: GeoFencePlan, system_id: number): Message.MissionItemInt[] => { + const items: Message.MissionItemInt[] = [] + + plan.polygons.forEach((polygon) => { + if (polygon.vertices.length < 3) return + const command = polygon.inclusion + ? MavCmd.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION + : MavCmd.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION + const vertexCount = polygon.vertices.length + polygon.vertices.forEach(([lat, lon]) => { + items.push(buildFenceItem(items.length, system_id, command, MavFrame.MAV_FRAME_GLOBAL, vertexCount, lat, lon, 0)) + }) + }) + + plan.circles.forEach((circle) => { + const command = circle.inclusion + ? MavCmd.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION + : MavCmd.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION + const [lat, lon] = circle.center + items.push(buildFenceItem(items.length, system_id, command, MavFrame.MAV_FRAME_GLOBAL, circle.radius, lat, lon, 0)) + }) + + if (plan.breachReturn) { + const [lat, lon] = plan.breachReturn.coordinates + items.push( + buildFenceItem( + items.length, + system_id, + MavCmd.MAV_CMD_NAV_FENCE_RETURN_POINT, + MavFrame.MAV_FRAME_GLOBAL_RELATIVE_ALT, + 0, + lat, + lon, + plan.breachReturn.altitude + ) + ) + } + + return items +} + +const POLYGON_INCLUSION = 'MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION' +const POLYGON_EXCLUSION = 'MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION' +const CIRCLE_INCLUSION = 'MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION' +const CIRCLE_EXCLUSION = 'MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION' +const RETURN_POINT = 'MAV_CMD_NAV_FENCE_RETURN_POINT' + +const isPolygonCommand = (cmd: string): boolean => cmd === POLYGON_INCLUSION || cmd === POLYGON_EXCLUSION + +// `item.x` and `item.y` are 1e7 fixed-point degrees on the wire; dividing by +// `COORD_SCALE` recovers floating-point degrees. The division can introduce +// sub-centimeter rounding artifacts (binary doubles can't represent every +// 1e-7 step exactly), but the residual sits well below GPS noise, so the +// downstream renderer and re-encoder treat the round-trip as lossless. +const itemCoordinates = (item: Message.MissionItemInt): FenceLatLng => [item.x / COORD_SCALE, item.y / COORD_SCALE] + +/** + * Decodes a flat `MissionItemInt[]` (downloaded from the vehicle with + * `mission_type = MAV_MISSION_TYPE_FENCE`) back into a `GeoFencePlan`. + * + * Polygons are reassembled by accumulating consecutive items with the same + * polygon command until `param1` (vertex count) vertices have been collected. + * Mismatched commands or vertex counts mid-polygon throw a descriptive error + * so callers can flag corrupted/partial fence downloads. + * @param { Message.MissionItemInt[] } items Items downloaded from the vehicle. + * @returns { GeoFencePlan } The decoded geofence plan. + */ +export const convertMavlinkToGeoFencePlan = (items: Message.MissionItemInt[]): GeoFencePlan => { + const polygons: FencePolygon[] = [] + const circles: FenceCircle[] = [] + let breachReturn: BreachReturnPoint | undefined = undefined + + let pendingVertices: FenceLatLng[] = [] + let pendingCommand: string | undefined = undefined + let pendingExpectedCount = 0 + + const flushPendingPolygon = (): void => { + if (pendingVertices.length === 0) return + if (pendingCommand === undefined) return + polygons.push({ + id: uuid(), + inclusion: pendingCommand === POLYGON_INCLUSION, + vertices: pendingVertices, + }) + pendingVertices = [] + pendingCommand = undefined + pendingExpectedCount = 0 + } + + for (const item of items) { + const command = item.command.type as string + + if (isPolygonCommand(command)) { + const vertexCount = Math.round(item.param1) + if (pendingCommand === undefined) { + pendingCommand = command + pendingExpectedCount = vertexCount + pendingVertices = [itemCoordinates(item)] + } else if (pendingCommand !== command || pendingExpectedCount !== vertexCount) { + // Bad polygon item format — vertex count or command changed mid-polygon. + throw new Error( + `[Fence download] Bad polygon item format at seq ${item.seq}. Polygon vertices have inconsistent command or count.` + ) + } else { + pendingVertices.push(itemCoordinates(item)) + } + + if (pendingVertices.length === pendingExpectedCount) { + flushPendingPolygon() + } + continue + } + + if (pendingVertices.length > 0) { + // Incomplete polygon — non-vertex command appears before the polygon is closed. + throw new Error( + `[Fence download] Incomplete polygon at seq ${item.seq}. Expected ${pendingExpectedCount} vertices, got ${pendingVertices.length}.` + ) + } + + if (command === CIRCLE_INCLUSION || command === CIRCLE_EXCLUSION) { + circles.push({ + id: uuid(), + inclusion: command === CIRCLE_INCLUSION, + center: itemCoordinates(item), + radius: item.param1, + }) + continue + } + + if (command === RETURN_POINT) { + breachReturn = { + coordinates: itemCoordinates(item), + altitude: item.z, + } + continue + } + + throw new Error(`[Fence download] Unsupported command '${command}' at seq ${item.seq}.`) + } + + if (pendingVertices.length > 0) { + throw new Error( + `[Fence download] Incomplete polygon at end of items. Expected ${pendingExpectedCount} vertices, got ${pendingVertices.length}.` + ) + } + + return { version: 2, polygons, circles, breachReturn } +} + +/** + * Creates an empty geofence plan. + * @returns { GeoFencePlan } An empty plan with no polygons, circles, or breach return. + */ +export const emptyGeoFencePlan = (): GeoFencePlan => ({ version: 2, polygons: [], circles: [] }) diff --git a/src/libs/vehicle/mavlink/vehicle.ts b/src/libs/vehicle/mavlink/vehicle.ts index 10e9106174..29f10bb161 100644 --- a/src/libs/vehicle/mavlink/vehicle.ts +++ b/src/libs/vehicle/mavlink/vehicle.ts @@ -31,6 +31,11 @@ import { settingsManager } from '@/libs/settings-management' import { Signal, SignalTyped } from '@/libs/signal' import { degrees, frequencyHzToIntervalUs, isEqual, round, sleep } from '@/libs/utils' import { defaultMessageIntervalsOptions } from '@/libs/vehicle/mavlink/defaults' +import { + convertGeoFencePlanToMavlink, + convertMavlinkToGeoFencePlan, + emptyGeoFencePlan, +} from '@/libs/vehicle/mavlink/geofence-conversion' import { type MAVLinkParameterSetData, type MessageIntervalOptions, @@ -52,6 +57,7 @@ import { StatusText, Velocity, } from '@/libs/vehicle/types' +import type { GeoFencePlan } from '@/types/geofence' import { type MissionLoadingCallback, type Waypoint, defaultLoadingCallback } from '@/types/mission' import { flattenData } from '../common/data-flattener' @@ -1179,9 +1185,35 @@ export abstract class MAVLinkVehicle extends Vehicle.AbstractVehicle { - // Only deal with regular mission items for now - const missionType = MavMissionType.MAV_MISSION_TYPE_MISSION + const missionItems = await this._fetchMissionItems( + MavMissionType.MAV_MISSION_TYPE_MISSION, + loadingCallback, + timeoutBetweenItems + ) + this._currentMavlinkMissionItemsOnVehicle = missionItems + return convertMavlinkWaypointsToCockpit(missionItems) + } + /** + * Generic mission download. Implements the MAVLink mission micro-service + * download handshake (`MISSION_REQUEST_LIST` → `MISSION_COUNT` → + * `MISSION_REQUEST_INT` loop → `MISSION_ACK`) for any `MAV_MISSION_TYPE` + * (regular mission, geofence, rally points), returning the raw items. + * + * Reads `MISSION_COUNT` and `MISSION_ITEM_INT` from the shared `_messages` + * cache, which is not partitioned by `mission_type`. Concurrent downloads + * across mission types would consume each other's replies, so callers must + * serialize fetches against uploads/clears within a vehicle session. + * @param { MavMissionType } missionType Which mission micro-service to talk to. + * @param { MissionLoadingCallback } loadingCallback Callback that returns the state of the loading progress. + * @param { number } timeoutBetweenItems Timeout between mission items in milliseconds. + * @returns { Promise } Raw mission items downloaded from the vehicle. + */ + protected async _fetchMissionItems( + missionType: MavMissionType, + loadingCallback: MissionLoadingCallback = defaultLoadingCallback, + timeoutBetweenItems = 10000 + ): Promise { // Get number of mission items to be downloaded const initTimeCount = new Date().getTime() let timeoutEpoch = new Date().getTime() @@ -1281,8 +1313,7 @@ export abstract class MAVLinkVehicle extends Vehicle.AbstractVehicle extends Vehicle.AbstractVehicle { + const items = convertGeoFencePlanToMavlink(plan, this.currentSystemId) + await this._uploadMissionItems(MavMissionType.MAV_MISSION_TYPE_FENCE, items, loadingCallback) + await this._ensureArduPilotPolygonFenceTypeBit(plan) + } + + /** + * On ArduPilot, makes sure the `Polygon` bit (4) of the `FENCE_TYPE` bitmask + * is enabled whenever the uploaded plan contains polygon or circle items — + * the autopilot won't enforce uploaded fences otherwise. Other bits of + * `FENCE_TYPE` (AltMax, Circle radius, AltMin) are preserved untouched so + * users can still drive them from the parameters panel. + * @param { GeoFencePlan } plan The plan that was just uploaded. + */ + private async _ensureArduPilotPolygonFenceTypeBit(plan: GeoFencePlan): Promise { + if (this.firmware() !== Vehicle.Firmware.ArduPilot) return + if (plan.polygons.length === 0 && plan.circles.length === 0) return + const POLYGON_BIT = 4 + const current = await this.requestParameterValue('FENCE_TYPE') + const currentMask = current === undefined ? 0 : Math.round(current) + const desired = currentMask | POLYGON_BIT + if (desired === currentMask) return + this.setParameter({ + id: 'FENCE_TYPE', + value: desired, + // @ts-ignore: The correct type is indeed a Type + type: { type: MavParamType.MAV_PARAM_TYPE_UINT8 }, + }) + } + + /** + * Download the geofence plan currently stored on the vehicle. Returns an + * empty plan when the vehicle reports zero items. + * @param { MissionLoadingCallback } loadingCallback Callback that returns the state of the download progress. + * @returns { Promise } The decoded fence plan from the vehicle. + */ + async fetchFence(loadingCallback: MissionLoadingCallback = defaultLoadingCallback): Promise { + const items = await this._fetchMissionItems(MavMissionType.MAV_MISSION_TYPE_FENCE, loadingCallback) + if (items.length === 0) return emptyGeoFencePlan() + return convertMavlinkToGeoFencePlan(items) + } + + /** + * Clear the geofence currently stored on the vehicle and await the + * `MISSION_ACK` reply, mirroring the symmetric handshake of + * `uploadFence`/`fetchFence`. Throws if no acknowledgment arrives within + * `timeoutMs` or if the autopilot rejects the clear. + * @param { number } timeoutMs Timeout in milliseconds before giving up. + * @returns { Promise } Resolves once the autopilot acknowledges the clear. + */ + async clearFence(timeoutMs = 3000): Promise { + const initTime = new Date().getTime() + const message: Message.MissionClearAll = { + type: MAVLinkType.MISSION_CLEAR_ALL, + target_system: this.currentSystemId, + target_component: 1, + mission_type: { type: MavMissionType.MAV_MISSION_TYPE_FENCE }, + } + sendMavlinkMessage(message) + + while (new Date().getTime() - initTime < timeoutMs) { + await sleep(50) + const ack = this._messages.get(MAVLinkType.MISSION_ACK) + if (ack === undefined || ack.epoch < initTime) continue + if (ack.mavtype.type !== MavMissionResult.MAV_MISSION_ACCEPTED) { + throw Error(`Failed clearing fence. Result received: ${ack.mavtype.type}.`) + } + return + } + throw Error('Timeout waiting for fence clear acknowledgment.') + } + /** * Reset vehicle mode to LOITER/ALT_HOLD */ @@ -1472,10 +1589,32 @@ export abstract class MAVLinkVehicle extends Vehicle.AbstractVehicle { + console.debug(`[Mission upload] MAVLink waypoints: ${JSON.stringify(mavlinkWaypoints, null, 2)}`) // Say to the vehicle how many mission items we are going to send this.sendMissionCount(mavlinkWaypoints.length, missionType) From c4e12c8a4226a94cbc57cc880ea34314546f072a Mon Sep 17 00:00:00 2001 From: Arturo Manzoli Date: Thu, 7 May 2026 09:33:06 -0300 Subject: [PATCH 5/9] stores: main-vehicle: expose capabilities and fence helpers Surfaces the new MAVLink capability and fence APIs through the existing main-vehicle Pinia store so consumers don't need to reach into the underlying MAVLinkVehicle instance: - \`capabilities\` ref + \`onCapabilities\` subscription, kept in sync as AUTOPILOT_VERSION arrives. - \`uploadFence\` / \`fetchFence\` / \`clearFence\` proxy actions, mirroring the existing mission helpers (and emitting the same "Geofence deleted from vehicle" snackbar on clear). - \`requestParameter\` proxy so other stores (notably the upcoming geo-fence store) can pull individual parameter values without duplicating the MAVLink boilerplate. Signed-off-by: Arturo Manzoli --- src/stores/mainVehicle.ts | 54 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/src/stores/mainVehicle.ts b/src/stores/mainVehicle.ts index 6d0d38a8b4..f2b6e73071 100644 --- a/src/stores/mainVehicle.ts +++ b/src/stores/mainVehicle.ts @@ -49,6 +49,7 @@ import type { import { Coordinates } from '@/libs/vehicle/types' import * as Vehicle from '@/libs/vehicle/vehicle' import { VehicleFactory } from '@/libs/vehicle/vehicle-factory' +import type { GeoFencePlan } from '@/types/geofence' import type { MissionLoadingCallback, Waypoint } from '@/types/mission' import { useControllerStore } from './controller' @@ -143,6 +144,7 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { const vehiclePositionMaxSampleRate = useStorage('cockpit-vehicle-position-max-sampling-ms', 200) // Limits the frequency of vehicle position updates const reachedMissionItemSequences = ref([]) const currentMissionSeq = ref(undefined) + const capabilities = ref(undefined) const markMissionItemAsReached = (sequence: number): void => { if (reachedMissionItemSequences.value.includes(sequence)) return @@ -541,6 +543,47 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { openSnackbar({ message: 'Mission deleted from vehicle', variant: 'info' }) } + /** + * Upload a geofence plan to the vehicle. + * @param { GeoFencePlan } plan The plan to upload. + * @param { MissionLoadingCallback } loadingCallback Callback invoked with upload progress. + * @returns { Promise } Resolves when the vehicle acks the upload. + */ + async function uploadFence(plan: GeoFencePlan, loadingCallback: MissionLoadingCallback): Promise { + if (!mainVehicle.value) throw new Error('No vehicle available to upload fence.') + return await mainVehicle.value.uploadFence(plan, loadingCallback) + } + + /** + * Download the geofence plan currently stored on the vehicle. + * @param { MissionLoadingCallback } loadingCallback Callback invoked with download progress. + * @returns { Promise } The decoded geofence plan from the vehicle. + */ + async function fetchFence(loadingCallback: MissionLoadingCallback): Promise { + if (!mainVehicle.value) throw new Error('No vehicle available to fetch fence.') + return await mainVehicle.value.fetchFence(loadingCallback) + } + + /** + * Clear the geofence currently stored on the vehicle. + * @returns { Promise } Resolves once the vehicle acknowledges the clear. + */ + async function clearFence(): Promise { + if (!mainVehicle.value) throw new Error('No vehicle available to clear fence.') + await mainVehicle.value.clearFence() + openSnackbar({ message: 'Geofence deleted from vehicle', variant: 'info' }) + } + + /** + * Request a single onboard parameter from the vehicle by name. + * The reply arrives asynchronously via the standard `PARAM_VALUE` channel. + * @param { string } paramName Onboard parameter id (max 16 chars). + */ + function requestParameter(paramName: string): void { + if (!mainVehicle.value) return + mainVehicle.value.requestParameter(paramName) + } + /** * Start mission that is on the vehicle */ @@ -641,6 +684,12 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { currentMissionSeq.value = seq }) + // Track autopilot capability bitmask so feature gates (e.g. geofence) can react. + capabilities.value = mainVehicle.value.capabilities() + mainVehicle.value.onCapabilities.add((bits: number) => { + capabilities.value = bits + }) + mainVehicle.value.onAltitude.add((newAltitude: Altitude) => { Object.assign(altitude, newAltitude) }) @@ -1047,6 +1096,11 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { fetchMission, uploadMission, clearMissions, + uploadFence, + fetchFence, + clearFence, + requestParameter, + capabilities, startMission, pauseMission, returnHome, From a4364865a8600c4c42ca9a0accfe5591676b743d Mon Sep 17 00:00:00 2001 From: Arturo Manzoli Date: Thu, 2 Jul 2026 13:10:12 -0300 Subject: [PATCH 6/9] libs: geo-fence: add fence geometry and breach helpers Framework-agnostic geofence geometry extracted so it stays unit-testable and reusable outside the store: WGS84 metric offsetting for default shape sizing, turf-backed point-in-polygon and haversine distance, plan/vertex deep-clone helpers, and detectMissionBreaches, which flags waypoints that fall outside every inclusion shape or inside any exclusion shape. Also houses the shared placement constants (default/max polygon half-side and circle radius) consumed by both the store and the editor draft. --- src/libs/geo-fence.ts | 148 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 src/libs/geo-fence.ts diff --git a/src/libs/geo-fence.ts b/src/libs/geo-fence.ts new file mode 100644 index 0000000000..04491ef2cf --- /dev/null +++ b/src/libs/geo-fence.ts @@ -0,0 +1,148 @@ +import * as turf from '@turf/turf' + +import type { FenceCircle, FenceLatLng, FencePolygon, GeoFencePlan } from '@/types/geofence' + +export const POLYGON_DEFAULT_HALF_SIDE_M = 100 +export const POLYGON_MAX_HALF_SIDE_M = 1500 +export const CIRCLE_DEFAULT_RADIUS_M = 100 +export const CIRCLE_MAX_RADIUS_M = 1500 + +const EARTH_RADIUS_M = 6378137 + +/** + * Minimal shape required by `detectMissionBreaches`. Accepts both Cockpit's + * `Waypoint` and any caller-supplied object that exposes `[lat, lng]` + * coordinates, so the breach check stays decoupled from the mission types. + */ +export type WaypointLike = { + /** + * Geographic location of the waypoint as `[latitude, longitude]` in degrees. + */ + coordinates: [number, number] +} + +/** + * Result of a `detectMissionBreaches` call. + */ +export type MissionBreachReport = { + /** + * True when at least one waypoint breaches the active fence. + */ + hasBreaches: boolean + /** + * Indices (within the input waypoint list) of every breaching waypoint. + */ + breachedIndices: number[] + /** + * Total number of waypoints inspected. + */ + totalChecked: number + /** + * False when no fence plan was available to check against; callers can use + * this to differentiate "no breaches" from "no fence to check". + */ + hadPlan: boolean +} + +/** + * Deep-copies a vertex ring so mutations on the copy don't alias the source. + * @param { FenceLatLng[] } vertices Vertices to clone. + * @returns { FenceLatLng[] } A fresh array of fresh `[lat, lng]` tuples. + */ +export const cloneVertices = (vertices: FenceLatLng[]): FenceLatLng[] => vertices.map(([lat, lng]) => [lat, lng]) + +/** + * Deep-copies a geofence plan via JSON round-trip. + * @param { GeoFencePlan } plan Plan to clone. + * @returns { GeoFencePlan } A structurally independent copy. + */ +export const clonePlan = (plan: GeoFencePlan): GeoFencePlan => JSON.parse(JSON.stringify(plan)) as GeoFencePlan + +/** + * Computes the offset (in degrees) needed to move a point on the WGS84 + * spheroid by `dxMeters` east and `dyMeters` north. Used to seed default + * polygon and circle sizes around a given anchor. + * @param { FenceLatLng } anchor The anchor point `[lat, lng]`. + * @param { number } dxMeters Offset east in meters. + * @param { number } dyMeters Offset north in meters. + * @returns { FenceLatLng } The offset coordinates. + */ +export const offsetCoordinates = (anchor: FenceLatLng, dxMeters: number, dyMeters: number): FenceLatLng => { + const [lat, lng] = anchor + const dLat = (dyMeters / EARTH_RADIUS_M) * (180 / Math.PI) + const dLng = ((dxMeters / EARTH_RADIUS_M) * (180 / Math.PI)) / Math.cos((lat * Math.PI) / 180) + return [lat + dLat, lng + dLng] +} + +/** + * Tests whether a `[lat, lng]` point lies inside the polygon defined by the + * given vertex ring. Vertices are given in `[lat, lng]` order, the ring is + * implicitly closed, and the check uses turf's well-tested + * `booleanPointInPolygon` (ray casting). + * @param { FenceLatLng } point The `[lat, lng]` point to test. + * @param { FenceLatLng[] } vertices Polygon vertices `[lat, lng]`, open ring. + * @returns { boolean } True if the point is inside the polygon. + */ +export const isPointInsidePolygon = (point: FenceLatLng, vertices: FenceLatLng[]): boolean => { + if (vertices.length < 3) return false + const closed = [...vertices, vertices[0]] + const turfPoly = turf.polygon([closed.map(([lat, lng]) => [lng, lat])]) + const turfPoint = turf.point([point[1], point[0]]) + return turf.booleanPointInPolygon(turfPoint, turfPoly) +} + +/** + * Great-circle distance in meters between two `[lat, lng]` points using + * turf's haversine implementation. + * @param { FenceLatLng } a First `[lat, lng]` point. + * @param { FenceLatLng } b Second `[lat, lng]` point. + * @returns { number } Distance between the two points in meters. + */ +export const distanceMeters = (a: FenceLatLng, b: FenceLatLng): number => { + return turf.distance(turf.point([a[1], a[0]]), turf.point([b[1], b[0]]), { units: 'meters' }) +} + +/** + * Checks a list of waypoints against a set of fence shapes. A waypoint is + * flagged when it sits outside every inclusion shape (and at least one + * inclusion shape exists) or inside any exclusion shape. + * @param { WaypointLike[] } waypoints Waypoints to test. + * @param { FencePolygon[] } polygons Polygons to test against. + * @param { FenceCircle[] } circles Circles to test against. + * @returns { MissionBreachReport } Breach summary with offending indices. + */ +export const detectMissionBreaches = ( + waypoints: WaypointLike[], + polygons: FencePolygon[], + circles: FenceCircle[] +): MissionBreachReport => { + if (polygons.length === 0 && circles.length === 0) { + return { hasBreaches: false, breachedIndices: [], totalChecked: waypoints.length, hadPlan: false } + } + + const inclusionPolygons = polygons.filter((p) => p.inclusion) + const exclusionPolygons = polygons.filter((p) => !p.inclusion) + const inclusionCircles = circles.filter((c) => c.inclusion) + const exclusionCircles = circles.filter((c) => !c.inclusion) + const hasInclusion = inclusionPolygons.length > 0 || inclusionCircles.length > 0 + + const breachedIndices: number[] = [] + waypoints.forEach((wp, index) => { + const point: FenceLatLng = [wp.coordinates[0], wp.coordinates[1]] + const insideAnyInclusion = + !hasInclusion || + inclusionPolygons.some((p) => isPointInsidePolygon(point, p.vertices)) || + inclusionCircles.some((c) => distanceMeters(point, c.center) <= c.radius) + const insideAnyExclusion = + exclusionPolygons.some((p) => isPointInsidePolygon(point, p.vertices)) || + exclusionCircles.some((c) => distanceMeters(point, c.center) <= c.radius) + if (!insideAnyInclusion || insideAnyExclusion) breachedIndices.push(index) + }) + + return { + hasBreaches: breachedIndices.length > 0, + breachedIndices, + totalChecked: waypoints.length, + hadPlan: true, + } +} From 991b70f6856a22e6fffe8af787609ee7db376cc8 Mon Sep 17 00:00:00 2001 From: Arturo Manzoli Date: Thu, 2 Jul 2026 13:10:12 -0300 Subject: [PATCH 7/9] stores: add geo-fence store Pinia store that owns the persisted editor-side geofencing state and vehicle sync, delegating pure geometry to libs/geo-fence and transient draw/interaction state to the useGeoFenceEditorDraft composable: - Reactive polygons, circles and breach-return point with `cockpit-draft-fence` (in-progress) and `cockpit-vehicle-fence` (last uploaded) persistence so the user doesn't lose work across reloads. - Vehicle sync: `uploadToVehicle` (auto-enables FENCE_ENABLE and FENCE_AUTOENABLE on ArduPilot once the upload completes), `downloadFromVehicle`, `clearOnVehicle`, plus `setFenceEnabled` / `setFenceAutoEnable` that write the parameters through the vehicle. - `FENCE_ENABLE` polling watch (10s) tied to ArduPilot + a loaded plan so the Map widget toggle reflects autopilot-side flips (e.g. takeoff-time activation) the autopilot doesn't broadcast. - Capability gate via `isFenceSupported` (`MISSION_FENCE` bit) and a `detectMissionBreaches` wrapper that picks the editor plan (or the last uploaded plan as fallback) so the UI can warn before mission upload. --- src/composables/useGeoFenceEditorDraft.ts | 148 ++++++ src/stores/geoFence.ts | 599 ++++++++++++++++++++++ 2 files changed, 747 insertions(+) create mode 100644 src/composables/useGeoFenceEditorDraft.ts create mode 100644 src/stores/geoFence.ts diff --git a/src/composables/useGeoFenceEditorDraft.ts b/src/composables/useGeoFenceEditorDraft.ts new file mode 100644 index 0000000000..884403fef2 --- /dev/null +++ b/src/composables/useGeoFenceEditorDraft.ts @@ -0,0 +1,148 @@ +import { reactive } from 'vue' + +import { CIRCLE_MAX_RADIUS_M } from '@/libs/geo-fence' +import type { FenceLatLng } from '@/types/geofence' + +/** + * Transient, non-persisted editor interaction state for the geofence editor: + * which shape is currently selected and the in-progress polygon/circle draw + * buffers. Kept out of the `geo-fence` store (which owns the persisted plan + * and vehicle sync) because this is ephemeral UI state shared between the + * editor panel and the map layer, not app data. Exposed as a single module + * singleton so every consumer mutates and observes the same draft. + */ +export interface GeoFenceEditorDraft { + /** + * Id of the shape currently in edit mode, or `undefined` when none is selected. + */ + interactiveShapeId: string | undefined + /** + * True while the click-to-draw polygon flow is active. + */ + isDrawingPolygon: boolean + /** + * Whether the polygon being drawn is an inclusion fence. + */ + pendingPolygonInclusion: boolean + /** + * Vertices accumulated for the polygon currently being drawn. + */ + pendingPolygonVertices: FenceLatLng[] + /** + * True while the click-to-draw circle flow is active. + */ + isDrawingCircle: boolean + /** + * Whether the circle being drawn is an inclusion fence. + */ + pendingCircleInclusion: boolean + /** + * Center of the circle currently being drawn, or `undefined` before the first click. + */ + pendingCircleCenter: FenceLatLng | undefined + /** + * Live radius (meters) of the circle currently being drawn. + */ + pendingCircleRadius: number + /** + * Selects the shape with the given id as the interactive one. + * @param { string | undefined } id Shape id to select, or `undefined` to clear. + */ + setInteractive: (id: string | undefined) => void + /** + * Enters the click-to-draw polygon flow, clearing any pending vertices. + * @param { boolean } inclusion Whether the polygon being drawn is an inclusion fence. + */ + startDrawingPolygon: (inclusion: boolean) => void + /** + * Appends a vertex to the polygon currently being drawn. No-op outside drawing mode. + * @param { FenceLatLng } vertex The vertex to append, in `[lat, lng]`. + */ + addPendingPolygonVertex: (vertex: FenceLatLng) => void + /** + * Removes the last vertex appended to the polygon currently being drawn. + */ + popPendingPolygonVertex: () => void + /** + * Aborts the polygon currently being drawn, discarding pending vertices. + */ + cancelDrawingPolygon: () => void + /** + * Enters the click-to-draw circle flow, clearing any pending center/radius. + * @param { boolean } inclusion Whether the circle being drawn is an inclusion fence. + */ + startDrawingCircle: (inclusion: boolean) => void + /** + * Sets the center of the circle currently being drawn (first click). No-op outside drawing mode. + * @param { FenceLatLng } center The center coordinates, in `[lat, lng]`. + */ + setPendingCircleCenter: (center: FenceLatLng) => void + /** + * Updates the radius (meters) of the circle currently being drawn, clamped to `[1, CIRCLE_MAX_RADIUS_M]`. + * @param { number } radius The new radius in meters. + */ + setPendingCircleRadius: (radius: number) => void + /** + * Aborts the circle currently being drawn, discarding pending data. + */ + cancelDrawingCircle: () => void +} + +const draft = reactive({ + interactiveShapeId: undefined, + isDrawingPolygon: false, + pendingPolygonInclusion: true, + pendingPolygonVertices: [], + isDrawingCircle: false, + pendingCircleInclusion: true, + pendingCircleCenter: undefined, + pendingCircleRadius: 0, + setInteractive(id) { + draft.interactiveShapeId = id + }, + startDrawingPolygon(inclusion) { + draft.pendingPolygonInclusion = inclusion + draft.pendingPolygonVertices.splice(0) + draft.isDrawingPolygon = true + draft.interactiveShapeId = undefined + }, + addPendingPolygonVertex(vertex) { + if (!draft.isDrawingPolygon) return + draft.pendingPolygonVertices.push([vertex[0], vertex[1]]) + }, + popPendingPolygonVertex() { + if (!draft.isDrawingPolygon) return + draft.pendingPolygonVertices.pop() + }, + cancelDrawingPolygon() { + draft.pendingPolygonVertices.splice(0) + draft.isDrawingPolygon = false + }, + startDrawingCircle(inclusion) { + draft.pendingCircleInclusion = inclusion + draft.pendingCircleCenter = undefined + draft.pendingCircleRadius = 0 + draft.isDrawingCircle = true + draft.interactiveShapeId = undefined + }, + setPendingCircleCenter(center) { + if (!draft.isDrawingCircle) return + draft.pendingCircleCenter = [center[0], center[1]] + draft.pendingCircleRadius = 0 + }, + setPendingCircleRadius(radius) { + if (!draft.isDrawingCircle || !draft.pendingCircleCenter) return + draft.pendingCircleRadius = Math.max(1, Math.min(CIRCLE_MAX_RADIUS_M, radius)) + }, + cancelDrawingCircle() { + draft.pendingCircleCenter = undefined + draft.pendingCircleRadius = 0 + draft.isDrawingCircle = false + }, +}) + +/** + * Access the shared geofence editor draft (interaction + in-progress draw state). + * @returns { GeoFenceEditorDraft } The module-singleton draft state and its mutators. + */ +export const useGeoFenceEditorDraft = (): GeoFenceEditorDraft => draft diff --git a/src/stores/geoFence.ts b/src/stores/geoFence.ts new file mode 100644 index 0000000000..635f75ed06 --- /dev/null +++ b/src/stores/geoFence.ts @@ -0,0 +1,599 @@ +import { tryOnScopeDispose, useDebounceFn, useDocumentVisibility } from '@vueuse/core' +import { defineStore } from 'pinia' +import { v4 as uuid } from 'uuid' +import { computed, reactive, ref, watch } from 'vue' + +import { useBlueOsStorage } from '@/composables/settingsSyncer' +import { useSnackbar } from '@/composables/snackbar' +import { useGeoFenceEditorDraft } from '@/composables/useGeoFenceEditorDraft' +import { MavParamType } from '@/libs/connection/m2r/messages/mavlink2rest-enum' +import { + type MissionBreachReport, + type WaypointLike, + CIRCLE_DEFAULT_RADIUS_M, + CIRCLE_MAX_RADIUS_M, + clonePlan, + cloneVertices, + detectMissionBreaches as detectMissionBreachesInShapes, + offsetCoordinates, + POLYGON_DEFAULT_HALF_SIDE_M, + POLYGON_MAX_HALF_SIDE_M, +} from '@/libs/geo-fence' +import type { Parameter } from '@/libs/vehicle/types' +import * as Vehicle from '@/libs/vehicle/vehicle' +import type { BreachReturnPoint, FenceCircle, FenceLatLng, FencePolygon, GeoFencePlan } from '@/types/geofence' +import type { MissionLoadingCallback } from '@/types/mission' + +import { useMainVehicleStore } from './mainVehicle' + +export const useGeoFenceStore = defineStore('geo-fence', () => { + const mainVehicleStore = useMainVehicleStore() + const { openSnackbar } = useSnackbar() + const draft = useGeoFenceEditorDraft() + + const polygons = reactive([]) + const circles = reactive([]) + const breachReturn = ref(undefined) + + const dirty = ref(false) + const syncInProgress = ref(false) + const lastUploadedPlan = ref(undefined) + + const draftFence = useBlueOsStorage('cockpit-draft-fence', { + version: 2, + polygons: [], + circles: [], + }) + const persistedVehicleFence = useBlueOsStorage('cockpit-vehicle-fence', null) + + // Restore previously persisted last-uploaded plan so the live overlay can + // appear immediately after a page reload without re-downloading. + if (persistedVehicleFence.value) { + lastUploadedPlan.value = clonePlan(persistedVehicleFence.value) + } + + // Restore the user's draft into the reactive working model. + if (draftFence.value) { + polygons.push(...(draftFence.value.polygons ?? [])) + circles.push(...(draftFence.value.circles ?? [])) + breachReturn.value = draftFence.value.breachReturn + } + + const markDirty = (): void => { + dirty.value = true + } + + const persistDraft = (): void => { + draftFence.value = exportPlan() + } + + // Debounced variant for high-frequency callers (vertex drag updates fire on + // every mousemove). Atomic actions stay on the immediate `persistDraft` so + // discrete edits (add/delete/finish-drawing/clear/load/setBreachReturn) + // are flushed to storage right away. + const persistDraftDebounced = useDebounceFn(persistDraft, 300) + + // MAV_PROTOCOL_CAPABILITY_MISSION_FENCE — bit 14 of AUTOPILOT_VERSION.capabilities. + // The dialect generator doesn't expose this enum yet (the field is typed as a + // generic BitFlag), so we mirror the single value we need. + const MAV_PROTOCOL_CAPABILITY_MISSION_FENCE = 1 << 14 + + /** + * Whether the connected vehicle advertises the geofence MAVLink service. + * Optimistic by default: if no `AUTOPILOT_VERSION` has been received yet, + * the editor remains enabled. + * @returns { boolean } True when the editor should be enabled for the current vehicle. + */ + const isFenceSupported = computed(() => { + const bits = mainVehicleStore.capabilities + if (bits === undefined) return true + return (bits & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) !== 0 + }) + + /** + * True when the connected vehicle is an ArduPilot Plane. ArduPilot Plane + * requires a polygon to be present for any fence parameter (incl. ALT_MAX) + * to take effect — surfaced as a UI hint. + * @returns { boolean } True for ArduPilot Plane vehicles. + */ + const isArduPilotPlane = computed(() => { + const v = mainVehicleStore.mainVehicle + if (!v) return false + return v.firmware() === Vehicle.Firmware.ArduPilot && v.type() === Vehicle.Type.Plane + }) + + /** + * True when the connected vehicle is PX4. PX4 ANDs multiple inclusion + * polygons (intersection) — surfaced as a UI hint. + * @returns { boolean } True for PX4 vehicles. + */ + const isPx4 = computed(() => { + const v = mainVehicleStore.mainVehicle + if (!v) return false + return v.firmware() === Vehicle.Firmware.PX4 + }) + + /** + * True when the connected vehicle runs ArduPilot firmware. Used to gate + * features that exist only on ArduPilot (e.g. the `FENCE_ENABLE` runtime + * enforcement toggle, which has no PX4 equivalent). + * @returns { boolean } True for any ArduPilot vehicle. + */ + const isArduPilot = computed(() => { + const v = mainVehicleStore.mainVehicle + if (!v) return false + return v.firmware() === Vehicle.Firmware.ArduPilot + }) + + // Tracks the vehicle-side fence enforcement state (ArduPilot's `FENCE_ENABLE`). + // `undefined` until the first PARAM_VALUE reply lands, so the UI can render a + // neutral state instead of falsely claiming the fence is disabled. + const fenceEnabled = ref(undefined) + + // Tracks the current `FENCE_AUTOENABLE` mode (0–4 on ArduPilot). Drives the + // "Auto enable fence on takeoff" switch in the editor; non-zero is treated + // as ON because every supported mode auto-enables enforcement at some + // arming/takeoff point. + const fenceAutoEnableMode = ref(undefined) + + /** + * Asks the vehicle for the current `FENCE_ENABLE` value. The reply is + * received asynchronously via PARAM_VALUE and updates `fenceEnabled`. + * No-op on non-ArduPilot vehicles or when no vehicle is connected. + */ + const refreshFenceEnabled = (): void => { + const v = mainVehicleStore.mainVehicle + if (!v) return + if (!isArduPilot.value) return + v.requestParameter('FENCE_ENABLE') + } + + /** + * Asks the vehicle for the current `FENCE_AUTOENABLE` value. The reply + * arrives asynchronously via PARAM_VALUE and updates `fenceAutoEnableMode`. + * No-op on non-ArduPilot vehicles or when no vehicle is connected. + */ + const refreshFenceAutoEnable = (): void => { + const v = mainVehicleStore.mainVehicle + if (!v) return + if (!isArduPilot.value) return + v.requestParameter('FENCE_AUTOENABLE') + } + + /** + * Toggles vehicle-side geofence enforcement by writing the `FENCE_ENABLE` + * parameter on ArduPilot. Performs an optimistic local update; the real + * value is reconfirmed when the resulting PARAM_VALUE reply arrives. + * @param { boolean } enabled Whether enforcement should be active on the vehicle. + */ + const setFenceEnabled = (enabled: boolean): void => { + const v = mainVehicleStore.mainVehicle + if (!v) throw new Error('No vehicle connected.') + if (!isArduPilot.value) { + throw new Error('Fence enforcement toggle is only supported on ArduPilot vehicles.') + } + v.setParameter({ + id: 'FENCE_ENABLE', + value: enabled ? 1 : 0, + type: { type: MavParamType.MAV_PARAM_TYPE_UINT8 }, + }) + fenceEnabled.value = enabled + } + + /** + * Writes `FENCE_AUTOENABLE` on ArduPilot, controlling whether the autopilot + * automatically arms fence enforcement on auto-takeoff. Mode `1` is the + * common "enable on takeoff, disable on landing" preset; `0` disables it. + * @param { number } mode `FENCE_AUTOENABLE` mode (vendor-defined; typically 0–4). + */ + const setFenceAutoEnable = (mode: number): void => { + const v = mainVehicleStore.mainVehicle + if (!v) throw new Error('No vehicle connected.') + if (!isArduPilot.value) { + throw new Error('FENCE_AUTOENABLE is only supported on ArduPilot vehicles.') + } + v.setParameter({ + id: 'FENCE_AUTOENABLE', + value: mode, + type: { type: MavParamType.MAV_PARAM_TYPE_UINT8 }, + }) + fenceAutoEnableMode.value = mode + } + + // Wire PARAM_VALUE updates from the vehicle into `fenceEnabled` / + // `fenceAutoEnableMode` so that any change — whether triggered by us, by + // another GCS, or auto-applied by the autopilot itself (e.g. + // `FENCE_AUTOENABLE` on takeoff) — is reflected in the UI. Re-binds + // whenever the active vehicle instance changes. + let attachedVehicle: Vehicle.Abstract | undefined + const fenceParamSlot = ([param]: [Parameter, number | undefined]): void => { + if (!param) return + if (param.name === 'FENCE_ENABLE') { + fenceEnabled.value = Math.round(param.value) === 1 + } else if (param.name === 'FENCE_AUTOENABLE') { + fenceAutoEnableMode.value = Math.round(param.value) + } + } + watch( + () => mainVehicleStore.mainVehicle, + (newVehicle) => { + if (attachedVehicle) attachedVehicle.onParameter.remove(fenceParamSlot) + attachedVehicle = newVehicle + if (newVehicle) newVehicle.onParameter.add(fenceParamSlot) + }, + { immediate: true } + ) + + // The autopilot does not emit unsolicited PARAM_VALUE messages when its own + // logic flips `FENCE_ENABLE` (for example, on `FENCE_AUTOENABLE`-driven + // takeoff-time activation). To keep the Map widget toggle in sync with the + // real enforcement state we poll `FENCE_ENABLE` while a fence is loaded on + // the vehicle. `FENCE_AUTOENABLE` itself never changes autopilot-side, so + // we only need to refresh it on the rising edge. Polling pauses whenever + // the tab is hidden so we don't generate idle MAVLink traffic in the + // background. The interval is intentionally lazy — enforcement state rarely + // changes mid-flight, so a 10s cadence is enough to keep the UI honest + // without flooding the link. + const FENCE_ENABLE_POLL_INTERVAL_MS = 10000 + let fenceEnablePollHandle: ReturnType | undefined + const visibility = useDocumentVisibility() + watch( + [isArduPilot, lastUploadedPlan, () => mainVehicleStore.isVehicleOnline, visibility], + ([isAP, plan, online, vis]) => { + if (fenceEnablePollHandle !== undefined) { + clearInterval(fenceEnablePollHandle) + fenceEnablePollHandle = undefined + } + const baselineActive = Boolean(isAP && plan && online) + if (!baselineActive) { + fenceEnabled.value = undefined + fenceAutoEnableMode.value = undefined + return + } + if (vis !== 'visible') return + refreshFenceEnabled() + refreshFenceAutoEnable() + fenceEnablePollHandle = setInterval(refreshFenceEnabled, FENCE_ENABLE_POLL_INTERVAL_MS) + }, + { immediate: true } + ) + + // Pinia stores are app-singletons in production, so this never fires in the + // running app. Kept for tests and any future caller that explicitly + // `$dispose()`s the store, so the polling timer and parameter listener + // don't outlive the store's effect scope. + tryOnScopeDispose(() => { + if (fenceEnablePollHandle !== undefined) { + clearInterval(fenceEnablePollHandle) + fenceEnablePollHandle = undefined + } + if (attachedVehicle) { + attachedVehicle.onParameter.remove(fenceParamSlot) + attachedVehicle = undefined + } + }) + + const clearAll = (): void => { + polygons.splice(0) + circles.splice(0) + breachReturn.value = undefined + draft.setInteractive(undefined) + markDirty() + persistDraft() + } + + const addPolygon = (anchor: FenceLatLng, inclusion: boolean): FencePolygon => { + const halfSide = POLYGON_DEFAULT_HALF_SIDE_M + const vertices: FenceLatLng[] = [ + offsetCoordinates(anchor, -halfSide, halfSide), + offsetCoordinates(anchor, halfSide, halfSide), + offsetCoordinates(anchor, halfSide, -halfSide), + offsetCoordinates(anchor, -halfSide, -halfSide), + ] + const polygon: FencePolygon = { id: uuid(), inclusion, vertices } + polygons.push(polygon) + draft.setInteractive(polygon.id) + markDirty() + persistDraft() + return polygon + } + + const addCircle = (anchor: FenceLatLng, inclusion: boolean): FenceCircle => { + const circle: FenceCircle = { + id: uuid(), + inclusion, + center: [anchor[0], anchor[1]], + radius: CIRCLE_DEFAULT_RADIUS_M, + } + circles.push(circle) + draft.setInteractive(circle.id) + markDirty() + persistDraft() + return circle + } + + const updatePolygon = (id: string, update: Partial>): void => { + const polygon = polygons.find((p) => p.id === id) + if (!polygon) return + if (update.inclusion !== undefined) polygon.inclusion = update.inclusion + if (update.vertices !== undefined) polygon.vertices = cloneVertices(update.vertices) + markDirty() + persistDraftDebounced() + } + + const updateCircle = (id: string, update: Partial>): void => { + const circle = circles.find((c) => c.id === id) + if (!circle) return + if (update.inclusion !== undefined) circle.inclusion = update.inclusion + if (update.center !== undefined) circle.center = [update.center[0], update.center[1]] + if (update.radius !== undefined) circle.radius = Math.max(1, Math.min(CIRCLE_MAX_RADIUS_M, update.radius)) + markDirty() + persistDraftDebounced() + } + + const togglePolygonInclusion = (id: string): void => { + const polygon = polygons.find((p) => p.id === id) + if (!polygon) return + updatePolygon(id, { inclusion: !polygon.inclusion }) + } + + const toggleCircleInclusion = (id: string): void => { + const circle = circles.find((c) => c.id === id) + if (!circle) return + updateCircle(id, { inclusion: !circle.inclusion }) + } + + const deletePolygon = (id: string): void => { + const idx = polygons.findIndex((p) => p.id === id) + if (idx === -1) return + polygons.splice(idx, 1) + if (draft.interactiveShapeId === id) draft.setInteractive(undefined) + markDirty() + persistDraft() + } + + const deleteCircle = (id: string): void => { + const idx = circles.findIndex((c) => c.id === id) + if (idx === -1) return + circles.splice(idx, 1) + if (draft.interactiveShapeId === id) draft.setInteractive(undefined) + markDirty() + persistDraft() + } + + /** + * Commits the polygon currently being drawn into the persistent fence + * model. Requires at least 3 vertices, otherwise it cancels silently. + * @returns { FencePolygon | undefined } The committed polygon, or `undefined` if not enough vertices. + */ + const finishDrawingPolygon = (): FencePolygon | undefined => { + if (!draft.isDrawingPolygon || draft.pendingPolygonVertices.length < 3) { + draft.cancelDrawingPolygon() + return undefined + } + const polygon: FencePolygon = { + id: uuid(), + inclusion: draft.pendingPolygonInclusion, + vertices: cloneVertices(draft.pendingPolygonVertices), + } + polygons.push(polygon) + draft.cancelDrawingPolygon() + draft.setInteractive(polygon.id) + markDirty() + persistDraft() + return polygon + } + + /** + * Commits the circle currently being drawn into the persistent fence + * model. Requires a center and a radius >= 1m, otherwise it cancels + * silently. + * @returns { FenceCircle | undefined } The committed circle, or `undefined` if not enough data. + */ + const finishDrawingCircle = (): FenceCircle | undefined => { + if (!draft.isDrawingCircle || !draft.pendingCircleCenter || draft.pendingCircleRadius < 1) { + draft.cancelDrawingCircle() + return undefined + } + const circle: FenceCircle = { + id: uuid(), + inclusion: draft.pendingCircleInclusion, + center: [draft.pendingCircleCenter[0], draft.pendingCircleCenter[1]], + radius: draft.pendingCircleRadius, + } + circles.push(circle) + draft.cancelDrawingCircle() + draft.setInteractive(circle.id) + markDirty() + persistDraft() + return circle + } + + const setBreachReturn = (point: BreachReturnPoint | undefined): void => { + breachReturn.value = point ? { coordinates: [...point.coordinates], altitude: point.altitude } : undefined + markDirty() + persistDraft() + } + + /** + * Loads a complete fence plan into the editor, replacing the current draft. + * @param { GeoFencePlan } plan The plan to load. + */ + const loadFromPlan = (plan: GeoFencePlan): void => { + polygons.splice( + 0, + polygons.length, + ...plan.polygons.map((p) => ({ + id: p.id ?? uuid(), + inclusion: p.inclusion, + vertices: cloneVertices(p.vertices), + })) + ) + circles.splice( + 0, + circles.length, + ...plan.circles.map((c) => ({ + id: c.id ?? uuid(), + inclusion: c.inclusion, + center: [c.center[0], c.center[1]] as FenceLatLng, + radius: c.radius, + })) + ) + breachReturn.value = plan.breachReturn + ? { coordinates: [...plan.breachReturn.coordinates], altitude: plan.breachReturn.altitude } + : undefined + draft.setInteractive(undefined) + markDirty() + persistDraft() + } + + /** + * Snapshots the current editor state into a `GeoFencePlan` (e.g. for upload + * or file export). Always returns a fresh deep copy. + * @returns { GeoFencePlan } The current plan. + */ + const exportPlan = (): GeoFencePlan => ({ + version: 2, + polygons: polygons.map((p) => ({ id: p.id, inclusion: p.inclusion, vertices: cloneVertices(p.vertices) })), + circles: circles.map((c) => ({ + id: c.id, + inclusion: c.inclusion, + center: [c.center[0], c.center[1]], + radius: c.radius, + })), + breachReturn: breachReturn.value + ? { coordinates: [...breachReturn.value.coordinates], altitude: breachReturn.value.altitude } + : undefined, + }) + + const inclusionPolygonCount = computed(() => polygons.filter((p) => p.inclusion).length) + const hasItems = computed( + () => polygons.length > 0 || circles.length > 0 || breachReturn.value !== undefined + ) + + /** + * Checks the given list of waypoints against the current editor fence + * plan, with the uploaded plan as a fallback when the editor is empty. + * @param { WaypointLike[] } waypoints Waypoints to test. + * @returns { MissionBreachReport } Breach summary with offending indices. + */ + const detectMissionBreaches = (waypoints: WaypointLike[]): MissionBreachReport => { + // Read straight from the reactive editor state when it has shapes, + // otherwise fall back to the (already plain) `lastUploadedPlan`. Avoids + // a deep copy of every fence shape on every upload attempt. + const editorHasFences = polygons.length > 0 || circles.length > 0 + const sourcePolygons: FencePolygon[] = editorHasFences ? polygons : lastUploadedPlan.value?.polygons ?? [] + const sourceCircles: FenceCircle[] = editorHasFences ? circles : lastUploadedPlan.value?.circles ?? [] + return detectMissionBreachesInShapes(waypoints, sourcePolygons, sourceCircles) + } + + /** + * Uploads the current editor state to the vehicle. Caches the uploaded + * plan in `lastUploadedPlan` (also persisted to BlueOS) so the live + * overlay on the flight Map widget can render it without re-downloading. + * @param { MissionLoadingCallback } loadingCallback Callback invoked with progress. + */ + const uploadToVehicle = async (loadingCallback?: MissionLoadingCallback): Promise => { + syncInProgress.value = true + try { + const plan = exportPlan() + await mainVehicleStore.uploadFence(plan, loadingCallback ?? (async () => undefined)) + lastUploadedPlan.value = clonePlan(plan) + persistedVehicleFence.value = clonePlan(plan) + dirty.value = false + + // Auto-enable enforcement on ArduPilot so users don't need a second click + // after upload, and arm `FENCE_AUTOENABLE` so subsequent auto-takeoffs + // re-engage the fence even if it was manually disabled in between. + // Failures are non-fatal: the user can still flip the dedicated toggle + // from the Map widget and tweak the parameter from the parameters panel. + if (isArduPilot.value) { + try { + setFenceEnabled(true) + setFenceAutoEnable(1) + } catch (error) { + console.warn('Auto-enable of vehicle geofence failed:', error) + } + } + + openSnackbar({ variant: 'success', message: 'Geofence uploaded to vehicle.', duration: 3000 }) + } finally { + syncInProgress.value = false + } + } + + /** + * Downloads the current geofence from the vehicle, replaces the editor + * state with it, and updates the cached `lastUploadedPlan`. + * @param { MissionLoadingCallback } loadingCallback Callback invoked with progress. + */ + const downloadFromVehicle = async (loadingCallback?: MissionLoadingCallback): Promise => { + syncInProgress.value = true + try { + const plan = await mainVehicleStore.fetchFence(loadingCallback ?? (async () => undefined)) + loadFromPlan(plan) + lastUploadedPlan.value = clonePlan(plan) + persistedVehicleFence.value = clonePlan(plan) + dirty.value = false + openSnackbar({ variant: 'success', message: 'Geofence downloaded from vehicle.', duration: 3000 }) + } finally { + syncInProgress.value = false + } + } + + /** + * Clears the geofence currently stored on the vehicle, and clears the + * cached overlay so the live overlay control disappears. + */ + const clearOnVehicle = async (): Promise => { + syncInProgress.value = true + try { + await mainVehicleStore.clearFence() + lastUploadedPlan.value = undefined + persistedVehicleFence.value = null + } finally { + syncInProgress.value = false + } + } + + return { + polygons, + circles, + breachReturn, + dirty, + syncInProgress, + lastUploadedPlan, + isFenceSupported, + isArduPilotPlane, + isPx4, + isArduPilot, + fenceEnabled, + fenceAutoEnableMode, + setFenceEnabled, + setFenceAutoEnable, + refreshFenceEnabled, + refreshFenceAutoEnable, + inclusionPolygonCount, + hasItems, + detectMissionBreaches, + addPolygon, + addCircle, + updatePolygon, + updateCircle, + togglePolygonInclusion, + toggleCircleInclusion, + deletePolygon, + deleteCircle, + finishDrawingPolygon, + finishDrawingCircle, + setBreachReturn, + clearAll, + loadFromPlan, + exportPlan, + uploadToVehicle, + downloadFromVehicle, + clearOnVehicle, + polygonDefaults: { defaultHalfSideMeters: POLYGON_DEFAULT_HALF_SIDE_M, maxHalfSideMeters: POLYGON_MAX_HALF_SIDE_M }, + circleDefaults: { defaultRadiusMeters: CIRCLE_DEFAULT_RADIUS_M, maxRadiusMeters: CIRCLE_MAX_RADIUS_M }, + } +}) From a457afa171f1a3eebc7940a6e75440ca39d3fe58 Mon Sep 17 00:00:00 2001 From: Arturo Manzoli Date: Thu, 2 Jul 2026 16:31:55 -0300 Subject: [PATCH 8/9] =?UTF-8?q?review:=20round=201=20=E2=80=94=20guard=20F?= =?UTF-8?q?ENCE=5FTYPE=20read=20before=20setting=20polygon=20bit?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bail from _ensureArduPilotPolygonFenceTypeBit when the FENCE_TYPE read returns undefined (timeout) instead of assuming 0 and writing bit 4 alone, which would wipe the user's AltMax/AltMin/Circle bits. Logically belongs folded into "lib: vehicle-mavlink: add fence upload, fetch and clear" (00081637a); kept separate because folding it would rewrite the stacked base and force a rebuild of the #2808/#2809 [drop] chains — do that explicitly, not autonomously. --- src/libs/vehicle/mavlink/vehicle.ts | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/libs/vehicle/mavlink/vehicle.ts b/src/libs/vehicle/mavlink/vehicle.ts index 29f10bb161..c08d9659ec 100644 --- a/src/libs/vehicle/mavlink/vehicle.ts +++ b/src/libs/vehicle/mavlink/vehicle.ts @@ -1424,7 +1424,10 @@ export abstract class MAVLinkVehicle extends Vehicle.AbstractVehicle Date: Fri, 3 Jul 2026 14:00:19 -0300 Subject: [PATCH 9/9] =?UTF-8?q?review:=20round=201=20=E2=80=94=20centraliz?= =?UTF-8?q?e=20empty=20geofence=20plan=20factory?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Move emptyGeoFencePlan into libs/geo-fence and reuse it for the cockpit-draft-fence default and the MAVLink conversion re-export so the empty-plan literal is defined once. Addresses nit 10.3 from the local all-threshold review; logically folds into the geo-fence libs commit on a stack reshape. --- src/libs/geo-fence.ts | 6 ++++++ src/libs/vehicle/mavlink/geofence-conversion.ts | 8 ++------ src/stores/geoFence.ts | 7 ++----- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/libs/geo-fence.ts b/src/libs/geo-fence.ts index 04491ef2cf..ab19c06a7e 100644 --- a/src/libs/geo-fence.ts +++ b/src/libs/geo-fence.ts @@ -58,6 +58,12 @@ export const cloneVertices = (vertices: FenceLatLng[]): FenceLatLng[] => vertice */ export const clonePlan = (plan: GeoFencePlan): GeoFencePlan => JSON.parse(JSON.stringify(plan)) as GeoFencePlan +/** + * Creates an empty geofence plan. + * @returns { GeoFencePlan } An empty plan with no polygons, circles, or breach return. + */ +export const emptyGeoFencePlan = (): GeoFencePlan => ({ version: 2, polygons: [], circles: [] }) + /** * Computes the offset (in degrees) needed to move a point on the WGS84 * spheroid by `dxMeters` east and `dyMeters` north. Used to seed default diff --git a/src/libs/vehicle/mavlink/geofence-conversion.ts b/src/libs/vehicle/mavlink/geofence-conversion.ts index 9e77743181..8742c8cac5 100644 --- a/src/libs/vehicle/mavlink/geofence-conversion.ts +++ b/src/libs/vehicle/mavlink/geofence-conversion.ts @@ -4,6 +4,8 @@ import { MavCmd, MavFrame, MAVLinkType, MavMissionType } from '@/libs/connection import type { Message } from '@/libs/connection/m2r/messages/mavlink2rest-message' import type { BreachReturnPoint, FenceCircle, FenceLatLng, FencePolygon, GeoFencePlan } from '@/types/geofence' +export { emptyGeoFencePlan } from '@/libs/geo-fence' + const COORD_SCALE = 1e7 /** @@ -209,9 +211,3 @@ export const convertMavlinkToGeoFencePlan = (items: Message.MissionItemInt[]): G return { version: 2, polygons, circles, breachReturn } } - -/** - * Creates an empty geofence plan. - * @returns { GeoFencePlan } An empty plan with no polygons, circles, or breach return. - */ -export const emptyGeoFencePlan = (): GeoFencePlan => ({ version: 2, polygons: [], circles: [] }) diff --git a/src/stores/geoFence.ts b/src/stores/geoFence.ts index 635f75ed06..7a480ecbc0 100644 --- a/src/stores/geoFence.ts +++ b/src/stores/geoFence.ts @@ -15,6 +15,7 @@ import { clonePlan, cloneVertices, detectMissionBreaches as detectMissionBreachesInShapes, + emptyGeoFencePlan, offsetCoordinates, POLYGON_DEFAULT_HALF_SIDE_M, POLYGON_MAX_HALF_SIDE_M, @@ -39,11 +40,7 @@ export const useGeoFenceStore = defineStore('geo-fence', () => { const syncInProgress = ref(false) const lastUploadedPlan = ref(undefined) - const draftFence = useBlueOsStorage('cockpit-draft-fence', { - version: 2, - polygons: [], - circles: [], - }) + const draftFence = useBlueOsStorage('cockpit-draft-fence', emptyGeoFencePlan()) const persistedVehicleFence = useBlueOsStorage('cockpit-vehicle-fence', null) // Restore previously persisted last-uploaded plan so the live overlay can