MAVLink between teensy 4.1 and speedybee f405 v4 stack not doing anything

ArduCopter
1

Hello,
I’m trying to learn to use MAVLink with my drone. I have arducopter installed on a speedybee f405 v4 stack, and I’m trying to communicate with a Teensy 4.1 board.

The wiring is as follows:
Teensy 4.1 → speedybee f405
vin → 5v
gnd → gnd
Rx1 → T1
Tx1 → Rx1

As for the programming, I’ve been following the code from this tutorial: MAVLink and Arduino: step by step

Here is what I have for now:

#include <Arduino.h>
#include <MAVLink.h>
#include <ardupilotmega/mavlink.h>

#define DEBUG 1

unsigned long previousMillisMAVLink = 0;
const long intervalMAVLink = 1000;

// # of heartbeats to wait before activating STREAMS from Pixhawk. 60 = one minute.
const int num_hbs = 60;

int num_hbs_pasados = num_hbs;


void Send_Data();
void Mav_Request_Data();
void Receive_Data();

void setup() {
    Serial.begin(57600);
    delay(2000);
    Serial.println("Starting MAVLink");
}

void loop() {
    Send_Data();
    Receive_Data();
}

void Send_Data() {
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];

    // Pack the message
    mavlink_msg_heartbeat_pack(1, 0, &msg, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, MAV_MODE_PREFLIGHT, 0, MAV_STATE_ACTIVE);

    // Copy the message to the send buffer
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

    unsigned long currentMillisMAVLink = millis();

    if (currentMillisMAVLink - previousMillisMAVLink >= intervalMAVLink) {
        // Modfiy the timing variables
        previousMillisMAVLink = currentMillisMAVLink;

        // Send the message
        Serial1.write(buf, len);

        // Request data from flight controller - requests data every num_hbs heartbeats, so every minute in this case
        num_hbs_pasados++;
        if (num_hbs_pasados >= num_hbs) {
            Mav_Request_Data();
            num_hbs_pasados = 0;
        }
    }
}

void Mav_Request_Data() {
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];

    const int maxStreams = 2;
    const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_EXTENDED_STATUS, MAV_DATA_STREAM_EXTRA1};
    const uint16_t MAVRates[maxStreams] = {0x02,0x05};

    for (int i=0; i < maxStreams; i++) {
        mavlink_msg_request_data_stream_pack(2, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
        uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

        Serial1.write(buf, len);
    }
}

void Receive_Data() {
    mavlink_message_t msg;
    mavlink_status_t status;

    while (Serial1.available() > 0) {
        uint8_t c = Serial1.read();

        #if DEBUG
            Serial.println("Serial is available");
        #endif

        // Try to get a new message
        if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {

            // Handle message
            switch(msg.msgid) {
                case MAVLINK_MSG_ID_HEARTBEAT: {
                    #if DEBUG
                        Serial.println("\nReceived heartbeat");
                    #endif
                }
                break;
            }
        }
    }
}

It’s very similar to the tutorial. I’ve modified the function names, and changed Serial to Serial1 for any communications between the teensy 4.1 and the speedybee flight controller.

Apart from that, it’s all very similar. There’s a few things that may be problematic. The first is what I should set for my heartbeat message and for my data request for the system Id and component Id. I couldn’t find any resources as to what you should set it to.

The big problem is that I’m receiving absolutely no response in my Receive_Data function. Nothing is happening.
I ran a basic condition, and Serial1.available() is always 0 when I run it.

Does anyone know what I’m doing wrong? Have I set my component or system ID values incorrectly, or is communicating with a speedybee and a teensy board just not possible?

Thank you for any help, I’m a bit lost here.

2

On the Speedybee side, have you verified that you know what UART port is associated with the output port attached to the Teensy?

It sounds like there’s no data coming in - check out Mission Planner->Setup->Mandatory Hardware->Serial Ports and try some different settings there?

3

I modified the settings there to the correct baudrate and to use Mavlink 1.

It alll works as it should now and I can succesfully get the attitude data.

Thank you so much, I had no idea that you had to change the settings there.

You have no idea how much this helped.

1 Like