vtol prep

README.md

@@ -35,7 +35,7 @@ while True:
 
 ```python
 from mavcore.protocols import HeartbeatProtocol, SetModeProtocol
-from mavcore.messages import FlightMode
+from mavcore.mavtypes import FlightMode
 
 # Sends periodic heartbeats from device and sets mode
 hb_protocol = device.run_protocol(HeartbeatProtocol())  # will auto send at 1 hz

dev/mar_test.py

@@ -3,7 +3,9 @@
 import mavcore
 import mavcore.messages as messages
 import mavcore.protocols as protocols
+from mavcore.mavtypes import FlightMode
 
+########## NOTE THIS WILL ONLY WORK FOR COPTER ##########
 
 device = mavcore.MAVDevice("udp:127.0.0.1:14550")
 
@@ -36,7 +38,7 @@
 request_arm = protocols.ArmProtocol()
 device.run_protocol(request_arm)
 
-request_guided = protocols.SetModeProtocol(messages.FlightMode.GUIDED)
+request_guided = protocols.SetModeProtocol(FlightMode.GUIDED)
 device.run_protocol(request_guided)
 
 takeoff = protocols.TakeoffProtocol(300.0)
@@ -49,7 +51,7 @@
 dive = protocols.AttitudeSetpointProtocol(local_pos, imu, boot_time_ms)
 device.run_protocol(dive)
 
-request_brake = protocols.SetModeProtocol(messages.FlightMode.BRAKE)
+request_brake = protocols.SetModeProtocol(FlightMode.BRAKE)
 device.run_protocol(request_brake)
 
 highest = 0.0

dev/test_takeoff.py

@@ -2,6 +2,9 @@
 import mavcore.messages as messages
 import mavcore.protocols as protocols
 import time
+from mavcore.mavtypes import FlightMode
+
+########## NOTE THIS WILL ONLY WORK FOR COPTER ##########
 
 device = mav_device.MAVDevice("udp:127.0.0.1:14550")
 
@@ -25,7 +28,7 @@
 # while True:
 time.sleep(2)
 set_mode_protocol = protocols.SetModeProtocol(
-    messages.FlightMode.GUIDED
+    FlightMode.GUIDED
 )  # 15/AUTOTUNE is GUIDED in plane
 print("--Setting GUIDED mode")
 device.run_protocol(set_mode_protocol)
@@ -42,7 +45,7 @@
     time.sleep(0.2)
 print("Takeoff complete")
 time.sleep(2)
-set_mode_protocol = protocols.SetModeProtocol(messages.FlightMode.RTL)
+set_mode_protocol = protocols.SetModeProtocol(FlightMode.RTL)
 print("--Setting RTL mode")
 device.run_protocol(set_mode_protocol)
 print(f"Set RTL mode ack: {set_mode_protocol.ack_msg}")

mavtypes/__init__.py

@@ -1,2 +1,4 @@
 from .mav_pose import Pose as Pose
 from .waypoint import Waypoint as Waypoint
+from .modes import FlightMode as FlightMode
+from .modes import FlightModePlane as FlightModePlane

mavtypes/modes.py

@@ -0,0 +1,56 @@
+from enum import IntEnum
+
+
+class FlightMode(IntEnum):
+    UNKNOWN = -1
+    STABILIZE = 0
+    ACRO = 1
+    ALTHOLD = 2
+    AUTO = 3
+    GUIDED = 4
+    LOITER = 5
+    RTL = 6
+    CIRCLE = 7
+    LAND = 9
+    DRIFT = 11
+    SPORT = 13
+    FLIP = 14
+    AUTOTUNE = 15
+    POSHOLD = 16
+    BRAKE = 17
+    THROW = 18
+    AVOID_ADSB = 19
+    GUIDED_NOGPS = 20
+    SMART_RTL = 21
+    FLOWHOLD = 22
+    FOLLOW = 23
+    ZIGZAG = 24
+    SYSTEMID = 25
+    HELI_AUTOROTATE = 26
+    AUTO_RTL = 27
+    TURTLE = 28
+
+
+class FlightModePlane(IntEnum):
+    UNKNOWN = -1
+    MANUAL = 0
+    CIRCLE = 1
+    STABILIZE = 2
+    TRAINING = 3
+    ACRO = 4
+    FLY_BY_WIRE_A = 5
+    FLY_BY_WIRE_B = 6
+    CRUISE = 7
+    AUTOTUNE = 8
+    AUTO = 10
+    RTL = 11
+    LOITER = 12
+    TAKEOFF = 13
+    AVOID_ADSB = 14
+    GUIDED = 15
+    INITIALISING = 16
+    QSTABILIZE = 17
+    QHOVER = 18
+    QLOITER = 19
+    QLAND = 20
+    QRTL = 21

messages/__init__.py

@@ -15,7 +15,6 @@
     FenceMissionItemInt as FenceMissionItemInt,
 )
 from .gps_raw_int_msg import FixType as FixType
-from .heartbeat_msg import FlightMode as FlightMode
 from .full_pose_msg import FullPose as FullPose
 from .global_position_msg import GlobalPosition as GlobalPosition
 from .gps_raw_int_msg import GPSRaw as GPSRaw

messages/heartbeat_msg.py

@@ -4,6 +4,7 @@
 import time
 
 from ..mav_message import MAVMessage, thread_safe
+from ..mavtypes import FlightMode, FlightModePlane
 
 MAV_MODE_CUSTOM = 0
 NO_FLAGS = 0
@@ -33,36 +34,6 @@ class MAVState(IntEnum):
     FLIGHT_TERMINATION = 8
 
 
-class FlightMode(IntEnum):
-    UNKNOWN = -1
-    STABILIZE = 0
-    ACRO = 1
-    ALTHOLD = 2
-    AUTO = 3
-    GUIDED = 4
-    LOITER = 5
-    RTL = 6
-    CIRCLE = 7
-    LAND = 9
-    DRIFT = 11
-    SPORT = 13
-    FLIP = 14
-    AUTOTUNE = 15
-    POSHOLD = 16
-    BRAKE = 17
-    THROW = 18
-    AVOID_ADSB = 19
-    GUIDED_NOGPS = 20
-    SMART_RTL = 21
-    FLOWHOLD = 22
-    FOLLOW = 23
-    ZIGZAG = 24
-    SYSTEMID = 25
-    HELI_AUTOROTATE = 26
-    AUTO_RTL = 27
-    TURTLE = 28
-
-
 class Heartbeat(MAVMessage):
     """
     Heartbeat message to send and receive heartbeats.
@@ -112,7 +83,11 @@ def decode(self, msg):
             self.src_sys = msg.get_srcSystem()
             self.src_comp = msg.get_srcComponent()
             self.mask = msg.base_mode
-            self.mode = FlightMode(msg.custom_mode)
+
+            if MAVType(self.type_id) == MAVType.QUADROTOR:
+                self.mode = FlightMode(msg.custom_mode)
+            else:
+                self.mode = FlightModePlane(msg.custom_mode)
 
     @thread_safe
     def __repr__(self) -> str:

messages/set_mode_msg.py

@@ -1,15 +1,20 @@
 import pymavlink.dialects.v20.all as dialect
 
 from ..mav_message import MAVMessage, thread_safe
-from .heartbeat_msg import FlightMode
+from ..mavtypes import FlightMode, FlightModePlane
 
 
 class SetMode(MAVMessage):
     """
     Allows to set mode of device. Uses FlightMode defined in heartbeat_msg.
     """
 
-    def __init__(self, target_system: int, target_component: int, mode: FlightMode):
+    def __init__(
+        self,
+        target_system: int,
+        target_component: int,
+        mode: FlightMode | FlightModePlane,
+    ):
         super().__init__("CUSTOM_SET_MODE")
         self.target_system = target_system
         self.target_component = target_component

messages/setpoint_velocity_msg.py

@@ -1,6 +1,7 @@
 import pymavlink.dialects.v20.all as dialect
 import time
 import numpy as np
+import math
 from ..mav_message import MAVMessage
 
 
@@ -18,6 +19,7 @@ def __init__(
         vx: float,
         vy: float,
         vz: float,
+        do_yaw: bool = True,
     ):
         super().__init__("CUSTOM_SETPOINT_VELOCITY")
         self.target_system = target_system
@@ -30,7 +32,14 @@ def __init__(
         self.vy = vy  # velocity East in m/s
         self.vz = vz  # velocity Down in m/s
 
+        self.do_yaw = do_yaw
+
     def encode(self, system_id, component_id):
+        if self.do_yaw:
+            target_yaw = math.atan2(self.vy, self.vx)
+        else:
+            target_yaw = 0.0
+
         return dialect.MAVLink_set_position_target_local_ned_message(
             time_boot_ms=int(time.time() * 1000 - self.boot_time_ms),
             target_system=int(self.target_system),
@@ -48,7 +57,7 @@ def encode(self, system_id, component_id):
             afx=float(0.0),
             afy=float(0.0),
             afz=float(0.0),
-            yaw=float(0.0),
+            yaw=float(target_yaw),
             yaw_rate=float(0.0),
         )
 

protocols/set_mode_protocol.py

@@ -1,5 +1,6 @@
 from ..mav_protocol import MAVProtocol
-from ..messages import SetMode, FlightMode
+from ..messages import SetMode
+from ..mavtypes import FlightMode, FlightModePlane
 from ..messages.command_ack_msg import CommandAck
 
 
@@ -9,7 +10,10 @@ class SetModeProtocol(MAVProtocol):
     """
 
     def __init__(
-        self, mode: FlightMode, target_system: int = 1, target_component: int = 0
+        self,
+        mode: FlightMode | FlightModePlane,
+        target_system: int = 1,
+        target_component: int = 0,
     ):
         super().__init__()
         self.mode = mode