Update reposition function to use set global position int command

radio/app/controllers/flightModesController.py

@@ -152,7 +152,6 @@ def setCurrentFlightMode(self, flightMode: int) -> Response:
             response = self.drone.master.recv_match(
                 type="COMMAND_ACK", blocking=True, timeout=3
             )
-            print(response)
 
             if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_SET_MODE):
                 self.drone.is_listening = True

radio/app/controllers/navController.py

@@ -194,38 +194,48 @@ def reposition(self, lat: float, lon: float, alt: float) -> Response:
         Returns:
             Response: The response from the reposition command
         """
+        guidedModeSetResult = self.drone.flightModesController.setGuidedMode()
+        if not guidedModeSetResult["success"]:
+            return guidedModeSetResult
+
         self.drone.is_listening = False
 
-        self.drone.sendCommandInt(
-            message=mavutil.mavlink.MAV_CMD_DO_REPOSITION,
-            param2=1,  # Change to Guided mode immediately
-            x=lat,
-            y=lon,
-            z=alt,
+        self.drone.master.mav.set_position_target_global_int_send(
+            0,
+            self.drone.target_system,
+            self.drone.target_component,
+            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+            65016,  # Bitmask to ignore all values except for x, y and z
+            int(lat * 1e7),
+            int(lon * 1e7),
+            alt,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
         )
 
-        try:
-            response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True)
-            self.drone.is_listening = True
+        self.drone.logger.info(f"Reposition command sent to {lat}, {lon}, {alt}m")
 
-            if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_REPOSITION):
-                self.drone.logger.info("Reposition command send successfully")
-                return {
-                    "success": True,
-                    "message": "Reposition command sent successfully",
-                    "data": {
-                        "lat": lat,
-                        "lon": lon,
-                        "alt": alt,
-                    },
-                }
-            else:
-                return {
-                    "success": False,
-                    "message": "Could not reposition",
-                }
+        self.drone.is_listening = True
+
+        try:
+            return {
+                "success": True,
+                "message": "Reposition command sent successfully",
+                "data": {
+                    "lat": lat,
+                    "lon": lon,
+                    "alt": alt,
+                },
+            }
         except serial.serialutil.SerialException:
             self.drone.is_listening = True
+            self.drone.logger.error("Reposition command not accepted, serial exception")
             return {
                 "success": False,
                 "message": "Could not reposition, serial exception",

radio/app/drone.py

@@ -485,8 +485,6 @@ def checkForMessages(self) -> None:
                 elif msg.msgname == "STATUSTEXT":
                     self.logger.info(msg.text)
 
-                # self.logger.debug(msg.msgname)
-
                 if msg.msgname in self.message_listeners:
                     self.message_queue.put([msg.msgname, msg])