Getting Started: Using Teensy 4.1 as a Companion Computer for PX4 - Part 1

This guide provides a step-by-step approach to setting up a Teensy 4.1 as a companion computer for a Detect and Avoid (DAA) application using the PX4 autopilot. Specifically, it covers:

• Installing MAVLink v2 on Teensy
• Establishing Serial Communication between Teensy 4.1 and CubeOrange+ (PX4)
• Sending a Heartbeat from Teensy to PX4 via MAVLink
• Receiving Position and Altitude Data from PX4 to Teensy

Prerequisites

Hardware

• Teensy 4.1 (or any compatible microcontroller)
• CubePilot Orange Plus (or any Pixhawk running PX4 v1.15)

Software

• Arduino IDE 2.3.4 (or newer)
• Teensy 4.1 Board Package for Arduino IDE

Linux Users (Ubuntu-specific Setup)

If using Ubuntu, you need to configure UDEV rules to allow proper serial communication. Follow the guide here:
Teensy UDEV Setup

1. Installing MAVLink v2 on Teensy

1. Open Arduino IDE
2. Go to Library Manager (Sketch → Include Library → Manage Libraries...)
3. Search for "MAVLink"
4. Click Install

✅ That’s it! MAVLink is now installed on your Teensy.

2. Wiring: Connecting Teensy 4.1 to CubeOrange+ (Serial Communication)

To establish serial communication via MAVLink, connect the Teensy 4.1 to the Cube ADS-B Carrier Board as follows:

Powering the Devices

Connect both the Teensy and Cube to your computer via USB for power.

Reference: TELEM1 of the Cube ADS-B Carrier Board connects to GND, Pin 0 (RX), and Pin 1 (TX) on Teensy 4.1.

TELEM1, TELEM2 ports of CubeOrnage Plus​

I have added colors for description only otherwise pixhawk has all black (see https://docs.px4.io/main/en/assembly/cable_wiring.html)

Next carefully connect both teensy and Cube to your computer with micro usb to give them power supply and to see the output on serial console

3. Sending heartbeat from Teensy to PX4

Up Arduino IDE and select Teensy4.1 board from the borads dropdown

In your arduino sketch paste the following snippet from mavlink v2 arduino reposioty 

#include

void setup() {
  Serial.begin(57600);
}

void loop() {
  // Send HEARTBEAT message to Serial once a second
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  mavlink_msg_heartbeat_pack(1, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0, MAV_STATE_STANDBY);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  Serial.write(buf, len);
  delay(1000);
}

Upload your code to the teensy and press reboot button on teensy 4.1 

Open your ground station I used QGroundControl > Analyze Tools > MAVLink Inspector and look for HEARTBEAT message

In order to receive HEARTBEAT from the flight controller modify arduino sketch a bit 

#include 

// Use Serial1 for MAVLink communication with flight controller
// Use Serial for debug output to your computer
void setup() {
  Serial.begin(115200);   // Debug console
  Serial1.begin(57600);   // MAVLink communication
  Serial.println("Teensy MAVLink Test");
}

void loop() {
  // Send HEARTBEAT message
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  mavlink_msg_heartbeat_pack(1, MAV_COMP_ID_PERIPHERAL, &msg, MAV_TYPE_GENERIC, MAV_AUTOPILOT_INVALID, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0, MAV_STATE_STANDBY);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  Serial1.write(buf, len);
  Serial.println("Sent heartbeat");
  
  // Check for incoming messages
  mavlink_message_t rx_msg;
  mavlink_status_t status;
  
  uint32_t start_time = millis();
  while (millis() - start_time < 1000) {  // Check for 1 second
    while (Serial1.available()) {
      uint8_t c = Serial1.read();
      if (mavlink_parse_char(MAVLINK_COMM_0, c, &rx_msg, &status)) {
        // Message received
        if (rx_msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
          mavlink_heartbeat_t heartbeat;
          mavlink_msg_heartbeat_decode(&rx_msg, &heartbeat);
          
          Serial.println("Received heartbeat from system:");
          Serial.print("  System ID: "); Serial.println(rx_msg.sysid);
          Serial.print("  Component ID: "); Serial.println(rx_msg.compid);
          Serial.print("  Type: "); Serial.println(heartbeat.type);
        }
      }
    }
  }
}

Upload the sketch again and now open up your serial console in arduino IDE and you should be able to see the heartbeat messages from PX4

4. Receiving Position and Altitude Data from PX4 to Teensy

Now the main part getting the real flight data from flight controller via MAVLink 

#include 

// Serial port configuration
#define DEBUG_SERIAL Serial    // Connected to computer
#define PX4_SERIAL Serial1     // Connected to PX4
#define DEBUG_BAUD 115200
#define PX4_BAUD 57600

// System IDs
#define TEENSY_SYS_ID 1
#define TEENSY_COMP_ID 158     // MAV_COMP_ID_PERIPHERAL
#define PX4_SYS_ID 1
#define PX4_COMP_ID 1

// Timing
#define HEARTBEAT_INTERVAL 1000    // ms
#define DATA_REQUEST_INTERVAL 500  // ms

// Data storage
float altitude_amsl = 0;      // Altitude above mean sea level (m)
float altitude_relative = 0;  // Relative altitude (m)
float position_x = 0;         // Local X position (m)
float position_y = 0;         // Local Y position (m)
float position_z = 0;         // Local Z position (m)
float battery_voltage = 0;    // Battery voltage (V)
float battery_current = 0;    // Battery current (A)
float battery_remaining = 0;  // Battery remaining (%)

// Timing variables
unsigned long last_heartbeat_time = 0;
unsigned long last_data_request_time = 0;

void setup() {
  DEBUG_SERIAL.begin(DEBUG_BAUD);
  PX4_SERIAL.begin(PX4_BAUD);
  
  delay(2000);  // Give serial ports time to initialize
  DEBUG_SERIAL.println("Teensy-PX4 MAVLink Communication Started");
  DEBUG_SERIAL.println("Requesting altitude, position, and battery data");
}

void loop() {
  // Send heartbeat periodically
  if (millis() - last_heartbeat_time >= HEARTBEAT_INTERVAL) {
    send_heartbeat();
    last_heartbeat_time = millis();
  }
  
  // Request data periodically
  if (millis() - last_data_request_time >= DATA_REQUEST_INTERVAL) {
    request_data();
    last_data_request_time = millis();
  }
  
  // Process incoming MAVLink messages
  process_mavlink_messages();
}

void send_heartbeat() {
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  
  // Pack heartbeat message (per MAVLink common message spec)
  mavlink_msg_heartbeat_pack(
    TEENSY_SYS_ID,
    TEENSY_COMP_ID,
    &msg,
    MAV_TYPE_GENERIC,            // Type of system
    MAV_AUTOPILOT_INVALID,       // Type of autopilot
    MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,  // System mode
    0,                           // Custom mode
    MAV_STATE_ACTIVE             // System state
  );
  
  // Copy message to send buffer and send
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  PX4_SERIAL.write(buf, len);
  
  DEBUG_SERIAL.println("Heartbeat sent to PX4");
}

void request_data() {
  // Request specific data from PX4 using REQUEST_MESSAGE command
  request_message(MAVLINK_MSG_ID_ALTITUDE);             // Request altitude data
  request_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED);   // Request position data
  request_message(MAVLINK_MSG_ID_BATTERY_STATUS);       // Request battery data
  
  DEBUG_SERIAL.println("Data requests sent to PX4");
}

void request_message(uint32_t message_id) {
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  
  // REQUEST_MESSAGE command (ID #512)
  mavlink_msg_command_long_pack(
    TEENSY_SYS_ID,
    TEENSY_COMP_ID,
    &msg,
    PX4_SYS_ID,
    PX4_COMP_ID,
    MAV_CMD_REQUEST_MESSAGE,  // Command ID
    0,                        // Confirmation
    message_id,               // Param1: Message ID to request
    0, 0, 0, 0, 0, 0          // Param2-7: (unused)
  );
  
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  PX4_SERIAL.write(buf, len);
}

void process_mavlink_messages() {
  mavlink_message_t msg;
  mavlink_status_t status;
  
  // Read all available bytes from PX4
  while (PX4_SERIAL.available()) {
    uint8_t c = PX4_SERIAL.read();
    
    // Try to parse the message
    if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
      // Process based on message ID
      switch (msg.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT:
          process_heartbeat(&msg);
          break;
          
        case MAVLINK_MSG_ID_ALTITUDE:
          process_altitude(&msg);
          break;
          
        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
          process_local_position(&msg);
          break;
          
        case MAVLINK_MSG_ID_BATTERY_STATUS:
          process_battery_status(&msg);
          break;
          
        default:
          // Uncomment for debugging all message types
          // DEBUG_SERIAL.print("Received message ID: ");
          // DEBUG_SERIAL.println(msg.msgid);
          break;
      }
    }
  }
}

void process_heartbeat(const mavlink_message_t* msg) {
  mavlink_heartbeat_t heartbeat;
  mavlink_msg_heartbeat_decode(msg, &heartbeat);
  
  DEBUG_SERIAL.println("PX4 Heartbeat received");
  DEBUG_SERIAL.print("  System status: ");
  DEBUG_SERIAL.println(heartbeat.system_status);
  DEBUG_SERIAL.print("  System mode: 0x");
  DEBUG_SERIAL.println(heartbeat.base_mode, HEX);
}

void process_altitude(const mavlink_message_t* msg) {
  mavlink_altitude_t altitude;
  mavlink_msg_altitude_decode(msg, &altitude);
  
  // Store altitude values
  altitude_amsl = altitude.altitude_amsl;
  altitude_relative = altitude.altitude_relative;
  
  // Print for debugging
  DEBUG_SERIAL.println("\n--- ALTITUDE DATA ---");
  DEBUG_SERIAL.print("  AMSL: ");
  DEBUG_SERIAL.print(altitude_amsl);
  DEBUG_SERIAL.println(" m");
  DEBUG_SERIAL.print("  Relative: ");
  DEBUG_SERIAL.print(altitude_relative);
  DEBUG_SERIAL.println(" m");
}

void process_local_position(const mavlink_message_t* msg) {
  mavlink_local_position_ned_t pos;
  mavlink_msg_local_position_ned_decode(msg, &pos);
  
  // Store position values
  position_x = pos.x;
  position_y = pos.y;
  position_z = pos.z;
  
  // Print for debugging
  DEBUG_SERIAL.println("\n--- POSITION DATA (NED) ---");
  DEBUG_SERIAL.print("  X: ");
  DEBUG_SERIAL.print(position_x);
  DEBUG_SERIAL.println(" m");
  DEBUG_SERIAL.print("  Y: ");
  DEBUG_SERIAL.print(position_y);
  DEBUG_SERIAL.println(" m");
  DEBUG_SERIAL.print("  Z: ");
  DEBUG_SERIAL.print(position_z);
  DEBUG_SERIAL.println(" m");
}

void process_battery_status(const mavlink_message_t* msg) {
  mavlink_battery_status_t battery;
  mavlink_msg_battery_status_decode(msg, &battery);
  
  // Store battery values
  // Voltage is in millivolts, convert to volts
  if (battery.voltages[0] != UINT16_MAX) {
    battery_voltage = battery.voltages[0] / 1000.0f;
  }
  
  // Current is in centiamperes (0.01A), convert to amperes
  battery_current = battery.current_battery / 100.0f;
  
  // Battery remaining (0-100%)
  battery_remaining = battery.battery_remaining;
  
  // Print for debugging
  DEBUG_SERIAL.println("\n--- BATTERY DATA ---");
  DEBUG_SERIAL.print("  Voltage: ");
  DEBUG_SERIAL.print(battery_voltage);
  DEBUG_SERIAL.println(" V");
  DEBUG_SERIAL.print("  Current: ");
  DEBUG_SERIAL.print(battery_current);
  DEBUG_SERIAL.println(" A");
  DEBUG_SERIAL.print("  Remaining: ");
  DEBUG_SERIAL.print(battery_remaining);
  DEBUG_SERIAL.println(" %");
}

See the flight data the arduino console!

Awesome!