STM32控制APM飞控（三）MAVLINK整合并适配stm32串口的收发

最新推荐文章于 2023-04-23 10:14:50 发布

星之河泛孤舟

最新推荐文章于 2023-04-23 10:14:50 发布

阅读量4.9k

点赞数 5

分类专栏： APM飞控文章标签： MAVLINK APM STM32 无人机

本文链接：https://blog.csdn.net/u011919377/article/details/78415507

版权

stm32底层串口代码更改能收发MAVLINK协议包

一.在上一次移植好的工程基础上进行如下改动

1.将两个文件mavlink_usart_fifo.h mavlink_usart_fifo.c添加到工程里（都是关于缓冲区的底层串口加缓冲区函数）

2.usart.c中屏蔽USART1_IRQHandler函数，而正点原子的usart.c全代码为

3.添加mavlink_avoid_errors.h里面的代码如下,这个代码是用来避免错误的跟mdk编译器相关的

4.添加open_tel_mavlink.h和open_tel_mavlink.c，这两个函数是用来测试mavlink通信的上层代码

5.添加define.h函数，这里是上层函数的一些结构类型定义，为避免上层报错就添加上(有兴趣可以自己精简)

6.主函数也有更改，这里是main.c所有代码

7.mavlink_helpers.h，在以前基础上增加如下代码(如有重复函数，请屏蔽以前的函数)

8.checksum未经任何修改

9.mavlink_conversions.h修改了一个地方就是将参数定义提到了前面）

10.mavlink_types.h跟源代码相比较屏蔽了下面这段代码

11.protocol.h未做任何修改，附上全代码

12.其他代码均未修改

二：实验测试代码是否成功移植

1.测试开发板发送功能然后重启或者复位开发板，会有数据返回（注意：选中16进制显示）。如图收到如图数据表示发送移植成功。复位一次返回一次同样的数据

2.测试开发板接收功能

三：分析数据

stm32底层串口代码更改能收发MAVLINK协议包

一.在上一次移植好的工程基础上进行如下改动

1.将两个文件mavlink_usart_fifo.h mavlink_usart_fifo.c添加到工程里（都是关于缓冲区的底层串口加缓冲区函数）

mavlink_usart_fifo.h文件内容如下

#ifndef _USART_FIFO_H_
#define _USART_FIFO_H_
#include "stdint.h" 
#define true 1
#define false 0
typedef struct _fifo 
{
    uint8_t* buf;
    uint16_t length;
    uint16_t head;
    uint16_t tail;
} fifo_t;
uint8_t fifo_read_ch(fifo_t* fifo, uint8_t* ch);
uint8_t fifo_write_ch(fifo_t* fifo, uint8_t ch);
uint16_t fifo_free(fifo_t* fifo);
uint16_t fifo_used(fifo_t* fifo);
void fifo_init(fifo_t* fifo, uint8_t* buf, uint16_t length);
uint8_t serial_write_buf(uint8_t* buf, uint16_t length);
uint8_t serial_read_ch(void);
uint16_t serial_free(void);
uint16_t serial_available(void);
#endif  /*_USART_FIFO_H_*/

mavlink_usart_fifo.c文件内容如下

#include "sys.h"
#include "mavlink_usart_fifo.h"
#define UART_TX_BUFFER_SIZE 255
#define UART_RX_BUFFER_SIZE 255
fifo_t uart_rx_fifo, uart_tx_fifo;
uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE], uart_rx_buf[UART_RX_BUFFER_SIZE];
/** @brief 读FIFO
* @param fifo 待读缓冲区
* *ch 读到的数据
* @return
* 正确读取，1；无数据，0
*/
uint8_t fifo_read_ch(fifo_t* fifo, uint8_t* ch)
{
if(fifo->tail == fifo->head) return false;
*ch = fifo->buf[fifo->tail];

if(++fifo->tail >= fifo->length) fifo->tail = 0;
return true;
}
/** @brief 写一字节数据到FIFO
* @param fifo 待写入缓冲区
* ch 待写入的数据
* @return
* 正确，1；缓冲区满，0
*/
uint8_t fifo_write_ch(fifo_t* fifo, uint8_t ch)
{
uint16_t h = fifo->head;

if(++h >= fifo->length) h = 0;
if(h == fifo->tail) return false;

fifo->buf[fifo->head] = ch;
fifo->head = h;
return true;
}
/** @brief 返回缓冲区剩余字节长度
* @param fifo
* @return
* 剩余空间
*
* @note 剩余字节长度大于等于2时，才可写入数据
*/
uint16_t fifo_free(fifo_t* fifo)
{
uint16_t free;

if(fifo->head >= fifo->tail) free = fifo->tail + (fifo->length - fifo->head);
else free = fifo->tail - fifo->head;

return free;
}
uint16_t fifo_used(fifo_t* fifo)
{
uint16_t used;

if(fifo->head >= fifo->tail) used = fifo->head - fifo->tail;
else used = fifo->head + (fifo->length - fifo->tail);

return used;
}
/** @brief 初始化缓冲区
* @param *fifo
* *buf
* length
*/
void fifo_init(fifo_t* fifo, uint8_t* buf, uint16_t length)
{
uint16_t i;

fifo->buf = buf;
fifo->length = length;
fifo->head = 0;
fifo->tail = 0;

for(i=0; i<length; i++) fifo->buf[i] = 0;
}
/** @brief 写数据到串口，启动发射
*
* @note 数据写入发射缓冲区后，启动发射中断，在中断程序，数据自动发出
*/
uint8_t serial_write_buf(uint8_t* buf, uint16_t length) {
uint16_t i;

if(length == 0) return false;
for(i = 0; length > 0; length--, i++) {
fifo_write_ch(&uart_tx_fifo, buf[i]);
}
USART_ITConfig(USART1, USART_IT_TXE, ENABLE);

return true;
}
/** @brief 自串口读数据
* @return 一字节数据
*/
uint8_t serial_read_ch(void){
uint8_t ch;
fifo_read_ch(&uart_rx_fifo, &ch);
return ch;
}
/** @breif 检测发射缓冲区剩余字节长度
* @return 剩余字节长度
*/
uint16_t serial_free(void){
return fifo_free(&uart_tx_fifo);
}
uint16_t serial_available(void){
return fifo_used(&uart_rx_fifo);
}
void USART1_IRQHandler(void)
{
uint8_t c;
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)//数据接收终端
{
c = USART_ReceiveData(USART1);
fifo_write_ch(&uart_rx_fifo, c);
//USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);
}
if(USART_GetITStatus(USART1, USART_IT_TXE) != RESET)//数据发送中断
{
if(fifo_read_ch(&uart_tx_fifo, &c))
USART_SendData(USART1, c);
else
USART_SendData(USART1, 0x55);
if (fifo_used(&uart_tx_fifo) == 0) // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
{
// Disable the EVAL_COM1 Transmit interrupt
USART_ITConfig(USART1, USART_IT_TXE, DISABLE);
}
}
}

2.usart.c中屏蔽USART1_IRQHandler函数，而正点原子的usart.c全代码为

#include "sys.h"
#include "usart.h"	
// 	 
//如果使用ucos,则包括下面的头文件即可.
#if SYSTEM_SUPPORT_OS
#include "includes.h"					//ucos 使用	  
#endif
//	 
//本程序只供学习使用，未经作者许可，不得用于其它任何用途
//ALIENTEK STM32F4探索者开发板
//串口1初始化		   
//正点原子@ALIENTEK
//技术论坛:www.openedv.com
//修改日期:2014/6/10
//版本：V1.5
//版权所有，盗版必究。
//Copyright(C) 广州市星翼电子科技有限公司 2009-2019
//All rights reserved
//********************************************************************************
//V1.3修改说明 
//支持适应不同频率下的串口波特率设置.
//加入了对printf的支持
//增加了串口接收命令功能.
//修正了printf第一个字符丢失的bug
//V1.4修改说明
//1,修改串口初始化IO的bug
//2,修改了USART_RX_STA,使得串口最大接收字节数为2的14次方
//3,增加了USART_REC_LEN,用于定义串口最大允许接收的字节数(不大于2的14次方)
//4,修改了EN_USART1_RX的使能方式
//V1.5修改说明
//1,增加了对UCOSII的支持
// 	  
 
//
//加入以下代码,支持printf函数,而不需要选择use MicroLIB	  
#if 1
#pragma import(__use_no_semihosting)             
//标准库需要的支持函数                 
struct __FILE 
{ 
	int handle; 
}; 
FILE __stdout;       
//定义_sys_exit()以避免使用半主机模式    
_sys_exit(int x) 
{ 
	x = x; 
} 
//重定义fputc函数 
int fputc(int ch, FILE *f)
{ 	
	while((USART1->SR&0X40)==0);//循环发送,直到发送完毕   
	USART1->DR = (u8) ch;      
	return ch;
}
#endif
 
#if EN_USART1_RX   //如果使能了接收
//串口1中断服务程序
//注意,读取USARTx->SR能避免莫名其妙的错误   	
u8 USART_RX_BUF[USART_REC_LEN];     //接收缓冲,最大USART_REC_LEN个字节.
//接收状态
//bit15，	接收完成标志
//bit14，	接收到0x0d
//bit13~0，	接收到的有效字节数目
u16 USART_RX_STA=0;       //接收状态标记	
//初始化IO 串口1 
//bound:波特率
void uart_init(u32 bound){
   //GPIO端口设置
  GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
 
	//串口1对应引脚复用映射
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
	
	//USART1端口配置
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;	//速度50MHz
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
	GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9，PA10
   //USART1 初始化设置
	USART_InitStructure.USART_BaudRate = bound;//波特率设置
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
	USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
	USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//收发模式
  USART_Init(USART1, &USART_InitStructure); //初始化串口1
	
  USART_Cmd(USART1, ENABLE);  //使能串口1 
	
	//USART_ClearFlag(USART1, USART_FLAG_TC);
	
#if EN_USART1_RX	
	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
	//Usart1 NVIC 配置
  NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
	NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;		//子优先级3
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			//IRQ通道使能
	NVIC_Init(&NVIC_InitStructure);	//根据指定的参数初始化VIC寄存器、
#endif
	
}
//void USART1_IRQHandler(void)                	//串口1中断服务程序
//{
//	u8 Res;
//#if SYSTEM_SUPPORT_OS 		//如果SYSTEM_SUPPORT_OS为真，则需要支持OS.
//	OSIntEnter();    
//#endif
//	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
//	{
//		Res =USART_ReceiveData(USART1);//(USART1->DR);	//读取接收到的数据
//		
//		if((USART_RX_STA&0x8000)==0)//接收未完成
//		{
//			if(USART_RX_STA&0x4000)//接收到了0x0d
//			{
//				if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
//				else USART_RX_STA|=0x8000;	//接收完成了 
//			}
//			else //还没收到0X0D
//			{	
//				if(Res==0x0d)USART_RX_STA|=0x4000;
//				else
//				{
//					USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
//					USART_RX_STA++;
//					if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收	  
//				}		 
//			}
//		}   		 
//  } 
//#if SYSTEM_SUPPORT_OS 	//如果SYSTEM_SUPPORT_OS为真，则需要支持OS.
//	OSIntExit();  											 
//#endif
//} 
#endif

3.添加mavlink_avoid_errors.h里面的代码如下,这个代码是用来避免错误的跟mdk编译器相关的

#ifndef MAVLINK_AVOID_ERRORS_H
#define MAVLINK_AVOID_ERRORS_H
#include "stdio.h"
#include "stdint.h"
/*解决..\MAVLINK\common\../mavlink_types.h(53): error: #20: identifier "pack" is undefined*/
#define MAVPACKED( __Declaration__ ) __Declaration__
/*解决..\MAVLINK\common\../mavlink_types.h(53): error: #3092: anonymous unions are only supported in --gnu mode, or when enabled with #pragma anon_unions*/
#pragma anon_unions
#define inline __inline
//#ifndef memset//由王朔添加 2018-08-24
//void *memset(void *dest, int data, size_t length) {
// uint32_t i;
// int *point = dest;
// for(i=0; i<length; i++) point[i] = data;
// return dest;
//}
//#endif
//#ifndef memcpy//由王朔添加 2018-08-24
//void *memcpy(void *dest, const void *src, size_t n)
//{
// unsigned char *pout = (unsigned char*)dest;
// unsigned char *pin = (unsigned char*)src;
// while (n-- > 0) *pout++ = *pin++;
// return dest;
//}
//#endif
#include "mavlink_types.h"
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
//#define MAVLINK_SEPARATE_HELPERS
//mavlink_system_t mavlink_system = {0,0};
//mavlink_system_t mavlink_system = {
// 1,
// 1
//}; // System ID, 1-255, Component/Subsystem ID, 1-255
//void comm_send_ch(mavlink_channel_t chan, uint8_t buf)
//{
// chan=chan;
// USART_SendData(USART1, buf); //向串口1发送数据
// while(USART_GetFlagStatus(USART1,USART_FLAG_TC)!=SET);//等待发送结束
//}
#include "mavlink.h"
#include "mavlink_helpers.h"
#endif //AVLINK_AVOID_ERRORS_H

4.添加open_tel_mavlink.h和open_tel_mavlink.c，这两个函数是用来测试mavlink通信的上层代码

open_tel_mavlink.h如下

#ifndef __OPEN_TEL_MAVLINK_H
#define __OPEN_TEL_MAVLINK_H
//#include "./minimal/minimal/minimal.h"
#include "define.h"
#include "mavlink_avoid_errors.h"
#include "stdint.h"
void mavlink_send_message(mavlink_channel_t chan,enum ap_message id,uint16_t packet_drops);
void update(void);
void handleMessage(mavlink_message_t* msg);
#endif /*__OPENTEL_MAVLINK_H*/

open_tel_mavlink.c代码如下:

#include "open_tel_mavlink.h"
#include "mavlink_usart_fifo.h"
//#include "mavlink_avoid_errors.h"
//#include "mavlink_types.h"
//#include "mavlink_avoid_errors.h"
#include "define.h"
#include "stdint.h"
Add By BigW
typedef uint8_t bool;
//typedef struct {
// char c;
//} prog_char_t;
//
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
static int8_t control_mode = STABILIZE;
mavlink_channel_t chan;
//uint16_t packet_drops;
mavlink_heartbeat_t heartbeat;
mavlink_attitude_t attitude;
mavlink_global_position_int_t position;
//mavlink_ahrs_t ahrs;
uint8_t buf[100];
End Add By BigW
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// this costs us 51 bytes, but means that low priority
// messages don't block the CPU
static mavlink_statustext_t pending_status;
// true when we have received at least 1 MAVLink packet
static bool mavlink_active;
// check if a message will fit in the payload space available
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false
void handleMessage(mavlink_message_t* msg);
/*
* !!NOTE!!
*
* the use of NOINLINE separate functions for each message type avoids
* a compiler bug in gcc that would cause it to use far more stack
* space than is needed. Without the NOINLINE we use the sum of the
* stack needed for each message type. Please be careful to follow the
* pattern below when adding any new messages
*/
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
// work out the base_mode. This value is not very useful
// for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about
// what the MAV is up to. The actual bit values are highly
// ambiguous for most of the APM flight modes. In practice, you
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
switch (control_mode) {
case AUTO:
case RTL:
case LOITER:
case GUIDED:
case CIRCLE:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break;
}
// all modes except INITIALISING have some form of manual
// override if stick mixing is enabled
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
#if HIL_MODE != HIL_MODE_DISABLED
base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
#endif
// we are armed if we are not initialising
if (0){//motors.armed()) {
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
// indicate we have set a custom mode
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
mavlink_msg_heartbeat_send(
chan,
MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_ARDUPILOTMEGA,
base_mode,
custom_mode,
system_status);
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
{
mavlink_msg_attitude_send(
chan,
++buf[1],//millis(),
++buf[2],//ahrs.roll,
++buf[3],//ahrs.pitch,
++buf[4],//ahrs.yaw,
++buf[5],//omega.x,
++buf[6],//omega.y,
++buf[7]);//omega.z);
}

static void NOINLINE send_location(mavlink_channel_t chan)
{
//Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
1,//millis(),
2,//current_loc.lat, // in 1E7 degrees
3,//current_loc.lng, // in 1E7 degrees
4,//g_gps->altitude * 10, // millimeters above sea level
5,//(current_loc.alt - home.alt) * 10, // millimeters above ground
6,//g_gps->ground_speed * rot.a.x, // X speed cm/s
7,//g_gps->ground_speed * rot.b.x, // Y speed cm/s
8,//g_gps->ground_speed * rot.c.x,
9);//g_gps->ground_course); // course in 1/100 degree
}
//static void NOINLINE send_ahrs(mavlink_channel_t chan)
//{
// //Vector3f omega_I = ahrs.get_gyro_drift();
// mavlink_msg_ahrs_send(
// chan,
// ++buf[8],//omega_I.x,
// ++buf[9],//omega_I.y,
// ++buf[10],//omega_I.z,
// 1,
// 0,
// ++buf[11],//ahrs.get_error_rp(),
// ++buf[12]);//ahrs.get_error_yaw());
//}
static void NOINLINE send_statustext(mavlink_channel_t chan)
{
}
// are we still delaying telemetry to try to avoid Xbee bricking?
static bool telemetry_delayed(mavlink_channel_t chan)
{
return false;
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
int16_t payload_space = serial_free();
if (telemetry_delayed(chan)) {
return false;
}
switch(id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;

// case MSG_AHRS:
// CHECK_PAYLOAD_SIZE(AHRS);
// send_ahrs(chan);
// break;
case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE(STATUSTEXT);
send_statustext(chan);
break;
default:
break;
}
return true;
}
#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
static struct mavlink_queue {
enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[2];
// send a message using mavlink
void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
uint8_t i, nextid;
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message(chan,
q->deferred_messages[q->next_deferred_message],
packet_drops)) {
break;
}
q->next_deferred_message++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
q->next_deferred_message = 0;
}
q->num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
if (q->deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES) {
nextid = 0;
}
}
if (q->num_deferred_messages != 0 ||
!mavlink_try_send_message(chan, id, packet_drops)) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferred_message + q->num_deferred_messages;
if (nextid >= MAX_DEFERRED_MESSAGES) {
nextid -= MAX_DEFERRED_MESSAGES;
}
q->deferred_messages[nextid] = id;
q->num_deferred_messages++;
}
}
void mavlink_send_text(mavlink_channel_t chan, enum gcs_severity severity, char *str)
{
if (telemetry_delayed(chan)) {
return;
}
if (severity == SEVERITY_LOW) {
// send via the deferred queuing system
pending_status.severity = (uint8_t)severity;
mav_array_memcpy((char *)pending_status.text, str, sizeof(pending_status.text));
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
} else {
// send immediately
mavlink_msg_statustext_send(
chan,
severity,
str);
}
}
void update(void)
{
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
status.packet_rx_drop_count = 0;
// process received bytes
while(serial_available())
{
uint8_t c = serial_read_ch();
// Try to get a new message
if (mavlink_parse_char(chan, c, &msg, &status)) {
mavlink_active = true;

//printf("%c",c);
printf("Received message with ID %d, sequence: %d from component %d of system %d",\
msg.msgid, msg.seq, msg.compid, msg.sysid);
handleMessage(&msg);
}
}
}
void handleMessage(mavlink_message_t* msg)
{
//struct Location tell_command = {}; // command for telemetry
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_msg_heartbeat_decode(msg, &he

最低0.47元/天解锁文章

星之河泛孤舟

关注

5
点赞
踩
14

收藏

觉得还不错? 一键收藏
0
评论
STM32控制APM飞控（三）MAVLINK整合并适配stm32串口的收发

把mavlink协议真正放入到stm32工程中，能够用stm32串口按MAVLINK协议发送数据和接受APM飞控返回的MAVLINK信息报并解析出想要的信息
复制链接

扫一扫