From 64797be9a73fc075a46c2eaf5e76d0376eadd9cd Mon Sep 17 00:00:00 2001 From: Elijah Wade Date: Sat, 27 Feb 2021 15:00:37 -0600 Subject: [PATCH 1/5] changes --- flight/states/early_laps.py | 10 ++- flight/states/land.py | 2 +- flight/states/to_mast.py | 6 +- flight/utils/movement_controller.py | 112 ++++++++-------------------- 4 files changed, 41 insertions(+), 89 deletions(-) diff --git a/flight/states/early_laps.py b/flight/states/early_laps.py index 00c15691..96f6df37 100644 --- a/flight/states/early_laps.py +++ b/flight/states/early_laps.py @@ -25,18 +25,22 @@ 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, True, config.FLYING_ALT) logging.info("Arrived at pylon 1") async for i in arange(config.NUM_LAPS): 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, True, config.FLYING_ALT + ) # move to pylon 2 logging.debug("Lap %d: Turn one", i) await mover.turn(drone) # 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, True, config.FLYING_ALT + ) # move to pylon 1 logging.debug("Lap %d: Turn two", i) await mover.turn(drone) # turn around pylon 1 diff --git a/flight/states/land.py b/flight/states/land.py index 022b895a..10752ab5 100644 --- a/flight/states/land.py +++ b/flight/states/land.py @@ -31,7 +31,7 @@ async def run(self, drone: System) -> State: ) await asyncio.sleep(config.THINK_FOR_S) - await mover.move_to_takeoff(drone, config.takeoff_pos) + await mover.move_to(drone, config.takeoff_pos, False, 2) await asyncio.sleep(config.THINK_FOR_S) logging.info("Preparing to land") await mover.manual_land(drone) diff --git a/flight/states/to_mast.py b/flight/states/to_mast.py index 9875f87e..911f92eb 100644 --- a/flight/states/to_mast.py +++ b/flight/states/to_mast.py @@ -19,13 +19,13 @@ 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, True, 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) + await asyncio.sleep(5) return Land() else: return Land() diff --git a/flight/utils/movement_controller.py b/flight/utils/movement_controller.py index 2a6719e0..0ec7ec7f 100644 --- a/flight/utils/movement_controller.py +++ b/flight/utils/movement_controller.py @@ -5,6 +5,7 @@ import mavsdk as sdk import logging import math +import asyncio class MovementController: @@ -14,12 +15,16 @@ class MovementController: Calculates and uses gps coordinates of the drone to move to the location of the target pylon """ - async def move_to(self, drone: System, pylon: LatLon) -> bool: + async def move_to( + self, drone: System, pylon: LatLon, offset: bool, fly_at: int + ) -> bool: """ Function to calculate movement velocity: Parameters: Drone(System): Our drone object Pylon(LatLon): Targets for the drone found using GPS Latitude and Longitude + Offset(bool): offset the move to position to avoid colllision + Fly At(int): desired altitude to fly at Return: bool: True or false if the target is within range None: If we don't reach the target @@ -33,9 +38,9 @@ async def move_to(self, drone: System, pylon: LatLon) -> bool: # at or above, go down (positive) # below tolerance, go up (negative) - if altitude >= config.ALT_RANGE_MAX: + if altitude >= fly_at + (fly_at * config.ALT_PERCENT_ACCURACY): alt = config.ALT_CORRECTION_SPEED # go down m/s - elif altitude <= config.ALT_RANGE_MIN: + elif altitude <= fly_at - (fly_at * config.ALT_PERCENT_ACCURACY): alt = -config.ALT_CORRECTION_SPEED # go up m/s else: alt = -0.15 # don't move @@ -47,15 +52,23 @@ async def move_to(self, drone: System, pylon: LatLon) -> bool: # Only for first run through loop if count == 0: - # How many degrees we need to turn in order to look at the pylon - # Think of a unit circle - deg_to_pylon: float = current.heading_initial(pylon) - # Creating a new position we need to go to - # Distance to the offset point from the pylon - offset_point: float = pylon.offset( - deg_to_pylon + config.DEG_OFFSET, config.OFFSET - ) - logging.debug(offset_point.to_string("d% %m% %S% %H")) # you are here + if offset: + # How many degrees we need to turn in order to look at the pylon + # Think of a unit circle + deg_to_pylon: float = current.heading_initial(pylon) + # Creating a new position we need to go to + # Distance to the offset point from the pylon + offset_point: float = pylon.offset( + deg_to_pylon + config.DEG_OFFSET, config.OFFSET + ) + logging.debug( + offset_point.to_string("d% %m% %S% %H") + ) # you are here + else: + offset_point: float = pylon + logging.debug( + offset_point.to_string("d% %m% %S% %H") + ) # you are here # distance we have to go in order to get to the offset point dist: float = current.distance(offset_point) # degrees needed to change to get to offset position @@ -89,9 +102,12 @@ async def move_to(self, drone: System, pylon: LatLon) -> bool: # if inside the circle, move on to the next # if outside of the circle, keep running to you get inside if ( - abs(x) <= reference_x * config.POINT_PERCENT_ACCURACY - and abs(y) <= reference_y * config.POINT_PERCENT_ACCURACY + abs(x) <= 1 # reference_x * config.POINT_PERCENT_ACCURACY + and abs(y) <= 1 # reference_y * config.POINT_PERCENT_ACCURACY ): + await drone.offboard.set_velocity_ned( + sdk.offboard.VelocityNedYaw(0, 0, alt, deg) + ) return True count += 1 @@ -141,74 +157,6 @@ async def takeoff(self, drone: System): return - async def move_to_takeoff(self, drone: System, takeoff_location: LatLon) -> None: - """ - Similar to move_to function, but heights are changed so drone only descends when moving - Parameters: - drone(System): our drone object - takeoff_location(LatLon): gives lat & lon of takeoff location - Return: - None - """ - # Moves drone to initial takeoff location - logging.info("Moving to Takeoff location") - count: int = 0 - async for gps in drone.telemetry.position(): - altitude: float = round(gps.relative_altitude_m, 2) - # not allowed to go past 15m - # at or above, go down (positive) - # below tolerance, go up (negative) - - if altitude > 2: - alt = config.ALT_CORRECTION_SPEED # go down m/s - elif altitude < 2: - alt = -config.ALT_CORRECTION_SPEED # go up m/s - else: - alt = -0.15 # don't move - - # Configure current position and store it - lat: float = round(gps.latitude_deg, 8) - lon: float = round(gps.longitude_deg, 8) - current: float = LatLon(lat, lon) # you are here - - # distance we have to go in order to get to the offset point - dist: float = current.distance(takeoff_location) - # degrees needed to change to get to offset position - deg: float = current.heading_initial(takeoff_location) - - # East, West - x: float = dist * math.sin(math.radians(deg)) * 1000 # from km to m - # North, South - y: float = dist * math.cos(math.radians(deg)) * 1000 # from km to m - - if count == 0: - reference_x: float = abs(x) - reference_y: float = abs(y) - - dx: float = math.copysign( - config.MAX_SPEED - * math.cos(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), - x, - ) - dy: float = math.copysign( - config.MAX_SPEED - * math.sin(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), - y, - ) - # continuously update information on the drone's location - # and update the velocity of the drone - await drone.offboard.set_velocity_ned( - sdk.offboard.VelocityNedYaw(dy, dx, alt, deg) - ) - # if the x and y values are close enough (2m) to the original position * precision - # if inside the circle, move on to the next - # if outside of the circle, keep running to you get inside - if ( - abs(x) <= reference_x * config.POINT_PERCENT_ACCURACY - and abs(y) <= reference_y * config.POINT_PERCENT_ACCURACY - ): - return True - async def manual_land(self, drone: System) -> None: """ Function to slowly land the drone vertically From bc740eabd31ec83d2169db7ca0881c5a13181805 Mon Sep 17 00:00:00 2001 From: Elijah Wade Date: Tue, 2 Mar 2021 19:18:18 -0600 Subject: [PATCH 2/5] found error in movement --- flight/utils/movement_controller.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/flight/utils/movement_controller.py b/flight/utils/movement_controller.py index 0ec7ec7f..ef07a78b 100644 --- a/flight/utils/movement_controller.py +++ b/flight/utils/movement_controller.py @@ -83,16 +83,16 @@ async def move_to( reference_x: float = abs(x) reference_y: float = abs(y) - dx: float = math.copysign( - config.MAX_SPEED - * math.cos(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), - x, - ) - dy: float = math.copysign( - config.MAX_SPEED - * math.sin(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), - y, - ) + dx: float = math.copysign( + config.MAX_SPEED + * math.cos(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), + x, + ) + dy: float = math.copysign( + config.MAX_SPEED + * math.sin(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), + y, + ) # continuously update information on the drone's location # and update the velocity of the drone await drone.offboard.set_velocity_ned( From 817e7a567cb29a673e5845cd27f686ebd6847792 Mon Sep 17 00:00:00 2001 From: Elijah Wade Date: Tue, 9 Mar 2021 21:10:38 -0600 Subject: [PATCH 3/5] added all exit and returning laps with a lot of edits to movement --- flight/config.py | 17 +++--- flight/states/early_laps.py | 22 ++++---- flight/states/exit_early_lap.py | 39 ++++++++++++++ flight/states/exit_return_lap.py | 37 +++++++++++++ flight/states/land.py | 4 +- flight/states/return_laps.py | 53 ++++++++++++++++++ flight/states/to_mast.py | 10 ++-- flight/utils/movement_controller.py | 83 ++++++++++++++++++----------- 8 files changed, 209 insertions(+), 56 deletions(-) create mode 100644 flight/states/exit_early_lap.py create mode 100644 flight/states/exit_return_lap.py create mode 100644 flight/states/return_laps.py diff --git a/flight/config.py b/flight/config.py index 7d16c216..a903d68f 100644 --- a/flight/config.py +++ b/flight/config.py @@ -41,15 +41,19 @@ MAST_LON: Longitude = Longitude(degree=-91, minute=-47, second=-5.0) MAST_LOCATION: LatLon = LatLon(MAST_LAT, MAST_LON) -OFFSET: float = 0.005 # km -DEG_OFFSET: int = 90 # deg +# OFFSET: float = 0.005 # km +# DEG_OFFSET: int = 90 # deg + +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} NUM_LAPS: int = 2 THINK_FOR_S: float = 2.0 FAST_THINK_S: float = 1.0 - -run_states = {"early_laps": True, "to_mast": True} +run_states = {"early_laps": True, "to_mast": True, "return_laps": True} async def config_params(drone: System): @@ -68,8 +72,3 @@ async def config_params(drone: System): # 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) diff --git a/flight/states/early_laps.py b/flight/states/early_laps.py index 96f6df37..e6533597 100644 --- a/flight/states/early_laps.py +++ b/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,25 +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, True, config.FLYING_ALT) + 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, True, config.FLYING_ALT + 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, True, config.FLYING_ALT + 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() diff --git a/flight/states/exit_early_lap.py b/flight/states/exit_early_lap.py new file mode 100644 index 00000000..094c1dd5 --- /dev/null +++ b/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() diff --git a/flight/states/exit_return_lap.py b/flight/states/exit_return_lap.py new file mode 100644 index 00000000..f47e4602 --- /dev/null +++ b/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() diff --git a/flight/states/land.py b/flight/states/land.py index 10752ab5..f5bdad83 100644 --- a/flight/states/land.py +++ b/flight/states/land.py @@ -30,8 +30,8 @@ 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(drone, config.takeoff_pos, False, 2) + # await asyncio.sleep(config.THINK_FOR_S) + await mover.move_to(drone, config.takeoff_pos, config.OFFSET_BACK, 2) await asyncio.sleep(config.THINK_FOR_S) logging.info("Preparing to land") await mover.manual_land(drone) diff --git a/flight/states/return_laps.py b/flight/states/return_laps.py new file mode 100644 index 00000000..cc208db5 --- /dev/null +++ b/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() diff --git a/flight/states/to_mast.py b/flight/states/to_mast.py index 911f92eb..76aec435 100644 --- a/flight/states/to_mast.py +++ b/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, True, config.FLYING_ALT) + 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.01, 0.0) ) await asyncio.sleep(5) - return Land() + return ReturnLaps() else: - return Land() + return ReturnLaps() diff --git a/flight/utils/movement_controller.py b/flight/utils/movement_controller.py index ef07a78b..d16df760 100644 --- a/flight/utils/movement_controller.py +++ b/flight/utils/movement_controller.py @@ -16,7 +16,7 @@ class MovementController: """ async def move_to( - self, drone: System, pylon: LatLon, offset: bool, fly_at: int + self, drone: System, pylon: LatLon, offset: {float, int}, fly_at: int ) -> bool: """ Function to calculate movement velocity: @@ -52,23 +52,15 @@ async def move_to( # Only for first run through loop if count == 0: - if offset: - # How many degrees we need to turn in order to look at the pylon - # Think of a unit circle - deg_to_pylon: float = current.heading_initial(pylon) - # Creating a new position we need to go to - # Distance to the offset point from the pylon - offset_point: float = pylon.offset( - deg_to_pylon + config.DEG_OFFSET, config.OFFSET - ) - logging.debug( - offset_point.to_string("d% %m% %S% %H") - ) # you are here - else: - offset_point: float = pylon - logging.debug( - offset_point.to_string("d% %m% %S% %H") - ) # you are here + # How many degrees we need to turn in order to look at the pylon + # Think of a unit circle + deg_to_pylon: float = current.heading_initial(pylon) + # Creating a new position we need to go to + # Distance to the offset point from the pylon + offset_point: float = pylon.offset( + deg_to_pylon + offset["DEG"], offset["KM"] + ) + logging.debug(offset_point.to_string("d% %m% %S% %H")) # you are here # distance we have to go in order to get to the offset point dist: float = current.distance(offset_point) # degrees needed to change to get to offset position @@ -83,16 +75,20 @@ async def move_to( reference_x: float = abs(x) reference_y: float = abs(y) - dx: float = math.copysign( - config.MAX_SPEED - * math.cos(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), - x, - ) - dy: float = math.copysign( - config.MAX_SPEED - * math.sin(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), - y, - ) + try: + dx = math.copysign(config.MAX_SPEED * math.cos(math.atan(y / x)), x) + dy = math.copysign(config.MAX_SPEED * math.sin(math.atan(y / x)), y) + except: + dx: float = math.copysign( + config.MAX_SPEED + * math.cos(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), + x, + ) + dy: float = math.copysign( + config.MAX_SPEED + * math.sin(math.asin(y / (math.sqrt((x ** 2) + (y ** 2))))), + y, + ) # continuously update information on the drone's location # and update the velocity of the drone await drone.offboard.set_velocity_ned( @@ -102,8 +98,8 @@ async def move_to( # if inside the circle, move on to the next # if outside of the circle, keep running to you get inside if ( - abs(x) <= 1 # reference_x * config.POINT_PERCENT_ACCURACY - and abs(y) <= 1 # reference_y * config.POINT_PERCENT_ACCURACY + abs(x) <= 0.25 # reference_x * config.POINT_PERCENT_ACCURACY + and abs(y) <= 0.25 # reference_y * config.POINT_PERCENT_ACCURACY ): await drone.offboard.set_velocity_ned( sdk.offboard.VelocityNedYaw(0, 0, alt, deg) @@ -111,7 +107,7 @@ async def move_to( return True count += 1 - async def turn(self, drone: System) -> bool: + async def turn(self, drone: System, deg: int) -> bool: """ Turns the drone around the pylon it is currently at Parameters: @@ -121,7 +117,7 @@ async def turn(self, drone: System) -> bool: async for tel in drone.telemetry.attitude_euler(): current: float = (360 + round(tel.yaw_deg)) % 360 if count == 0: - temp = (current + 180) % 360 + temp = (current + deg) % 360 await drone.offboard.set_velocity_body( sdk.offboard.VelocityBodyYawspeed(5, -3, -0.1, -60) @@ -185,3 +181,26 @@ async def manual_land(self, drone: System) -> None: sdk.offboard.VelocityBodyYawspeed(0.0, 0.0, 0.0, 0.0) ) return + + async def turn_right(self, drone: System, deg: int) -> bool: + """ + Turns the drone around the pylon it is currently at + Parameters: + Drone(System): Our drone object + """ + count: int = 0 + async for tel in drone.telemetry.attitude_euler(): + current: float = (360 + round(tel.yaw_deg)) % 360 + if count == 0: + temp = (current + deg) % 360 + + await drone.offboard.set_velocity_body( + sdk.offboard.VelocityBodyYawspeed(5, 3, -0.1, 60) + ) + # await asyncio.sleep(config.FAST_THINK_S) + val = abs(current - temp) + # TODO: Add case so that it can overshoot the point and still complete + if val < 10: + logging.debug("Finished Turn") + return True + count += 1 From 9016dbca598a5a208f1c5d562400947b46274a90 Mon Sep 17 00:00:00 2001 From: Elijah Wade Date: Wed, 17 Mar 2021 17:55:16 -0500 Subject: [PATCH 4/5] cleaning a few things up --- flight/config.py | 35 ++++++++++++----------------- flight/utils/movement_controller.py | 7 +++--- 2 files changed, 18 insertions(+), 24 deletions(-) diff --git a/flight/config.py b/flight/config.py index a903d68f..8408b412 100644 --- a/flight/config.py +++ b/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,43 +22,31 @@ 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(degree=37, minute=56, second=53.0) # 37.948056 +MAST_LON: Longitude = Longitude(degree=-91, minute=-47, second=-5.0) # -91.784722 MAST_LOCATION: LatLon = LatLon(MAST_LAT, MAST_LON) -# OFFSET: float = 0.005 # km -# DEG_OFFSET: int = 90 # deg 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} -NUM_LAPS: int = 2 - THINK_FOR_S: float = 2.0 FAST_THINK_S: float = 1.0 -run_states = {"early_laps": True, "to_mast": True, "return_laps": True} async def config_params(drone: System): @@ -68,7 +61,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) diff --git a/flight/utils/movement_controller.py b/flight/utils/movement_controller.py index d16df760..69e4238a 100644 --- a/flight/utils/movement_controller.py +++ b/flight/utils/movement_controller.py @@ -97,10 +97,11 @@ async def move_to( # if the x and y values are close enough (2m) to the original position * precision # if inside the circle, move on to the next # if outside of the circle, keep running to you get inside - if ( - abs(x) <= 0.25 # reference_x * config.POINT_PERCENT_ACCURACY - and abs(y) <= 0.25 # reference_y * config.POINT_PERCENT_ACCURACY + if ( # will always undershoot target at slower speeds + abs(x) <= reference_x * config.POINT_PERCENT_ACCURACY + and abs(y) <= reference_y * config.POINT_PERCENT_ACCURACY ): + # get to drone to halt before moving on to the next thing await drone.offboard.set_velocity_ned( sdk.offboard.VelocityNedYaw(0, 0, alt, deg) ) From 995595fb9805047967c710e3d80be6bf98df2b3a Mon Sep 17 00:00:00 2001 From: Elijah Wade Date: Wed, 17 Mar 2021 18:19:01 -0500 Subject: [PATCH 5/5] last bit of testing --- flight/config.py | 10 ++++++++-- flight/states/land.py | 4 +++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/flight/config.py b/flight/config.py index 8408b412..3a2e047a 100644 --- a/flight/config.py +++ b/flight/config.py @@ -35,15 +35,21 @@ takeoff_pos = LatLon # Position for the mast -MAST_LAT: Latitude = Latitude(degree=37, minute=56, second=53.0) # 37.948056 -MAST_LON: Longitude = Longitude(degree=-91, minute=-47, second=-5.0) # -91.784722 +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_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 diff --git a/flight/states/land.py b/flight/states/land.py index f5bdad83..5da8dca5 100644 --- a/flight/states/land.py +++ b/flight/states/land.py @@ -31,7 +31,9 @@ async def run(self, drone: System) -> State: ) # await asyncio.sleep(config.THINK_FOR_S) - await mover.move_to(drone, config.takeoff_pos, config.OFFSET_BACK, 2) + + # 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)