移植MAVLINK到STM32详细教程之三

在前面教程的基础上继续移植优化，之前的没有加缓冲区，没有接收函数功能，这里进行统一的讲解作者：恒久力行qq:624668529缓冲区对于接受来说很有必要，为了数据的稳定性和实时性必须要加上缓冲。没有缓冲很容易造成数据丢失

一：利用之前移植好的工程，在其基础上进行改动

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

mavlink_usart_fifo.h

#ifndef_USART_FIFO_H_//作者：恒久力行qq:624668529

#define_USART_FIFO_H_

#include"stdint.h"

#definetrue1

#definefalse0

typedefstruct_fifo{

uint8_t*buf;

uint16_tlength;

uint16_thead;

uint16_ttail;

}fifo_t;

uint8_tfifo_read_ch(fifo_t*fifo,uint8_t*ch);

uint8_tfifo_write_ch(fifo_t*fifo,uint8_tch);

uint16_tfifo_free(fifo_t*fifo);

uint16_tfifo_used(fifo_t*fifo);

voidfifo_init(fifo_t*fifo,uint8_t*buf,uint16_tlength);

uint8_tserial_write_buf(uint8_t*buf,uint16_tlength);

uint8_tserial_read_ch(void);

uint16_tserial_free(void);

uint16_tserial_available(void);

#endif/*_USART_FIFO_H_*/

[/code]
mavlink_usart_fifo.c

#include"sys.h"//作者：恒久力行qq:624668529

#include"mavlink_usart_fifo.h"

#defineUART_TX_BUFFER_SIZE255

#defineUART_RX_BUFFER_SIZE255

fifo_tuart_rx_fifo,uart_tx_fifo;

uint8_tuart_tx_buf[UART_TX_BUFFER_SIZE],uart_rx_buf[UART_RX_BUFFER_SIZE];

/**@brief读FIFO

*@paramfifo待读缓冲区

**ch读到的数据

*@return

*正确读取，1；无数据，0

*/

uint8_tfifo_read_ch(fifo_t*fifo,uint8_t*ch)

{

if(fifo->tail==fifo->head)returnfalse;

*ch=fifo->buf[fifo->tail];

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

returntrue;

}

/**@brief写一字节数据到FIFO

*@paramfifo待写入缓冲区

*ch待写入的数据

*@return

*正确，1；缓冲区满，0

*/

uint8_tfifo_write_ch(fifo_t*fifo,uint8_tch)

{

uint16_th=fifo->head;

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

if(h==fifo->tail)returnfalse;

fifo->buf[fifo->head]=ch;

fifo->head=h;

returntrue;

}

/**@brief返回缓冲区剩余字节长度

*@paramfifo

*@return

*剩余空间

*

*@note剩余字节长度大于等于2时，才可写入数据

*/

uint16_tfifo_free(fifo_t*fifo)

{

uint16_tfree;

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

elsefree=fifo->tail-fifo->head;

returnfree;

}

uint16_tfifo_used(fifo_t*fifo)

{

uint16_tused;

if(fifo->head>=fifo->tail)used=fifo->head-fifo->tail;

elseused=fifo->head+(fifo->length-fifo->tail);

returnused;

}

/**@brief初始化缓冲区

*@param*fifo

**buf

*length

*/

voidfifo_init(fifo_t*fifo,uint8_t*buf,uint16_tlength)

{

uint16_ti;

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_tserial_write_buf(uint8_t*buf,uint16_tlength){

uint16_ti;

if(length==0)returnfalse;

for(i=0;length>0;length--,i++){

fifo_write_ch(&uart_tx_fifo,buf[i]);

}

USART_ITConfig(USART1,USART_IT_TXE,ENABLE);

returntrue;

}

/**@brief自串口读数据

*@return一字节数据

*/

uint8_tserial_read_ch(void){

uint8_tch;

fifo_read_ch(&uart_rx_fifo,&ch);

returnch;

}

/**@breif检测发射缓冲区剩余字节长度

*@return剩余字节长度

*/

uint16_tserial_free(void){

returnfifo_free(&uart_tx_fifo);

}

uint16_tserial_available(void){

returnfifo_used(&uart_rx_fifo);

}

voidUSART1_IRQHandler(void)

{

uint8_tc;

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)//Checkifalldataistransmitted.ifyesdisabletransmitterUDREinterrupt

{

//DisabletheEVAL_COM1Transmitinterrupt

USART_ITConfig(USART1,USART_IT_TXE,DISABLE);

}

}

}

[/code]

2.在usart.c中屏蔽USART1_IRQHandler函数
usart.c全代码

#include"sys.h"//作者：恒久力行qq:624668529

#include"usart.h"

//////////////////////////////////////////////////////////////////////////////////

//如果使用ucos,则包括下面的头文件即可.

#ifSYSTEM_SUPPORT_OS

#include"includes.h"//ucos使用

#endif

//////////////////////////////////////////////////////////////////////////////////

//本程序只供学习使用，未经作者许可，不得用于其它任何用途

//ALIENTEKSTM32F4探索者开发板

//串口1初始化

//正点原子@ALIENTEK

//技术论坛:www.openedv.com

//修改日期:2014/6/10

//版本：V1.5

//版权所有，盗版必究。

//Copyright(C)广州市星翼电子科技有限公司2009-2019

//Allrightsreserved

//********************************************************************************

//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函数,而不需要选择useMicroLIB

#if1

#pragmaimport(__use_no_semihosting)

//标准库需要的支持函数

struct__FILE

{

inthandle;

};

FILE__stdout;

//定义_sys_exit()以避免使用半主机模式

_sys_exit(intx)

{

x=x;

}

//重定义fputc函数

intfputc(intch,FILE*f)

{

while((USART1->SR&0X40)==0);//循环发送,直到发送完毕

USART1->DR=(u8)ch;

returnch;

}

#endif

#ifEN_USART1_RX//如果使能了接收

//串口1中断服务程序

//注意,读取USARTx->SR能避免莫名其妙的错误

u8USART_RX_BUF[USART_REC_LEN];//接收缓冲,最大USART_REC_LEN个字节.

//接收状态

//bit15，接收完成标志

//bit14，接收到0x0d

//bit13~0，接收到的有效字节数目

u16USART_RX_STA=0;//接收状态标记

//初始化IO串口1

//bound:波特率

voiduart_init(u32bound){

//GPIO端口设置

GPIO_InitTypeDefGPIO_InitStructure;

USART_InitTypeDefUSART_InitStructure;

NVIC_InitTypeDefNVIC_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);

#ifEN_USART1_RX

USART_ITConfig(USART1,USART_IT_RXNE,ENABLE);//开启相关中断

//Usart1NVIC配置

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

}

//voidUSART1_IRQHandler(void)//串口1中断服务程序

//{

//u8Res;

//#ifSYSTEM_SUPPORT_OS//如果SYSTEM_SUPPORT_OS为真，则需要支持OS.

//OSIntEnter();

//#endif

//if(USART_GetITStatus(USART1,USART_IT_RXNE)!=RESET)//接收中断(接收到的数据必须是0x0d0x0a结尾)

//{

//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;//接收错误,重新开始

//elseUSART_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;//接收数据错误,重新开始接收

//}

//}

//}

//}

//#ifSYSTEM_SUPPORT_OS//如果SYSTEM_SUPPORT_OS为真，则需要支持OS.

//OSIntExit();

//#endif

//}

#endif

[/code]

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

/**@filemavlink_avoid_errors.h//作者：恒久力行qq:624668529

*@简介：本文件是由王朔添加，用来统一解决mavlink报错信息

*@seeQQ624668529

*/

#ifndefMAVLINK_AVOID_ERRORS_H

#defineMAVLINK_AVOID_ERRORS_H

#include"stdio.h"

#include"stdint.h"

/*解决..\MAVLINK\common\../mavlink_types.h(53):error:#20:identifier"pack"isundefined*/

#defineMAVPACKED(__Declaration__)__Declaration__

/*解决..\MAVLINK\common\../mavlink_types.h(53):error:#3092:anonymousunionsareonlysupportedin--gnumode,orwhenenabledwith#pragmaanon_unions*/

#pragmaanon_unions

#defineinline__inline

//#ifndefmemset//由王朔添加2018-08-24

//void*memset(void*dest,intdata,size_tlength){

//uint32_ti;

//int*point=dest;

//for(i=0;i<length;i++)point[i]=data;

//returndest;

//}

//#endif

//#ifndefmemcpy//由王朔添加2018-08-24

//void*memcpy(void*dest,constvoid*src,size_tn)

//{

//unsignedchar*pout=(unsignedchar*)dest;

//unsignedchar*pin=(unsignedchar*)src;

//while(n-->0)*pout++=*pin++;

//returndest;

//}

//#endif

#include"mavlink_types.h"

#defineMAVLINK_USE_CONVENIENCE_FUNCTIONS

//#defineMAVLINK_SEPARATE_HELPERS

//mavlink_system_tmavlink_system={0,0};

//mavlink_system_tmavlink_system={

//1,

//1

//};//SystemID,1-255,Component/SubsystemID,1-255

//voidcomm_send_ch(mavlink_channel_tchan,uint8_tbuf)

//{

//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

[/code]

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

open_tel_mavlink.h

#ifndef__OPEN_TEL_MAVLINK_H

//作者：恒久力行qq:624668529

#define__OPEN_TEL_MAVLINK_H

//#include"./minimal/minimal/minimal.h"

#include"define.h"

#include"mavlink_avoid_errors.h"

#include"stdint.h"

voidmavlink_send_message(mavlink_channel_tchan,enumap_messageid,uint16_tpacket_drops);

voidupdate(void);

voidhandleMessage(mavlink_message_t*msg);

#endif/*__OPENTEL_MAVLINK_H*/

[/code]open_tel_mavlink.c

#include"open_tel_mavlink.h"

//作者：恒久力行qq:624668529

////#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"

////AddByBigW

typedefuint8_tbool;

//typedefstruct{

//charc;

//}prog_char_t;

//

//Thisisthestateoftheflightcontrolsystem

//TherearemultiplestatesdefinedsuchasSTABILIZE,ACRO,

staticint8_tcontrol_mode=STABILIZE;

mavlink_channel_tchan;

//uint16_tpacket_drops;

mavlink_heartbeat_theartbeat;

mavlink_attitude_tattitude;

mavlink_global_position_int_tposition;

//mavlink_ahrs_tahrs;

uint8_tbuf[100];

////EndAddByBigW

////-*-tab-width:4;Mode:C++;c-basic-offset:4;indent-tabs-mode:nil-*-

//thiscostsus51bytes,butmeansthatlowpriority

//messagesdon'tblocktheCPU

staticmavlink_statustext_tpending_status;

//truewhenwehavereceivedatleast1MAVLinkpacket

staticboolmavlink_active;

//checkifamessagewillfitinthepayloadspaceavailable

#defineCHECK_PAYLOAD_SIZE(id)if(payload_space<MAVLINK_MSG_ID_##id##_LEN)returnfalse

voidhandleMessage(mavlink_message_t*msg);

/*

*!!NOTE!!

*

*theuseofNOINLINEseparatefunctionsforeachmessagetypeavoids

*acompilerbugingccthatwouldcauseittousefarmorestack

*spacethanisneeded.WithouttheNOINLINEweusethesumofthe

*stackneededforeachmessagetype.Pleasebecarefultofollowthe

*patternbelowwhenaddinganynewmessages

*/

staticNOINLINEvoidsend_heartbeat(mavlink_channel_tchan)

{

uint8_tbase_mode=MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;

uint8_tsystem_status=MAV_STATE_ACTIVE;

uint32_tcustom_mode=control_mode;

//workoutthebase_mode.Thisvalueisnotveryuseful

//forAPM,butwecalculateitasbestwecansoageneric

//MAVLinkenabledgroundstationcanworkoutsomethingabout

//whattheMAVisupto.Theactualbitvaluesarehighly

//ambiguousformostoftheAPMflightmodes.Inpractice,you

//onlygetusefulinformationfromthecustom_mode,whichmapsto

//theAPMflightmodeandhasawelldefinedmeaninginthe

//ArduPlanedocumentation

base_mode=MAV_MODE_FLAG_STABILIZE_ENABLED;

switch(control_mode){

caseAUTO:

caseRTL:

caseLOITER:

caseGUIDED:

caseCIRCLE:

base_mode|=MAV_MODE_FLAG_GUIDED_ENABLED;

//notethatMAV_MODE_FLAG_AUTO_ENABLEDdoesnotmatchwhat

//APMdoesinanymode,asthatisdefinedas"systemfindsitsowngoal

//positions",whichAPMdoesnotcurrentlydo

break;

}

//allmodesexceptINITIALISINGhavesomeformofmanual

//overrideifstickmixingisenabled

base_mode|=MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;

#ifHIL_MODE!=HIL_MODE_DISABLED

base_mode|=MAV_MODE_FLAG_HIL_ENABLED;

#endif

//wearearmedifwearenotinitialising

if(0){//motors.armed()){

base_mode|=MAV_MODE_FLAG_SAFETY_ARMED;

}

//indicatewehavesetacustommode

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);

}

staticNOINLINEvoidsend_attitude(mavlink_channel_tchan)

{

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);

}

staticvoidNOINLINEsend_location(mavlink_channel_tchan)

{

//Matrix3frot=ahrs.get_dcm_matrix();//neglectingangleofattackfornow

mavlink_msg_global_position_int_send(

chan,

1,//millis(),

2,//current_loc.lat,//in1E7degrees

3,//current_loc.lng,//in1E7degrees

4,//g_gps->altitude*10,//millimetersabovesealevel

5,//(current_loc.alt-home.alt)*10,//millimetersaboveground

6,//g_gps->ground_speed*rot.a.x,//Xspeedcm/s

7,//g_gps->ground_speed*rot.b.x,//Yspeedcm/s

8,//g_gps->ground_speed*rot.c.x,

9);//g_gps->ground_course);//coursein1/100degree

}

//staticvoidNOINLINEsend_ahrs(mavlink_channel_tchan)

//{

////Vector3fomega_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());

//}

staticvoidNOINLINEsend_statustext(mavlink_channel_tchan)

{

}

//arewestilldelayingtelemetrytotrytoavoidXbeebricking?

staticbooltelemetry_delayed(mavlink_channel_tchan)

{

returnfalse;

}

//trytosendamessage,returnfalseifitwon'tfitintheserialtxbuffer

staticboolmavlink_try_send_message(mavlink_channel_tchan,enumap_messageid,uint16_tpacket_drops)

{

int16_tpayload_space=serial_free();

if(telemetry_delayed(chan)){

returnfalse;

}

switch(id){

caseMSG_HEARTBEAT:

CHECK_PAYLOAD_SIZE(HEARTBEAT);

send_heartbeat(chan);

break;

caseMSG_ATTITUDE:

CHECK_PAYLOAD_SIZE(ATTITUDE);

send_attitude(chan);

break;

caseMSG_LOCATION:

CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);

send_location(chan);

break;

//caseMSG_AHRS:

//CHECK_PAYLOAD_SIZE(AHRS);

//send_ahrs(chan);

//break;

caseMSG_STATUSTEXT:

CHECK_PAYLOAD_SIZE(STATUSTEXT);

send_statustext(chan);

break;

default:

break;

}

returntrue;

}

#defineMAX_DEFERRED_MESSAGESMSG_RETRY_DEFERRED

staticstructmavlink_queue{

enumap_messagedeferred_messages[MAX_DEFERRED_MESSAGES];

uint8_tnext_deferred_message;

uint8_tnum_deferred_messages;

}mavlink_queue[2];

//sendamessageusingmavlink

voidmavlink_send_message(mavlink_channel_tchan,enumap_messageid,uint16_tpacket_drops)

{

uint8_ti,nextid;

structmavlink_queue*q=&mavlink_queue[(uint8_t)chan];

//seeifwecansendthedeferredmessages,ifany

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;

}

//thismessageidmightalreadybedeferred

for(i=0,nextid=q->next_deferred_message;i<q->num_deferred_messages;i++){

if(q->deferred_messages[nextid]==id){

//itsalreadydeferred,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'tsenditnow,sodeferit

if(q->num_deferred_messages==MAX_DEFERRED_MESSAGES){

//thedeferbufferisfull,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++;

}

}

voidmavlink_send_text(mavlink_channel_tchan,enumgcs_severityseverity,char*str)

{

if(telemetry_delayed(chan)){

return;

}

if(severity==SEVERITY_LOW){

//sendviathedeferredqueuingsystem

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{

//sendimmediately

mavlink_msg_statustext_send(

chan,

severity,

str);

}

}

voidupdate(void)

{

//receivenewpackets

mavlink_message_tmsg;

mavlink_status_tstatus;

status.packet_rx_drop_count=0;

//processreceivedbytes

while(serial_available())

{

uint8_tc=serial_read_ch();

//Trytogetanewmessage

if(mavlink_parse_char(chan,c,&msg,&status)){

mavlink_active=true;

//printf("%c",c);

printf("ReceivedmessagewithID%d,sequence:%dfromcomponent%dofsystem%d",\

msg.msgid,msg.seq,msg.compid,msg.sysid);

handleMessage(&msg);

}

}

}

voidhandleMessage(mavlink_message_t*msg)

{

//structLocationtell_command={};//commandfortelemetry

switch(msg->msgid){

caseMAVLINK_MSG_ID_HEARTBEAT:{

mavlink_msg_heartbeat_decode(msg,&heartbeat);

break;

}

caseMAVLINK_MSG_ID_ATTITUDE:{

mavlink_msg_attitude_decode(msg,&attitude);

break;

}

caseMAVLINK_MSG_ID_GLOBAL_POSITION_INT:{

mavlink_msg_global_position_int_decode(msg,&position);

break;

}

//

//caseMAVLINK_MSG_ID_AHRS:{

//mavlink_msg_ahrs_decode(msg,&ahrs);

//break;

//}

default:

break;

}//endswitch

}//endhandlemavlink

[/code]

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

//-*-tab-width:4;Mode:C++;c-basic-offset:4;indent-tabs-mode:nil-*-

#ifndef_DEFINES_H

//作者：恒久力行qq:624668529

#define_DEFINES_H

//thisavoidsaverycommonconfigerror

#defineENABLEENABLED

#defineDISABLEDISABLED

//Flightmodes

//------------

#defineYAW_HOLD0

#defineYAW_ACRO1

#defineYAW_AUTO2

#defineYAW_LOOK_AT_HOME3

#defineYAW_TOY4//THORThisistheYawmode

#defineROLL_PITCH_STABLE0

#defineROLL_PITCH_ACRO1

#defineROLL_PITCH_AUTO2

#defineROLL_PITCH_STABLE_OF3

#defineROLL_PITCH_TOY4//THORThisistheRollandPitch

//mode

#defineTHROTTLE_MANUAL0

#defineTHROTTLE_HOLD1

#defineTHROTTLE_AUTO2

//activealtitudesensor

//----------------------

#defineSONAR0

#defineBARO1

#defineSONAR_SOURCE_ADC1

#defineSONAR_SOURCE_ANALOG_PIN2

//CH7control

#defineCH7_DO_NOTHING0

#defineCH7_SET_HOVER1

#defineCH7_FLIP2

#defineCH7_SIMPLE_MODE3

#defineCH7_RTL4

#defineCH7_AUTO_TRIM5

#defineCH7_ADC_FILTER6

#defineCH7_SAVE_WP7

#defineCH7_MULTI_MODE8

//Frametypes

#defineQUAD_FRAME0

#defineTRI_FRAME1

#defineHEXA_FRAME2

#defineY6_FRAME3

#defineOCTA_FRAME4

#defineHELI_FRAME5

#defineOCTA_QUAD_FRAME6

#definePLUS_FRAME0

#defineX_FRAME1

#defineV_FRAME2

//LEDoutput

#defineNORMAL_LEDS0

#defineAUTO_TRIM_LEDS1

#defineCH_7_PWM_TRIGGER1800

#defineCH_6_PWM_TRIGGER_HIGH1800

#defineCH_6_PWM_TRIGGER1500

#defineCH_6_PWM_TRIGGER_LOW1200

//Internaldefines,don'teditandexpectthingstowork

//-------------------------------------------------------

#defineTRUE1

#defineFALSE0

#defineToRad(x)(x*0.01745329252)//*pi/180

#defineToDeg(x)(x*57.2957795131)//*180/pi

#defineDEBUG0

#defineLOITER_RANGE60//forcalculatingpoweroutsideofloiterradius

#defineT61000000

#defineT710000000

//GPStypecodes-usethenames,notthenumbers

#defineGPS_PROTOCOL_NONE-1

#defineGPS_PROTOCOL_NMEA0

#defineGPS_PROTOCOL_SIRF1

#defineGPS_PROTOCOL_UBLOX2

#defineGPS_PROTOCOL_IMU3

#defineGPS_PROTOCOL_MTK4

#defineGPS_PROTOCOL_HIL5

#defineGPS_PROTOCOL_MTK166

#defineGPS_PROTOCOL_AUTO7

#defineCH_ROLLCH_1

#defineCH_PITCHCH_2

#defineCH_THROTTLECH_3

#defineCH_RUDDERCH_4

#defineCH_YAWCH_4

#defineRC_CHANNEL_ANGLE0

#defineRC_CHANNEL_RANGE1

#defineRC_CHANNEL_ANGLE_RAW2

//HILenumerations

#defineHIL_MODE_DISABLED0

#defineHIL_MODE_ATTITUDE1

#defineHIL_MODE_SENSORS2

#defineASCENDING1

#defineDESCENDING-1

#defineREACHED_ALT0

//AutoPilotmodes

//----------------

#defineSTABILIZE0//holdlevelposition

#defineACRO1//ratecontrol

#defineALT_HOLD2//AUTOcontrol

#defineAUTO3//AUTOcontrol

#defineGUIDED4//AUTOcontrol

#defineLOITER5//Holdasinglelocation

#defineRTL6//AUTOcontrol

#defineCIRCLE7//AUTOcontrol

#definePOSITION8//AUTOcontrol

#defineLAND9//AUTOcontrol

#defineOF_LOITER10//Holdasinglelocationusingopticalflow

//sensor

#defineTOY_A11//THOREnumforToymode

#defineTOY_M12//THOREnumforToymode

#defineNUM_MODES13

#defineSIMPLE_11

#defineSIMPLE_22

#defineSIMPLE_34

#defineSIMPLE_48

#defineSIMPLE_516

#defineSIMPLE_632

//CH_6Tuning

//-----------

#defineCH6_NONE0

//Attitude

#defineCH6_STABILIZE_KP1

#defineCH6_STABILIZE_KI2

#defineCH6_STABILIZE_KD29//duplicatewithCH6_DAMP

#defineCH6_YAW_KP3

#defineCH6_YAW_KI24

//Rate

#defineCH6_ACRO_KP25

#defineCH6_RATE_KP4

#defineCH6_RATE_KI5

#defineCH6_RATE_KD21

#defineCH6_YAW_RATE_KP6

#defineCH6_YAW_RATE_KD26

//Altituderatecontroller

#defineCH6_THROTTLE_KP7

//Extras

#defineCH6_TOP_BOTTOM_RATIO8

#defineCH6_RELAY9

//Navigation

#defineCH6_TRAVERSE_SPEED10//maximumspeedtonextwaypoint

#defineCH6_NAV_KP11

#defineCH6_LOITER_KP12

#defineCH6_LOITER_KI27

//TradHelispecific

#defineCH6_HELI_EXTERNAL_GYRO13

//altitudecontroller

#defineCH6_THR_HOLD_KP14

#defineCH6_Z_GAIN15

#defineCH6_DAMP16//duplicatewithCH6_YAW_RATE_KD

//opticalflowcontroller

#defineCH6_OPTFLOW_KP17

#defineCH6_OPTFLOW_KI18

#defineCH6_OPTFLOW_KD19

#defineCH6_NAV_I20

#defineCH6_LOITER_RATE_KP22

#defineCH6_LOITER_RATE_KI28

#defineCH6_LOITER_RATE_KD23

#defineCH6_AHRS_YAW_KP30

#defineCH6_AHRS_KP31

//navbytemask

//-------------

#defineNAV_LOCATION1

#defineNAV_ALTITUDE2

#defineNAV_DELAY4

//Commands-NotethatAPMnowusesasubsetoftheMAVLinkprotocol

//commands.SeeenumMAV_CMDintheGCS_Mavlinklibrary

#defineCMD_BLANK0//thereisnocommandstoredinthememlocation

//requested

#defineNO_COMMAND0

#defineLOITER_MODE1

#defineWP_MODE2

#defineCIRCLE_MODE3

#defineNO_NAV_MODE4

#defineTOY_MODE5//THORThismodedefinestheVirtual

//WPfollowingmode

//TOYmixingoptions

#defineTOY_LOOKUP_TABLE0

#defineTOY_LINEAR_MIXER1

#defineTOY_EXTERNAL_MIXER2

//Waypointoptions

#defineMASK_OPTIONS_RELATIVE_ALT1

#defineWP_OPTION_ALT_CHANGE2

#defineWP_OPTION_YAW4

#defineWP_OPTION_ALT_REQUIRED8

#defineWP_OPTION_RELATIVE16

//#defineWP_OPTION_32

//#defineWP_OPTION_64

#defineWP_OPTION_NEXT_CMD128

//repeatingevents

#defineNO_REPEAT0

#defineCH_5_TOGGLE1

#defineCH_6_TOGGLE2

#defineCH_7_TOGGLE3

#defineCH_8_TOGGLE4

#defineRELAY_TOGGLE5

#defineSTOP_REPEAT10

//GCSMessageID's

///NOTE:toensureweneverblockonsendingMAVLinkmessages

///pleasekeepeachMSG_toasingleMAVLinkmessage.Ifneedbe

///createnewMSG_IDsforadditionalmessagesonthesame

///stream

enumap_message{

MSG_HEARTBEAT,

MSG_ATTITUDE,

MSG_LOCATION,

MSG_EXTENDED_STATUS1,

MSG_EXTENDED_STATUS2,

MSG_NAV_CONTROLLER_OUTPUT,

MSG_CURRENT_WAYPOINT,

MSG_VFR_HUD,

MSG_RADIO_OUT,

MSG_RADIO_IN,

MSG_RAW_IMU1,

MSG_RAW_IMU2,

MSG_RAW_IMU3,

MSG_GPS_STATUS,

MSG_GPS_RAW,

MSG_SERVO_OUT,

MSG_NEXT_WAYPOINT,

MSG_NEXT_PARAM,

MSG_STATUSTEXT,

MSG_LIMITS_STATUS,

MSG_AHRS,

MSG_SIMSTATE,

MSG_HWSTATUS,

MSG_RETRY_DEFERRED//thismustbelast

};

enumgcs_severity{

SEVERITY_LOW=1,

SEVERITY_MEDIUM,

SEVERITY_HIGH,

SEVERITY_CRITICAL

};

//Loggingparameters

#defineTYPE_AIRSTART_MSG0x00

#defineTYPE_GROUNDSTART_MSG0x01

#defineLOG_ATTITUDE_MSG0x01

#defineLOG_GPS_MSG0x02

#defineLOG_MODE_MSG0x03

#defineLOG_CONTROL_TUNING_MSG0x04

#defineLOG_NAV_TUNING_MSG0x05

#defineLOG_PERFORMANCE_MSG0x06

#defineLOG_RAW_MSG0x07

#defineLOG_CMD_MSG0x08

#defineLOG_CURRENT_MSG0x09

#defineLOG_STARTUP_MSG0x0A

#defineLOG_MOTORS_MSG0x0B

#defineLOG_OPTFLOW_MSG0x0C

#defineLOG_DATA_MSG0x0D

#defineLOG_PID_MSG0x0E

#defineLOG_ITERM_MSG0x0F

#defineLOG_DMP_MSG0x10

#defineLOG_INDEX_MSG0xF0

#defineMAX_NUM_LOGS50

#defineMASK_LOG_ATTITUDE_FAST(1<<0)

#defineMASK_LOG_ATTITUDE_MED(1<<1)

#defineMASK_LOG_GPS(1<<2)

#defineMASK_LOG_PM(1<<3)

#defineMASK_LOG_CTUN(1<<4)

#defineMASK_LOG_NTUN(1<<5)

#defineMASK_LOG_MODE(1<<6)

#defineMASK_LOG_RAW(1<<7)

#defineMASK_LOG_CMD(1<<8)

#defineMASK_LOG_CUR(1<<9)

#defineMASK_LOG_MOTORS(1<<10)

#defineMASK_LOG_OPTFLOW(1<<11)

#defineMASK_LOG_PID(1<<12)

#defineMASK_LOG_ITERM(1<<13)

//WaypointModes

//----------------

#defineABS_WP0

#defineREL_WP1

//CommandQueues

//---------------

#defineCOMMAND_MUST0

#defineCOMMAND_MAY1

#defineCOMMAND_NOW2

//Events

//------

#defineEVENT_WILL_REACH_WAYPOINT1

#defineEVENT_SET_NEW_WAYPOINT_INDEX2

#defineEVENT_LOADED_WAYPOINT3

#defineEVENT_LOOP4

//Climbratecalculations

#defineALTITUDE_HISTORY_LENGTH8//Numberof(time,altitude)pointsto

//regressaclimbratefrom

#defineBATTERY_VOLTAGE(x)(x*(g.input_voltage/1024.0))*g.volt_div_ratio

#defineCURRENT_AMPS(x)((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt

//#defineBARO_FILTER_SIZE8

/****************************************************************/

/*ExpansionPIN'sthatpeoplecanuseforvariousthings.*/

//AN0-7arelocatedatedgeofIMUPCB"above"pressuresensorand

//Expansionport

//AN0-5arealsolocatednexttovoltagedividersandslidingSW2switch

//AN0-3has10kOhmresistorinserial,include3.9kOhmtomakeitas

//voltagedivider

//AN4-5aredirectGPIOpinsfromatmega1280andtheyarethelatestpins

//nexttoSW2switch

//LookmoreArduCopterWikiforvoltagedividersandotherports

#defineAN054//resistor,vdivuse,divider1closesttorelay

#defineAN155//resistor,vdivuse,divider2

#defineAN256//resistor,vdivuse,divider3

#defineAN357//resistor,vdivuse,divider4closesttoSW2

#defineAN458//directGPIOpin,defaultasanaloginput,nexttoSW2

//switch

#defineAN559//directGPIOpin,defaultasanaloginput,nexttoSW2

//switch

#defineAN660//directGPIOpin,defaultasanaloginput,closeto

//Pressuresensor,ExpansionPorts

#defineAN761//directGPIOpin,defaultasanaloginput,closeto

//Pressuresensor,ExpansionPorts

//AN8-15arelocatedatedgeofIMUPCB"above"pressuresensorand

//Expansionport

//AN8-15PINsarenotconnectedanywhere,theyarelocatedaslast8pins

//onedgeoftheboardaboveExpansionPorts

//evenpins(8,10,12,14)areatedgeofboard,Oddpins(9,11,13,15)areon

//innerrow

#defineAN862//NC

#defineAN963//NC

#defineAN1064//NC

#defineAN1165//NC

#defineAN1266//NC

#defineAN1367//NC

#defineAN1468//NC

#defineAN1569//NC

#defineRELAY_PIN47

#definePIEZO_PINAN5//LastpinonthebackADCconnector

//sonar

//#defineSonarToCm(x)(x*1.26)//Sonarrawvaluetocentimeters

//RADIANS

#defineRADX1000.000174532925

#defineDEGX1005729.57795

//EEPROMaddresses

#defineEEPROM_MAX_ADDR4096

//parametersgetthefirst1536bytesofEEPROM,remainderisforwaypoints

#defineWP_START_BYTE0x600//whereinmemoryhomeWPisstored+allother

//WP

#defineWP_SIZE15

#defineONBOARD_PARAM_NAME_LENGTH15

//fencepointsarestoredattheendoftheEEPROM

#defineMAX_FENCEPOINTS6

#defineFENCE_WP_SIZEsizeof(Vector2l)

#defineFENCE_START_BYTE(EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE))

#defineMAX_WAYPOINTS((FENCE_START_BYTE-WP_START_BYTE)/WP_SIZE)-1//-

//1

//to

//be

//safe

//markafunctionasnottobeinlined

#defineNOINLINE__attribute__((noinline))

//IMUselection

#defineCONFIG_IMU_OILPAN1

#defineCONFIG_IMU_MPU60002

//APMHardwareselection

#defineAPM_HARDWARE_APM11

#defineAPM_HARDWARE_APM22

#defineAP_BARO_BMP0851

#defineAP_BARO_MS56112

#defineLOGGING_SIMPLE1

#defineLOGGING_VERBOSE2

//ChannelConfigselection

#defineCHANNEL_CONFIG_DEFAULT1

#defineCHANNEL_CONFIG_CUSTOM2

#endif//_DEFINES_H

//作者：恒久力行qq:624668529

[/code]

6.主函数也有更改这里是主函数代码

main.c

#include"sys.h"

//作者：恒久力行qq:624668529

#include"delay.h"

#include"usart.h"

#include"led.h"

#include"beep.h"

#include"key.h"

#include"mavlink_avoid_errors.h"

#include"mavlink_usart_fifo.h"

#include"open_tel_mavlink.h"

//ALIENTEK探索者STM32F407开发板实验4

//串口通信实验-库函数版本

//技术支持：www.openedv.com

//淘宝店铺：http://eboard.taobao.com

//广州市星翼电子科技有限公司

//作者：正点原子@ALIENTEK

mavlink_system_tmavlink_system;

#defineUART_TX_BUFFER_SIZE255

#defineUART_RX_BUFFER_SIZE255

externfifo_tuart_rx_fifo,uart_tx_fifo;

externuint8_tuart_tx_buf[UART_TX_BUFFER_SIZE],uart_rx_buf[UART_RX_BUFFER_SIZE];

intmain(void)

{

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2

delay_init(168);//延时初始化

uart_init(115200);//串口初始化波特率为115200

LED_Init();//初始化与LED连接的硬件接口

fifo_init(&uart_tx_fifo,uart_tx_buf,UART_TX_BUFFER_SIZE);

fifo_init(&uart_rx_fifo,uart_rx_buf,UART_RX_BUFFER_SIZE);

mavlink_system.sysid=MAV_TYPE_GENERIC;

mavlink_system.compid=MAV_AUTOPILOT_GENERIC;

while(1)

{

mavlink_send_message(0,MSG_HEARTBEAT,0);

mavlink_send_message(0,MSG_LOCATION,0);

while(1)

{

//if(tranlTimer>100)

//{

//tranlTimer=0;

//mavlink_send_message(0,MSG_HEARTBEAT,0);

//mavlink_send_message(0,MSG_ATTITUDE,0);

//mavlink_send_message(0,MSG_AHRS,0);

//}

update();

}

}

}

[/code]

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

跟原来官方不同的还有MAVLINK_HELPERuint16_tmavlink_msg_to_send_buffer(uint8_t*buffer,constmavlink_message_t*msg)这个函数参数定义到函数最前面了。全代码是修改后的，有兴趣的可以去比较。不修改的话会报错。

#include"mavlink_usart_fifo.h"

externmavlink_system_tmavlink_system;

MAVLINK_HELPERvoid_mavlink_send_uart(mavlink_channel_tchan,constchar*buf,uint16_tlen)

{

serial_write_buf((uint8_t*)buf,len);

}

[/code]
mavlink_helpers.h全代码

#ifndef_MAVLINK_HELPERS_H_

//作者：恒久力行qq:624668529

#define_MAVLINK_HELPERS_H_

#include"string.h"

#include"checksum.h"

#include"mavlink_types.h"

#include"mavlink_conversions.h"

#ifndefMAVLINK_HELPER

#defineMAVLINK_HELPER

#endif

externmavlink_system_tmavlink_system;

/*

*Internalfunctiontogiveaccesstothechannelstatusforeachchannel

*/

#ifndefMAVLINK_GET_CHANNEL_STATUS

MAVLINK_HELPERmavlink_status_t*mavlink_get_channel_status(uint8_tchan)

{

#ifdefMAVLINK_EXTERNAL_RX_STATUS

//Nom_mavlink_statusarraydefinedinfunction,

//hastobedefinedexternally

#else

staticmavlink_status_tm_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];

#endif

return&m_mavlink_status[chan];

}

#endif

/*

*Internalfunctiontogiveaccesstothechannelbufferforeachchannel

*/

#ifndefMAVLINK_GET_CHANNEL_BUFFER

MAVLINK_HELPERmavlink_message_t*mavlink_get_channel_buffer(uint8_tchan)

{

#ifdefMAVLINK_EXTERNAL_RX_BUFFER

//Nom_mavlink_bufferarraydefinedinfunction,

//hastobedefinedexternally

#else

staticmavlink_message_tm_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];

#endif

return&m_mavlink_buffer[chan];

}

#endif

/**

*@briefResetthestatusofachannel.

*/

MAVLINK_HELPERvoidmavlink_reset_channel_status(uint8_tchan)

{

mavlink_status_t*status=mavlink_get_channel_status(chan);

status->parse_state=MAVLINK_PARSE_STATE_IDLE;

}

/**

*@briefFinalizeaMAVLinkmessagewithchannelassignment

*

*Thisfunctioncalculatesthechecksumandsetslengthandaircraftidcorrectly.

*Itassumesthatthemessageidandthepayloadarealreadycorrectlyset.Thisfunction

*canalsobeusedifthemessageheaderhasalreadybeenwrittenbefore(asinmavlink_msg_xxx_pack

*insteadofmavlink_msg_xxx_pack_headerless),itjustintroduceslittleextraoverhead.

*

*@parammsgMessagetofinalize

*@paramsystem_idIdofthesending(this)system,1-127

*@paramlengthMessagelength

*/

#ifMAVLINK_CRC_EXTRA

MAVLINK_HELPERuint16_tmavlink_finalize_message_chan(mavlink_message_t*msg,uint8_tsystem_id,uint8_tcomponent_id,

uint8_tchan,uint8_tmin_length,uint8_tlength,uint8_tcrc_extra)

#else

MAVLINK_HELPERuint16_tmavlink_finalize_message_chan(mavlink_message_t*msg,uint8_tsystem_id,uint8_tcomponent_id,

uint8_tchan,uint8_tlength)

#endif

{

//Thisisonlyusedforthev2protocolandwesilenceithere

(void)min_length;

//Thiscodepartisthesameforallmessages;

msg->magic=MAVLINK_STX;

msg->len=length;

msg->sysid=system_id;

msg->compid=component_id;

//Onesequencenumberperchannel

msg->seq=mavlink_get_channel_status(chan)->current_tx_seq;

mavlink_get_channel_status(chan)->current_tx_seq=mavlink_get_channel_status(chan)->current_tx_seq+1;

msg->checksum=crc_calculate(((constuint8_t*)(msg))+3,MAVLINK_CORE_HEADER_LEN);

crc_accumulate_buffer(&msg->checksum,_MAV_PAYLOAD(msg),msg->len);

#ifMAVLINK_CRC_EXTRA

crc_accumulate(crc_extra,&msg->checksum);

#endif

mavlink_ck_a(msg)=(uint8_t)(msg->checksum&0xFF);

mavlink_ck_b(msg)=(uint8_t)(msg->checksum>>8);

returnlength+MAVLINK_NUM_NON_PAYLOAD_BYTES;

}

/**

*@briefFinalizeaMAVLinkmessagewithMAVLINK_COMM_0asdefaultchannel

*/

#ifMAVLINK_CRC_EXTRA

MAVLINK_HELPERuint16_tmavlink_finalize_message(mavlink_message_t*msg,uint8_tsystem_id,uint8_tcomponent_id,

uint8_tmin_length,uint8_tlength,uint8_tcrc_extra)

{

returnmavlink_finalize_message_chan(msg,system_id,component_id,MAVLINK_COMM_0,min_length,length,crc_extra);

}

#else

MAVLINK_HELPERuint16_tmavlink_finalize_message(mavlink_message_t*msg,uint8_tsystem_id,uint8_tcomponent_id,

uint8_tlength)

{

returnmavlink_finalize_message_chan(msg,system_id,component_id,MAVLINK_COMM_0,length);

}

#endif

#ifdefMAVLINK_USE_CONVENIENCE_FUNCTIONS

MAVLINK_HELPERvoid_mavlink_send_uart(mavlink_channel_tchan,constchar*buf,uint16_tlen);

/**

*@briefFinalizeaMAVLinkmessagewithchannelassignmentandsend

*/

#ifMAVLINK_CRC_EXTRA

MAVLINK_HELPERvoid_mav_finalize_message_chan_send(mavlink_channel_tchan,uint8_tmsgid,constchar*packet,

uint8_tmin_length,uint8_tlength,uint8_tcrc_extra)

#else

MAVLINK_HELPERvoid_mav_finalize_message_chan_send(mavlink_channel_tchan,uint8_tmsgid,constchar*packet,uint8_tlength)

#endif

{

uint16_tchecksum;

uint8_tbuf[MAVLINK_NUM_HEADER_BYTES];

uint8_tck[2];

mavlink_status_t*status=mavlink_get_channel_status(chan);

buf[0]=MAVLINK_STX;

buf[1]=length;

buf[2]=status->current_tx_seq;

buf[3]=mavlink_system.sysid;

buf[4]=mavlink_system.compid;

buf[5]=msgid;

status->current_tx_seq++;

checksum=crc_calculate((constuint8_t*)&buf[1],MAVLINK_CORE_HEADER_LEN);

crc_accumulate_buffer(&checksum,packet,length);

#ifMAVLINK_CRC_EXTRA

crc_accumulate(crc_extra,&checksum);

#endif

ck[0]=(uint8_t)(checksum&0xFF);

ck[1]=(uint8_t)(checksum>>8);

MAVLINK_START_UART_SEND(chan,MAVLINK_NUM_NON_PAYLOAD_BYTES+(uint16_t)length);

_mavlink_send_uart(chan,(constchar*)buf,MAVLINK_NUM_HEADER_BYTES);

_mavlink_send_uart(chan,packet,length);

_mavlink_send_uart(chan,(constchar*)ck,2);

MAVLINK_END_UART_SEND(chan,MAVLINK_NUM_NON_PAYLOAD_BYTES+(uint16_t)length);

}

/**

*@briefre-sendamessageoverauartchannel

*thisismorestackefficientthanre-marshallingthemessage

*/

MAVLINK_HELPERvoid_mavlink_resend_uart(mavlink_channel_tchan,constmavlink_message_t*msg)

{

uint8_tck[2];

ck[0]=(uint8_t)(msg->checksum&0xFF);

ck[1]=(uint8_t)(msg->checksum>>8);

//XXXusetherightsequencehere

MAVLINK_START_UART_SEND(chan,MAVLINK_NUM_NON_PAYLOAD_BYTES+msg->len);

_mavlink_send_uart(chan,(constchar*)&msg->magic,MAVLINK_NUM_HEADER_BYTES);

_mavlink_send_uart(chan,_MAV_PAYLOAD(msg),msg->len);

_mavlink_send_uart(chan,(constchar*)ck,2);

MAVLINK_END_UART_SEND(chan,MAVLINK_NUM_NON_PAYLOAD_BYTES+msg->len);

}

#endif//MAVLINK_USE_CONVENIENCE_FUNCTIONS

/**

*@briefPackamessagetosenditoveraserialbytestream

*/

MAVLINK_HELPERuint16_tmavlink_msg_to_send_buffer(uint8_t*buffer,constmavlink_message_t*msg)

{

uint8_t*ck;

memcpy(buffer,(constuint8_t*)&msg->magic,MAVLINK_NUM_HEADER_BYTES+(uint16_t)msg->len);

ck=buffer+(MAVLINK_NUM_HEADER_BYTES+(uint16_t)msg->len);

ck[0]=(uint8_t)(msg->checksum&0xFF);

ck[1]=(uint8_t)(msg->checksum>>8);

returnMAVLINK_NUM_NON_PAYLOAD_BYTES+(uint16_t)msg->len;

}

union__mavlink_bitfield{

uint8_tuint8;

int8_tint8;

uint16_tuint16;

int16_tint16;

uint32_tuint32;

int32_tint32;

};

MAVLINK_HELPERvoidmavlink_start_checksum(mavlink_message_t*msg)

{

crc_init(&msg->checksum);

}

MAVLINK_HELPERvoidmavlink_update_checksum(mavlink_message_t*msg,uint8_tc)

{

crc_accumulate(c,&msg->checksum);

}

/**

*Thisisavarientofmavlink_frame_char()butwithcallersupplied

*parsingbuffers.ItisusefulwhenyouwanttocreateaMAVLink

*parserinalibrarythatdoesn'tuseanyglobalvariables

*

*@paramrxmsgparsingmessagebuffer

*@paramstatusparsingstarusbuffer

*@paramcThechartoparse

*

*@paramreturnMsgNULLifnomessagecouldbedecoded,themessagedataelse

*@paramreturnStatsifamessagewasdecoded,thisisfilledwiththechannel'sstats

*@return0ifnomessagecouldbedecoded,1ongoodmessageandCRC,2onbadCRC

*

*Atypicalusescenarioofthisfunctioncallis:

*

*@code

*#include<mavlink.h>

*

*mavlink_message_tmsg;

*intchan=0;

*

*

*while(serial.bytesAvailable>0)

*{

*uint8_tbyte=serial.getNextByte();

*if(mavlink_frame_char(chan,byte,&msg)!=MAVLINK_FRAMING_INCOMPLETE)

*{

*printf("ReceivedmessagewithID%d,sequence:%dfromcomponent%dofsystem%d",msg.msgid,msg.seq,msg.compid,msg.sysid);

*}

*}

*

*

*@endcode

*/

MAVLINK_HELPERuint8_tmavlink_frame_char_buffer(mavlink_message_t*rxmsg,

mavlink_status_t*status,

uint8_tc,

mavlink_message_t*r_message,

mavlink_status_t*r_mavlink_status)

{

/*

defaultmessagecrcfunction.Youcanoverridethisper-systemto

putthisdatainadifferentmemorysegment

*/

#ifMAVLINK_CRC_EXTRA

#ifndefMAVLINK_MESSAGE_CRC

staticconstuint8_tmavlink_message_crcs[256]=MAVLINK_MESSAGE_CRCS;

#defineMAVLINK_MESSAGE_CRC(msgid)mavlink_message_crcs[msgid]

#endif

#endif

/*Enablethisoptiontocheckthelengthofeachmessage.

Thisallowsinvalidmessagestobecaughtmuchsooner.Useifthetransmission

mediumispronetomissing(orextra)characters(e.g.aradiothatfadesin

andout).Onlyuseifthechannelwillonlycontainmessagestypeslistedin

theheaders.

*/

#ifdefMAVLINK_CHECK_MESSAGE_LENGTH

#ifndefMAVLINK_MESSAGE_LENGTH

staticconstuint8_tmavlink_message_lengths[256]=MAVLINK_MESSAGE_LENGTHS;

#defineMAVLINK_MESSAGE_LENGTH(msgid)mavlink_message_lengths[msgid]

#endif

#endif

intbufferIndex=0;

status->msg_received=MAVLINK_FRAMING_INCOMPLETE;

switch(status->parse_state)

{

caseMAVLINK_PARSE_STATE_UNINIT:

caseMAVLINK_PARSE_STATE_IDLE:

if(c==MAVLINK_STX)

{

status->parse_state=MAVLINK_PARSE_STATE_GOT_STX;

rxmsg->len=0;

rxmsg->magic=c;

mavlink_start_checksum(rxmsg);

}

break;

caseMAVLINK_PARSE_STATE_GOT_STX:

if(status->msg_received

/*Supportshorterbuffersthanthe

defaultmaximumpacketsize*/

#if(MAVLINK_MAX_PAYLOAD_LEN<255)

||c>MAVLINK_MAX_PAYLOAD_LEN

#endif

)

{

status->buffer_overrun++;

status->parse_error++;

status->msg_received=0;

status->parse_state=MAVLINK_PARSE_STATE_IDLE;

}

else

{

//NOTcountingSTX,LENGTH,SEQ,SYSID,COMPID,MSGID,CRC1andCRC2

rxmsg->len=c;

status->packet_idx=0;

mavlink_update_checksum(rxmsg,c);

status->parse_state=MAVLINK_PARSE_STATE_GOT_LENGTH;

}

break;

caseMAVLINK_PARSE_STATE_GOT_LENGTH:

rxmsg->seq=c;

mavlink_update_checksum(rxmsg,c);

status->parse_state=MAVLINK_PARSE_STATE_GOT_SEQ;

break;

caseMAVLINK_PARSE_STATE_GOT_SEQ:

rxmsg->sysid=c;

mavlink_update_checksum(rxmsg,c);

status->parse_state=MAVLINK_PARSE_STATE_GOT_SYSID;

break;

caseMAVLINK_PARSE_STATE_GOT_SYSID:

rxmsg->compid=c;

mavlink_update_checksum(rxmsg,c);

status->parse_state=MAVLINK_PARSE_STATE_GOT_COMPID;

break;

caseMAVLINK_PARSE_STATE_GOT_COMPID:

#ifdefMAVLINK_CHECK_MESSAGE_LENGTH

if(rxmsg->len!=MAVLINK_MESSAGE_LENGTH(c))

{

status->parse_error++;

status->parse_state=MAVLINK_PARSE_STATE_IDLE;

break;

}

#endif

rxmsg->msgid=c;

mavlink_update_checksum(rxmsg,c);

if(rxmsg->len==0)

{

status->parse_state=MAVLINK_PARSE_STATE_GOT_PAYLOAD;

}

else

{

status->parse_state=MAVLINK_PARSE_STATE_GOT_MSGID;

}

break;

caseMAVLINK_PARSE_STATE_GOT_MSGID:

_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++]=(char)c;

mavlink_update_checksum(rxmsg,c);

if(status->packet_idx==rxmsg->len)

{

status->parse_state=MAVLINK_PARSE_STATE_GOT_PAYLOAD;

}

break;

caseMAVLINK_PARSE_STATE_GOT_PAYLOAD:

#ifMAVLINK_CRC_EXTRA

mavlink_update_checksum(rxmsg,MAVLINK_MESSAGE_CRC(rxmsg->msgid));

#endif

if(c!=(rxmsg->checksum&0xFF)){

status->parse_state=MAVLINK_PARSE_STATE_GOT_BAD_CRC1;

}else{

status->parse_state=MAVLINK_PARSE_STATE_GOT_CRC1;

}

_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx]=(char)c;

break;

caseMAVLINK_PARSE_STATE_GOT_CRC1:

caseMAVLINK_PARSE_STATE_GOT_BAD_CRC1:

if(status->parse_state==MAVLINK_PARSE_STATE_GOT_BAD_CRC1||c!=(rxmsg->checksum>>8)){

//gotabadCRCmessage

status->msg_received=MAVLINK_FRAMING_BAD_CRC;

}else{

//Successfullygotmessage

status->msg_received=MAVLINK_FRAMING_OK;

}

status->parse_state=MAVLINK_PARSE_STATE_IDLE;

_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1]=(char)c;

memcpy(r_message,rxmsg,sizeof(mavlink_message_t));

break;

}

bufferIndex++;

//Ifamessagehasbeensucessfullydecoded,checkindex

if(status->msg_received==MAVLINK_FRAMING_OK)

{

//while(status->current_seq!=rxmsg->seq)

//{

//status->packet_rx_drop_count++;

//status->current_seq++;

//}

status->current_rx_seq=rxmsg->seq;

//Initialcondition:Ifnopackethasbeenreceivedsofar,dropcountisundefined

if(status->packet_rx_success_count==0)status->packet_rx_drop_count=0;

//Countthispacketasreceived

status->packet_rx_success_count++;

}

r_message->len=rxmsg->len;//Providevisibilityonhowfarweareintocurrentmsg

r_mavlink_status->parse_state=status->parse_state;

r_mavlink_status->packet_idx=status->packet_idx;

r_mavlink_status->current_rx_seq=status->current_rx_seq+1;

r_mavlink_status->packet_rx_success_count=status->packet_rx_success_count;

r_mavlink_status->packet_rx_drop_count=status->parse_error;

status->parse_error=0;

if(status->msg_received==MAVLINK_FRAMING_BAD_CRC){

/*

theCRCcameoutwrong.Wenowneedtooverwritethe

msgCRCwiththeoneonthewiresothatifthe

callerdecidestoforwardthemessageanywaythat

mavlink_msg_to_send_buffer()won'toverwritethe

checksum

*/

r_message->checksum=_MAV_PAYLOAD(rxmsg)[status->packet_idx]|(_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);

}

returnstatus->msg_received;

}

/**

*ThisisaconveniencefunctionwhichhandlesthecompleteMAVLinkparsing.

*thefunctionwillparseonebyteatatimeandreturnthecompletepacketonce

*itcouldbesuccessfullydecoded.Thisfunctionwillreturn0,1or

*2(MAVLINK_FRAMING_INCOMPLETE,MAVLINK_FRAMING_OKorMAVLINK_FRAMING_BAD_CRC)

*

*Messagesareparsedintoaninternalbuffer(oneforeachchannel).Whenacomplete

*messageisreceiveditiscopiesinto*returnMsgandthechannel'sstatusis

*copiedinto*returnStats.

*

*@paramchanIDofthecurrentchannel.Thisallowstoparsedifferentchannelswiththisfunction.

*achannelisnotaphysicalmessagechannellikeaserialport,butalogicpartitionof

*thecommunicationstreamsinthiscase.COMM_NBisthelimitforthenumberofchannels

*onMCU(e.g.ARM7),whileCOMM_NB_HIGHisthelimitforthenumberofchannelsinLinux/Windows

*@paramcThechartoparse

*

*@paramreturnMsgNULLifnomessagecouldbedecoded,themessagedataelse

*@paramreturnStatsifamessagewasdecoded,thisisfilledwiththechannel'sstats

*@return0ifnomessagecouldbedecoded,1ongoodmessageandCRC,2onbadCRC

*

*Atypicalusescenarioofthisfunctioncallis:

*

*@code

*#include<mavlink.h>

*

*mavlink_message_tmsg;

*intchan=0;

*

*

*while(serial.bytesAvailable>0)

*{

*uint8_tbyte=serial.getNextByte();

*if(mavlink_frame_char(chan,byte,&msg)!=MAVLINK_FRAMING_INCOMPLETE)

*{

*printf("ReceivedmessagewithID%d,sequence:%dfromcomponent%dofsystem%d",msg.msgid,msg.seq,msg.compid,msg.sysid);

*}

*}

*

*

*@endcode

*/

MAVLINK_HELPERuint8_tmavlink_frame_char(uint8_tchan,uint8_tc,mavlink_message_t*r_message,mavlink_status_t*r_mavlink_status)

{

returnmavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),

mavlink_get_channel_status(chan),

c,

r_message,

r_mavlink_status);

}

/**

*ThisisaconveniencefunctionwhichhandlesthecompleteMAVLinkparsing.

*thefunctionwillparseonebyteatatimeandreturnthecompletepacketonce

*itcouldbesuccessfullydecoded.Thisfunctionwillreturn0or1.

*

*Messagesareparsedintoaninternalbuffer(oneforeachchannel).Whenacomplete

*messageisreceiveditiscopiesinto*returnMsgandthechannel'sstatusis

*copiedinto*returnStats.

*

*@paramchanIDofthecurrentchannel.Thisallowstoparsedifferentchannelswiththisfunction.

*achannelisnotaphysicalmessagechannellikeaserialport,butalogicpartitionof

*thecommunicationstreamsinthiscase.COMM_NBisthelimitforthenumberofchannels

*onMCU(e.g.ARM7),whileCOMM_NB_HIGHisthelimitforthenumberofchannelsinLinux/Windows

*@paramcThechartoparse

*

*@paramreturnMsgNULLifnomessagecouldbedecoded,themessagedataelse

*@paramreturnStatsifamessagewasdecoded,thisisfilledwiththechannel'sstats

*@return0ifnomessagecouldbedecodedorbadCRC,1ongoodmessageandCRC

*

*Atypicalusescenarioofthisfunctioncallis:

*

*@code

*#include<mavlink.h>

*

*mavlink_message_tmsg;

*intchan=0;

*

*

*while(serial.bytesAvailable>0)

*{

*uint8_tbyte=serial.getNextByte();

*if(mavlink_parse_char(chan,byte,&msg))

*{

*printf("ReceivedmessagewithID%d,sequence:%dfromcomponent%dofsystem%d",msg.msgid,msg.seq,msg.compid,msg.sysid);

*}

*}

*

*

*@endcode

*/

MAVLINK_HELPERuint8_tmavlink_parse_char(uint8_tchan,uint8_tc,mavlink_message_t*r_message,mavlink_status_t*r_mavlink_status)

{

uint8_tmsg_received=mavlink_frame_char(chan,c,r_message,r_mavlink_status);

if(msg_received==MAVLINK_FRAMING_BAD_CRC){

//wegotabadCRC.Treatasaparsefailure

mavlink_message_t*rxmsg=mavlink_get_channel_buffer(chan);

mavlink_status_t*status=mavlink_get_channel_status(chan);

status->parse_error++;

status->msg_received=MAVLINK_FRAMING_INCOMPLETE;

status->parse_state=MAVLINK_PARSE_STATE_IDLE;

if(c==MAVLINK_STX)

{

status->parse_state=MAVLINK_PARSE_STATE_GOT_STX;

rxmsg->len=0;

mavlink_start_checksum(rxmsg);

}

return0;

}

returnmsg_received;

}

/**

*@briefPutabitfieldoflength1-32bitintothebuffer

*

*@parambthevaluetoadd,willbeencodedinthebitfield

*@parambitsnumberofbitstousetoencodeb,e.g.1forboolean,2,3,etc.

*@parampacket_indexthepositioninthepacket(theindexofthefirstbytetouse)

*@parambit_indexthepositioninthebyte(theindexofthefirstbittouse)

*@parambufferpacketbuffertowriteinto

*@returnnewpositionofthelastusedbyteinthebuffer

*/

MAVLINK_HELPERuint8_tput_bitfield_n_by_index(int32_tb,uint8_tbits,uint8_tpacket_index,uint8_tbit_index,uint8_t*r_bit_index,uint8_t*buffer)

{

uint16_tbits_remain=bits;

//Transformnumberintonetworkorder

int32_tv;

uint8_ti_bit_index,i_byte_index,curr_bits_n;

#ifMAVLINK_NEED_BYTE_SWAP

union{

int32_ti;

uint8_tb[4];

}bin,bout;

bin.i=b;

bout.b[0]=bin.b[3];

bout.b[1]=bin.b[2];

bout.b[2]=bin.b[1];

bout.b[3]=bin.b[0];

v=bout.i;

#else

v=b;

#endif

//bufferin

//01100000010000000000000011110001

//bufferout

//11110001000000000100000001100000

//Existingpartlyfilledbyte(fourfreeslots)

//0111xxxx

//Masknfreebits

//00001111=2^0+2^1+2^2+2^3=2^n-1

//=((uint32_t)(1<<n))-1;//=2^n-1

//Shiftnbitsintotherightposition

//out=in>>n;

//Maskandshiftbytes

i_bit_index=bit_index;

i_byte_index=packet_index;

if(bit_index>0)

{

//Ifbitswereavailableatstart,theywereavailable

//inthebytebeforethecurrentindex

i_byte_index--;

}

//Whilebitshavenotbeenpackedyet

while(bits_remain>0)

{

//Bitsstillhavetobepacked

//therecanbemorethan8bits,so

//wemighthavetopackthemintomorethanonebyte

//Firstpackeverythingwecanintothecurrent'open'byte

//curr_bits_n=bits_remain<<3;//Equalsbits_remainmod8

//FIXME

if(bits_remain<=(uint8_t)(8-i_bit_index))

{

//Enoughspace

curr_bits_n=(uint8_t)bits_remain;

}

else

{

curr_bits_n=(8-i_bit_index);

}

//Packthesenbitsintothecurrentbyte

//Maskoutwhateverwasatthatpositionwithones(xxx11111)

buffer[i_byte_index]&=(0xFF>>(8-curr_bits_n));

//Putcontenttothisposition,bymaskingoutthenon-usedpart

buffer[i_byte_index]|=((0x00<<curr_bits_n)&v);

//Incrementthebitindex

i_bit_index+=curr_bits_n;

//Nowproceedtothenextbyte,ifnecessary

bits_remain-=curr_bits_n;

if(bits_remain>0)

{

//Offeranother8bits/onebyte

i_byte_index++;

i_bit_index=0;

}

}

*r_bit_index=i_bit_index;

//Ifapartlyfilledbyteispresent,markthisasconsumed

if(i_bit_index!=7)i_byte_index++;

returni_byte_index-packet_index;

}

#ifdefMAVLINK_USE_CONVENIENCE_FUNCTIONS

//TomakeMAVLinkworkonyourMCU,definecomm_send_ch()ifyouwish

//tosend1byteatatime,orMAVLINK_SEND_UART_BYTES()tosenda

//wholepacketatatime

/*

#include"mavlink_types.h"

voidcomm_send_ch(mavlink_channel_tchan,uint8_tch)

{

if(chan==MAVLINK_COMM_0)

{

uart0_transmit(ch);

}

if(chan==MAVLINK_COMM_1)

{

uart1_transmit(ch);

}

}

*/

#include"mavlink_usart_fifo.h"

MAVLINK_HELPERvoid_mavlink_send_uart(mavlink_channel_tchan,constchar*buf,uint16_tlen)

{

serial_write_buf((uint8_t*)buf,len);

}

//MAVLINK_HELPERvoid_mavlink_send_uart(mavlink_channel_tchan,constchar*buf,uint16_tlen)

//{

//#ifdefMAVLINK_SEND_UART_BYTES

///*thisisthemoreefficientapproach,iftheplatform

//definesit*/

//MAVLINK_SEND_UART_BYTES(chan,(constuint8_t*)buf,len);

//#else

///*fallbacktoonebyteatatime*/

//uint16_ti;

//for(i=0;i<len;i++){

//comm_send_ch(chan,(uint8_t)buf[i]);

//}

//#endif

//}

#endif//MAVLINK_USE_CONVENIENCE_FUNCTIONS

#endif/*_MAVLINK_HELPERS_H_*/

[/code]

8.checksum未经任何修改（为了大家方便看附上全代码）

checksum.h

#ifdef__cplusplus

//作者：恒久力行qq:624668529

extern"C"{

#endif

#ifndef_CHECKSUM_H_

#define_CHECKSUM_H_

//VisualStudioversionsbefore2010don'thavestdint.h,sowejusterrorout.

#if(defined_MSC_VER)&&(_MSC_VER<1600)

#error"TheC-MAVLinkimplementationrequiresVisualStudio2010orgreater"

#endif

#include<stdint.h>

/**

*

*CALCULATETHECHECKSUM

*

*/

#defineX25_INIT_CRC0xffff

#defineX25_VALIDATE_CRC0xf0b8

#ifndefHAVE_CRC_ACCUMULATE

/**

*@briefAccumulatetheX.25CRCbyaddingonecharatatime.

*

*Thechecksumfunctionaddsthehashofonecharatatimetothe

*16bitchecksum(uint16_t).

*

*@paramdatanewchartohash

*@paramcrcAccumthealreadyaccumulatedchecksum

**/

staticinlinevoidcrc_accumulate(uint8_tdata,uint16_t*crcAccum)

{

/*AccumulateonebyteofdataintotheCRC*/

uint8_ttmp;

tmp=data^(uint8_t)(*crcAccum&0xff);

tmp^=(tmp<<4);

*crcAccum=(*crcAccum>>8)^(tmp<<8)^(tmp<<3)^(tmp>>4);

}

#endif

/**

*@briefInitiliazethebufferfortheX.25CRC

*

*@paramcrcAccumthe16bitX.25CRC

*/

staticinlinevoidcrc_init(uint16_t*crcAccum)

{

*crcAccum=X25_INIT_CRC;

}

/**

*@briefCalculatestheX.25checksumonabytebuffer

*

*@parampBufferbuffercontainingthebytearraytohash

*@paramlengthlengthofthebytearray

*@returnthechecksumoverthebufferbytes

**/

staticinlineuint16_tcrc_calculate(constuint8_t*pBuffer,uint16_tlength)

{

uint16_tcrcTmp;

crc_init(&crcTmp);

while(length--){

crc_accumulate(*pBuffer++,&crcTmp);

}

returncrcTmp;

}

/**

*@briefAccumulatetheX.25CRCbyaddinganarrayofbytes

*

*Thechecksumfunctionaddsthehashofonecharatatimetothe

*16bitchecksum(uint16_t).

*

*@paramdatanewbytestohash

*@paramcrcAccumthealreadyaccumulatedchecksum

**/

staticinlinevoidcrc_accumulate_buffer(uint16_t*crcAccum,constchar*pBuffer,uint16_tlength)

{

constuint8_t*p=(constuint8_t*)pBuffer;

while(length--){

crc_accumulate(*p++,crcAccum);

}

}

#endif/*_CHECKSUM_H_*/

#ifdef__cplusplus

}

#endif

[/code]

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

（就是这个函数MAVLINK_HELPERvoidmavlink_dcm_to_quaternion(constfloatdcm[3][3],floatquaternion[4])mavlink_conversions.h全代码

#ifndef_MAVLINK_CONVERSIONS_H_

//作者：恒久力行qq:624668529

#define_MAVLINK_CONVERSIONS_H_

/*enablemathdefinesonWindows*/

#ifdef_MSC_VER

#ifndef_USE_MATH_DEFINES

#define_USE_MATH_DEFINES

#endif

#endif

#include<math.h>

#ifndefM_PI_2

#defineM_PI_2((float)asin(1))

#endif

/**

*@filemavlink_conversions.h

*

*TheseconversionfunctionsfollowtheNASArotationstandardsdefinitionfile

*availableonline.

*

*TheirintentistolowerthebarrierforMAVLinkadopterstousegimbal-lockfree

*(bothrotationmatrices,sometimescalledDCM,andquaternionsaregimbal-lockfree)

*rotationrepresentations.Eulerangles(roll,pitch,yaw)willbephasedoutofthe

*protocolaswidelyaspossible.

*

*@authorJamesGoppert

*@authorThomasGubler<thomasgubler@gmail.com>

*/

/**

*Convertsaquaterniontoarotationmatrix

*

*@paramquaterniona[w,x,y,z]orderedquaternion(null-rotationbeing1000)

*@paramdcma3x3rotationmatrix

*/

MAVLINK_HELPERvoidmavlink_quaternion_to_dcm(constfloatquaternion[4],floatdcm[3][3])

{

doublea=quaternion[0];

doubleb=quaternion[1];

doublec=quaternion[2];

doubled=quaternion[3];

doubleaSq=a*a;

doublebSq=b*b;

doublecSq=c*c;

doubledSq=d*d;

dcm[0][0]=aSq+bSq-cSq-dSq;

dcm[0][1]=2*(b*c-a*d);

dcm[0][2]=2*(a*c+b*d);

dcm[1][0]=2*(b*c+a*d);

dcm[1][1]=aSq-bSq+cSq-dSq;

dcm[1][2]=2*(c*d-a*b);

dcm[2][0]=2*(b*d-a*c);

dcm[2][1]=2*(a*b+c*d);

dcm[2][2]=aSq-bSq-cSq+dSq;

}

/**

*Convertsarotationmatrixtoeulerangles

*

*@paramdcma3x3rotationmatrix

*@paramrolltherollangleinradians

*@parampitchthepitchangleinradians

*@paramyawtheyawangleinradians

*/

MAVLINK_HELPERvoidmavlink_dcm_to_euler(constfloatdcm[3][3],float*roll,float*pitch,float*yaw)

{

floatphi,theta,psi;

theta=asin(-dcm[2][0]);

if(fabsf(theta-(float)M_PI_2)<1.0e-3f){

phi=0.0f;

psi=(atan2f(dcm[1][2]-dcm[0][1],

dcm[0][2]+dcm[1][1])+phi);

}elseif(fabsf(theta+(float)M_PI_2)<1.0e-3f){

phi=0.0f;

psi=atan2f(dcm[1][2]-dcm[0][1],

dcm[0][2]+dcm[1][1]-phi);

}else{

phi=atan2f(dcm[2][1],dcm[2][2]);

psi=atan2f(dcm[1][0],dcm[0][0]);

}

*roll=phi;

*pitch=theta;

*yaw=psi;

}

/**

*Convertsaquaterniontoeulerangles

*

*@paramquaterniona[w,x,y,z]orderedquaternion(null-rotationbeing1000)

*@paramrolltherollangleinradians

*@parampitchthepitchangleinradians

*@paramyawtheyawangleinradians

*/

MAVLINK_HELPERvoidmavlink_quaternion_to_euler(constfloatquaternion[4],float*roll,float*pitch,float*yaw)

{

floatdcm[3][3];

mavlink_quaternion_to_dcm(quaternion,dcm);

mavlink_dcm_to_euler((constfloat(*)[3])dcm,roll,pitch,yaw);

}

/**

*Convertseuleranglestoaquaternion

*

*@paramrolltherollangleinradians

*@parampitchthepitchangleinradians

*@paramyawtheyawangleinradians

*@paramquaterniona[w,x,y,z]orderedquaternion(null-rotationbeing1000)

*/

MAVLINK_HELPERvoidmavlink_euler_to_quaternion(floatroll,floatpitch,floatyaw,floatquaternion[4])

{

floatcosPhi_2=cosf(roll/2);

floatsinPhi_2=sinf(roll/2);

floatcosTheta_2=cosf(pitch/2);

floatsinTheta_2=sinf(pitch/2);

floatcosPsi_2=cosf(yaw/2);

floatsinPsi_2=sinf(yaw/2);

quaternion[0]=(cosPhi_2*cosTheta_2*cosPsi_2+

sinPhi_2*sinTheta_2*sinPsi_2);

quaternion[1]=(sinPhi_2*cosTheta_2*cosPsi_2-

cosPhi_2*sinTheta_2*sinPsi_2);

quaternion[2]=(cosPhi_2*sinTheta_2*cosPsi_2+

sinPhi_2*cosTheta_2*sinPsi_2);

quaternion[3]=(cosPhi_2*cosTheta_2*sinPsi_2-

sinPhi_2*sinTheta_2*cosPsi_2);

}

/**

*Convertsarotationmatrixtoaquaternion

*Reference:

*-Shoemake,Quaternions,

*'target='_blank'>http://www.cs.ucr.edu/~vbz/resources/quatut.pdf[/code]
*

*@paramdcma3x3rotationmatrix

*@paramquaterniona[w,x,y,z]orderedquaternion(null-rotationbeing1000)

*/

MAVLINK_HELPERvoidmavlink_dcm_to_quaternion(constfloatdcm[3][3],floatquaternion[4])

{

intdcm_j,dcm_k;

floats;

floattr=dcm[0][0]+dcm[1][1]+dcm[2][2];

if(tr>0.0f){

floats=sqrtf(tr+1.0f);

quaternion[0]=s*0.5f;

s=0.5f/s;

quaternion[1]=(dcm[2][1]-dcm[1][2])*s;

quaternion[2]=(dcm[0][2]-dcm[2][0])*s;

quaternion[3]=(dcm[1][0]-dcm[0][1])*s;

}else{

/*Findmaximumdiagonalelementindcm

*storeindexindcm_i*/

intdcm_i=0;

inti;

for(i=1;i<3;i++){

if(dcm[i][i]>dcm[dcm_i][dcm_i]){

dcm_i=i;

}

}

dcm_j=(dcm_i+1)%3;

dcm_k=(dcm_i+2)%3;

s=sqrtf((dcm[dcm_i][dcm_i]-dcm[dcm_j][dcm_j]-

dcm[dcm_k][dcm_k])+1.0f);

quaternion[dcm_i+1]=s*0.5f;

s=0.5f/s;

quaternion[dcm_j+1]=(dcm[dcm_i][dcm_j]+dcm[dcm_j][dcm_i])*s;

quaternion[dcm_k+1]=(dcm[dcm_k][dcm_i]+dcm[dcm_i][dcm_k])*s;

quaternion[0]=(dcm[dcm_k][dcm_j]-dcm[dcm_j][dcm_k])*s;

}

}



/**

*Convertseuleranglestoarotationmatrix

*

*@paramrolltherollangleinradians

*@parampitchthepitchangleinradians

*@paramyawtheyawangleinradians

*@paramdcma3x3rotationmatrix

*/

MAVLINK_HELPERvoidmavlink_euler_to_dcm(floatroll,floatpitch,floatyaw,floatdcm[3][3])

{

floatcosPhi=cosf(roll);

floatsinPhi=sinf(roll);

floatcosThe=cosf(pitch);

floatsinThe=sinf(pitch);

floatcosPsi=cosf(yaw);

floatsinPsi=sinf(yaw);


dcm[0][0]=cosThe*cosPsi;

dcm[0][1]=-cosPhi*sinPsi+sinPhi*sinThe*cosPsi;

dcm[0][2]=sinPhi*sinPsi+cosPhi*sinThe*cosPsi;


dcm[1][0]=cosThe*sinPsi;

dcm[1][1]=cosPhi*cosPsi+sinPhi*sinThe*sinPsi;

dcm[1][2]=-sinPhi*cosPsi+cosPhi*sinThe*sinPsi;


dcm[2][0]=-sinThe;

dcm[2][1]=sinPhi*cosThe;

dcm[2][2]=cosPhi*cosThe;

}


#endif

[/code]
10.mavlink_types.h跟源代码相比较屏蔽了下面这段代码
（关于MAVPACKED相关定义在mavlink_avoid_errors.h中）
//Macrotodefinepackedstructures

//#ifdef__GNUC__

//#defineMAVPACKED(__Declaration__)__Declaration____attribute__((packed))

//#else

//#defineMAVPACKED(__Declaration__)__pragma(pack(push,1))__Declaration____pragma(pack(pop))

//#endif

[/code]mavlink_types.h全代码
#ifndefMAVLINK_TYPES_H_

#defineMAVLINK_TYPES_H_


//VisualStudioversionsbefore2010don'thavestdint.h,sowejusterrorout.

#if(defined_MSC_VER)&&(_MSC_VER<1600)

#error"TheC-MAVLinkimplementationrequiresVisualStudio2010orgreater"

#endif


#include<stdint.h>


//Macrotodefinepackedstructures

//#ifdef__GNUC__

//#defineMAVPACKED(__Declaration__)__Declaration____attribute__((packed))

//#else

//#defineMAVPACKED(__Declaration__)__pragma(pack(push,1))__Declaration____pragma(pack(pop))

//#endif


#ifndefMAVLINK_MAX_PAYLOAD_LEN

//itispossibletooverridethis,butbecareful!

#defineMAVLINK_MAX_PAYLOAD_LEN255///<Maximumpayloadlength

#endif


#defineMAVLINK_CORE_HEADER_LEN5///<Lengthofcoreheader(ofthecomm.layer):messagelength(1byte)+messagesequence(1byte)+messagesystemid(1byte)+messagecomponentid(1byte)+messagetypeid(1byte)

#defineMAVLINK_NUM_HEADER_BYTES(MAVLINK_CORE_HEADER_LEN+1)///<Lengthofallheaderbytes,includingcoreandchecksum

#defineMAVLINK_NUM_CHECKSUM_BYTES2

#defineMAVLINK_NUM_NON_PAYLOAD_BYTES(MAVLINK_NUM_HEADER_BYTES+MAVLINK_NUM_CHECKSUM_BYTES)


#defineMAVLINK_MAX_PACKET_LEN(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_NON_PAYLOAD_BYTES)///<Maximumpacketlength


#defineMAVLINK_MSG_ID_EXTENDED_MESSAGE255

#defineMAVLINK_EXTENDED_HEADER_LEN14


#if(defined_MSC_VER)||((defined__APPLE__)&&(defined__MACH__))||(defined__linux__)

/*fullfledged32bit++OS*/

#defineMAVLINK_MAX_EXTENDED_PACKET_LEN65507

#else

/*smallmicrocontrollers*/

#defineMAVLINK_MAX_EXTENDED_PACKET_LEN2048

#endif


#defineMAVLINK_MAX_EXTENDED_PAYLOAD_LEN(MAVLINK_MAX_EXTENDED_PACKET_LEN-MAVLINK_EXTENDED_HEADER_LEN-MAVLINK_NUM_NON_PAYLOAD_BYTES)



/**

*Old-style4byteparamunion

*

*Thisstructisthedataformattobeusedwhensending

*parameters.Theparametershouldbecopiedtothenative

*type(withouttypeconversion)

*andre-instantedonthereceivingsideusingthe

*nativetypeaswell.

*/

MAVPACKED(

typedefstructparam_union{

union{

floatparam_float;

int32_tparam_int32;

uint32_tparam_uint32;

int16_tparam_int16;

uint16_tparam_uint16;

int8_tparam_int8;

uint8_tparam_uint8;

uint8_tbytes[4];

};

uint8_ttype;

})mavlink_param_union_t;



/**

*New-style8byteparamunion

*mavlink_param_union_double_twillbe8byteslong,andtreatedasneeding8bytealignmentforthepurposesofMAVLink1.0fieldordering.

*Themavlink_param_union_double_twillbetreatedasalittle-endianstructure.

*

*Ifis_doubleis1thenthetypeisadouble,andtheremaining63bitsarethedouble,withthelowestbitofthemantissazero.

*Theintentionisthatbyreplacingtheis_doublebitwith0thetypecanbedirectlyusedasadouble(astheis_doublebitcorrespondstothe

*lowestmantissabitofadouble).Ifis_doubleis0thenmavlink_typegivesthetypeintheunion.

*Themavlink_types.hheaderwillalsoneedtohaveshifts/maskstodefinethebitboundariesintheabove,

*asbitfieldorderingisn’tconsistentbetweenplatforms.Theaboveisintendedtobeforgcconx86,

*whichshouldbethesameasgcconlittle-endianarm.Whenusingshifts/masksthevaluewillbetreatedasa64bitunsignednumber,

*andthebitspulledoutusingtheshifts/masks.

*/

MAVPACKED(

typedefstructparam_union_extended{

union{

struct{

uint8_tis_double:1;

uint8_tmavlink_type:7;

union{

charc;

uint8_tuint8;

int8_tint8;

uint16_tuint16;

int16_tint16;

uint32_tuint32;

int32_tint32;

floatf;

uint8_talign[7];

};

};

uint8_tdata[8];

};

})mavlink_param_union_double_t;


/**

*Thisstructureisrequiredtomakethemavlink_send_xxxconveniencefunctions

*work,asittellsthelibrarywhatthecurrentsystemandcomponentIDare.

*/

MAVPACKED(

typedefstruct__mavlink_system{

uint8_tsysid;///<UsedbytheMAVLinkmessage_xx_send()conveniencefunction

uint8_tcompid;///<UsedbytheMAVLinkmessage_xx_send()conveniencefunction

})mavlink_system_t;


MAVPACKED(

typedefstruct__mavlink_message{

uint16_tchecksum;///<sentatendofpacket

uint8_tmagic;///<protocolmagicmarker

uint8_tlen;///<Lengthofpayload

uint8_tseq;///<Sequenceofpacket

uint8_tsysid;///<IDofmessagesendersystem/aircraft

uint8_tcompid;///<IDofthemessagesendercomponent

uint8_tmsgid;///<IDofmessageinpayload

uint64_tpayload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];

})mavlink_message_t;


MAVPACKED(

typedefstruct__mavlink_extended_message{

mavlink_message_tbase_msg;

int32_textended_payload_len;///<Lengthofextendedpayloadifany

uint8_textended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];

})mavlink_extended_message_t;


typedefenum{

MAVLINK_TYPE_CHAR=0,

MAVLINK_TYPE_UINT8_T=1,

MAVLINK_TYPE_INT8_T=2,

MAVLINK_TYPE_UINT16_T=3,

MAVLINK_TYPE_INT16_T=4,

MAVLINK_TYPE_UINT32_T=5,

MAVLINK_TYPE_INT32_T=6,

MAVLINK_TYPE_UINT64_T=7,

MAVLINK_TYPE_INT64_T=8,

MAVLINK_TYPE_FLOAT=9,

MAVLINK_TYPE_DOUBLE=10

}mavlink_message_type_t;


#defineMAVLINK_MAX_FIELDS64


typedefstruct__mavlink_field_info{

constchar*name;//nameofthisfield

constchar*print_format;//printingformathint,orNULL

mavlink_message_type_ttype;//typeofthisfield

unsignedintarray_length;//ifnon-zero,fieldisanarray

unsignedintwire_offset;//offsetofeachfieldinthepayload

unsignedintstructure_offset;//offsetinaCstructure

}mavlink_field_info_t;


//notethatinthisstructuretheorderoffieldsistheorder

//intheXMLfile,notnecessarythewireorder

typedefstruct__mavlink_message_info{

constchar*name;//nameofthemessage

unsignednum_fields;//howmanyfieldsinthismessage

mavlink_field_info_tfields[MAVLINK_MAX_FIELDS];//fieldinformation

}mavlink_message_info_t;


#define_MAV_PAYLOAD(msg)((constchar*)(&((msg)->payload64[0])))

#define_MAV_PAYLOAD_NON_CONST(msg)((char*)(&((msg)->payload64[0])))


//checksumisimmediatelyafterthepayloadbytes

#definemavlink_ck_a(msg)*((msg)->len+(uint8_t*)_MAV_PAYLOAD_NON_CONST(msg))

#definemavlink_ck_b(msg)*(((msg)->len+(uint16_t)1)+(uint8_t*)_MAV_PAYLOAD_NON_CONST(msg))


typedefenum{

MAVLINK_COMM_0,

MAVLINK_COMM_1,

MAVLINK_COMM_2,

MAVLINK_COMM_3

}mavlink_channel_t;


/*

*applicationscansetMAVLINK_COMM_NUM_BUFFERStothemaximumnumber

*ofbufferstheywilluse.Ifmoreareused,thentheresultwillbe

*astackoverrun

*/

#ifndefMAVLINK_COMM_NUM_BUFFERS

#if(definedlinux)|(defined__linux)|(defined__MACH__)|(defined_WIN32)

#defineMAVLINK_COMM_NUM_BUFFERS16

#else

#defineMAVLINK_COMM_NUM_BUFFERS4

#endif

#endif


typedefenum{

MAVLINK_PARSE_STATE_UNINIT=0,

MAVLINK_PARSE_STATE_IDLE,

MAVLINK_PARSE_STATE_GOT_STX,

MAVLINK_PARSE_STATE_GOT_SEQ,

MAVLINK_PARSE_STATE_GOT_LENGTH,

MAVLINK_PARSE_STATE_GOT_SYSID,

MAVLINK_PARSE_STATE_GOT_COMPID,

MAVLINK_PARSE_STATE_GOT_MSGID,

MAVLINK_PARSE_STATE_GOT_PAYLOAD,

MAVLINK_PARSE_STATE_GOT_CRC1,

MAVLINK_PARSE_STATE_GOT_BAD_CRC1

}mavlink_parse_state_t;///<Thestatemachineforthecommparser


typedefenum{

MAVLINK_FRAMING_INCOMPLETE=0,

MAVLINK_FRAMING_OK=1,

MAVLINK_FRAMING_BAD_CRC=2

}mavlink_framing_t;


typedefstruct__mavlink_status{

uint8_tmsg_received;///<Numberofreceivedmessages

uint8_tbuffer_overrun;///<Numberofbufferoverruns

uint8_tparse_error;///<Numberofparseerrors

mavlink_parse_state_tparse_state;///<Parsingstatemachine

uint8_tpacket_idx;///<Indexincurrentpacket

uint8_tcurrent_rx_seq;///<Sequencenumberoflastpacketreceived

uint8_tcurrent_tx_seq;///<Sequencenumberoflastpacketsent

uint16_tpacket_rx_success_count;///<Receivedpackets

uint16_tpacket_rx_drop_count;///<Numberofpacketdrops

}mavlink_status_t;


#defineMAVLINK_BIG_ENDIAN0

#defineMAVLINK_LITTLE_ENDIAN1


#endif/*MAVLINK_TYPES_H_*/

[/code]
11.protocol.h未做任何修改，附上全代码
protocol.h全代码
#ifndef_MAVLINK_PROTOCOL_H_
//作者：恒久力行qq:624668529
#define_MAVLINK_PROTOCOL_H_


#include"string.h"

#include"mavlink_types.h"


/*

IfyouwantMAVLinkonasystemthatisnativebig-endian,

youneedtodefineNATIVE_BIG_ENDIAN

*/

#ifdefNATIVE_BIG_ENDIAN

#defineMAVLINK_NEED_BYTE_SWAP(MAVLINK_ENDIAN==MAVLINK_LITTLE_ENDIAN)

#else

#defineMAVLINK_NEED_BYTE_SWAP(MAVLINK_ENDIAN!=MAVLINK_LITTLE_ENDIAN)

#endif


#ifndefMAVLINK_STACK_BUFFER

#defineMAVLINK_STACK_BUFFER0

#endif


#ifndefMAVLINK_AVOID_GCC_STACK_BUG

#defineMAVLINK_AVOID_GCC_STACK_BUGdefined(__GNUC__)

#endif


#ifndefMAVLINK_ASSERT

#defineMAVLINK_ASSERT(x)

#endif


#ifndefMAVLINK_START_UART_SEND

#defineMAVLINK_START_UART_SEND(chan,length)

#endif


#ifndefMAVLINK_END_UART_SEND

#defineMAVLINK_END_UART_SEND(chan,length)

#endif


/*optiontoprovidealternativeimplementationofmavlink_helpers.h*/

#ifdefMAVLINK_SEPARATE_HELPERS


#defineMAVLINK_HELPER


/*declsinsyncwiththoseinmavlink_helpers.h*/

#ifndefMAVLINK_GET_CHANNEL_STATUS

MAVLINK_HELPERmavlink_status_t*mavlink_get_channel_status(uint8_tchan);

#endif

MAVLINK_HELPERvoidmavlink_reset_channel_status(uint8_tchan);

#ifMAVLINK_CRC_EXTRA

MAVLINK_HELPERuint16_tmavlink_finalize_message_chan(mavlink_message_t*msg,uint8_tsystem_id,uint8_tcomponent_id,

uint8_tchan,uint8_tmin_length,uint8_tlength,uint8_tcrc_extra);

MAVLINK_HELPERuint16_tmavlink_finalize_message(mavlink_message_t*msg,uint8_tsystem_id,uint8_tcomponent_id,

uint8_tmin_length,uint8_tlength,uint8_tcrc_extra);

#ifdefMAVLINK_USE_CONVENIENCE_FUNCTIONS

MAVLINK_HELPERvoid_mav_finalize_message_chan_send(mavlink_channel_tchan,uint8_tmsgid,constchar*packet,

uint8_tmin_length,uint8_tlength,uint8_tcrc_extra);

#endif

#else

MAVLINK_HELPERuint16_tmavlink_finalize_message_chan(mavlink_message_t*msg,uint8_tsystem_id,uint8_tcomponent_id,

uint8_tchan,uint8_tlength);

MAVLINK_HELPERuint16_tmavlink_finalize_message(mavlink_message_t*msg,uint8_tsystem_id,uint8_tcomponent_id,

uint8_tlength);

#ifdefMAVLINK_USE_CONVENIENCE_FUNCTIONS

MAVLINK_HELPERvoid_mav_finalize_message_chan_send(mavlink_channel_tchan,uint8_tmsgid,constchar*packet,uint8_tlength);

#endif

#endif//MAVLINK_CRC_EXTRA

MAVLINK_HELPERuint16_tmavlink_msg_to_send_buffer(uint8_t*buffer,constmavlink_message_t*msg);

MAVLINK_HELPERvoidmavlink_start_checksum(mavlink_message_t*msg);

MAVLINK_HELPERvoidmavlink_update_checksum(mavlink_message_t*msg,uint8_tc);

MAVLINK_HELPERuint8_tmavlink_frame_char_buffer(mavlink_message_t*rxmsg,

mavlink_status_t*status,

uint8_tc,

mavlink_message_t*r_message,

mavlink_status_t*r_mavlink_status);

MAVLINK_HELPERuint8_tmavlink_frame_char(uint8_tchan,uint8_tc,mavlink_message_t*r_message,mavlink_status_t*r_mavlink_status);

MAVLINK_HELPERuint8_tmavlink_parse_char(uint8_tchan,uint8_tc,mavlink_message_t*r_message,mavlink_status_t*r_mavlink_status);

MAVLINK_HELPERuint8_tput_bitfield_n_by_index(int32_tb,uint8_tbits,uint8_tpacket_index,uint8_tbit_index,

uint8_t*r_bit_index,uint8_t*buffer);

#ifdefMAVLINK_USE_CONVENIENCE_FUNCTIONS

MAVLINK_HELPERvoid_mavlink_send_uart(mavlink_channel_tchan,constchar*buf,uint16_tlen);

MAVLINK_HELPERvoid_mavlink_resend_uart(mavlink_channel_tchan,constmavlink_message_t*msg);

#endif


#else


#defineMAVLINK_HELPERstaticinline

#include"mavlink_helpers.h"


#endif//MAVLINK_SEPARATE_HELPERS


/**

*@briefGettherequiredbuffersizeforthismessage

*/

staticinlineuint16_tmavlink_msg_get_send_buffer_length(constmavlink_message_t*msg)

{

returnmsg->len+MAVLINK_NUM_NON_PAYLOAD_BYTES;

}


#ifMAVLINK_NEED_BYTE_SWAP

staticinlinevoidbyte_swap_2(char*dst,constchar*src)

{

dst[0]=src[1];

dst[1]=src[0];

}

staticinlinevoidbyte_swap_4(char*dst,constchar*src)

{

dst[0]=src[3];

dst[1]=src[2];

dst[2]=src[1];

dst[3]=src[0];

}

staticinlinevoidbyte_swap_8(char*dst,constchar*src)

{

dst[0]=src[7];

dst[1]=src[6];

dst[2]=src[5];

dst[3]=src[4];

dst[4]=src[3];

dst[5]=src[2];

dst[6]=src[1];

dst[7]=src[0];

}

#elif!MAVLINK_ALIGNED_FIELDS

staticinlinevoidbyte_copy_2(char*dst,constchar*src)

{

dst[0]=src[0];

dst[1]=src[1];

}

staticinlinevoidbyte_copy_4(char*dst,constchar*src)

{

dst[0]=src[0];

dst[1]=src[1];

dst[2]=src[2];

dst[3]=src[3];

}

staticinlinevoidbyte_copy_8(char*dst,constchar*src)

{

memcpy(dst,src,8);

}

#endif


#define_mav_put_uint8_t(buf,wire_offset,b)buf[wire_offset]=(uint8_t)b

#define_mav_put_int8_t(buf,wire_offset,b)buf[wire_offset]=(int8_t)b

#define_mav_put_char(buf,wire_offset,b)buf[wire_offset]=b


#ifMAVLINK_NEED_BYTE_SWAP

#define_mav_put_uint16_t(buf,wire_offset,b)byte_swap_2(&buf[wire_offset],(constchar*)&b)

#define_mav_put_int16_t(buf,wire_offset,b)byte_swap_2(&buf[wire_offset],(constchar*)&b)

#define_mav_put_uint32_t(buf,wire_offset,b)byte_swap_4(&buf[wire_offset],(constchar*)&b)

#define_mav_put_int32_t(buf,wire_offset,b)byte_swap_4(&buf[wire_offset],(constchar*)&b)

#define_mav_put_uint64_t(buf,wire_offset,b)byte_swap_8(&buf[wire_offset],(constchar*)&b)

#define_mav_put_int64_t(buf,wire_offset,b)byte_swap_8(&buf[wire_offset],(constchar*)&b)

#define_mav_put_float(buf,wire_offset,b)byte_swap_4(&buf[wire_offset],(constchar*)&b)

#define_mav_put_double(buf,wire_offset,b)byte_swap_8(&buf[wire_offset],(constchar*)&b)

#elif!MAVLINK_ALIGNED_FIELDS

#define_mav_put_uint16_t(buf,wire_offset,b)byte_copy_2(&buf[wire_offset],(constchar*)&b)

#define_mav_put_int16_t(buf,wire_offset,b)byte_copy_2(&buf[wire_offset],(constchar*)&b)

#define_mav_put_uint32_t(buf,wire_offset,b)byte_copy_4(&buf[wire_offset],(constchar*)&b)

#define_mav_put_int32_t(buf,wire_offset,b)byte_copy_4(&buf[wire_offset],(constchar*)&b)

#define_mav_put_uint64_t(buf,wire_offset,b)byte_copy_8(&buf[wire_offset],(constchar*)&b)

#define_mav_put_int64_t(buf,wire_offset,b)byte_copy_8(&buf[wire_offset],(constchar*)&b)

#define_mav_put_float(buf,wire_offset,b)byte_copy_4(&buf[wire_offset],(constchar*)&b)

#define_mav_put_double(buf,wire_offset,b)byte_copy_8(&buf[wire_offset],(constchar*)&b)

#else

#define_mav_put_uint16_t(buf,wire_offset,b)*(uint16_t*)&buf[wire_offset]=b

#define_mav_put_int16_t(buf,wire_offset,b)*(int16_t*)&buf[wire_offset]=b

#define_mav_put_uint32_t(buf,wire_offset,b)*(uint32_t*)&buf[wire_offset]=b

#define_mav_put_int32_t(buf,wire_offset,b)*(int32_t*)&buf[wire_offset]=b

#define_mav_put_uint64_t(buf,wire_offset,b)*(uint64_t*)&buf[wire_offset]=b

#define_mav_put_int64_t(buf,wire_offset,b)*(int64_t*)&buf[wire_offset]=b

#define_mav_put_float(buf,wire_offset,b)*(float*)&buf[wire_offset]=b

#define_mav_put_double(buf,wire_offset,b)*(double*)&buf[wire_offset]=b

#endif


/*

likememcpy(),butifsrcisNULL,doamemsettozero

*/

staticinlinevoidmav_array_memcpy(void*dest,constvoid*src,size_tn)

{

if(src==NULL){

memset(dest,0,n);

}else{

memcpy(dest,src,n);

}

}


/*

*Placeachararrayintoabuffer

*/

staticinlinevoid_mav_put_char_array(char*buf,uint8_twire_offset,constchar*b,uint8_tarray_length)

{

mav_array_memcpy(&buf[wire_offset],b,array_length);


}


/*

*Placeauint8_tarrayintoabuffer

*/

staticinlinevoid_mav_put_uint8_t_array(char*buf,uint8_twire_offset,constuint8_t*b,uint8_tarray_length)

{

mav_array_memcpy(&buf[wire_offset],b,array_length);


}


/*

*Placeaint8_tarrayintoabuffer

*/

staticinlinevoid_mav_put_int8_t_array(char*buf,uint8_twire_offset,constint8_t*b,uint8_tarray_length)

{

mav_array_memcpy(&buf[wire_offset],b,array_length);


}


#ifMAVLINK_NEED_BYTE_SWAP

#define_MAV_PUT_ARRAY(TYPE,V)\

staticinlinevoid_mav_put_##TYPE##_array(char*buf,uint8_twire_offset,constTYPE*b,uint8_tarray_length)\

{\

if(b==NULL){\

memset(&buf[wire_offset],0,array_length*sizeof(TYPE));\

}else{\

uint16_ti;\

for(i=0;i<array_length;i++){\

_mav_put_##TYPE(buf,wire_offset+(i*sizeof(TYPE)),b[i]);\

}\

}\

}

#else

#define_MAV_PUT_ARRAY(TYPE,V)\

staticinlinevoid_mav_put_##TYPE##_array(char*buf,uint8_twire_offset,constTYPE*b,uint8_tarray_length)\

{\

mav_array_memcpy(&buf[wire_offset],b,array_length*sizeof(TYPE));\

}

#endif


_MAV_PUT_ARRAY(uint16_t,u16)

_MAV_PUT_ARRAY(uint32_t,u32)

_MAV_PUT_ARRAY(uint64_t,u64)

_MAV_PUT_ARRAY(int16_t,i16)

_MAV_PUT_ARRAY(int32_t,i32)

_MAV_PUT_ARRAY(int64_t,i64)

_MAV_PUT_ARRAY(float,f)

_MAV_PUT_ARRAY(double,d)


#define_MAV_RETURN_char(msg,wire_offset)(constchar)_MAV_PAYLOAD(msg)[wire_offset]

#define_MAV_RETURN_int8_t(msg,wire_offset)(int8_t)_MAV_PAYLOAD(msg)[wire_offset]

#define_MAV_RETURN_uint8_t(msg,wire_offset)(uint8_t)_MAV_PAYLOAD(msg)[wire_offset]


#ifMAVLINK_NEED_BYTE_SWAP

#define_MAV_MSG_RETURN_TYPE(TYPE,SIZE)\

staticinlineTYPE_MAV_RETURN_##TYPE(constmavlink_message_t*msg,uint8_tofs)\

{TYPEr;byte_swap_##SIZE((char*)&r,&_MAV_PAYLOAD(msg)[ofs]);returnr;}


_MAV_MSG_RETURN_TYPE(uint16_t,2)

_MAV_MSG_RETURN_TYPE(int16_t,2)

_MAV_MSG_RETURN_TYPE(uint32_t,4)

_MAV_MSG_RETURN_TYPE(int32_t,4)

_MAV_MSG_RETURN_TYPE(uint64_t,8)

_MAV_MSG_RETURN_TYPE(int64_t,8)

_MAV_MSG_RETURN_TYPE(float,4)

_MAV_MSG_RETURN_TYPE(double,8)


#elif!MAVLINK_ALIGNED_FIELDS

#define_MAV_MSG_RETURN_TYPE(TYPE,SIZE)\

staticinlineTYPE_MAV_RETURN_##TYPE(constmavlink_message_t*msg,uint8_tofs)\

{TYPEr;byte_copy_##SIZE((char*)&r,&_MAV_PAYLOAD(msg)[ofs]);returnr;}


_MAV_MSG_RETURN_TYPE(uint16_t,2)

_MAV_MSG_RETURN_TYPE(int16_t,2)

_MAV_MSG_RETURN_TYPE(uint32_t,4)

_MAV_MSG_RETURN_TYPE(int32_t,4)

_MAV_MSG_RETURN_TYPE(uint64_t,8)

_MAV_MSG_RETURN_TYPE(int64_t,8)

_MAV_MSG_RETURN_TYPE(float,4)

_MAV_MSG_RETURN_TYPE(double,8)

#else//nicelyaligned,noswap

#define_MAV_MSG_RETURN_TYPE(TYPE)\

staticinlineTYPE_MAV_RETURN_##TYPE(constmavlink_message_t*msg,uint8_tofs)\

{return*(constTYPE*)(&_MAV_PAYLOAD(msg)[ofs]);}


_MAV_MSG_RETURN_TYPE(uint16_t)

_MAV_MSG_RETURN_TYPE(int16_t)

_MAV_MSG_RETURN_TYPE(uint32_t)

_MAV_MSG_RETURN_TYPE(int32_t)

_MAV_MSG_RETURN_TYPE(uint64_t)

_MAV_MSG_RETURN_TYPE(int64_t)

_MAV_MSG_RETURN_TYPE(float)

_MAV_MSG_RETURN_TYPE(double)

#endif//MAVLINK_NEED_BYTE_SWAP


staticinlineuint16_t_MAV_RETURN_char_array(constmavlink_message_t*msg,char*value,

uint8_tarray_length,uint8_twire_offset)

{

memcpy(value,&_MAV_PAYLOAD(msg)[wire_offset],array_length);

returnarray_length;

}


staticinlineuint16_t_MAV_RETURN_uint8_t_array(constmavlink_message_t*msg,uint8_t*value,

uint8_tarray_length,uint8_twire_offset)

{

memcpy(value,&_MAV_PAYLOAD(msg)[wire_offset],array_length);

returnarray_length;

}


staticinlineuint16_t_MAV_RETURN_int8_t_array(constmavlink_message_t*msg,int8_t*value,

uint8_tarray_length,uint8_twire_offset)

{

memcpy(value,&_MAV_PAYLOAD(msg)[wire_offset],array_length);

returnarray_length;

}


#ifMAVLINK_NEED_BYTE_SWAP

#define_MAV_RETURN_ARRAY(TYPE,V)\

staticinlineuint16_t_MAV_RETURN_##TYPE##_array(constmavlink_message_t*msg,TYPE*value,\

uint8_tarray_length,uint8_twire_offset)\

{\

uint16_ti;\

for(i=0;i<array_length;i++){\

value[i]=_MAV_RETURN_##TYPE(msg,wire_offset+(i*sizeof(value[0])));\

}\

returnarray_length*sizeof(value[0]);\

}

#else

#define_MAV_RETURN_ARRAY(TYPE,V)\

staticinlineuint16_t_MAV_RETURN_##TYPE##_array(constmavlink_message_t*msg,TYPE*value,\

uint8_tarray_length,uint8_twire_offset)\

{\

memcpy(value,&_MAV_PAYLOAD(msg)[wire_offset],array_length*sizeof(TYPE));\

returnarray_length*sizeof(TYPE);\

}

#endif


_MAV_RETURN_ARRAY(uint16_t,u16)

_MAV_RETURN_ARRAY(uint32_t,u32)

_MAV_RETURN_ARRAY(uint64_t,u64)

_MAV_RETURN_ARRAY(int16_t,i16)

_MAV_RETURN_ARRAY(int32_t,i32)

_MAV_RETURN_ARRAY(int64_t,i64)

_MAV_RETURN_ARRAY(float,f)

_MAV_RETURN_ARRAY(double,d)


#endif//_MAVLINK_PROTOCOL_H_
//作者：恒久力行qq:624668529
[/code]
12.其他代码均未修改，详细移植步骤请参考教程二和教程一。
现在代码已经移植成功。
二：实验测试代码是否移植成功
将代码下载到stm32f407的开发板中去。打开串口调试助手，设置波特率115200,16进制显示接收到的数据。不细讲，看图
1.测试开发板发送功能
然后重启或者复位开发板，会有数据返回（注意：选中16进制显示）。如图收到如图数据表示发送移植成功。复位一次返回一次同样的数据//作者：恒久力行qq:624668529


2.测试开发板接收功能
将接收到的数据原封不动的拷贝下来，（复制下来的是16进制的哦，别弄成字符乱码了，注意从FE到结尾的空格都不能少），然后选中16进制发送同时取消16进制显示。[/b]然后将数据粘贴在发送窗(发送窗一定要清空，不能有空格或者回车让光标删除到最前端)，先清空接收，点击发送，会有如下字符返回，如图表示接受移植成功。点击一次发送返回一次同样的字符//作者：恒久力行qq:624668529


三：分析数据
1.下面是串口助手接收到的开发板发过来的数据
FE0900000000000000000203510403855DFE1C01000021010000000200000003000000040000000500000006000700080009002A16

[/code]
2.下图是消息包的发送格式说明

3.将从开发板收到的16进制的数据以FE为分界将两条消息按格式展开如图




4分析
开发板发出的数据，首先都是以FE开头，先看第一帧数据的第二个字节，LEN代表有效载荷PAYLOAD的长度，经自己亲自数数PAYLOAD确实是9个字节（注意16进制要转换成10进制），同理，再来看第二帧数据的第二个字节1C转换成10进制是28.数一数PAYLOAD的长度确实为28.表示移植数据发送成功。
开发板接收的数据是我们原封不动发回去的数据，其收到后，会对数据进行解析。其对两帧数据解析的结果是（即第二次发回来的字符）：ReceivedmessagewithID0,sequence:0fromcomponent0ofsystem0ReceivedmessagewithID33,sequence:1fromcomponent0ofsystem0
对比messagewithID即是MSG(10进制)[/b]sequence既是SEQ[/b]
[/b]component[/b]既是COMP[/b]
[/b]system[/b]既是SYS[/b]
对比之后发现数据一一对应。即开发板收到数据后成功的解析了数据。表示移植开发板接收部分成功

移植成功。作者：恒久力行qq:624668529

来自为知笔记(Wiz)

附件列表