Teensy4.1 and PX4 via MAVLink through Serial Communication Part 1
This guide covers installing MAVLink v2, connecting Teensy to CubeOrange+ via serial communication, sending heartbeat signals, and receiving position & altitude data from PX4. Perfect for UAV developers and PX4 enthusiasts!

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
- Open Arduino IDE
- Go to Library Manager (
Sketch
→Include Library
→Manage Libraries...
) - Search for "MAVLink"
- 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:
Teensy 4.1 | CubeOrange+ (TELEM1 Port) |
---|---|
GND | Pin 6 (GND) |
RX (Pin 0) | Pin 2 (TX) |
TX (Pin 1) | Pin 3 (RX) |
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
Pin | Signal | Volt |
---|---|---|
1 (red) | VCC | +5V |
2 (blk) | TX (OUT) | +3.3V |
3 (blk) | RX (IN) | +3.3V |
4 (blk) | CTS (IN) | +3.3V |
5 (blk) | RTS (OUT) | +3.3V |
6 (blk) | GND | GND |
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!
What's Your Reaction?






