First pass at a MAVlink part

donkeycar/parts/mavlink.py

@@ -0,0 +1,73 @@
+import time
+import math
+
+debug = False
+
+try:
+    from pymavlink import mavutil
+except ImportError:
+    print("Pymavlink not found.  Please install: pip install pymavlink")
+
+
+class MavlinkIMUPart:
+    def __init__(self, connection_string):
+        # Create the connection to Pixhawk
+        self.master = mavutil.mavlink_connection(connection_string, baud=115200)
+        self.master.wait_heartbeat()
+        print("Heartbeat from system (system %u component %u)" % (self.master.target_system, self.master.target_component))
+
+        # IMU data
+        self.roll = 0
+        self.pitch = 0
+        self.yaw = 0
+
+        # Position data
+        self.lat = 0.0
+        self.lon = 0.0
+        self.alt = 0.0
+
+        # Velocity data
+        self.vx = 0.0
+        self.vy = 0.0
+        self.vz = 0.0
+
+        # Flag to control the data reading thread
+        self.running = True
+
+
+    def update(self):
+        while self.running:
+            self.poll()
+
+    def poll(self):
+       message = self.master.recv_match()
+       if message is not None:
+           attitude_msg = self.master.recv_match(type='ATTITUDE', blocking=True)
+           position_msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
+           velocity_msg = self.master.recv_match(type='VFR_HUD', blocking=True)
+
+            # Updating IMU data
+           self.roll = attitude_msg.roll
+           self.pitch = attitude_msg.pitch
+           self.yaw = attitude_msg.yaw
+
+           # Updating Position data
+           self.lat = position_msg.lat / 1e7  # Convert to degrees
+           self.lon = position_msg.lon / 1e7  # Convert to degrees
+           self.alt = position_msg.alt / 1e3  # Convert to meters
+
+           # Updating Velocity data (VFR_HUD provides ground speeds)
+           self.vx = velocity_msg.groundspeed * math.cos(self.yaw)
+           self.vy = velocity_msg.groundspeed * math.sin(self.yaw)
+           self.vz = velocity_msg.climb
+       else:
+           print("no Mavlink data")
+
+    def run_threaded(self):
+        return (self.roll, self.pitch, self.yaw, 
+                self.lat, self.lon, self.alt,
+                self.vx, self.vy, self.vz)
+
+    def shutdown(self):
+        self.master.close()
+        self.running = False

donkeycar/parts/serial_port.py

@@ -110,7 +110,7 @@ def read(self, count:int=0) -> Tuple[bool, str]:
         try:
             return (ok, bytestring.decode(self.charset))
         except UnicodeDecodeError:
-            # the first read often includes mis-framed garbase
+            # the first read often includes mis-framed garbage
             return (False, "")
 
     def readln(self) -> Tuple[bool, str]:
@@ -139,7 +139,7 @@ def readln(self) -> Tuple[bool, str]:
             return (False, "")
         except UnicodeDecodeError:
             # the first read often includes mis-framed garbase
-            logger.warn("failed decoding unicode line from serial port")
+            # logger.warn("failed decoding unicode line from serial port")
             return (False, "")
 
     def writeBytes(self, value:bytes):

donkeycar/templates/cfg_complete.py

@@ -347,6 +347,10 @@
 ODOM_PIN = 13                        # if using GPIO, which GPIO board mode pin to use as input
 ODOM_DEBUG = False                  # Write out values on vel and distance as it runs
 
+#MAVLINK 
+USE_MAVLINK = False                 # Set to True to use a Pixhawk-based drone flight controller for IMU and GPS
+MAVLINK_SERIAL_PORT = '/dev/ttyACM0'  # Serial port for the MAVLink connection (e.g. "/dev/ttyACM0" on a Raspberry Pi)
+
 # #LIDAR
 USE_LIDAR = False
 LIDAR_TYPE = 'RP' #(RP|YD)

donkeycar/templates/complete.py

@@ -269,6 +269,14 @@ def show_record_count_status():
     add_imu(V, cfg)
 
 
+    if cfg.USE_MAVLINK:
+        from donkeycar.parts.mavlink import MavlinkIMUPart
+        mavlink_imu = MavlinkIMUPart(connection_string=cfg.MAVLINK_SERIAL_PORT)
+        V.add(mavlink_imu, outputs=['imu_roll', 'imu_pitch', 'imu_yaw', 
+                            'lat', 'lon', 'alt', 
+                            'vx', 'vy', 'vz'], threaded=True)
+
+
     # Use the FPV preview, which will show the cropped image output, or the full frame.
     if cfg.USE_FPV:
         V.add(WebFpv(), inputs=['cam/image_array'], threaded=True)

setup.cfg

@@ -51,7 +51,7 @@ pi =
     adafruit-circuitpython-ssd1306
     adafruit-circuitpython-rplidar
     RPi.GPIO
-    tensorflow-aarch64==2.9.3
+    tensorflow-aarch64==2.15
     opencv-contrib-python
 
 nano =