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")
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
def main(args=None): rclpy.init(args=args) node = QGCcommNode() rclpy.spin(node) rclpy.shutdown()
if name == "main": main()
this is the messages that the QGC is receiving: (you can see that the frequency is different than what i'm sending)