Significant processing delays when receiving data

[yonatan8070]

I recently started using MAVLink in a robotics project, and I was passing IMU data using MAVLink over a direct Ethernet from a Teensy 4.1 to an NVIDIA Jetson Orin nano.

I ran some tests, and the results show that on the receiving end (the Jetson Orin Nano) there are processing delays of ~30ms
To run the test, I connected one of the Teensy's GPIO pins to the Jetson's GPIO, and used the following code to send a heartbeat message while pulsing the GPIO line:

ml->heartbeat();
Serial.println("Sent heartbeat");
digitalWrite(5, LOW);
Serial.println("Toggled GPIO");

The heartbeat() method is just a small wrapper:

void MAVLink::heartbeat() {
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  mavlink_msg_heartbeat_pack(this->system_id,
                             this->component_id,
                             &msg,
                             MAV_TYPE_GROUND_ROVER,
                             MAV_AUTOPILOT_GENERIC,
                             0,
                             0,
                             MAV_STATE_ACTIVE);

  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  this->send_buffer(buf, len);
}

The send_buffer function is also just a simple wrapper around Arduino's built-in UDP functionality

On the Jetson side, I wrote a Python script that waits for the GPIO falling edge, then measures the time until the MAVLink message is received:

#!/usr/bin/env python3

import Jetson.GPIO as GPIO
import time
import socket
from pymavlink import mavutil

GPIO_PIN = 21
GPIO.setmode(GPIO.BCM)
GPIO.setup(GPIO_PIN, GPIO.IN)

mavlink_connection = mavutil.mavlink_connection('udpin:0.0.0.0:14550')

#sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
#sock.bind(("", 14550))

def wait_for_heartbeat():
    while True:
        msg = mavlink_connection.recv_match(blocking=False)
        # msg, addr = sock.recvfrom(1024)
        if msg:
            return time.time()

try:
    # Wait for GPIO to go LOW
    GPIO.wait_for_edge(GPIO_PIN, GPIO.FALLING)
    start_time = time.time()
    
    # Measure time to first MAVLink heartbeat
    heartbeat_time = wait_for_heartbeat()
    elapsed_time = heartbeat_time - start_time
    
    print(f"Time from GPIO LOW to MAVLink heartbeat: {elapsed_time * 1000:.4f}ms")
except KeyboardInterrupt:
    pass
finally:
    GPIO.cleanup()

When commenting out the MAVLink code and uncommenting the raw UDP code in this script, the time drops from ~30ms to ~0.5ms. Am I doing something wrong on the receive code that causes such a massive delay? Or is this an fundamental limitation of pymavlink?