Alpha 0.1.10/569 write missions to drone (#589)

1Blademaster · Jopat2409 · 1Blademaster · commit 0ae1b84879f5 · 2025-09-28T21:46:09.000+01:00
* alpha-0.1.9/hf/583-fix-camera-not-working-on-electron Fixed electron build camera issues (#584) * Add mission write functionality * Add change to waypoints parsing function to allow for rally and fence types * Add tests for missions * Add improvements from copilot review --------- Co-authored-by: Joe <joantpat@gmail.com>
diff --git a/.vscode/launch.json b/.vscode/launch.json
@@ -0,0 +1,11 @@
+{
+  "configurations": [
+    {
+      "name": "Python Debugger: Backend",
+      "type": "debugpy",
+      "request": "launch",
+      "program": "${workspaceFolder}/radio/app.py",
+      "console": "integratedTerminal"
+    }
+  ]
+}
diff --git a/gcs/src/missions.jsx b/gcs/src/missions.jsx
@@ -135,10 +135,19 @@ export default function Missions() {
       showSuccessNotification(`${data.mission_type} read successfully`)
     })
 
+    socket.on("write_mission_result", (data) => {
+      if (data.success) {
+        showSuccessNotification(data.message)
+      } else {
+        showErrorNotification(data.message)
+      }
+    })
+
     return () => {
       socket.off("incoming_msg")
       socket.off("home_position_result")
       socket.off("current_mission")
+      socket.off("write_mission_result")
     }
   }, [connected])
 
@@ -183,7 +192,16 @@ export default function Missions() {
   }
 
   function writeMissionToDrone() {
-    return
+    if (activeTab === "mission") {
+      socket.emit("write_current_mission", {
+        type: "mission",
+        items: missionItems,
+      })
+    } else if (activeTab === "fence") {
+      socket.emit("write_current_mission", { type: "fence", items: fenceItems })
+    } else if (activeTab === "rally") {
+      socket.emit("write_current_mission", { type: "rally", items: rallyItems })
+    }
   }
 
   function importMissionFromFile() {
diff --git a/radio/app/controllers/missionController.py b/radio/app/controllers/missionController.py
@@ -5,7 +5,7 @@
 
 import serial
 from app.customTypes import Response
-from app.utils import commandAccepted, wpToMissionItemInt
+from app.utils import commandAccepted
 from pymavlink import mavutil, mavwp
 
 if TYPE_CHECKING:
@@ -238,7 +238,7 @@ def getItemDetails(
         if not mission_type_check.get("success"):
             return mission_type_check
 
-        failure_message = f"Failed to get mission item {item_number}/{mission_count} for mission type {mission_type}"
+        failure_message = f"Failed to get mission item {item_number}/{mission_count - 1} for mission type {mission_type}"
 
         self.drone.is_listening = False
 
@@ -260,7 +260,7 @@ def getItemDetails(
 
             if response:
                 self.drone.logger.debug(
-                    f"Got response for mission item {item_number}/{mission_count} for mission type {mission_type}"
+                    f"Got response for mission item {item_number}/{mission_count - 1} for mission type {mission_type}"
                 )
                 return {
                     "success": True,
@@ -269,7 +269,7 @@ def getItemDetails(
 
             else:
                 self.drone.logger.error(
-                    f"Got no response for mission item {item_number}/{mission_count} for mission type {mission_type}"
+                    f"Got no response for mission item {item_number}/{mission_count - 1} for mission type {mission_type}"
                 )
                 return {
                     "success": False,
@@ -279,7 +279,7 @@ def getItemDetails(
         except serial.serialutil.SerialException:
             self.drone.is_listening = True
             self.drone.logger.error(
-                f"Got no response for mission item {item_number}/{mission_count}, serial exception"
+                f"Got no response for mission item {item_number}/{mission_count - 1}, serial exception"
             )
             return {
                 "success": False,
@@ -454,9 +454,63 @@ def loadWaypointFile(self, file_path: str, mission_type: int) -> Response:
             "message": f"Waypoint file loaded {loader.count()} points successfully",
         }
 
-    def uploadMission(self, mission_type: int) -> Response:
+    def _parseWaypointsListIntoLoader(
+        self, waypoints: List[dict], mission_type: int
+    ) -> mavwp.MAVWPLoader:
         """
-        Uploads the current mission to the drone.
+        Parses a list of waypoints into a MAVWPLoader object.
+        """
+
+        if mission_type == TYPE_MISSION:
+            loader = mavwp.MAVWPLoader(
+                target_system=self.drone.target_system,
+                target_component=self.drone.target_component,
+            )
+        elif mission_type == TYPE_FENCE:
+            loader = mavwp.MissionItemProtocol_Fence(
+                target_system=self.drone.target_system,
+                target_component=self.drone.target_component,
+            )
+        elif mission_type == TYPE_RALLY:
+            loader = mavwp.MissionItemProtocol_Rally(
+                target_system=self.drone.target_system,
+                target_component=self.drone.target_component,
+            )
+
+        for wp in waypoints:
+            if isinstance(wp, mavutil.mavlink.MAVLink_mission_item_int_message):
+                loader.add(wp)
+            elif isinstance(wp, dict):
+                # Convert dict to MAVLink mission item int message
+                p = mavutil.mavlink.MAVLink_mission_item_int_message(
+                    self.drone.target_system,
+                    self.drone.target_component,
+                    wp["seq"],
+                    wp["frame"],
+                    wp["command"],
+                    wp["current"],
+                    wp["autocontinue"],
+                    wp["param1"],
+                    wp["param2"],
+                    wp["param3"],
+                    wp["param4"],
+                    int(wp["x"]),
+                    int(wp["y"]),
+                    wp["z"],
+                    wp["mission_type"],
+                )
+                loader.add(p)
+            else:
+                self.drone.logger.error(
+                    f"Invalid waypoint type {type(wp)} in waypoints list"
+                )
+                raise ValueError(f"Invalid waypoint type {type(wp)} in waypoints list")
+
+        return loader
+
+    def uploadMission(self, mission_type: int, waypoints: List[dict]) -> Response:
+        """
+        Uploads the current mission to the drone. This method overwrites the current loader if the upload is successful.
 
         Args:
             mission_type (int): The type of mission to upload. 0=Mission,1=Fence,2=Rally.
@@ -465,14 +519,9 @@ def uploadMission(self, mission_type: int) -> Response:
         if not mission_type_check.get("success"):
             return mission_type_check
 
-        if mission_type == TYPE_MISSION:
-            loader = self.missionLoader
-        elif mission_type == TYPE_FENCE:
-            loader = self.fenceLoader
-        else:
-            loader = self.rallyLoader
+        new_loader = self._parseWaypointsListIntoLoader(waypoints, mission_type)
 
-        if loader.count() == 0:
+        if new_loader.count() == 0:
             return {
                 "success": False,
                 "message": f"No waypoints loaded for the mission type of {mission_type}",
@@ -487,7 +536,7 @@ def uploadMission(self, mission_type: int) -> Response:
         self.drone.master.mav.mission_count_send(
             self.drone.target_system,
             self.drone.target_component,
-            loader.count(),
+            new_loader.count(),
             mission_type=mission_type,
         )
 
@@ -515,13 +564,11 @@ def uploadMission(self, mission_type: int) -> Response:
                     }
                 elif response.mission_type == mission_type:
                     self.drone.logger.debug(
-                        f"Sending mission item {response.seq} out of {loader.count()}"
-                    )
-                    self.drone.master.mav.send(
-                        wpToMissionItemInt(loader.item(response.seq))
+                        f"Sending mission item {response.seq} out of {new_loader.count()-1}"
                     )
+                    self.drone.master.mav.send(new_loader.item(response.seq))
 
-                    if response.seq == loader.count() - 1:
+                    if response.seq == new_loader.count() - 1:
                         mission_ack_response = self.drone.master.recv_match(
                             type=[
                                 "MISSION_ACK",
@@ -537,6 +584,12 @@ def uploadMission(self, mission_type: int) -> Response:
                             and mission_ack_response.mission_type == mission_type
                         ):
                             self.drone.logger.info("Uploaded mission successfully")
+                            if mission_type == TYPE_MISSION:
+                                self.missionLoader = new_loader
+                            elif mission_type == TYPE_FENCE:
+                                self.fenceLoader = new_loader
+                            else:
+                                self.rallyLoader = new_loader
                             return {
                                 "success": True,
                                 "message": "Mission uploaded successfully",
diff --git a/radio/app/endpoints/mission.py b/radio/app/endpoints/mission.py
@@ -9,6 +9,11 @@ class CurrentMissionType(TypedDict):
     type: str
 
 
+class WriteCurrentMissionType(TypedDict):
+    type: str
+    items: list[dict]
+
+
 class ControlMissionType(TypedDict):
     action: str
 
@@ -90,6 +95,51 @@ def getCurrentMissionAll() -> None:
     )
 
 
+@socketio.on("write_current_mission")
+def writeCurrentMission(data: WriteCurrentMissionType) -> None:
+    """
+    Writes the current mission to the drone, only works if missions screen is loaded.
+    """
+    if droneStatus.state != "missions":
+        socketio.emit(
+            "params_error",
+            {
+                "message": "You must be on the missions screen to write the current mission."
+            },
+        )
+        logger.debug(f"Current state: {droneStatus.state}")
+        return
+
+    if not droneStatus.drone:
+        return notConnectedError(action="write current mission")
+
+    mission_type = data.get("type")
+    mission_type_array = ["mission", "fence", "rally"]
+
+    if mission_type not in mission_type_array:
+        socketio.emit(
+            "write_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
+
+    items = data.get("items", [])
+
+    result = droneStatus.drone.missionController.uploadMission(
+        mission_type_array.index(mission_type), items
+    )
+    if not result.get("success"):
+        logger.error(result.get("message"))
+
+    socketio.emit("write_mission_result", result)
+
+
 @socketio.on("control_mission")
 def controlMission(data: ControlMissionType) -> None:
     """
diff --git a/radio/app/utils.py b/radio/app/utils.py
@@ -185,32 +185,6 @@ def sendMessage(msg: Any) -> None:
     socketio.emit("incoming_msg", data)
 
 
-def wpToMissionItemInt(
-    wp: mavutil.mavlink.MAVLink_message,
-) -> mavutil.mavlink.MAVLink_message:
-    if wp.get_type() == "MISSION_ITEM_INT":
-        return wp
-
-    wp_int = mavutil.mavlink.MAVLink_mission_item_int_message(
-        wp.target_system,
-        wp.target_component,
-        wp.seq,
-        wp.frame,
-        wp.command,
-        wp.current,
-        wp.autocontinue,
-        wp.param1,
-        wp.param2,
-        wp.param3,
-        wp.param4,
-        int(wp.x * 1e7),
-        int(wp.y * 1e7),
-        wp.z,
-        wp.mission_type,
-    )
-    return wp_int
-
-
 FIXED_WING_TYPES = [
     mavutil.mavlink.MAV_TYPE_FIXED_WING,
     mavutil.mavlink.MAV_TYPE_VTOL_DUOROTOR,
diff --git a/radio/tests/test_mission.py b/radio/tests/test_mission.py
