I’m trying to learn to use mavlink with my speedybee f405. I have it connected to a teensy 4.1, and, as of yet, I’ve managed to get attitude data from it succesfully.
However, I’m trying to send messages to it to control it, but my code doesn’t appear to do anything.
Here is the code responsible for the motor movement:
void Control_Motors() {
// for testing, set thrust to 10%
mavlink_manual_control_t manual_control;
manual_control.x = 0;
manual_control.y = 0;
manual_control.z = 100;
manual_control.r = 0;
manual_control.s = 0;
manual_control.t = 0;
mavlink_message_t msg;
mavlink_msg_manual_control_pack(1, 200, &msg, 2, manual_control.x, manual_control.y, manual_control.z, manual_control.r, 0, 0, 0, manual_control.s, manual_control.t, 0, 0, 0, 0, 0, 0);
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
Serial1.write(buf, len);
}
I’m having a very hard time understanding what messages I’m supposed to send, as the only documentation I can find is in the mavlink website under messages.
Is there any other documentation that I’m not aware of?
For anyone curious as to the rest of my code, here it is:
#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;
mavlink_attitude_t attitude;
void Send_Data();
void Mav_Request_Data();
void Receive_Data();
void Control_Motors();
void setup() {
Serial.begin(57600);
Serial1.begin(115200);
delay(2000);
Serial.println("Starting MAVLink");
}
void loop() {
Send_Data();
Receive_Data();
Control_Motors();
}
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();
// 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;
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t sys_status;
mavlink_msg_sys_status_decode(&msg, &sys_status);
// #if DEBUG
// Serial.println("\nReceived SYS_STATUS");
// Serial.print("Voltage: ");
// Serial.println(sys_status.voltage_battery);
// #endif
}
break;
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t param_value;
mavlink_msg_param_value_decode(&msg, ¶m_value);
#if DEBUG
Serial.println("\nReceived PARAM_VALUE");
Serial.print("Param ID: ");
Serial.println(param_value.param_id);
Serial.print("Param Value: ");
Serial.println(param_value.param_value);
#endif
}
break;
case MAVLINK_MSG_ID_RAW_IMU: {
mavlink_raw_imu_t raw_imu;
mavlink_msg_raw_imu_decode(&msg, &raw_imu);
#if DEBUG
Serial.println("\nReceived RAW_IMU");
Serial.print("Accel X: ");
Serial.println(raw_imu.xacc);
#endif
}
break;
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_msg_attitude_decode(&msg, &attitude);
#if DEBUG
Serial.print("Roll: ");
Serial.println(attitude.roll);
Serial.print("Pitch: ");
Serial.println(attitude.pitch);
Serial.print("Yaw: ");
Serial.println(attitude.yaw);
#endif
}
break;
}
}
}
}
void Control_Motors() {
// for testing, set thrust to 10%
mavlink_manual_control_t manual_control;
manual_control.x = 0;
manual_control.y = 0;
manual_control.z = 100;
manual_control.r = 0;
manual_control.s = 0;
manual_control.t = 0;
mavlink_message_t msg;
mavlink_msg_manual_control_pack(1, 200, &msg, 2, manual_control.x, manual_control.y, manual_control.z, manual_control.r, 0, 0, 0, manual_control.s, manual_control.t, 0, 0, 0, 0, 0, 0);
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
Serial1.write(buf, len);
}
I had seen in the documentation a command to request control of the vehicle, but all of my attempts to implement it seemed to not do anything either. Does anyone know of what I’m doing wrong or of any example code of people controlling drones with Mavlink, preferably with a Teensy board or something of the sort? I can’t find anything on google or the mavlink website.
Any help is really appreciated.
