Feature/movement

flight/config.py

@@ -3,12 +3,17 @@
 
 from mavsdk import System
 
+NUM_LAPS: int = 2
+
+run_states = {"early_laps": True, "to_mast": True, "return_laps": True}
 
 MAX_SPEED: float = 6.352  # m/s
+
 ALT_CORRECTION_SPEED: float = 0.25  # m/s down
 MAX_ALT: float = 9.0  # m
 TAKEOFF_ALT: float = 1.0  # m
 FLYING_ALT: float = 6.0  # m
+
 # What percentage of the hight can we loos/gain before unsafe
 ALT_PERCENT_ACCURACY: float = 0.15
 ALT_RANGE_MAX: float = FLYING_ALT + (FLYING_ALT * ALT_PERCENT_ACCURACY)  # m
@@ -17,40 +22,38 @@
 POINT_PERCENT_ACCURACY: float = 0.2
 
 # Position for pylon 1
-# lat1: Latitude = Latitude(37.9497800)
-# lon1: Longitude = Longitude(-92.7854470)
-lat1: Latitude = Latitude(degree=37, minute=56, second=55.6)
-lon1: Longitude = Longitude(degree=-91, minute=-47, second=-3.3)
+lat1: Latitude = Latitude(degree=37, minute=56, second=55.6)  # 37.948778
+lon1: Longitude = Longitude(degree=-91, minute=-47, second=-3.3)  # -91.78425
 pylon1: LatLon = LatLon(lat1, lon1)
 
 # Position for pylon 2
-# lat2: float = 37.9486433
-# lon2: float = -91.7839372
-# lat2: float = Latitude(37.9504260)
-# lon2: float = Longitude(-91.7848542)
-
-lat2: Latitude = Latitude(degree=37, minute=56, second=53.3)
-lon2: Longitude = Longitude(degree=-91, minute=-47, second=0)
+lat2: Latitude = Latitude(degree=37, minute=56, second=53.3)  # 37.948139
+lon2: Longitude = Longitude(degree=-91, minute=-47, second=0)  # -91.783333
 pylon2: LatLon = LatLon(lat2, lon2)
 
 # Takeoff Position set in takeoff.py
 takeoff_pos = LatLon
 
 # Position for the mast
-MAST_LAT: Latitude = Latitude(degree=37, minute=56, second=53.0)  # placeholder postion
-MAST_LON: Longitude = Longitude(degree=-91, minute=-47, second=-5.0)
+MAST_LAT: Latitude = Latitude(
+    37.9486054
+)  # degree=37, minute=56, second=53.0)  # 37.948056 placeholder
+MAST_LON: Longitude = Longitude(
+    -91.7843514
+)  # degree=-91, minute=-47, second=-5.0)  # -91.784722
 MAST_LOCATION: LatLon = LatLon(MAST_LAT, MAST_LON)
+# flight test lat: 37.9486054, lon: -91.7843514
 
-OFFSET: float = 0.005  # km
-DEG_OFFSET: int = 90  # deg
 
-NUM_LAPS: int = 2
+OFFSET_RIGHT = {"KM": 0.005, "DEG": 90}
+OFFSET_LEFT = {"KM": 0.005, "DEG": -90}
+OFFSET_BACK = {"KM": 0.005, "DEG": 180}
+OFFSET_FRONT = {"KM": 0.005, "DEG": 0}
+OFFSET_NONE = {"KM": 0, "DEG": 0}
 
 THINK_FOR_S: float = 2.0
 FAST_THINK_S: float = 1.0
 
-run_states = {"early_laps": True, "to_mast": True}
-
 
 async def config_params(drone: System):
     await drone.action.set_maximum_speed(MAX_SPEED)
@@ -64,12 +67,7 @@ async def config_params(drone: System):
     # Set offboard loss failsafe mode HOLD
     await drone.param.set_param_int("COM_OBL_ACT", 1)
     # Set offboard loss failsafe mode when RC is available HOLD
-    await drone.param.set_param_int("COM_OBL_RC_ACT", 5)
-
-    # Set RC loss failsafe mode HOLD
+    await drone.param.set_param_int(
+        "COM_OBL_RC_ACT", 5
+    )  # Set RC loss failsafe mode HOLD
     await drone.param.set_param_int("NAV_RCL_ACT", 1)
-    await drone.param.set_param_float("LNDMC_XY_VEL_MAX", 0.5)
-    # await drone.param.set_param_float("LNDMC_FFALL_THR", 3)
-    # await drone.param.set_param_float("LNDMC_FFALL_TTRI", 0.15)
-    await drone.param.set_param_float("LNDMC_ALT_MAX", MAX_ALT)
-    # await drone.param.set_param_float("LNDMC_LOW_T_THR", 0.2)

flight/states/early_laps.py

@@ -7,7 +7,7 @@
 
 from flight.utils.movement_controller import MovementController
 from .state import State
-from .to_mast import ToMast
+from .exit_early_lap import ExitEarlyLap
 
 
 async def arange(count):
@@ -25,21 +25,29 @@ async def run(self, drone):
             mover: MovementController = MovementController()
             # Go to pylon 1
             logging.info("Moving to pylon 1")
-            await mover.move_to(drone, config.pylon1)
+            await mover.move_to(
+                drone, config.pylon1, config.OFFSET_RIGHT, config.FLYING_ALT
+            )
+
             logging.info("Arrived at pylon 1")
-            async for i in arange(config.NUM_LAPS):
+            async for i in arange(config.NUM_LAPS - 1):
+
                 logging.info("Starting lap: %d", i)
                 logging.debug("Lap %d: Straight one", i)
-                await mover.move_to(drone, config.pylon2)  # move to pylon 2
+                await mover.move_to(
+                    drone, config.pylon2, config.OFFSET_RIGHT, config.FLYING_ALT
+                )  # move to pylon 2
 
                 logging.debug("Lap %d: Turn one", i)
-                await mover.turn(drone)  # turn around pylon 2
+                await mover.turn(drone, 180)  # turn around pylon 2
 
                 logging.debug("Lap %d: Straight two", i)
-                await mover.move_to(drone, config.pylon1)  # move to pylon 1
+                await mover.move_to(
+                    drone, config.pylon1, config.OFFSET_RIGHT, config.FLYING_ALT
+                )  # move to pylon 1
 
                 logging.debug("Lap %d: Turn two", i)
-                await mover.turn(drone)  # turn around pylon 1
-            return ToMast()
+                await mover.turn(drone, 180)  # turn around pylon 1
+            return ExitEarlyLap()
         else:
-            return ToMast()
+            return ExitEarlyLap()

flight/states/exit_early_lap.py

@@ -0,0 +1,39 @@
+"""Runs the Final lab to move to mast"""
+import logging
+import asyncio
+import mavsdk as sdk
+
+from flight import config
+
+from flight.utils.movement_controller import MovementController
+from .state import State
+from .to_mast import ToMast
+
+
+class ExitEarlyLap(State):
+    """Handles getting the drone around the two pylons for the final times"""
+
+    async def run(self, drone):
+        """Runs the final lap"""
+        if config.run_states["early_laps"]:
+            mover: MovementController = MovementController()
+
+            logging.info("Starting final lap")
+            logging.debug("Final Lap: Straight one")
+            await mover.move_to(
+                drone, config.pylon2, config.OFFSET_RIGHT, config.FLYING_ALT
+            )  # move to pylon 2
+
+            logging.debug("Final Lap: Turn one")
+            await mover.turn(drone, 180)  # turn around pylon 2
+
+            logging.debug("Final Lap: Straight two")
+            await mover.move_to(
+                drone, config.pylon1, config.OFFSET_RIGHT, config.FLYING_ALT
+            )  # move to pylon 1
+
+            logging.debug("Final Lap: Turn two")
+            await mover.turn(drone, -90)  # turn around pylon 1
+            return ToMast()
+        else:
+            return ToMast()

flight/states/exit_return_lap.py

@@ -0,0 +1,37 @@
+"""Runs the Final lab to move to land"""
+import logging
+import asyncio
+import mavsdk as sdk
+
+from flight import config
+
+from flight.utils.movement_controller import MovementController
+from .state import State
+from .land import Land
+
+
+class ExitReturnLap(State):
+    """Handles getting the drone around the two pylons for the final times"""
+
+    async def run(self, drone):
+        """Runs the final lap"""
+        if config.run_states["return_laps"]:
+            mover: MovementController = MovementController()
+
+            logging.info("Starting final lap")
+            logging.debug("Final Lap: Straight one")
+            await mover.move_to(
+                drone, config.pylon2, config.OFFSET_LEFT, config.FLYING_ALT
+            )  # move to pylon 2
+
+            logging.debug("Final Lap: Turn one")
+            await mover.turn_right(drone, 180)  # turn around pylon 2
+
+            logging.debug("Final Lap: Straight two")
+            await mover.move_to(
+                drone, config.pylon1, config.OFFSET_LEFT, config.FLYING_ALT
+            )  # move to pylon 1
+
+            return Land()
+        else:
+            return Land()

flight/states/land.py

@@ -30,8 +30,10 @@ async def run(self, drone: System) -> State:
             sdk.offboard.VelocityBodyYawspeed(0, 0, 0, 0)
         )
 
-        await asyncio.sleep(config.THINK_FOR_S)
-        await mover.move_to_takeoff(drone, config.takeoff_pos)
+        # await asyncio.sleep(config.THINK_FOR_S)
+
+        # change config.OFFSET_ depending on what physical drone does
+        await mover.move_to(drone, config.takeoff_pos, config.OFFSET_FRONT, 2)
         await asyncio.sleep(config.THINK_FOR_S)
         logging.info("Preparing to land")
         await mover.manual_land(drone)

flight/states/return_laps.py

@@ -0,0 +1,53 @@
+""""""
+import logging
+import asyncio
+import mavsdk as sdk
+
+from flight import config
+
+from flight.utils.movement_controller import MovementController
+from .state import State
+from .exit_return_lap import ExitReturnLap
+
+
+async def arange(count):
+    """Needed to allows us to do a range asynchronously"""
+    for i in range(count):
+        yield i
+
+
+class ReturnLaps(State):
+    """"""
+
+    async def run(self, drone):
+        """"""
+        if config.run_states["return_laps"]:
+            mover: MovementController = MovementController()
+            # Go to pylon 1
+            logging.info("Moving to pylon 1")
+            await mover.move_to(
+                drone, config.pylon1, config.OFFSET_LEFT, config.FLYING_ALT
+            )
+
+            logging.info("Arrived at pylon 1")
+            async for i in arange(config.NUM_LAPS - 1):
+
+                logging.info("Starting lap: %d", i)
+                logging.debug("Lap %d: Straight one", i)
+                await mover.move_to(
+                    drone, config.pylon2, config.OFFSET_LEFT, config.FLYING_ALT
+                )  # move to pylon 2
+
+                logging.debug("Lap %d: Turn one", i)
+                await mover.turn_right(drone, 180)  # turn around pylon 2
+
+                logging.debug("Lap %d: Straight two", i)
+                await mover.move_to(
+                    drone, config.pylon1, config.OFFSET_LEFT, config.FLYING_ALT
+                )  # move to pylon 1
+
+                logging.debug("Lap %d: Turn two", i)
+                await mover.turn_right(drone, 180)  # turn around pylon 1
+            return ExitReturnLap()
+        else:
+            return ExitReturnLap()

flight/states/to_mast.py

@@ -6,7 +6,7 @@
 from flight import config
 
 from flight.utils.movement_controller import MovementController
-from .land import Land
+from .return_laps import ReturnLaps
 from .state import State
 
 
@@ -19,13 +19,15 @@ async def run(self, drone):
             mover: MovementController = MovementController()
             # Go to the mast
             logging.info("Moving to mast")
-            await mover.move_to(drone, config.MAST_LOCATION)
+            await mover.move_to(
+                drone, config.MAST_LOCATION, config.OFFSET_BACK, config.FLYING_ALT
+            )
             logging.info("Arrived at mast")
             # (NSm/s, EWm/s, DUm/s, Ydeg) Stop moving
             await drone.offboard.set_velocity_ned(
-                sdk.offboard.VelocityNedYaw(0.0, 0.0, 0.0, 0.0)
+                sdk.offboard.VelocityNedYaw(0.0, 0.0, -0.01, 0.0)
             )
-            await asyncio.sleep(20)
-            return Land()
+            await asyncio.sleep(5)
+            return ReturnLaps()
         else:
-            return Land()
+            return ReturnLaps()