diff --git a/mavctl/messages/advanced.py b/mavctl/messages/advanced.py index f81a675..9f6fb59 100644 --- a/mavctl/messages/advanced.py +++ b/mavctl/messages/advanced.py @@ -1,16 +1,20 @@ -import time -from messages import util -from messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal +import time +from typing import Callable, Literal from math import radians, atan -from messages.util import Heading, LatLon_to_Distance + +from src.modules.mavctl.mavctl.messages.navigator import LandingTarget, Navigator +from src.modules.mavctl.mavctl.messages import util +from src.modules.mavctl.mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal +from src.modules.mavctl.mavctl.messages.util import Heading, LatLon_to_Distance # This file is meant for implementing more advanced features in mavctl # This tests the wait target reached and has been verified to work def simple_goto_local(master, x, y, z): - - # Mimics simple_goto from dronekit, includes the right yaw angle so that the drone faces where it needs to go. + """Mimics simple_goto from dronekit, + includes the right yaw angle so that + the drone faces where it needs to go.""" type_mask = master.generate_typemask([0, 1, 2, 9]) angle = atan(y/x) diff --git a/mavctl/messages/location.py b/mavctl/messages/location.py index 36f2aaf..dc20821 100644 --- a/mavctl/messages/location.py +++ b/mavctl/messages/location.py @@ -26,6 +26,14 @@ def __init__(self, north, east, down): self.east = east self.down = down +class Altitude(object): + def __init__(self, mono, amsl, local, relative, terrain, clearance): + self.mono = mono + self.amsl = amsl + self.local = local + self.relative = relative + self.terrain = terrain + self.clearance = clearance class Battery(object): diff --git a/mavctl/messages/Navigator.py b/mavctl/messages/navigator.py similarity index 77% rename from mavctl/messages/Navigator.py rename to mavctl/messages/navigator.py index ebd6b36..6f9a675 100644 --- a/mavctl/messages/Navigator.py +++ b/mavctl/messages/navigator.py @@ -1,20 +1,34 @@ -from pymavlink import mavutil +from typing import Literal, Optional, Tuple +from dataclasses import dataclass import time -from messages import util -from messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal, Velocity from math import sqrt +from pymavlink import mavutil + +from src.modules.mavctl.mavctl.messages import util +from src.modules.mavctl.mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal, Velocity, Altitude + +@dataclass +class LandingTarget: + """ + Data class to store position of landing target in MAV_FRAME_BODY_FRD frame. + FRD local frame aligned to the vehicle's attitude (x: Forward, y: Right, z: Down) + with an origin that travels with vehicle. + """ + forward: float + right: float + altitude: float + class Navigator: def __init__(self, mav): self.mav = mav - + self.TOLERANCE_CE = 0.05 # For checking if the target position has been reached. This is a coefficient which is multiplied by the distance travelled. # The reason why a coefficient method was chosen is because the position tolerance should be a function of distance as opposed to being a constant # This method is preferred so that what happened at AEAC 2025 doesnt happen again. - def arm(self): """ Arms the drone. @@ -126,6 +140,26 @@ def get_global_position(self): alt = msg.alt / 1000 return LocationGlobal(lat, lon, alt) + + def get_geovector(point): + origin = self.get_global_position() + distance = util.LatLon_to_Distance(point, origin) + bearing = radians(util.Heading(point, origin)) + + vectorX = distance * sin(bearing) + vectorY = distance * cos(bearing) + + xRot = vectorX * cos(heading) - vectorY * sin(heading) + yRot = vectorx * sin(heading) + vectorY * cos(heading) + + return (xRot, yRot) + + def get_heading(self): + msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) + if msg: + hdg = hdg / 100 + return hdg + def get_local_position(self): """ @@ -180,6 +214,20 @@ def get_velocity(self): return Velocity(north, east, down) + def get_altitude(self): + """ + Gets the current system altitude, of various types + """ + msg = self.mav.recv_match(type='ALTITUDE', blocking=True) + if msg: + mono = msg.altitude_monotonic + amsl = msg.altiude_amsl + local = msg.altitude_local + relative = msg.altitude_relative + terrain = msg.altitude_terrain + clearance = msg.bottom_clearance + + return Altitude(mono, amsl, local, relative, terrain, clearance) def takeoff(self, altitude, pitch=15): @@ -232,6 +280,60 @@ def vtol_takeoff(self, altitude, pitch=15): self.wait_target_reached(LocationLocal(0, 0, -altitude)) + def land(self, + land_mode: Literal[0,1,2], + abort_alt: float = 0, + yaw_angle: Optional[float] = 0, + latitude: Optional[float] = 0, + longitude: Optional[float] = 0, + ): + """ + Initiate a landing sequence. + + Parameters: + abort_alt (float): + Minimum target altitude if landing is aborted + use system default if not specified. Units: meters. + + land_mode (0, 1, 2): + Precision land mode: + 0: Precision land disabled + 1: opportunistic + 2: required + + yaw_angle (float, optional): + Desired yaw angle in degrees. + Default is to follow the current system yaw heading mode + + latitude (float, optional): + Latitude in decimal degrees. + + longitude (float, optional): + Longitude in decimal degrees. + """ + + # Runtime validation + if land_mode == 0 and (latitude is None or longitude is None): + raise ValueError("specify latitude and longitude for disabled precision landing") + self.mav.mav.command_long_send( + self.mav.target_system, + self.mav.target_component, + mavutil.mavlink.MAV_CMD_NAV_LAND, + 0, + abort_alt, + land_mode, + 0, + yaw_angle, + latitude, + longitude, + 0) + + current_position = self.get_local_position() + current_position.alt = 0 + target = (current_position.north, current_position.down, 0) + + self.wait_target_reached(current_position) + def return_to_launch(self): print("MAVCTL: RTL") @@ -249,11 +351,6 @@ def return_to_launch(self): 0 ) - - - - - def generate_typemask(self, keeps): # Generates typemask based on which values to be included mask = 0 @@ -451,3 +548,19 @@ def wait_target_reached_global(self, target, timeout = 30): return True + def broadcast_landing_target(self, landing_target: LandingTarget) -> None: + """ + This function broadcasts the position of the landing target in MAV_FRAME_BODY_FRD frame. + """ + + time_usec = int(time.time() * 1000000) # convert to microseconds + + print(landing_target) + self.mav.mav.landing_target_send(time_usec=time_usec, + target_num=1, + frame=mavutil.mavlink.MAV_FRAME_BODY_FRD, + angle_x=-landing_target.forward, + angle_y=-landing_target.right, + distance=landing_target.altitude, + size_x=0, + size_y=0) diff --git a/mavctl/messages/util.py b/mavctl/messages/util.py index 9f2df42..f655908 100644 --- a/mavctl/messages/util.py +++ b/mavctl/messages/util.py @@ -1,7 +1,7 @@ import math from math import radians, degrees, cos, sin, atan2, sqrt from pymavlink import mavutil -from messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal +from src.modules.mavctl.mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal def distance_to_target(target1, target2): @@ -74,4 +74,3 @@ def Heading(point1, point2): return heading -