From a4ea5bc0ef05ea74df7799f7e713ec54fd850fb2 Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Sat, 2 Aug 2025 16:27:31 +0100 Subject: [PATCH 1/4] Add exporting waypoints to file functionality --- gcs/electron/main.ts | 12 +++- gcs/electron/preload.js | 44 +++++++------ gcs/src/missions.jsx | 39 +++++++++++- radio/app/controllers/missionController.py | 73 ++++++++++++++++++++-- radio/app/endpoints/mission.py | 56 ++++++++++++++++- 5 files changed, 194 insertions(+), 30 deletions(-) diff --git a/gcs/electron/main.ts b/gcs/electron/main.ts index 762cb3b81..977a43354 100644 --- a/gcs/electron/main.ts +++ b/gcs/electron/main.ts @@ -5,7 +5,7 @@ import path from 'node:path' import packageInfo from '../package.json' // @ts-expect-error - no types available -import openFile, { getRecentFiles, clearRecentFiles } from './fla' +import openFile, { clearRecentFiles, getRecentFiles } from './fla' // The built directory structure // // ├─┬─┬ dist @@ -448,6 +448,16 @@ app.whenReady().then(() => { // Clear recent logs ipcMain.handle('fla:clear-recent-logs', clearRecentFiles) + // Save mission file + ipcMain.handle('missions:get-save-mission-file-path', async (event, options) => { + const window = BrowserWindow.fromWebContents(event.sender); + if (!window) { + throw new Error('No active window found') + } + const result = await dialog.showSaveDialog(window, options); + return result; + }) + ipcMain.handle('app:get-node-env', () => app.isPackaged ? 'production' : 'development', ) diff --git a/gcs/electron/preload.js b/gcs/electron/preload.js index c52ab7d36..e7149140f 100644 --- a/gcs/electron/preload.js +++ b/gcs/electron/preload.js @@ -1,18 +1,22 @@ -import { contextBridge, ipcRenderer } from 'electron' +import { contextBridge, ipcRenderer } from "electron" // --------- Expose some API to the Renderer process --------- -contextBridge.exposeInMainWorld('ipcRenderer', { +contextBridge.exposeInMainWorld("ipcRenderer", { ...withPrototype(ipcRenderer), - loadFile: (data) => ipcRenderer.invoke('fla:open-file', data), - getRecentLogs: () => ipcRenderer.invoke('fla:get-recent-logs'), - clearRecentLogs: () => ipcRenderer.invoke('fla:clear-recent-logs'), - getNodeEnv: () => ipcRenderer.invoke('app:get-node-env'), - getVersion: () => ipcRenderer.invoke('app:get-version'), - getSettings: () => ipcRenderer.invoke('getSettings'), - saveSettings: (settings) => ipcRenderer.invoke('setSettings', settings), - openWebcamWindow: (id, name, aspect) => ipcRenderer.invoke("openWebcamWindow", id, name, aspect), - closeWebcamWindow: () => ipcRenderer.invoke('closeWebcamWindow'), - onCameraWindowClose: (callback) => ipcRenderer.on("webcam-closed", () => callback()) + loadFile: (data) => ipcRenderer.invoke("fla:open-file", data), + getRecentLogs: () => ipcRenderer.invoke("fla:get-recent-logs"), + clearRecentLogs: () => ipcRenderer.invoke("fla:clear-recent-logs"), + getSaveMissionFilePath: (options) => + ipcRenderer.invoke("missions:get-save-mission-file-path", options), + getNodeEnv: () => ipcRenderer.invoke("app:get-node-env"), + getVersion: () => ipcRenderer.invoke("app:get-version"), + getSettings: () => ipcRenderer.invoke("getSettings"), + saveSettings: (settings) => ipcRenderer.invoke("setSettings", settings), + openWebcamWindow: (id, name, aspect) => + ipcRenderer.invoke("openWebcamWindow", id, name, aspect), + closeWebcamWindow: () => ipcRenderer.invoke("closeWebcamWindow"), + onCameraWindowClose: (callback) => + ipcRenderer.on("webcam-closed", () => callback()), }) // `exposeInMainWorld` can't detect attributes and methods of `prototype`, manually patching it. @@ -22,7 +26,7 @@ function withPrototype(obj) { for (const [key, value] of Object.entries(protos)) { if (Object.prototype.hasOwnProperty.call(obj, key)) continue - if (typeof value === 'function') { + if (typeof value === "function") { // Some native APIs, like `NodeJS.EventEmitter['on']`, don't work in the Renderer process. Wrapping them into a function. obj[key] = function (...args) { return value.call(obj, ...args) @@ -35,12 +39,12 @@ function withPrototype(obj) { } // --------- Preload scripts loading --------- -function domReady(condition = ['complete', 'interactive']) { +function domReady(condition = ["complete", "interactive"]) { return new Promise((resolve) => { if (condition.includes(document.readyState)) { resolve(true) } else { - document.addEventListener('readystatechange', () => { + document.addEventListener("readystatechange", () => { if (condition.includes(document.readyState)) { resolve(true) } @@ -97,12 +101,12 @@ function useLoading() { z-index: 9; } ` - const oStyle = document.createElement('style') - const oDiv = document.createElement('div') + const oStyle = document.createElement("style") + const oDiv = document.createElement("div") - oStyle.id = 'app-loading-style' + oStyle.id = "app-loading-style" oStyle.innerHTML = styleContent - oDiv.className = 'app-loading-wrap' + oDiv.className = "app-loading-wrap" oDiv.innerHTML = `
` return { @@ -124,7 +128,7 @@ const { appendLoading, removeLoading } = useLoading() domReady().then(appendLoading) window.onmessage = (ev) => { - ev.data.payload === 'removeLoading' && removeLoading() + ev.data.payload === "removeLoading" && removeLoading() } setTimeout(removeLoading, 4999) diff --git a/gcs/src/missions.jsx b/gcs/src/missions.jsx index 89284af7a..187f2321b 100644 --- a/gcs/src/missions.jsx +++ b/gcs/src/missions.jsx @@ -180,6 +180,14 @@ export default function Missions() { } }) + socket.on("export_mission_result", (data) => { + if (data.success) { + showSuccessNotification(data.message) + } else { + showErrorNotification(data.message) + } + }) + return () => { socket.off("incoming_msg") socket.off("home_position_result") @@ -187,6 +195,7 @@ export default function Missions() { socket.off("current_mission") socket.off("write_mission_result") socket.off("import_mission_result") + socket.off("export_mission_result") } }, [connected]) @@ -333,8 +342,34 @@ export default function Missions() { importFileResetRef.current?.() } - function saveMissionToFile() { - return + async function saveMissionToFile() { + // The options for the save dialog + const options = { + title: "Save the mission to a file", + filters: [ + { name: "Waypoint Files", extensions: ["waypoints"] }, + { name: "All Files", extensions: ["*"] }, + ], + } + + const result = await window.ipcRenderer.getSaveMissionFilePath(options) + + if (!result.canceled) { + let items = [] + if (activeTab === "mission") { + items = missionItems + } else if (activeTab === "fence") { + items = fenceItems + } else if (activeTab === "rally") { + items = rallyItems + } + + socket.emit("export_mission_to_file", { + type: activeTab, + file_path: result.filePath, + items: items, + }) + } } return ( diff --git a/radio/app/controllers/missionController.py b/radio/app/controllers/missionController.py index 724b7e600..09cc7c78b 100644 --- a/radio/app/controllers/missionController.py +++ b/radio/app/controllers/missionController.py @@ -28,6 +28,9 @@ def __init__(self, drone: Drone) -> None: self.drone = drone + # Loaders are only used to manage the mission items that are currently loaded in the drone. + # Importing and exporting mission items to/from files do not use loaders as these waypoints + # are not then loaded into the drone's mission items. self.missionLoader = mavwp.MAVWPLoader( target_system=drone.target_system, target_component=drone.target_component ) @@ -464,6 +467,9 @@ def _parseWaypointsListIntoLoader( ) raise ValueError(f"Invalid waypoint type {type(wp)} in waypoints list") + self.drone.logger.debug( + f"Parsed {loader.count()} waypoints into loader for mission type {mission_type}" + ) return loader def uploadMission(self, mission_type: int, waypoints: List[dict]) -> Response: @@ -570,7 +576,7 @@ def uploadMission(self, mission_type: int, waypoints: List[dict]) -> Response: def importMissionFromFile(self, mission_type: int, file_path: str) -> Response: """ - Imports a mission from a file into the drone's mission loader, return the waypoints loaded. + Imports a mission from a file, return the waypoints loaded. Args: mission_type (int): The type of mission to import. 0=Mission,1=Fence,2=Rally. @@ -592,11 +598,20 @@ def importMissionFromFile(self, mission_type: int, file_path: str) -> Response: ) if mission_type == TYPE_MISSION: - loader = self.missionLoader + loader = mavwp.MAVWPLoader( + target_system=self.drone.target_system, + target_component=self.drone.target_component, + ) elif mission_type == TYPE_FENCE: - loader = self.fenceLoader + loader = mavwp.MissionItemProtocol_Fence( + target_system=self.drone.target_system, + target_component=self.drone.target_component, + ) else: - loader = self.rallyLoader + loader = mavwp.MissionItemProtocol_Rally( + target_system=self.drone.target_system, + target_component=self.drone.target_component, + ) try: loader.load(file_path) @@ -635,3 +650,53 @@ def importMissionFromFile(self, mission_type: int, file_path: str) -> Response: "message": f"Waypoint file loaded {loader.count()} points successfully", "data": [wp.to_dict() for wp in loader.wpoints], } + + def exportMissionToFile( + self, mission_type: int, file_path: str, waypoints: List[dict] + ) -> Response: + """ + Exports a mission to a file from a given list of waypoints. + + Args: + mission_type (int): The type of mission to import. 0=Mission,1=Fence,2=Rally. + file_path (str): The path to the waypoint file to import. + """ + mission_type_check = self._checkMissionType(mission_type) + if not mission_type_check.get("success"): + return mission_type_check + + loader = self._parseWaypointsListIntoLoader(waypoints, mission_type) + + for wp in loader.wpoints: + if hasattr(wp, "x") and hasattr(wp, "y"): + if isinstance(wp.x, int): + wp.x = wp.x / 1e7 + if isinstance(wp.y, int): + wp.y = wp.y / 1e7 + + if loader.count() == 0: + return { + "success": False, + "message": f"No waypoints loaded for the mission type of {mission_type}", + } + + self.drone.logger.debug( + f"Exporting waypoint file to {file_path} for mission type {mission_type}" + ) + + try: + loader.save(file_path) + except Exception as e: + self.drone.logger.error(f"Failed to save waypoint file: {e}") + return { + "success": False, + "message": f"Failed to save waypoint file: {e}", + } + + self.drone.logger.info( + f"Saved waypoint file with {loader.count()} points successfully to {file_path}" + ) + return { + "success": True, + "message": f"Waypoint file saved {loader.count()} points successfully to {file_path}", + } diff --git a/radio/app/endpoints/mission.py b/radio/app/endpoints/mission.py index ef02d2300..d5ef0f284 100644 --- a/radio/app/endpoints/mission.py +++ b/radio/app/endpoints/mission.py @@ -14,11 +14,17 @@ class WriteCurrentMissionType(TypedDict): items: list[dict] -class ImportMissionFromFileType(TypedDict): +class ImportMissionFileType(TypedDict): type: str file_path: str +class ExportMissionFileType(TypedDict): + type: str + file_path: str + items: list[dict] + + class ControlMissionType(TypedDict): action: str @@ -146,7 +152,7 @@ def writeCurrentMission(data: WriteCurrentMissionType) -> None: @socketio.on("import_mission_from_file") -def importMissionFromFile(data: ImportMissionFromFileType) -> None: +def importMissionFromFile(data: ImportMissionFileType) -> None: if droneStatus.state != "missions": socketio.emit( "params_error", @@ -165,7 +171,7 @@ def importMissionFromFile(data: ImportMissionFromFileType) -> None: if mission_type not in mission_type_array: socketio.emit( - "write_mission_result", + "import_mission_result", { "success": False, "message": f"Invalid mission type. Must be 'mission', 'fence', or 'rally', got {mission_type}.", @@ -197,6 +203,50 @@ def importMissionFromFile(data: ImportMissionFromFileType) -> None: ) +@socketio.on("export_mission_to_file") +def exportMissionToFile(data: ExportMissionFileType) -> None: + if droneStatus.state != "missions": + socketio.emit( + "params_error", + { + "message": "You must be on the missions screen to export a mission to a file." + }, + ) + logger.debug(f"Current state: {droneStatus.state}") + return + + if not droneStatus.drone: + return notConnectedError(action="export mission from file") + + mission_type = data.get("type") + mission_type_array = ["mission", "fence", "rally"] + + if mission_type not in mission_type_array: + socketio.emit( + "export_mission_result", + { + "success": False, + "message": f"Invalid mission type. Must be 'mission', 'fence', or 'rally', got {mission_type}.", + }, + ) + logger.error( + f"Invalid mission type: {mission_type}. Must be 'mission', 'fence', or 'rally'." + ) + return + + file_path = data.get("file_path", "") + items = data.get("items", []) + + result = droneStatus.drone.missionController.exportMissionToFile( + mission_type_array.index(mission_type), file_path, items + ) + + if not result.get("success"): + logger.error(result.get("message")) + + socketio.emit("export_mission_result", result) + + @socketio.on("control_mission") def controlMission(data: ControlMissionType) -> None: """ From d14253d42b68b99cf57978cf1cbaa8edc21ec463 Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Sat, 2 Aug 2025 16:30:42 +0100 Subject: [PATCH 2/4] Address copilot review comments --- radio/app/controllers/missionController.py | 5 +++-- radio/app/endpoints/mission.py | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/radio/app/controllers/missionController.py b/radio/app/controllers/missionController.py index 09cc7c78b..637cfe59d 100644 --- a/radio/app/controllers/missionController.py +++ b/radio/app/controllers/missionController.py @@ -658,8 +658,9 @@ def exportMissionToFile( Exports a mission to a file from a given list of waypoints. Args: - mission_type (int): The type of mission to import. 0=Mission,1=Fence,2=Rally. - file_path (str): The path to the waypoint file to import. + mission_type (int): The type of mission to export. 0=Mission,1=Fence,2=Rally. + file_path (str): The path to the waypoint file to export. + waypoints (List[dict]): The list of waypoints to upload. Each waypoint should be a dict with the required fields. """ mission_type_check = self._checkMissionType(mission_type) if not mission_type_check.get("success"): diff --git a/radio/app/endpoints/mission.py b/radio/app/endpoints/mission.py index d5ef0f284..a54b87f95 100644 --- a/radio/app/endpoints/mission.py +++ b/radio/app/endpoints/mission.py @@ -216,7 +216,7 @@ def exportMissionToFile(data: ExportMissionFileType) -> None: return if not droneStatus.drone: - return notConnectedError(action="export mission from file") + return notConnectedError(action="export mission to file") mission_type = data.get("type") mission_type_array = ["mission", "fence", "rally"] From eec86de797fa42ea3ef4f79bb6efd22fa177e9fc Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Sat, 2 Aug 2025 17:34:16 +0100 Subject: [PATCH 3/4] Fix bug with home position not updating --- gcs/src/missions.jsx | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/gcs/src/missions.jsx b/gcs/src/missions.jsx index 187f2321b..3338bc592 100644 --- a/gcs/src/missions.jsx +++ b/gcs/src/missions.jsx @@ -135,6 +135,7 @@ export default function Missions() { for (let missionItem of data.items) { missionItemsWithIds.push(addIdToItem(missionItem)) } + updateHomePositionBasedOnWaypoints(missionItemsWithIds) setMissionItems(missionItemsWithIds) } else if (data.mission_type === "fence") { setFenceItems(data.items) @@ -164,6 +165,9 @@ export default function Missions() { for (let missionItem of data.items) { missionItemsWithIds.push(addIdToItem(missionItem)) } + + updateHomePositionBasedOnWaypoints(missionItemsWithIds) + setMissionItems(missionItemsWithIds) } else if (data.mission_type === "fence") { setFenceItems(data.items) @@ -205,6 +209,29 @@ export default function Missions() { } }, [importFile]) + function updateHomePositionBasedOnWaypoints(waypoints) { + if (waypoints.length > 0) { + const potentialHomeLocation = waypoints[0] + if ( + potentialHomeLocation.frame === + parseInt( + Object.keys(MAV_FRAME_LIST).find( + (key) => MAV_FRAME_LIST[key] === "MAV_FRAME_GLOBAL", + ), + ) && + potentialHomeLocation.x !== 0 && + potentialHomeLocation.y !== 0 && + potentialHomeLocation.command === 16 + ) { + setHomePosition({ + lat: potentialHomeLocation.x, + lon: potentialHomeLocation.y, + alt: potentialHomeLocation.z, + }) + } + } + } + function getFlightMode() { if (aircraftType === 1) { return PLANE_MODES_FLIGHT_MODE_MAP[heartbeatData.custom_mode] From 1bb0d55b0141e8521888df2623ceacb197e2959d Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Sat, 2 Aug 2025 17:43:48 +0100 Subject: [PATCH 4/4] Address copilot review comments --- gcs/src/missions.jsx | 26 +++++++++++++--------- radio/app/controllers/missionController.py | 26 ++++++++++++++-------- 2 files changed, 32 insertions(+), 20 deletions(-) diff --git a/gcs/src/missions.jsx b/gcs/src/missions.jsx index 3338bc592..573478131 100644 --- a/gcs/src/missions.jsx +++ b/gcs/src/missions.jsx @@ -209,20 +209,24 @@ export default function Missions() { } }, [importFile]) + function isGlobalFrameHomeCommand(waypoint) { + const globalFrameValue = parseInt( + Object.keys(MAV_FRAME_LIST).find( + (key) => MAV_FRAME_LIST[key] === "MAV_FRAME_GLOBAL", + ), + ) + return ( + waypoint.frame === globalFrameValue && + waypoint.x !== 0 && + waypoint.y !== 0 && + waypoint.command === 16 + ) + } + function updateHomePositionBasedOnWaypoints(waypoints) { if (waypoints.length > 0) { const potentialHomeLocation = waypoints[0] - if ( - potentialHomeLocation.frame === - parseInt( - Object.keys(MAV_FRAME_LIST).find( - (key) => MAV_FRAME_LIST[key] === "MAV_FRAME_GLOBAL", - ), - ) && - potentialHomeLocation.x !== 0 && - potentialHomeLocation.y !== 0 && - potentialHomeLocation.command === 16 - ) { + if (isGlobalFrameHomeCommand(potentialHomeLocation)) { setHomePosition({ lat: potentialHomeLocation.x, lon: potentialHomeLocation.y, diff --git a/radio/app/controllers/missionController.py b/radio/app/controllers/missionController.py index 637cfe59d..4061bfb74 100644 --- a/radio/app/controllers/missionController.py +++ b/radio/app/controllers/missionController.py @@ -4,7 +4,7 @@ from typing import TYPE_CHECKING, Any, List import serial -from app.customTypes import Response +from app.customTypes import Number, Response from app.utils import commandAccepted from pymavlink import mavutil, mavwp @@ -49,6 +49,18 @@ def _checkMissionType(self, mission_type: int) -> Response: } return {"success": True} + def _convertCoordinate(self, coordinate) -> Number: + gps_coordinate_scale = 1e7 + + if isinstance(coordinate, float): + return int(coordinate * gps_coordinate_scale) + elif isinstance(coordinate, int): + return coordinate / gps_coordinate_scale + + raise ValueError( + f"Invalid coordinate type {type(coordinate)}. Must be int or float." + ) + def getCurrentMission(self, mission_type: int) -> Response: """ Get the current mission of a specific type from the drone. @@ -637,10 +649,8 @@ def importMissionFromFile(self, mission_type: int, file_path: str) -> Response: for wp in loader.wpoints: if hasattr(wp, "x") and hasattr(wp, "y"): - if isinstance(wp.x, float): - wp.x = int(wp.x * 1e7) - if isinstance(wp.y, float): - wp.y = int(wp.y * 1e7) + wp.x = self._convertCoordinate(wp.x) + wp.y = self._convertCoordinate(wp.y) self.drone.logger.info( f"Loaded waypoint file with {loader.count()} points successfully" @@ -670,10 +680,8 @@ def exportMissionToFile( for wp in loader.wpoints: if hasattr(wp, "x") and hasattr(wp, "y"): - if isinstance(wp.x, int): - wp.x = wp.x / 1e7 - if isinstance(wp.y, int): - wp.y = wp.y / 1e7 + wp.x = self._convertCoordinate(wp.x) + wp.y = self._convertCoordinate(wp.y) if loader.count() == 0: return {