Reset mission on takeoff

Author: tedtasman

pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name='MAVez'
-version='3.2.0'
+version='3.2.1'
 description='An intuitive MAVLink library for ArduPilot via pymavlink'
 authors=[
     {name='The Pennsylvania State University Unmanned Aerial Systems Club', email='psuuasofficial@gmail.com'}

src/MAVez/controller.py

@@ -710,13 +710,13 @@ async def receive_landing_status(self, timeout=TIMEOUT_DURATION) -> int:
             self.logger.error("[Controller] Bad response received for landing status")
             return self.BAD_RESPONSE_ERROR
 
-    async def set_message_interval(self, message_type, interval) -> int:
+    async def set_message_interval(self, message_type: int, interval: int) -> int:
         """
         Set the message interval for the specified message type.
 
         Args:
-            message_type (str): The type of message to set the interval for.
-            interval (int): The interval in milliseconds to set for the message type.
+            message_type (int): The type of message to set the interval for.
+            interval (int): The interval in microseconds to set for the message type.
             timeout (int): The timeout duration in seconds. Default is 5 seconds.
 
         Returns:
@@ -816,7 +816,7 @@ async def receive_current_mission_index(self) -> int:
             self.logger.error("[Controller] Bad response received for mission item reached")
             return self.BAD_RESPONSE_ERROR
 
-    async def set_current_mission_index(self, index) -> int:
+    async def set_current_mission_index(self, index: int, reset: bool = False) -> int:
         """
         sets the target mission index to the specified index
 
@@ -833,7 +833,7 @@ async def set_current_mission_index(self, index) -> int:
             mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT,  # command
             0,  # confirmation
             index,  # param1
-            0,  # param2
+            1 if reset else 0,  # param2
             0,  # param3
             0,  # param4
             0,  # param5

src/MAVez/flight_controller.py

@@ -105,19 +105,19 @@ async def takeoff(self, takeoff_mission_filename: str) -> int:
             self.logger.critical("[Flight] Takeoff failed, mission not sent")
             return response
 
-        # wait for mission to be fully received
-        # Countdown from 5
-        self.logger.info("[Flight] Takeoff in 5 seconds")
-        for _ in range(5, 0, -1):
-            time.sleep(1)
-
         # set the mode to AUTO
         response = await self.set_mode("AUTO")
 
         # verify that the mode was set successfully
         if response:
             self.logger.critical("[Flight] Takeoff failed, mode not set to AUTO")
             return response
+        
+        # reset mission index to 0
+        response = await self.set_current_mission_index(0, reset=True)
+        if response:
+            self.logger.critical("[Flight] Takeoff failed, could not reset mission index")
+            return response
 
         # arm ardupilot
         response = await self.arm()
@@ -244,7 +244,7 @@ async def wait_for_landing(self, timeout=60) -> int:
 
         # start receiving landing status
         response = await self.set_message_interval(
-            message_type=245, interval=1e6
+            message_type=245, interval=100000
         )  # 245 is landing status (EXTENDED_SYS_STATE), 1e6 is 1 second
         if response:
             self.logger.critical("[Flight] Failed waiting for landing.")