Hi, i am new to MAVLink and PX4 so i decided to learn by creating little project.
I have Pixhawk4 and old STM32F411CEU6 microcontroller and i want to capture information with it.
Ofcource i read greate article by Juan Pedro López (MAVLink and Arduino: step by step). It answered to many of my questions, but still it does not work in my case…
PROBLEM:
In the end I want to control custom gimbal but for now it would be nice to just receive some simplest MSGs from Pixhawk4 in STM32F411CEU6 , as it was in the article. BUT i can not figure out why it does not work. It is not clear what exactly the problem is. All i see is that inpSerial always empty whitch means either i have a problem with STM32 or requesting messages.
The good news is that i can see Heartbeat in MissionPlaner.
MY CODE AND HARDWARE:
I write in Arduino IDE (settings for STM32 i fount in some article) and use FTDI arduino to display messages in Termit app .
#include <Arduino.h>
#include <SoftwareSerial.h> // http://arduino.cc/en/Reference/softwareSerial
#include <mavlink.h>
#define rxInp PA3
#define txInp PA2
#define rxOut PB7
#define txOut PB6
SoftwareSerial inpSerial(rxInp, txInp); //PX -> STM32
SoftwareSerial outSerial(rxOut, txOut); //STM32 ->
//------------------------------ MACROS -----------------------------
#define MAV_TYPE_GIMBAL 26 //temporary here
// Debug:
#define DEBUG_MODE
//---------------------------- VARIABLES ----------------------------
// Mavlink variables:
unsigned long previousMillisMAVLink = 0; // will store last time MAVLink was transmitted and listened
unsigned long nextIntervalMAVLink = 1000; // next interval to count
const int c_num_hbs = 60; // № of heartbeats to wait before activating STREAMS from Pixhawk. 60 = one minute.
int num_hbs_pasados = c_num_hbs;
//Sets the data rate in bits per second
const int c_connectionSpeed = 57600;
//MAVLink
int sysid = 1; ///< ID 1: id of plane (stm32 is planes` component)
int compid = 158; ///< The component sending the message
int type = MAV_TYPE_GIMBAL /*MAV_TYPE_FIXED_WING*/; ///< Gimbal
// Define the system type, in this case an airplane -> on-board controller
uint8_t system_type = MAV_TYPE_GENERIC;
uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;
uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
uint32_t custom_mode = 0; ///< Custom mode, can be defined by user/adopter
uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight
void setup()
{
pinMode(rxInp, INPUT);
pinMode(txInp, OUTPUT);
pinMode(rxOut, INPUT);
pinMode(txOut, OUTPUT);
inpSerial.begin(c_connectionSpeed);
outSerial.begin(c_connectionSpeed);
}
void loop()
{
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
//mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
mavlink_msg_heartbeat_pack(1,0, &msg, type, autopilot_type, system_mode, custom_mode, system_state); //OLD
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message with the standard UART send function
// uart0_send might be named differently depending on
// the individual microcontroller / library in use.
unsigned long currentMillisMAVLink = millis();
if (currentMillisMAVLink - previousMillisMAVLink >= nextIntervalMAVLink) {
// Save the last time you changed the mode
previousMillisMAVLink = currentMillisMAVLink;
inpSerial.write(buf,len);
#ifdef DEBUG_MODE
outSerial.println(" HB [-----SENT-----]");
#endif
num_hbs_pasados++;
if(num_hbs_pasados>=c_num_hbs) {
Mav_Request_Data();
num_hbs_pasados=0;
}
}
comm_receive();
}
//------------------------ Mav_Request_Data()------------------------
// 1 - Request from Pixhawk what DATA SETs STM32 wants;
// 2 - Request transmission rate.
void Mav_Request_Data()
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// To be setup according to the needed information to be requested from the Pixhawk
const int maxStreams = 2;
const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_ALL/*MAV_DATA_STREAM_EXTENDED_STATUS*//*, MAV_DATA_STREAM_EXTRA1*/};
const uint16_t MAVRates[maxStreams] = {0x02/*, 0x05*/}; //data rate in Hz (times per second)
for (int i=0; i < maxStreams; i++) {
//Pack
//v1
mavlink_msg_request_data_stream_pack(2, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1); //OLD
// mavlink_msg_request_data_stream_pack(sysid, compid, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
// //v2
// const uint16_t MAVStreamID= 0; //HB
// const uint32_t MAVRates = 511;
// mavlink_msg_message_interval_pack(2, 200, &msg, MAVStreamID, MAVRates);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
//Send
inpSerial.write(buf,len);
#ifdef DEBUG_MODE
outSerial.println(" MAVLink [--REQUESTED!--].");
#endif
}
}
//-------------------------- comm_receive()--------------------------
// 1 - Focus on what messages we are interested in and how to decode them
void comm_receive()
{
#ifdef DEBUG_MODE
//outSerial.println(" comm_receive() [open].");
#endif
mavlink_message_t msg;
mavlink_status_t status;
while(inpSerial.available()>0) { // PROBLEM!!!!!!
uint8_t c = inpSerial.read();
#ifdef DEBUG_MODE
outSerial.println("----while works----");
#endif
// Try to get a new message
if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
#ifdef DEBUG_MODE
outSerial.print("DEBUG msgID: ");
outSerial.println(msg.msgid);
#endif
// Handle message
switch(msg.msgid)
{
#ifdef DEBUG_MODE <TEST>
case MAVLINK_MSG_ID_HEARTBEAT: // #0:
{
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(&msg, &hb);
outSerial.print("custom_mode: ");outSerial.println(hb.custom_mode);
outSerial.print("type: ");outSerial.println(hb.type);
outSerial.print("autopilot: ");outSerial.println(hb.autopilot);
outSerial.print("base_mode: ");outSerial.println(hb.base_mode);
outSerial.print("system_status: ");outSerial.println(hb.system_status);
outSerial.print("mavlink_version: ");outSerial.println(hb.mavlink_version);
}
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: // #1: SYS_STATUS
{
mavlink_rc_channels_raw_t chs;
mavlink_msg_rc_channels_raw_decode(&msg, &chs);
outSerial.print("Roll: ");outSerial.println(chs.chan1_raw);
}
break;
case MAVLINK_MSG_ID_ATTITUDE:
{
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&msg, &attitude);
outSerial.print("PX ATTITUDE: ");
outSerial.println(attitude.roll);
}
break;
#endif
case MAVLINK_MSG_ID_COMMAND_LONG:
{
if(outSerial.available()>0)
{
//outSerial.write(c);
#ifdef DEBUG_MODE
//Tets case
outSerial.println(" Income MSG [-COMMAND_LONG-].");
#endif
}
}
break;
default:
#ifdef DEBUG_MODE
outSerial.println(" Income MSG [--NO_SIMILAR--].");
#endif
break;
}
}
}
#ifdef DEBUG_MODE
//outSerial.println(" comm_receive() [close].");
#endif
}
PHOTO:
