diff --git a/donkeycar/parts/mavlink.py b/donkeycar/parts/mavlink.py new file mode 100644 index 000000000..395cceda5 --- /dev/null +++ b/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 diff --git a/donkeycar/parts/serial_port.py b/donkeycar/parts/serial_port.py index cb9b84bf6..de95169a9 100644 --- a/donkeycar/parts/serial_port.py +++ b/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): diff --git a/donkeycar/templates/cfg_complete.py b/donkeycar/templates/cfg_complete.py index 464e8837f..2ee031969 100644 --- a/donkeycar/templates/cfg_complete.py +++ b/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) diff --git a/donkeycar/templates/complete.py b/donkeycar/templates/complete.py index 32c4a7168..7869d25a4 100644 --- a/donkeycar/templates/complete.py +++ b/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) diff --git a/setup.cfg b/setup.cfg index bbfd61ad0..60a21f1dc 100644 --- a/setup.cfg +++ b/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 =