mavlink / qgroundcontrol

Cross-platform ground control station for drones (Android, iOS, Mac OS, Linux, Windows)
http://qgroundcontrol.io
3.08k stars 3.44k forks source link

can't connect to QGC via UDP connection - trying to pretend to be a flight stack #11638

Open hilaelb opened 1 week ago

hilaelb commented 1 week ago

My code is trying to pretend to be a flight stack and i want to connect QGC via UDP connection. i use the ardupilotmega.xml which is the defualt one and i trying to send 3 type of messages: system time status, heartbeat message (the sending is happened in def system_time_status(self)) and AHRS filter status. when i open the qgc on the mavlink inspector it sees my messages but still the main screen is disconnected. what can be the problem?

import os from .udp_server import UdpServer from .ardupilotmega import MAVLink, MAVError import random import time import math import rclpy from rclpy.node import Node

class QGCcommNode(Node): def init(self): super().init("qgc_comm")

Establishing UDP connection and creating MAVLink instances

        self.server_is_connected = True
        self.server = UdpServer(14575, self.parser, verbosity=1)  # This is assumed for all tigers
        self.mav = MAVLink(self, srcSystem=1)
        self.get_logger().info("Show me that you accepted my colcon build !!!!")

        #Create timers
        self.system_timer = self.create_timer(0.1111, self.system_time_status) #system time publishing timer
        self.ahrs_filter_timer = self.create_timer(5, self.AHRS_filter_status)

  def parser(self, input_bytes: bytes):
    """UDP callback function, handle received MAVLink messages over the UDP connection."""
    self.heartbeat_timer = int(time.time()*1e6)
    try:
        self.get_logger().info(input_bytes)
        msg_lst = self.mav.parse_buffer(input_bytes) # trying to parse the MAVLink message
        if msg_lst is not None:

            for msg in msg_lst:
                if (msg.id == 0):
                    self.qg_msg_timer = int(time.time() * 1e6)
                    self.get_logger().info("Received heartbeat from QGC")
                else:
                    self.get_logger().info(f"Received message: {msg}")

    except MAVError as e:
        self.get_logger().info(str(e))
        self.get_logger().info(input_bytes)
    except Exception as e:
        self.get_logger().info('PARSER EXCEPTION: ', str(e))

  def write(self, data: bytes):
        """
        UDP Write function.

        This function called from lar_dialect,
        when we want to send a MAVLINK massage on UPD channel,
        and calls the UdpServer write function.
        """
        # if self.server_is_connected == True:
        try:
              # DO NOT CHANGE ANY OF THE IP ADDRESSES!!! IF AN ADDITIONAL PC IS NEEDED JUST ADD A LINE AND A COMMENT!!!
              self.server.write(data, ('192.168.168.95', 14570)) # testing on Hila's computer

        except Exception as e:
              self.get_logger().info(str(e))
        finally:
              pass

  def AHRS_filter_status(self):
        # Roll, Pitch, Yaw in degrees
        roll = random.uniform(-180, 180)  # Roll can be between -180 and 180 degrees
        pitch = random.uniform(-90, 90)   # Pitch can be between -90 and 90 degrees
        yaw = random.uniform(-180, 180)   # Yaw can be between -180 and 180 degrees

        # Altitude in meters (example range from sea level to 12,000 meters)
        altitude = random.uniform(0, 12000)  

        # Latitude and Longitude
        lat = int(random.uniform(-90, 90) * 1e7)  # Latitude in the range of -90 to 90 degrees, scaled to int32
        lng = int(random.uniform(-180, 180) * 1e7)  # Longitude in the range of -180 to 180 degrees, scaled to int32

        # Call the send function with generated values
        self.mav.ahrs2_send(roll, pitch, yaw, altitude, lat, lng)
        self.get_logger().info("AHRS_filter_status has been sent")

  def system_time_status(self):
        # Generate current time in UNIX epoch time [us] (uint64_t)
        current_time_unix_sec = int(time.time())
        time_unix_usec = current_time_unix_sec * 1_000_000  # convert seconds to microseconds

        # Generate time since system boot in [ms] (uint32_t)
        # Assume a boot time up to 30 days (30 * 24 * 60 * 60 * 1000 ms)
        time_boot_ms = random.randint(0, 30 * 24 * 60 * 60 * 1000)

        # Call the send function with generated values
        self.mav.system_time_send(time_unix_usec, time_boot_ms)

        self.mav.heartbeat_send(
        type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
        autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
        base_mode=65,
        custom_mode=65536,
        system_status=mavutil.mavlink.MAV_STATE_STANDBY,
        mavlink_version=3
        )
        self.get_logger().info("system_time_status and heartbeat has been sent")

def main(args=None): rclpy.init(args=args) node = QGCcommNode() rclpy.spin(node) rclpy.shutdown()

if name == "main": main()

image

this is the messages that the QGC is receiving: (you can see that the frequency is different than what i'm sending) image

hilaelb commented 1 week ago

Another question that can help me solve the case - what do I require for a connection to be established with a generic autopilot?