Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
hilaelb opened this issue Jun 27, 2024 · 1 comment
Labels

Comments

@hilaelb
Copy link

hilaelb commented Jun 27, 2024

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
Copy link
Author

hilaelb commented Jun 29, 2024

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

No branches or pull requests

1 participant