在前面教程的基础上继续移植优化,之前的没有加缓冲区,没有接收函数功能,这里进行统一的讲解                            作者:恒久力行  qq:624668529

缓冲区对于接受来说很有必要,为了数据的稳定性和实时性必须要加上缓冲。没有缓冲很容易造成数据丢失

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

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

 mavlink_usart_fifo.h
  1. #ifndef _USART_FIFO_H_//作者:恒久力行 qq:624668529
  2. #define _USART_FIFO_H_
  3. #include"stdint.h"
  4. #definetrue1
  5. #definefalse0
  6. typedefstruct _fifo {
  7. uint8_t* buf;
  8. uint16_t length;
  9. uint16_t head;
  10. uint16_t tail;
  11. }fifo_t;
  12. uint8_t fifo_read_ch(fifo_t* fifo,uint8_t* ch);
  13. uint8_t fifo_write_ch(fifo_t* fifo,uint8_t ch);
  14. uint16_t fifo_free(fifo_t* fifo);
  15. uint16_t fifo_used(fifo_t* fifo);
  16. void fifo_init(fifo_t* fifo,uint8_t* buf,uint16_t length);
  17. uint8_t serial_write_buf(uint8_t* buf,uint16_t length);
  18. uint8_t serial_read_ch(void);
  19. uint16_t serial_free(void);
  20. uint16_t serial_available(void);
  21. #endif/*_USART_FIFO_H_*/
 
mavlink_usart_fifo.c
  1. #include"sys.h"//作者:恒久力行 qq:624668529
  2. #include"mavlink_usart_fifo.h"
  3. #define UART_TX_BUFFER_SIZE 255
  4. #define UART_RX_BUFFER_SIZE 255
  5. fifo_t uart_rx_fifo, uart_tx_fifo;
  6. uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE], uart_rx_buf[UART_RX_BUFFER_SIZE];
  7. /** @brief 读FIFO
  8. * @param fifo 待读缓冲区
  9. * *ch 读到的数据
  10. * @return
  11. * 正确读取,1; 无数据,0
  12. */
  13. uint8_t fifo_read_ch(fifo_t* fifo,uint8_t* ch)
  14. {
  15. if(fifo->tail == fifo->head)returnfalse;
  16. *ch = fifo->buf[fifo->tail];
  17. if(++fifo->tail >= fifo->length) fifo->tail =0;
  18. returntrue;
  19. }
  20. /** @brief 写一字节数据到FIFO
  21. * @param fifo 待写入缓冲区
  22. * ch 待写入的数据
  23. * @return
  24. * 正确,1; 缓冲区满,0
  25. */
  26. uint8_t fifo_write_ch(fifo_t* fifo,uint8_t ch)
  27. {
  28. uint16_t h = fifo->head;
  29. if(++h >= fifo->length) h =0;
  30. if(h == fifo->tail)returnfalse;
  31. fifo->buf[fifo->head]= ch;
  32. fifo->head = h;
  33. returntrue;
  34. }
  35. /** @brief 返回缓冲区剩余字节长度
  36. * @param fifo
  37. * @return
  38. * 剩余空间
  39. *
  40. * @note 剩余字节长度大于等于2时,才可写入数据
  41. */
  42. uint16_t fifo_free(fifo_t* fifo)
  43. {
  44. uint16_t free;
  45. if(fifo->head >= fifo->tail) free = fifo->tail +(fifo->length - fifo->head);
  46. else free = fifo->tail - fifo->head;
  47. return free;
  48. }
  49. uint16_t fifo_used(fifo_t* fifo)
  50. {
  51. uint16_t used;
  52. if(fifo->head >= fifo->tail) used = fifo->head - fifo->tail;
  53. else used = fifo->head +(fifo->length - fifo->tail);
  54. return used;
  55. }
  56. /** @brief 初始化缓冲区
  57. * @param *fifo
  58. * *buf
  59. * length
  60. */
  61. void fifo_init(fifo_t* fifo,uint8_t* buf,uint16_t length)
  62. {
  63. uint16_t i;
  64. fifo->buf = buf;
  65. fifo->length = length;
  66. fifo->head =0;
  67. fifo->tail =0;
  68. for(i=0; i<length; i++) fifo->buf[i]=0;
  69. }
  70. /** @brief 写数据到串口,启动发射
  71. *
  72. * @note 数据写入发射缓冲区后,启动发射中断,在中断程序,数据自动发出
  73. */
  74. uint8_t serial_write_buf(uint8_t* buf,uint16_t length){
  75. uint16_t i;
  76. if(length ==0)returnfalse;
  77. for(i =0; length >0; length--, i++){
  78. fifo_write_ch(&uart_tx_fifo, buf[i]);
  79. }
  80. USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
  81. returntrue;
  82. }
  83. /** @brief 自串口读数据
  84. * @return 一字节数据
  85. */
  86. uint8_t serial_read_ch(void){
  87. uint8_t ch;
  88. fifo_read_ch(&uart_rx_fifo,&ch);
  89. return ch;
  90. }
  91. /** @breif 检测发射缓冲区剩余字节长度
  92. * @return 剩余字节长度
  93. */
  94. uint16_t serial_free(void){
  95. return fifo_free(&uart_tx_fifo);
  96. }
  97. uint16_t serial_available(void){
  98. return fifo_used(&uart_rx_fifo);
  99. }
  100. void USART1_IRQHandler(void)
  101. {
  102. uint8_t c;
  103. if(USART_GetITStatus(USART1, USART_IT_RXNE)!= RESET)//数据接收终端
  104. {
  105. c = USART_ReceiveData(USART1);
  106. fifo_write_ch(&uart_rx_fifo, c);
  107. //USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);
  108. }
  109. if(USART_GetITStatus(USART1, USART_IT_TXE)!= RESET)//数据发送中断
  110. {
  111. if(fifo_read_ch(&uart_tx_fifo,&c))
  112. USART_SendData(USART1, c);
  113. else
  114. USART_SendData(USART1,0x55);
  115. if(fifo_used(&uart_tx_fifo)==0)// Check if all data is transmitted . if yes disable transmitter UDRE interrupt
  116. {
  117. // Disable the EVAL_COM1 Transmit interrupt
  118. USART_ITConfig(USART1, USART_IT_TXE, DISABLE);
  119. }
  120. }
  121. }
 

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

  1. #include"sys.h"//作者:恒久力行 qq:624668529
  2. #include"usart.h"
  3. //////////////////////////////////////////////////////////////////////////////////
  4. //如果使用ucos,则包括下面的头文件即可.
  5. #if SYSTEM_SUPPORT_OS
  6. #include"includes.h"//ucos 使用
  7. #endif
  8. //////////////////////////////////////////////////////////////////////////////////
  9. //本程序只供学习使用,未经作者许可,不得用于其它任何用途
  10. //ALIENTEK STM32F4探索者开发板
  11. //串口1初始化
  12. //正点原子@ALIENTEK
  13. //技术论坛:www.openedv.com
  14. //修改日期:2014/6/10
  15. //版本:V1.5
  16. //版权所有,盗版必究。
  17. //Copyright(C) 广州市星翼电子科技有限公司 2009-2019
  18. //All rights reserved
  19. //********************************************************************************
  20. //V1.3修改说明
  21. //支持适应不同频率下的串口波特率设置.
  22. //加入了对printf的支持
  23. //增加了串口接收命令功能.
  24. //修正了printf第一个字符丢失的bug
  25. //V1.4修改说明
  26. //1,修改串口初始化IO的bug
  27. //2,修改了USART_RX_STA,使得串口最大接收字节数为2的14次方
  28. //3,增加了USART_REC_LEN,用于定义串口最大允许接收的字节数(不大于2的14次方)
  29. //4,修改了EN_USART1_RX的使能方式
  30. //V1.5修改说明
  31. //1,增加了对UCOSII的支持
  32. //////////////////////////////////////////////////////////////////////////////////
  33. //////////////////////////////////////////////////////////////////
  34. //加入以下代码,支持printf函数,而不需要选择use MicroLIB
  35. #if 1
  36. #pragmaimport(__use_no_semihosting)
  37. //标准库需要的支持函数
  38. struct __FILE
  39. {
  40. int handle;
  41. };
  42. FILE __stdout;
  43. //定义_sys_exit()以避免使用半主机模式
  44. _sys_exit(int x)
  45. {
  46. x = x;
  47. }
  48. //重定义fputc函数
  49. int fputc(int ch,FILE*f)
  50. {
  51. while((USART1->SR&0X40)==0);//循环发送,直到发送完毕
  52. USART1->DR =(u8) ch;
  53. return ch;
  54. }
  55. #endif
  56. #if EN_USART1_RX //如果使能了接收
  57. //串口1中断服务程序
  58. //注意,读取USARTx->SR能避免莫名其妙的错误
  59. u8 USART_RX_BUF[USART_REC_LEN];//接收缓冲,最大USART_REC_LEN个字节.
  60. //接收状态
  61. //bit15, 接收完成标志
  62. //bit14, 接收到0x0d
  63. //bit13~0, 接收到的有效字节数目
  64. u16 USART_RX_STA=0;//接收状态标记
  65. //初始化IO 串口1
  66. //bound:波特率
  67. void uart_init(u32 bound){
  68. //GPIO端口设置
  69. GPIO_InitTypeDef GPIO_InitStructure;
  70. USART_InitTypeDef USART_InitStructure;
  71. NVIC_InitTypeDef NVIC_InitStructure;
  72. RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);//使能GPIOA时钟
  73. RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
  74. //串口1对应引脚复用映射
  75. GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1);//GPIOA9复用为USART1
  76. GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1);//GPIOA10复用为USART1
  77. //USART1端口配置
  78. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10;//GPIOA9与GPIOA10
  79. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
  80. GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//速度50MHz
  81. GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;//推挽复用输出
  82. GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉
  83. GPIO_Init(GPIOA,&GPIO_InitStructure);//初始化PA9,PA10
  84. //USART1 初始化设置
  85. USART_InitStructure.USART_BaudRate = bound;//波特率设置
  86. USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
  87. USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
  88. USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
  89. USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
  90. USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;//收发模式
  91. USART_Init(USART1,&USART_InitStructure);//初始化串口1
  92. USART_Cmd(USART1, ENABLE);//使能串口1
  93. //USART_ClearFlag(USART1, USART_FLAG_TC);
  94. #if EN_USART1_RX
  95. USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
  96. //Usart1 NVIC 配置
  97. NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
  98. NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
  99. NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;//子优先级3
  100. NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;//IRQ通道使能
  101. NVIC_Init(&NVIC_InitStructure);//根据指定的参数初始化VIC寄存器、
  102. #endif
  103. }
  104. //void USART1_IRQHandler(void) //串口1中断服务程序
  105. //{
  106. // u8 Res;
  107. //#if SYSTEM_SUPPORT_OS //如果SYSTEM_SUPPORT_OS为真,则需要支持OS.
  108. // OSIntEnter();
  109. //#endif
  110. // if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收中断(接收到的数据必须是0x0d 0x0a结尾)
  111. // {
  112. // Res =USART_ReceiveData(USART1);//(USART1->DR); //读取接收到的数据
  113. //
  114. // if((USART_RX_STA&0x8000)==0)//接收未完成
  115. // {
  116. // if(USART_RX_STA&0x4000)//接收到了0x0d
  117. // {
  118. // if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
  119. // else USART_RX_STA|=0x8000; //接收完成了
  120. // }
  121. // else //还没收到0X0D
  122. // {
  123. // if(Res==0x0d)USART_RX_STA|=0x4000;
  124. // else
  125. // {
  126. // USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
  127. // USART_RX_STA++;
  128. // if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收
  129. // }
  130. // }
  131. // }
  132. // }
  133. //#if SYSTEM_SUPPORT_OS //如果SYSTEM_SUPPORT_OS为真,则需要支持OS.
  134. // OSIntExit();
  135. //#endif
  136. //}
  137. #endif
 

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

  1. /** @file mavlink_avoid_errors.h//作者:恒久力行 qq:624668529
  2. * @简介:本文件是由624668529添加,用来统一解决mavlink报错信息
  3. * @see QQ624668529
  4. */
  5. #ifndef MAVLINK_AVOID_ERRORS_H
  6. #define MAVLINK_AVOID_ERRORS_H
  7. #include"stdio.h"
  8. #include"stdint.h"
  9. /*解决..\MAVLINK\common\../mavlink_types.h(53): error: #20: identifier "pack" is undefined*/
  10. #define MAVPACKED( __Declaration__ ) __Declaration__
  11. /*解决..\MAVLINK\common\../mavlink_types.h(53): error: #3092: anonymous unions are only supported in --gnu mode, or when enabled with #pragma anon_unions*/
  12. #pragma anon_unions
  13. #defineinline __inline
  14. //#ifndef memset//由624668529添加 2018-08-24
  15. //void *memset(void *dest, int data, size_t length) {
  16. // uint32_t i;
  17. // int *point = dest;
  18. // for(i=0; i<length; i++) point[i] = data;
  19. // return dest;
  20. //}
  21. //#endif
  22. //#ifndef memcpy//由624668529添加 2018-08-24
  23. //void *memcpy(void *dest, const void *src, size_t n)
  24. //{
  25. // unsigned char *pout = (unsigned char*)dest;
  26. // unsigned char *pin = (unsigned char*)src;
  27. // while (n-- > 0) *pout++ = *pin++;
  28. // return dest;
  29. //}
  30. //#endif
  31. #include"mavlink_types.h"
  32. #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
  33. //#define MAVLINK_SEPARATE_HELPERS
  34. //mavlink_system_t mavlink_system = {0,0};
  35. //mavlink_system_t mavlink_system = {
  36. // 1,
  37. // 1
  38. //}; // System ID, 1-255, Component/Subsystem ID, 1-255
  39. //void comm_send_ch(mavlink_channel_t chan, uint8_t buf)
  40. //{
  41. // chan=chan;
  42. // USART_SendData(USART1, buf); //向串口1发送数据
  43. // while(USART_GetFlagStatus(USART1,USART_FLAG_TC)!=SET);//等待发送结束
  44. //}
  45. #include"mavlink.h"
  46. #include"mavlink_helpers.h"
  47. #endif//AVLINK_AVOID_ERRORS_H
 

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

open_tel_mavlink.h
  1. #ifndef __OPEN_TEL_MAVLINK_H//作者:恒久力行  qq:624668529
  2. #define __OPEN_TEL_MAVLINK_H
  3. //#include "./minimal/minimal/minimal.h"
  4. #include"define.h"
  5. #include"mavlink_avoid_errors.h"
  6. #include"stdint.h"
  7. void mavlink_send_message(mavlink_channel_t chan,enum ap_message id,uint16_t packet_drops);
  8. void update(void);
  9. void handleMessage(mavlink_message_t* msg);
  10. #endif/*__OPENTEL_MAVLINK_H*/
open_tel_mavlink.c
  1. #include"open_tel_mavlink.h"//作者:恒久力行  qq:624668529
  2. ////#include "mavlink_usart_fifo.h"
  3. //#include "mavlink_avoid_errors.h"
  4. //#include "mavlink_types.h"
  5. //#include "mavlink_avoid_errors.h"
  6. #include"define.h"
  7. #include"stdint.h"
  8. ////Add By BigW
  9. typedefuint8_tbool;
  10. //typedef struct {
  11. // char c;
  12. //} prog_char_t;
  13. //
  14. // This is the state of the flight control system
  15. // There are multiple states defined such as STABILIZE, ACRO,
  16. staticint8_t control_mode = STABILIZE;
  17. mavlink_channel_t chan;
  18. //uint16_t packet_drops;
  19. mavlink_heartbeat_t heartbeat;
  20. mavlink_attitude_t attitude;
  21. mavlink_global_position_int_t position;
  22. //mavlink_ahrs_t ahrs;
  23. uint8_t buf[100];
  24. ////End Add By BigW
  25. //// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
  26. // this costs us 51 bytes, but means that low priority
  27. // messages don't block the CPU
  28. staticmavlink_statustext_t pending_status;
  29. // true when we have received at least 1 MAVLink packet
  30. staticbool mavlink_active;
  31. // check if a message will fit in the payload space available
  32. #define CHECK_PAYLOAD_SIZE(id)if(payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false
  33. void handleMessage(mavlink_message_t* msg);
  34. /*
  35. * !!NOTE!!
  36. *
  37. * the use of NOINLINE separate functions for each message type avoids
  38. * a compiler bug in gcc that would cause it to use far more stack
  39. * space than is needed. Without the NOINLINE we use the sum of the
  40. * stack needed for each message type. Please be careful to follow the
  41. * pattern below when adding any new messages
  42. */
  43. static NOINLINE void send_heartbeat(mavlink_channel_t chan)
  44. {
  45. uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
  46. uint8_t system_status = MAV_STATE_ACTIVE;
  47. uint32_t custom_mode = control_mode;
  48. // work out the base_mode. This value is not very useful
  49. // for APM, but we calculate it as best we can so a generic
  50. // MAVLink enabled ground station can work out something about
  51. // what the MAV is up to. The actual bit values are highly
  52. // ambiguous for most of the APM flight modes. In practice, you
  53. // only get useful information from the custom_mode, which maps to
  54. // the APM flight mode and has a well defined meaning in the
  55. // ArduPlane documentation
  56. base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
  57. switch(control_mode){
  58. case AUTO:
  59. case RTL:
  60. case LOITER:
  61. case GUIDED:
  62. case CIRCLE:
  63. base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
  64. // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
  65. // APM does in any mode, as that is defined as "system finds its own goal
  66. // positions", which APM does not currently do
  67. break;
  68. }
  69. // all modes except INITIALISING have some form of manual
  70. // override if stick mixing is enabled
  71. base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
  72. #if HIL_MODE != HIL_MODE_DISABLED
  73. base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
  74. #endif
  75. // we are armed if we are not initialising
  76. if(0){//motors.armed()) {
  77. base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
  78. }
  79. // indicate we have set a custom mode
  80. base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
  81. mavlink_msg_heartbeat_send(
  82. chan,
  83. MAV_TYPE_QUADROTOR,
  84. MAV_AUTOPILOT_ARDUPILOTMEGA,
  85. base_mode,
  86. custom_mode,
  87. system_status);
  88. }
  89. static NOINLINE void send_attitude(mavlink_channel_t chan)
  90. {
  91. mavlink_msg_attitude_send(
  92. chan,
  93. ++buf[1],//millis(),
  94. ++buf[2],//ahrs.roll,
  95. ++buf[3],//ahrs.pitch,
  96. ++buf[4],//ahrs.yaw,
  97. ++buf[5],//omega.x,
  98. ++buf[6],//omega.y,
  99. ++buf[7]);//omega.z);
  100. }
  101. staticvoid NOINLINE send_location(mavlink_channel_t chan)
  102. {
  103. //Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now
  104. mavlink_msg_global_position_int_send(
  105. chan,
  106. 1,//millis(),
  107. 2,//current_loc.lat, // in 1E7 degrees
  108. 3,//current_loc.lng, // in 1E7 degrees
  109. 4,//g_gps->altitude * 10, // millimeters above sea level
  110. 5,//(current_loc.alt - home.alt) * 10, // millimeters above ground
  111. 6,//g_gps->ground_speed * rot.a.x, // X speed cm/s
  112. 7,//g_gps->ground_speed * rot.b.x, // Y speed cm/s
  113. 8,//g_gps->ground_speed * rot.c.x,
  114. 9);//g_gps->ground_course); // course in 1/100 degree
  115. }
  116. //static void NOINLINE send_ahrs(mavlink_channel_t chan)
  117. //{
  118. // //Vector3f omega_I = ahrs.get_gyro_drift();
  119. // mavlink_msg_ahrs_send(
  120. // chan,
  121. // ++buf[8],//omega_I.x,
  122. // ++buf[9],//omega_I.y,
  123. // ++buf[10],//omega_I.z,
  124. // 1,
  125. // 0,
  126. // ++buf[11],//ahrs.get_error_rp(),
  127. // ++buf[12]);//ahrs.get_error_yaw());
  128. //}
  129. staticvoid NOINLINE send_statustext(mavlink_channel_t chan)
  130. {
  131. }
  132. // are we still delaying telemetry to try to avoid Xbee bricking?
  133. staticbool telemetry_delayed(mavlink_channel_t chan)
  134. {
  135. returnfalse;
  136. }
  137. // try to send a message, return false if it won't fit in the serial tx buffer
  138. staticbool mavlink_try_send_message(mavlink_channel_t chan,enum ap_message id,uint16_t packet_drops)
  139. {
  140. int16_t payload_space = serial_free();
  141. if(telemetry_delayed(chan)){
  142. returnfalse;
  143. }
  144. switch(id){
  145. case MSG_HEARTBEAT:
  146. CHECK_PAYLOAD_SIZE(HEARTBEAT);
  147. send_heartbeat(chan);
  148. break;
  149. case MSG_ATTITUDE:
  150. CHECK_PAYLOAD_SIZE(ATTITUDE);
  151. send_attitude(chan);
  152. break;
  153. case MSG_LOCATION:
  154. CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
  155. send_location(chan);
  156. break;
  157. // case MSG_AHRS:
  158. // CHECK_PAYLOAD_SIZE(AHRS);
  159. // send_ahrs(chan);
  160. // break;
  161. case MSG_STATUSTEXT:
  162. CHECK_PAYLOAD_SIZE(STATUSTEXT);
  163. send_statustext(chan);
  164. break;
  165. default:
  166. break;
  167. }
  168. returntrue;
  169. }
  170. #define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
  171. staticstruct mavlink_queue {
  172. enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
  173. uint8_t next_deferred_message;
  174. uint8_t num_deferred_messages;
  175. } mavlink_queue[2];
  176. // send a message using mavlink
  177. void mavlink_send_message(mavlink_channel_t chan,enum ap_message id,uint16_t packet_drops)
  178. {
  179. uint8_t i, nextid;
  180. struct mavlink_queue *q =&mavlink_queue[(uint8_t)chan];
  181. // see if we can send the deferred messages, if any
  182. while(q->num_deferred_messages !=0){
  183. if(!mavlink_try_send_message(chan,
  184. q->deferred_messages[q->next_deferred_message],
  185. packet_drops)){
  186. break;
  187. }
  188. q->next_deferred_message++;
  189. if(q->next_deferred_message == MAX_DEFERRED_MESSAGES){
  190. q->next_deferred_message =0;
  191. }
  192. q->num_deferred_messages--;
  193. }
  194. if(id == MSG_RETRY_DEFERRED){
  195. return;
  196. }
  197. // this message id might already be deferred
  198. for(i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++){
  199. if(q->deferred_messages[nextid]== id){
  200. // its already deferred, discard
  201. return;
  202. }
  203. nextid++;
  204. if(nextid == MAX_DEFERRED_MESSAGES){
  205. nextid =0;
  206. }
  207. }
  208. if(q->num_deferred_messages !=0||
  209. !mavlink_try_send_message(chan, id, packet_drops)){
  210. // can't send it now, so defer it
  211. if(q->num_deferred_messages == MAX_DEFERRED_MESSAGES){
  212. // the defer buffer is full, discard
  213. return;
  214. }
  215. nextid = q->next_deferred_message + q->num_deferred_messages;
  216. if(nextid >= MAX_DEFERRED_MESSAGES){
  217. nextid -= MAX_DEFERRED_MESSAGES;
  218. }
  219. q->deferred_messages[nextid]= id;
  220. q->num_deferred_messages++;
  221. }
  222. }
  223. void mavlink_send_text(mavlink_channel_t chan,enum gcs_severity severity,char*str)
  224. {
  225. if(telemetry_delayed(chan)){
  226. return;
  227. }
  228. if(severity == SEVERITY_LOW){
  229. // send via the deferred queuing system
  230. pending_status.severity =(uint8_t)severity;
  231. mav_array_memcpy((char*)pending_status.text, str,sizeof(pending_status.text));
  232. mavlink_send_message(chan, MSG_STATUSTEXT,0);
  233. }else{
  234. // send immediately
  235. mavlink_msg_statustext_send(
  236. chan,
  237. severity,
  238. str);
  239. }
  240. }
  241. void update(void)
  242. {
  243. // receive new packets
  244. mavlink_message_t msg;
  245. mavlink_status_t status;
  246. status.packet_rx_drop_count =0;
  247. // process received bytes
  248. while(serial_available())
  249. {
  250. uint8_t c = serial_read_ch();
  251. // Try to get a new message
  252. if(mavlink_parse_char(chan, c,&msg,&status)){
  253. mavlink_active =true;
  254. //printf("%c",c);
  255. printf("Received message with ID %d, sequence: %d from component %d of system %d",\
  256. msg.msgid, msg.seq, msg.compid, msg.sysid);
  257. handleMessage(&msg);
  258. }
  259. }
  260. }
  261. void handleMessage(mavlink_message_t* msg)
  262. {
  263. //struct Location tell_command = {}; // command for telemetry
  264. switch(msg->msgid){
  265. case MAVLINK_MSG_ID_HEARTBEAT:{
  266. mavlink_msg_heartbeat_decode(msg,&heartbeat);
  267. break;
  268. }
  269. case MAVLINK_MSG_ID_ATTITUDE:{
  270. mavlink_msg_attitude_decode(msg,&attitude);
  271. break;
  272. }
  273. case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:{
  274. mavlink_msg_global_position_int_decode(msg,&position);
  275. break;
  276. }
  277. //
  278. // case MAVLINK_MSG_ID_AHRS: {
  279. // mavlink_msg_ahrs_decode(msg, &ahrs);
  280. // break;
  281. // }
  282. default:
  283. break;
  284. }// end switch
  285. }// end handle mavlink

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

  1. // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
  2. #ifndef _DEFINES_H//作者:恒久力行  qq:624668529
  3. #define _DEFINES_H
  4. // this avoids a very common config error
  5. #define ENABLE ENABLED
  6. #define DISABLE DISABLED
  7. // Flight modes
  8. // ------------
  9. #define YAW_HOLD 0
  10. #define YAW_ACRO 1
  11. #define YAW_AUTO 2
  12. #define YAW_LOOK_AT_HOME 3
  13. #define YAW_TOY 4// THOR This is the Yaw mode
  14. #define ROLL_PITCH_STABLE 0
  15. #define ROLL_PITCH_ACRO 1
  16. #define ROLL_PITCH_AUTO 2
  17. #define ROLL_PITCH_STABLE_OF 3
  18. #define ROLL_PITCH_TOY 4// THOR This is the Roll and Pitch
  19. // mode
  20. #define THROTTLE_MANUAL 0
  21. #define THROTTLE_HOLD 1
  22. #define THROTTLE_AUTO 2
  23. // active altitude sensor
  24. // ----------------------
  25. #define SONAR 0
  26. #define BARO 1
  27. #define SONAR_SOURCE_ADC 1
  28. #define SONAR_SOURCE_ANALOG_PIN 2
  29. // CH 7 control
  30. #define CH7_DO_NOTHING 0
  31. #define CH7_SET_HOVER 1
  32. #define CH7_FLIP 2
  33. #define CH7_SIMPLE_MODE 3
  34. #define CH7_RTL 4
  35. #define CH7_AUTO_TRIM 5
  36. #define CH7_ADC_FILTER 6
  37. #define CH7_SAVE_WP 7
  38. #define CH7_MULTI_MODE 8
  39. // Frame types
  40. #define QUAD_FRAME 0
  41. #define TRI_FRAME 1
  42. #define HEXA_FRAME 2
  43. #define Y6_FRAME 3
  44. #define OCTA_FRAME 4
  45. #define HELI_FRAME 5
  46. #define OCTA_QUAD_FRAME 6
  47. #define PLUS_FRAME 0
  48. #define X_FRAME 1
  49. #define V_FRAME 2
  50. // LED output
  51. #define NORMAL_LEDS 0
  52. #define AUTO_TRIM_LEDS 1
  53. #define CH_7_PWM_TRIGGER 1800
  54. #define CH_6_PWM_TRIGGER_HIGH 1800
  55. #define CH_6_PWM_TRIGGER 1500
  56. #define CH_6_PWM_TRIGGER_LOW 1200
  57. // Internal defines, don't edit and expect things to work
  58. // -------------------------------------------------------
  59. #define TRUE 1
  60. #define FALSE 0
  61. #defineToRad(x)(x*0.01745329252)// *pi/180
  62. #defineToDeg(x)(x*57.2957795131)// *180/pi
  63. #define DEBUG 0
  64. #define LOITER_RANGE 60// for calculating power outside of loiter radius
  65. #define T6 1000000
  66. #define T7 10000000
  67. // GPS type codes - use the names, not the numbers
  68. #define GPS_PROTOCOL_NONE -1
  69. #define GPS_PROTOCOL_NMEA 0
  70. #define GPS_PROTOCOL_SIRF 1
  71. #define GPS_PROTOCOL_UBLOX 2
  72. #define GPS_PROTOCOL_IMU 3
  73. #define GPS_PROTOCOL_MTK 4
  74. #define GPS_PROTOCOL_HIL 5
  75. #define GPS_PROTOCOL_MTK16 6
  76. #define GPS_PROTOCOL_AUTO 7
  77. #define CH_ROLL CH_1
  78. #define CH_PITCH CH_2
  79. #define CH_THROTTLE CH_3
  80. #define CH_RUDDER CH_4
  81. #define CH_YAW CH_4
  82. #define RC_CHANNEL_ANGLE 0
  83. #define RC_CHANNEL_RANGE 1
  84. #define RC_CHANNEL_ANGLE_RAW 2
  85. // HIL enumerations
  86. #define HIL_MODE_DISABLED 0
  87. #define HIL_MODE_ATTITUDE 1
  88. #define HIL_MODE_SENSORS 2
  89. #define ASCENDING 1
  90. #define DESCENDING -1
  91. #define REACHED_ALT 0
  92. // Auto Pilot modes
  93. // ----------------
  94. #define STABILIZE 0// hold level position
  95. #define ACRO 1// rate control
  96. #define ALT_HOLD 2// AUTO control
  97. #define AUTO 3// AUTO control
  98. #define GUIDED 4// AUTO control
  99. #define LOITER 5// Hold a single location
  100. #define RTL 6// AUTO control
  101. #define CIRCLE 7// AUTO control
  102. #define POSITION 8// AUTO control
  103. #define LAND 9// AUTO control
  104. #define OF_LOITER 10// Hold a single location using optical flow
  105. // sensor
  106. #define TOY_A 11// THOR Enum for Toy mode
  107. #define TOY_M 12// THOR Enum for Toy mode
  108. #define NUM_MODES 13
  109. #define SIMPLE_1 1
  110. #define SIMPLE_2 2
  111. #define SIMPLE_3 4
  112. #define SIMPLE_4 8
  113. #define SIMPLE_5 16
  114. #define SIMPLE_6 32
  115. // CH_6 Tuning
  116. // -----------
  117. #define CH6_NONE 0
  118. // Attitude
  119. #define CH6_STABILIZE_KP 1
  120. #define CH6_STABILIZE_KI 2
  121. #define CH6_STABILIZE_KD 29// duplicate with CH6_DAMP
  122. #define CH6_YAW_KP 3
  123. #define CH6_YAW_KI 24
  124. // Rate
  125. #define CH6_ACRO_KP 25
  126. #define CH6_RATE_KP 4
  127. #define CH6_RATE_KI 5
  128. #define CH6_RATE_KD 21
  129. #define CH6_YAW_RATE_KP 6
  130. #define CH6_YAW_RATE_KD 26
  131. // Altitude rate controller
  132. #define CH6_THROTTLE_KP 7
  133. // Extras
  134. #define CH6_TOP_BOTTOM_RATIO 8
  135. #define CH6_RELAY 9
  136. // Navigation
  137. #define CH6_TRAVERSE_SPEED 10// maximum speed to next way point
  138. #define CH6_NAV_KP 11
  139. #define CH6_LOITER_KP 12
  140. #define CH6_LOITER_KI 27
  141. // Trad Heli specific
  142. #define CH6_HELI_EXTERNAL_GYRO 13
  143. // altitude controller
  144. #define CH6_THR_HOLD_KP 14
  145. #define CH6_Z_GAIN 15
  146. #define CH6_DAMP 16// duplicate with CH6_YAW_RATE_KD
  147. // optical flow controller
  148. #define CH6_OPTFLOW_KP 17
  149. #define CH6_OPTFLOW_KI 18
  150. #define CH6_OPTFLOW_KD 19
  151. #define CH6_NAV_I 20
  152. #define CH6_LOITER_RATE_KP 22
  153. #define CH6_LOITER_RATE_KI 28
  154. #define CH6_LOITER_RATE_KD 23
  155. #define CH6_AHRS_YAW_KP 30
  156. #define CH6_AHRS_KP 31
  157. // nav byte mask
  158. // -------------
  159. #define NAV_LOCATION 1
  160. #define NAV_ALTITUDE 2
  161. #define NAV_DELAY 4
  162. // Commands - Note that APM now uses a subset of the MAVLink protocol
  163. // commands. See enum MAV_CMD in the GCS_Mavlink library
  164. #define CMD_BLANK 0// there is no command stored in the mem location
  165. // requested
  166. #define NO_COMMAND 0
  167. #define LOITER_MODE 1
  168. #define WP_MODE 2
  169. #define CIRCLE_MODE 3
  170. #define NO_NAV_MODE 4
  171. #define TOY_MODE 5// THOR This mode defines the Virtual
  172. // WP following mode
  173. // TOY mixing options
  174. #define TOY_LOOKUP_TABLE 0
  175. #define TOY_LINEAR_MIXER 1
  176. #define TOY_EXTERNAL_MIXER 2
  177. // Waypoint options
  178. #define MASK_OPTIONS_RELATIVE_ALT 1
  179. #define WP_OPTION_ALT_CHANGE 2
  180. #define WP_OPTION_YAW 4
  181. #define WP_OPTION_ALT_REQUIRED 8
  182. #define WP_OPTION_RELATIVE 16
  183. //#define WP_OPTION_ 32
  184. //#define WP_OPTION_ 64
  185. #define WP_OPTION_NEXT_CMD 128
  186. //repeating events
  187. #define NO_REPEAT 0
  188. #define CH_5_TOGGLE 1
  189. #define CH_6_TOGGLE 2
  190. #define CH_7_TOGGLE 3
  191. #define CH_8_TOGGLE 4
  192. #define RELAY_TOGGLE 5
  193. #define STOP_REPEAT 10
  194. // GCS Message ID's
  195. /// NOTE: to ensure we never block on sending MAVLink messages
  196. /// please keep each MSG_ to a single MAVLink message. If need be
  197. /// create new MSG_ IDs for additional messages on the same
  198. /// stream
  199. enum ap_message {
  200. MSG_HEARTBEAT,
  201. MSG_ATTITUDE,
  202. MSG_LOCATION,
  203. MSG_EXTENDED_STATUS1,
  204. MSG_EXTENDED_STATUS2,
  205. MSG_NAV_CONTROLLER_OUTPUT,
  206. MSG_CURRENT_WAYPOINT,
  207. MSG_VFR_HUD,
  208. MSG_RADIO_OUT,
  209. MSG_RADIO_IN,
  210. MSG_RAW_IMU1,
  211. MSG_RAW_IMU2,
  212. MSG_RAW_IMU3,
  213. MSG_GPS_STATUS,
  214. MSG_GPS_RAW,
  215. MSG_SERVO_OUT,
  216. MSG_NEXT_WAYPOINT,
  217. MSG_NEXT_PARAM,
  218. MSG_STATUSTEXT,
  219. MSG_LIMITS_STATUS,
  220. MSG_AHRS,
  221. MSG_SIMSTATE,
  222. MSG_HWSTATUS,
  223. MSG_RETRY_DEFERRED // this must be last
  224. };
  225. enum gcs_severity {
  226. SEVERITY_LOW=1,
  227. SEVERITY_MEDIUM,
  228. SEVERITY_HIGH,
  229. SEVERITY_CRITICAL
  230. };
  231. // Logging parameters
  232. #define TYPE_AIRSTART_MSG 0x00
  233. #define TYPE_GROUNDSTART_MSG 0x01
  234. #define LOG_ATTITUDE_MSG 0x01
  235. #define LOG_GPS_MSG 0x02
  236. #define LOG_MODE_MSG 0x03
  237. #define LOG_CONTROL_TUNING_MSG 0x04
  238. #define LOG_NAV_TUNING_MSG 0x05
  239. #define LOG_PERFORMANCE_MSG 0x06
  240. #define LOG_RAW_MSG 0x07
  241. #define LOG_CMD_MSG 0x08
  242. #define LOG_CURRENT_MSG 0x09
  243. #define LOG_STARTUP_MSG 0x0A
  244. #define LOG_MOTORS_MSG 0x0B
  245. #define LOG_OPTFLOW_MSG 0x0C
  246. #define LOG_DATA_MSG 0x0D
  247. #define LOG_PID_MSG 0x0E
  248. #define LOG_ITERM_MSG 0x0F
  249. #define LOG_DMP_MSG 0x10
  250. #define LOG_INDEX_MSG 0xF0
  251. #define MAX_NUM_LOGS 50
  252. #define MASK_LOG_ATTITUDE_FAST (1<<0)
  253. #define MASK_LOG_ATTITUDE_MED (1<<1)
  254. #define MASK_LOG_GPS (1<<2)
  255. #define MASK_LOG_PM (1<<3)
  256. #define MASK_LOG_CTUN (1<<4)
  257. #define MASK_LOG_NTUN (1<<5)
  258. #define MASK_LOG_MODE (1<<6)
  259. #define MASK_LOG_RAW (1<<7)
  260. #define MASK_LOG_CMD (1<<8)
  261. #define MASK_LOG_CUR (1<<9)
  262. #define MASK_LOG_MOTORS (1<<10)
  263. #define MASK_LOG_OPTFLOW (1<<11)
  264. #define MASK_LOG_PID (1<<12)
  265. #define MASK_LOG_ITERM (1<<13)
  266. // Waypoint Modes
  267. // ----------------
  268. #define ABS_WP 0
  269. #define REL_WP 1
  270. // Command Queues
  271. // ---------------
  272. #define COMMAND_MUST 0
  273. #define COMMAND_MAY 1
  274. #define COMMAND_NOW 2
  275. // Events
  276. // ------
  277. #define EVENT_WILL_REACH_WAYPOINT 1
  278. #define EVENT_SET_NEW_WAYPOINT_INDEX 2
  279. #define EVENT_LOADED_WAYPOINT 3
  280. #define EVENT_LOOP 4
  281. // Climb rate calculations
  282. #define ALTITUDE_HISTORY_LENGTH 8//Number of (time,altitude) points to
  283. // regress a climb rate from
  284. #define BATTERY_VOLTAGE(x)(x*(g.input_voltage/1024.0))*g.volt_div_ratio
  285. #define CURRENT_AMPS(x)((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
  286. //#define BARO_FILTER_SIZE 8
  287. /* ************************************************************** */
  288. /* Expansion PIN's that people can use for various things. */
  289. // AN0 - 7 are located at edge of IMU PCB "above" pressure sensor and
  290. // Expansion port
  291. // AN0 - 5 are also located next to voltage dividers and sliding SW2 switch
  292. // AN0 - 3 has 10kOhm resistor in serial, include 3.9kOhm to make it as
  293. // voltage divider
  294. // AN4 - 5 are direct GPIO pins from atmega1280 and they are the latest pins
  295. // next to SW2 switch
  296. // Look more ArduCopter Wiki for voltage dividers and other ports
  297. #define AN0 54// resistor, vdiv use, divider 1 closest to relay
  298. #define AN1 55// resistor, vdiv use, divider 2
  299. #define AN2 56// resistor, vdiv use, divider 3
  300. #define AN3 57// resistor, vdiv use, divider 4 closest to SW2
  301. #define AN4 58// direct GPIO pin, default as analog input, next to SW2
  302. // switch
  303. #define AN5 59// direct GPIO pin, default as analog input, next to SW2
  304. // switch
  305. #define AN6 60// direct GPIO pin, default as analog input, close to
  306. // Pressure sensor, Expansion Ports
  307. #define AN7 61// direct GPIO pin, default as analog input, close to
  308. // Pressure sensor, Expansion Ports
  309. // AN8 - 15 are located at edge of IMU PCB "above" pressure sensor and
  310. // Expansion port
  311. // AN8 - 15 PINs are not connected anywhere, they are located as last 8 pins
  312. // on edge of the board above Expansion Ports
  313. // even pins (8,10,12,14) are at edge of board, Odd pins (9,11,13,15) are on
  314. // inner row
  315. #define AN8 62// NC
  316. #define AN9 63// NC
  317. #define AN10 64// NC
  318. #define AN11 65// NC
  319. #define AN12 66// NC
  320. #define AN13 67// NC
  321. #define AN14 68// NC
  322. #define AN15 69// NC
  323. #define RELAY_PIN 47
  324. #define PIEZO_PIN AN5 //Last pin on the back ADC connector
  325. // sonar
  326. //#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
  327. // RADIANS
  328. #define RADX100 0.000174532925
  329. #define DEGX100 5729.57795
  330. // EEPROM addresses
  331. #define EEPROM_MAX_ADDR 4096
  332. // parameters get the first 1536 bytes of EEPROM, remainder is for waypoints
  333. #define WP_START_BYTE 0x600// where in memory home WP is stored + all other
  334. // WP
  335. #define WP_SIZE 15
  336. #define ONBOARD_PARAM_NAME_LENGTH 15
  337. // fence points are stored at the end of the EEPROM
  338. #define MAX_FENCEPOINTS 6
  339. #define FENCE_WP_SIZE sizeof(Vector2l)
  340. #define FENCE_START_BYTE (EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE))
  341. #define MAX_WAYPOINTS ((FENCE_START_BYTE - WP_START_BYTE)/ WP_SIZE)-1// -
  342. // 1
  343. // to
  344. // be
  345. // safe
  346. // mark a function as not to be inlined
  347. #define NOINLINE __attribute__((noinline))
  348. // IMU selection
  349. #define CONFIG_IMU_OILPAN 1
  350. #define CONFIG_IMU_MPU6000 2
  351. // APM Hardware selection
  352. #define APM_HARDWARE_APM1 1
  353. #define APM_HARDWARE_APM2 2
  354. #define AP_BARO_BMP085 1
  355. #define AP_BARO_MS5611 2
  356. #define LOGGING_SIMPLE 1
  357. #define LOGGING_VERBOSE 2
  358. // Channel Config selection
  359. #define CHANNEL_CONFIG_DEFAULT 1
  360. #define CHANNEL_CONFIG_CUSTOM 2
  361. #endif// _DEFINES_H//作者:恒久力行  qq:624668529

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

main.c
  1. #include"sys.h"//作者:恒久力行  qq:624668529
  2. #include"delay.h"
  3. #include"usart.h"
  4. #include"led.h"
  5. #include"beep.h"
  6. #include"key.h"
  7. #include"mavlink_avoid_errors.h"
  8. #include"mavlink_usart_fifo.h"
  9. #include"open_tel_mavlink.h"
  10. //ALIENTEK 探索者STM32F407开发板 实验4
  11. //串口通信实验 -库函数版本
  12. //技术支持:www.openedv.com
  13. //淘宝店铺:http://eboard.taobao.com
  14. //广州市星翼电子科技有限公司
  15. //作者:正点原子 @ALIENTEK
  16. mavlink_system_t mavlink_system;
  17. #define UART_TX_BUFFER_SIZE 255
  18. #define UART_RX_BUFFER_SIZE 255
  19. externfifo_t uart_rx_fifo, uart_tx_fifo;
  20. externuint8_t uart_tx_buf[UART_TX_BUFFER_SIZE], uart_rx_buf[UART_RX_BUFFER_SIZE];
  21. int main(void)
  22. {
  23. NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
  24. delay_init(168);//延时初始化
  25. uart_init(115200);//串口初始化波特率为115200
  26. LED_Init();//初始化与LED连接的硬件接口
  27. fifo_init(&uart_tx_fifo, uart_tx_buf, UART_TX_BUFFER_SIZE);
  28. fifo_init(&uart_rx_fifo, uart_rx_buf, UART_RX_BUFFER_SIZE);
  29. mavlink_system.sysid = MAV_TYPE_GENERIC;
  30. mavlink_system.compid = MAV_AUTOPILOT_GENERIC;
  31. while(1)
  32. {
  33. mavlink_send_message(0, MSG_HEARTBEAT,0);
  34. mavlink_send_message(0, MSG_LOCATION,0);
  35. while(1)
  36. {
  37. // if(tranlTimer > 100)
  38. // {
  39. // tranlTimer = 0;
  40. // mavlink_send_message(0, MSG_HEARTBEAT, 0);
  41. // mavlink_send_message(0, MSG_ATTITUDE, 0);
  42. // mavlink_send_message(0, MSG_AHRS, 0);
  43. // }
  44. update();
  45. }
  46. }
  47. }

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

跟原来官方不同的还有MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t*buffer,constmavlink_message_t*msg)
这个函数参数定义到函数最前面了。全代码是修改后的,有兴趣的可以去比较。不修改的话会报错。
  1. #include"mavlink_usart_fifo.h"
  2. externmavlink_system_t mavlink_system;
  3. MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan,constchar*buf,uint16_t len)
  4. {
  5. serial_write_buf((uint8_t*)buf, len);
  6. }
 
mavlink_helpers.h全代码
  1. #ifndef _MAVLINK_HELPERS_H_//作者:恒久力行  qq:624668529
  2. #define _MAVLINK_HELPERS_H_
  3. #include"string.h"
  4. #include"checksum.h"
  5. #include"mavlink_types.h"
  6. #include"mavlink_conversions.h"
  7. #ifndef MAVLINK_HELPER
  8. #define MAVLINK_HELPER
  9. #endif
  10. externmavlink_system_t mavlink_system;
  11. /*
  12. * Internal function to give access to the channel status for each channel
  13. */
  14. #ifndef MAVLINK_GET_CHANNEL_STATUS
  15. MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
  16. {
  17. #ifdef MAVLINK_EXTERNAL_RX_STATUS
  18. // No m_mavlink_status array defined in function,
  19. // has to be defined externally
  20. #else
  21. staticmavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
  22. #endif
  23. return&m_mavlink_status[chan];
  24. }
  25. #endif
  26. /*
  27. * Internal function to give access to the channel buffer for each channel
  28. */
  29. #ifndef MAVLINK_GET_CHANNEL_BUFFER
  30. MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
  31. {
  32. #ifdef MAVLINK_EXTERNAL_RX_BUFFER
  33. // No m_mavlink_buffer array defined in function,
  34. // has to be defined externally
  35. #else
  36. staticmavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
  37. #endif
  38. return&m_mavlink_buffer[chan];
  39. }
  40. #endif
  41. /**
  42. * @brief Reset the status of a channel.
  43. */
  44. MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
  45. {
  46. mavlink_status_t*status = mavlink_get_channel_status(chan);
  47. status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  48. }
  49. /**
  50. * @brief Finalize a MAVLink message with channel assignment
  51. *
  52. * This function calculates the checksum and sets length and aircraft id correctly.
  53. * It assumes that the message id and the payload are already correctly set. This function
  54. * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
  55. * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
  56. *
  57. * @param msg Message to finalize
  58. * @param system_id Id of the sending (this) system, 1-127
  59. * @param length Message length
  60. */
  61. #if MAVLINK_CRC_EXTRA
  62. MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg,uint8_t system_id,uint8_t component_id,
  63. uint8_t chan,uint8_t min_length,uint8_t length,uint8_t crc_extra)
  64. #else
  65. MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg,uint8_t system_id,uint8_t component_id,
  66. uint8_t chan,uint8_t length)
  67. #endif
  68. {
  69. // This is only used for the v2 protocol and we silence it here
  70. (void)min_length;
  71. // This code part is the same for all messages;
  72. msg->magic = MAVLINK_STX;
  73. msg->len = length;
  74. msg->sysid = system_id;
  75. msg->compid = component_id;
  76. // One sequence number per channel
  77. msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
  78. mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
  79. msg->checksum = crc_calculate(((constuint8_t*)(msg))+3, MAVLINK_CORE_HEADER_LEN);
  80. crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
  81. #if MAVLINK_CRC_EXTRA
  82. crc_accumulate(crc_extra,&msg->checksum);
  83. #endif
  84. mavlink_ck_a(msg)=(uint8_t)(msg->checksum &0xFF);
  85. mavlink_ck_b(msg)=(uint8_t)(msg->checksum >>8);
  86. return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
  87. }
  88. /**
  89. * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
  90. */
  91. #if MAVLINK_CRC_EXTRA
  92. MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg,uint8_t system_id,uint8_t component_id,
  93. uint8_t min_length,uint8_t length,uint8_t crc_extra)
  94. {
  95. return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
  96. }
  97. #else
  98. MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg,uint8_t system_id,uint8_t component_id,
  99. uint8_t length)
  100. {
  101. return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
  102. }
  103. #endif
  104. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  105. MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan,constchar*buf,uint16_t len);
  106. /**
  107. * @brief Finalize a MAVLink message with channel assignment and send
  108. */
  109. #if MAVLINK_CRC_EXTRA
  110. MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan,uint8_t msgid,constchar*packet,
  111. uint8_t min_length,uint8_t length,uint8_t crc_extra)
  112. #else
  113. MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan,uint8_t msgid,constchar*packet,uint8_t length)
  114. #endif
  115. {
  116. uint16_t checksum;
  117. uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
  118. uint8_t ck[2];
  119. mavlink_status_t*status = mavlink_get_channel_status(chan);
  120. buf[0]= MAVLINK_STX;
  121. buf[1]= length;
  122. buf[2]= status->current_tx_seq;
  123. buf[3]= mavlink_system.sysid;
  124. buf[4]= mavlink_system.compid;
  125. buf[5]= msgid;
  126. status->current_tx_seq++;
  127. checksum = crc_calculate((constuint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
  128. crc_accumulate_buffer(&checksum, packet, length);
  129. #if MAVLINK_CRC_EXTRA
  130. crc_accumulate(crc_extra,&checksum);
  131. #endif
  132. ck[0]=(uint8_t)(checksum &0xFF);
  133. ck[1]=(uint8_t)(checksum >>8);
  134. MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES +(uint16_t)length);
  135. _mavlink_send_uart(chan,(constchar*)buf, MAVLINK_NUM_HEADER_BYTES);
  136. _mavlink_send_uart(chan, packet, length);
  137. _mavlink_send_uart(chan,(constchar*)ck,2);
  138. MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES +(uint16_t)length);
  139. }
  140. /**
  141. * @brief re-send a message over a uart channel
  142. * this is more stack efficient than re-marshalling the message
  143. */
  144. MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan,constmavlink_message_t*msg)
  145. {
  146. uint8_t ck[2];
  147. ck[0]=(uint8_t)(msg->checksum &0xFF);
  148. ck[1]=(uint8_t)(msg->checksum >>8);
  149. // XXX use the right sequence here
  150. MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
  151. _mavlink_send_uart(chan,(constchar*)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
  152. _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
  153. _mavlink_send_uart(chan,(constchar*)ck,2);
  154. MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
  155. }
  156. #endif// MAVLINK_USE_CONVENIENCE_FUNCTIONS
  157. /**
  158. * @brief Pack a message to send it over a serial byte stream
  159. */
  160. MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t*buffer,constmavlink_message_t*msg)
  161. {
  162. uint8_t*ck;
  163. memcpy(buffer,(constuint8_t*)&msg->magic, MAVLINK_NUM_HEADER_BYTES +(uint16_t)msg->len);
  164. ck = buffer +(MAVLINK_NUM_HEADER_BYTES +(uint16_t)msg->len);
  165. ck[0]=(uint8_t)(msg->checksum &0xFF);
  166. ck[1]=(uint8_t)(msg->checksum >>8);
  167. return MAVLINK_NUM_NON_PAYLOAD_BYTES +(uint16_t)msg->len;
  168. }
  169. union __mavlink_bitfield {
  170. uint8_tuint8;
  171. int8_tint8;
  172. uint16_tuint16;
  173. int16_tint16;
  174. uint32_tuint32;
  175. int32_tint32;
  176. };
  177. MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
  178. {
  179. crc_init(&msg->checksum);
  180. }
  181. MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg,uint8_t c)
  182. {
  183. crc_accumulate(c,&msg->checksum);
  184. }
  185. /**
  186. * This is a varient of mavlink_frame_char() but with caller supplied
  187. * parsing buffers. It is useful when you want to create a MAVLink
  188. * parser in a library that doesn't use any global variables
  189. *
  190. * @param rxmsg parsing message buffer
  191. * @param status parsing starus buffer
  192. * @param c The char to parse
  193. *
  194. * @param returnMsg NULL if no message could be decoded, the message data else
  195. * @param returnStats if a message was decoded, this is filled with the channel's stats
  196. * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
  197. *
  198. * A typical use scenario of this function call is:
  199. *
  200. * @code
  201. * #include <mavlink.h>
  202. *
  203. * mavlink_message_t msg;
  204. * int chan = 0;
  205. *
  206. *
  207. * while(serial.bytesAvailable > 0)
  208. * {
  209. * uint8_t byte = serial.getNextByte();
  210. * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
  211. * {
  212. * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
  213. * }
  214. * }
  215. *
  216. *
  217. * @endcode
  218. */
  219. MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
  220. mavlink_status_t* status,
  221. uint8_t c,
  222. mavlink_message_t* r_message,
  223. mavlink_status_t* r_mavlink_status)
  224. {
  225. /*
  226. default message crc function. You can override this per-system to
  227. put this data in a different memory segment
  228. */
  229. #if MAVLINK_CRC_EXTRA
  230. #ifndef MAVLINK_MESSAGE_CRC
  231. staticconstuint8_t mavlink_message_crcs[256]= MAVLINK_MESSAGE_CRCS;
  232. #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
  233. #endif
  234. #endif
  235. /* Enable this option to check the length of each message.
  236. This allows invalid messages to be caught much sooner. Use if the transmission
  237. medium is prone to missing (or extra) characters (e.g. a radio that fades in
  238. and out). Only use if the channel will only contain messages types listed in
  239. the headers.
  240. */
  241. #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
  242. #ifndef MAVLINK_MESSAGE_LENGTH
  243. staticconstuint8_t mavlink_message_lengths[256]= MAVLINK_MESSAGE_LENGTHS;
  244. #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid]
  245. #endif
  246. #endif
  247. int bufferIndex =0;
  248. status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
  249. switch(status->parse_state)
  250. {
  251. case MAVLINK_PARSE_STATE_UNINIT:
  252. case MAVLINK_PARSE_STATE_IDLE:
  253. if(c == MAVLINK_STX)
  254. {
  255. status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
  256. rxmsg->len =0;
  257. rxmsg->magic = c;
  258. mavlink_start_checksum(rxmsg);
  259. }
  260. break;
  261. case MAVLINK_PARSE_STATE_GOT_STX:
  262. if(status->msg_received
  263. /* Support shorter buffers than the
  264. default maximum packet size */
  265. #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
  266. || c > MAVLINK_MAX_PAYLOAD_LEN
  267. #endif
  268. )
  269. {
  270. status->buffer_overrun++;
  271. status->parse_error++;
  272. status->msg_received =0;
  273. status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  274. }
  275. else
  276. {
  277. // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
  278. rxmsg->len = c;
  279. status->packet_idx =0;
  280. mavlink_update_checksum(rxmsg, c);
  281. status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
  282. }
  283. break;
  284. case MAVLINK_PARSE_STATE_GOT_LENGTH:
  285. rxmsg->seq = c;
  286. mavlink_update_checksum(rxmsg, c);
  287. status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
  288. break;
  289. case MAVLINK_PARSE_STATE_GOT_SEQ:
  290. rxmsg->sysid = c;
  291. mavlink_update_checksum(rxmsg, c);
  292. status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
  293. break;
  294. case MAVLINK_PARSE_STATE_GOT_SYSID:
  295. rxmsg->compid = c;
  296. mavlink_update_checksum(rxmsg, c);
  297. status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
  298. break;
  299. case MAVLINK_PARSE_STATE_GOT_COMPID:
  300. #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
  301. if(rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
  302. {
  303. status->parse_error++;
  304. status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  305. break;
  306. }
  307. #endif
  308. rxmsg->msgid = c;
  309. mavlink_update_checksum(rxmsg, c);
  310. if(rxmsg->len ==0)
  311. {
  312. status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  313. }
  314. else
  315. {
  316. status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
  317. }
  318. break;
  319. case MAVLINK_PARSE_STATE_GOT_MSGID:
  320. _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++]=(char)c;
  321. mavlink_update_checksum(rxmsg, c);
  322. if(status->packet_idx == rxmsg->len)
  323. {
  324. status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
  325. }
  326. break;
  327. case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
  328. #if MAVLINK_CRC_EXTRA
  329. mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
  330. #endif
  331. if(c !=(rxmsg->checksum &0xFF)){
  332. status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
  333. }else{
  334. status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
  335. }
  336. _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx]=(char)c;
  337. break;
  338. case MAVLINK_PARSE_STATE_GOT_CRC1:
  339. case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
  340. if(status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c !=(rxmsg->checksum >>8)){
  341. // got a bad CRC message
  342. status->msg_received = MAVLINK_FRAMING_BAD_CRC;
  343. }else{
  344. // Successfully got message
  345. status->msg_received = MAVLINK_FRAMING_OK;
  346. }
  347. status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  348. _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1]=(char)c;
  349. memcpy(r_message, rxmsg,sizeof(mavlink_message_t));
  350. break;
  351. }
  352. bufferIndex++;
  353. // If a message has been sucessfully decoded, check index
  354. if(status->msg_received == MAVLINK_FRAMING_OK)
  355. {
  356. //while(status->current_seq != rxmsg->seq)
  357. //{
  358. // status->packet_rx_drop_count++;
  359. // status->current_seq++;
  360. //}
  361. status->current_rx_seq = rxmsg->seq;
  362. // Initial condition: If no packet has been received so far, drop count is undefined
  363. if(status->packet_rx_success_count ==0) status->packet_rx_drop_count =0;
  364. // Count this packet as received
  365. status->packet_rx_success_count++;
  366. }
  367. r_message->len = rxmsg->len;// Provide visibility on how far we are into current msg
  368. r_mavlink_status->parse_state = status->parse_state;
  369. r_mavlink_status->packet_idx = status->packet_idx;
  370. r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
  371. r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
  372. r_mavlink_status->packet_rx_drop_count = status->parse_error;
  373. status->parse_error =0;
  374. if(status->msg_received == MAVLINK_FRAMING_BAD_CRC){
  375. /*
  376. the CRC came out wrong. We now need to overwrite the
  377. msg CRC with the one on the wire so that if the
  378. caller decides to forward the message anyway that
  379. mavlink_msg_to_send_buffer() won't overwrite the
  380. checksum
  381. */
  382. r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx]|(_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);
  383. }
  384. return status->msg_received;
  385. }
  386. /**
  387. * This is a convenience function which handles the complete MAVLink parsing.
  388. * the function will parse one byte at a time and return the complete packet once
  389. * it could be successfully decoded. This function will return 0, 1 or
  390. * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC)
  391. *
  392. * Messages are parsed into an internal buffer (one for each channel). When a complete
  393. * message is received it is copies into *returnMsg and the channel's status is
  394. * copied into *returnStats.
  395. *
  396. * @param chan ID of the current channel. This allows to parse different channels with this function.
  397. * a channel is not a physical message channel like a serial port, but a logic partition of
  398. * the communication streams in this case. COMM_NB is the limit for the number of channels
  399. * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
  400. * @param c The char to parse
  401. *
  402. * @param returnMsg NULL if no message could be decoded, the message data else
  403. * @param returnStats if a message was decoded, this is filled with the channel's stats
  404. * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
  405. *
  406. * A typical use scenario of this function call is:
  407. *
  408. * @code
  409. * #include <mavlink.h>
  410. *
  411. * mavlink_message_t msg;
  412. * int chan = 0;
  413. *
  414. *
  415. * while(serial.bytesAvailable > 0)
  416. * {
  417. * uint8_t byte = serial.getNextByte();
  418. * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
  419. * {
  420. * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
  421. * }
  422. * }
  423. *
  424. *
  425. * @endcode
  426. */
  427. MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan,uint8_t c,mavlink_message_t* r_message,mavlink_status_t* r_mavlink_status)
  428. {
  429. return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan),
  430. mavlink_get_channel_status(chan),
  431. c,
  432. r_message,
  433. r_mavlink_status);
  434. }
  435. /**
  436. * This is a convenience function which handles the complete MAVLink parsing.
  437. * the function will parse one byte at a time and return the complete packet once
  438. * it could be successfully decoded. This function will return 0 or 1.
  439. *
  440. * Messages are parsed into an internal buffer (one for each channel). When a complete
  441. * message is received it is copies into *returnMsg and the channel's status is
  442. * copied into *returnStats.
  443. *
  444. * @param chan ID of the current channel. This allows to parse different channels with this function.
  445. * a channel is not a physical message channel like a serial port, but a logic partition of
  446. * the communication streams in this case. COMM_NB is the limit for the number of channels
  447. * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
  448. * @param c The char to parse
  449. *
  450. * @param returnMsg NULL if no message could be decoded, the message data else
  451. * @param returnStats if a message was decoded, this is filled with the channel's stats
  452. * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC
  453. *
  454. * A typical use scenario of this function call is:
  455. *
  456. * @code
  457. * #include <mavlink.h>
  458. *
  459. * mavlink_message_t msg;
  460. * int chan = 0;
  461. *
  462. *
  463. * while(serial.bytesAvailable > 0)
  464. * {
  465. * uint8_t byte = serial.getNextByte();
  466. * if (mavlink_parse_char(chan, byte, &msg))
  467. * {
  468. * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
  469. * }
  470. * }
  471. *
  472. *
  473. * @endcode
  474. */
  475. MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan,uint8_t c,mavlink_message_t* r_message,mavlink_status_t* r_mavlink_status)
  476. {
  477. uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
  478. if(msg_received == MAVLINK_FRAMING_BAD_CRC){
  479. // we got a bad CRC. Treat as a parse failure
  480. mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
  481. mavlink_status_t* status = mavlink_get_channel_status(chan);
  482. status->parse_error++;
  483. status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
  484. status->parse_state = MAVLINK_PARSE_STATE_IDLE;
  485. if(c == MAVLINK_STX)
  486. {
  487. status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
  488. rxmsg->len =0;
  489. mavlink_start_checksum(rxmsg);
  490. }
  491. return0;
  492. }
  493. return msg_received;
  494. }
  495. /**
  496. * @brief Put a bitfield of length 1-32 bit into the buffer
  497. *
  498. * @param b the value to add, will be encoded in the bitfield
  499. * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
  500. * @param packet_index the position in the packet (the index of the first byte to use)
  501. * @param bit_index the position in the byte (the index of the first bit to use)
  502. * @param buffer packet buffer to write into
  503. * @return new position of the last used byte in the buffer
  504. */
  505. MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b,uint8_t bits,uint8_t packet_index,uint8_t bit_index,uint8_t* r_bit_index,uint8_t* buffer)
  506. {
  507. uint16_t bits_remain = bits;
  508. // Transform number into network order
  509. int32_t v;
  510. uint8_t i_bit_index, i_byte_index, curr_bits_n;
  511. #if MAVLINK_NEED_BYTE_SWAP
  512. union{
  513. int32_t i;
  514. uint8_t b[4];
  515. } bin, bout;
  516. bin.i = b;
  517. bout.b[0]= bin.b[3];
  518. bout.b[1]= bin.b[2];
  519. bout.b[2]= bin.b[1];
  520. bout.b[3]= bin.b[0];
  521. v = bout.i;
  522. #else
  523. v = b;
  524. #endif
  525. // buffer in
  526. // 01100000 01000000 00000000 11110001
  527. // buffer out
  528. // 11110001 00000000 01000000 01100000
  529. // Existing partly filled byte (four free slots)
  530. // 0111xxxx
  531. // Mask n free bits
  532. // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
  533. // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
  534. // Shift n bits into the right position
  535. // out = in >> n;
  536. // Mask and shift bytes
  537. i_bit_index = bit_index;
  538. i_byte_index = packet_index;
  539. if(bit_index >0)
  540. {
  541. // If bits were available at start, they were available
  542. // in the byte before the current index
  543. i_byte_index--;
  544. }
  545. // While bits have not been packed yet
  546. while(bits_remain >0)
  547. {
  548. // Bits still have to be packed
  549. // there can be more than 8 bits, so
  550. // we might have to pack them into more than one byte
  551. // First pack everything we can into the current 'open' byte
  552. //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
  553. //FIXME
  554. if(bits_remain <=(uint8_t)(8- i_bit_index))
  555. {
  556. // Enough space
  557. curr_bits_n =(uint8_t)bits_remain;
  558. }
  559. else
  560. {
  561. curr_bits_n =(8- i_bit_index);
  562. }
  563. // Pack these n bits into the current byte
  564. // Mask out whatever was at that position with ones (xxx11111)
  565. buffer[i_byte_index]&=(0xFF>>(8- curr_bits_n));
  566. // Put content to this position, by masking out the non-used part
  567. buffer[i_byte_index]|=((0x00<< curr_bits_n)& v);
  568. // Increment the bit index
  569. i_bit_index += curr_bits_n;
  570. // Now proceed to the next byte, if necessary
  571. bits_remain -= curr_bits_n;
  572. if(bits_remain >0)
  573. {
  574. // Offer another 8 bits / one byte
  575. i_byte_index++;
  576. i_bit_index =0;
  577. }
  578. }
  579. *r_bit_index = i_bit_index;
  580. // If a partly filled byte is present, mark this as consumed
  581. if(i_bit_index !=7) i_byte_index++;
  582. return i_byte_index - packet_index;
  583. }
  584. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  585. // To make MAVLink work on your MCU, define comm_send_ch() if you wish
  586. // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
  587. // whole packet at a time
  588. /*
  589. #include "mavlink_types.h"
  590. void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
  591. {
  592. if (chan == MAVLINK_COMM_0)
  593. {
  594. uart0_transmit(ch);
  595. }
  596. if (chan == MAVLINK_COMM_1)
  597. {
  598. uart1_transmit(ch);
  599. }
  600. }
  601. */
  602. #include"mavlink_usart_fifo.h"
  603. MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan,constchar*buf,uint16_t len)
  604. {
  605. serial_write_buf((uint8_t*)buf, len);
  606. }
  607. //MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
  608. //{
  609. //#ifdef MAVLINK_SEND_UART_BYTES
  610. // /* this is the more efficient approach, if the platform
  611. // defines it */
  612. // MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len);
  613. //#else
  614. // /* fallback to one byte at a time */
  615. // uint16_t i;
  616. // for (i = 0; i < len; i++) {
  617. // comm_send_ch(chan, (uint8_t)buf[i]);
  618. // }
  619. //#endif
  620. //}
  621. #endif// MAVLINK_USE_CONVENIENCE_FUNCTIONS
  622. #endif/* _MAVLINK_HELPERS_H_ */

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

checksum.h
  1. #ifdef __cplusplus//作者:恒久力行  qq:624668529
  2. extern"C"{
  3. #endif
  4. #ifndef _CHECKSUM_H_
  5. #define _CHECKSUM_H_
  6. // Visual Studio versions before 2010 don't have stdint.h, so we just error out.
  7. #if (defined _MSC_VER) && (_MSC_VER < 1600)
  8. #error"The C-MAVLink implementation requires Visual Studio 2010 or greater"
  9. #endif
  10. #include<stdint.h>
  11. /**
  12. *
  13. * CALCULATE THE CHECKSUM
  14. *
  15. */
  16. #define X25_INIT_CRC 0xffff
  17. #define X25_VALIDATE_CRC 0xf0b8
  18. #ifndef HAVE_CRC_ACCUMULATE
  19. /**
  20. * @brief Accumulate the X.25 CRC by adding one char at a time.
  21. *
  22. * The checksum function adds the hash of one char at a time to the
  23. * 16 bit checksum (uint16_t).
  24. *
  25. * @param data new char to hash
  26. * @param crcAccum the already accumulated checksum
  27. **/
  28. staticinlinevoid crc_accumulate(uint8_t data,uint16_t*crcAccum)
  29. {
  30. /*Accumulate one byte of data into the CRC*/
  31. uint8_t tmp;
  32. tmp = data ^(uint8_t)(*crcAccum &0xff);
  33. tmp ^=(tmp<<4);
  34. *crcAccum =(*crcAccum>>8)^(tmp<<8)^(tmp <<3)^(tmp>>4);
  35. }
  36. #endif
  37. /**
  38. * @brief Initiliaze the buffer for the X.25 CRC
  39. *
  40. * @param crcAccum the 16 bit X.25 CRC
  41. */
  42. staticinlinevoid crc_init(uint16_t* crcAccum)
  43. {
  44. *crcAccum = X25_INIT_CRC;
  45. }
  46. /**
  47. * @brief Calculates the X.25 checksum on a byte buffer
  48. *
  49. * @param pBuffer buffer containing the byte array to hash
  50. * @param length length of the byte array
  51. * @return the checksum over the buffer bytes
  52. **/
  53. staticinlineuint16_t crc_calculate(constuint8_t* pBuffer,uint16_t length)
  54. {
  55. uint16_t crcTmp;
  56. crc_init(&crcTmp);
  57. while(length--){
  58. crc_accumulate(*pBuffer++,&crcTmp);
  59. }
  60. return crcTmp;
  61. }
  62. /**
  63. * @brief Accumulate the X.25 CRC by adding an array of bytes
  64. *
  65. * The checksum function adds the hash of one char at a time to the
  66. * 16 bit checksum (uint16_t).
  67. *
  68. * @param data new bytes to hash
  69. * @param crcAccum the already accumulated checksum
  70. **/
  71. staticinlinevoid crc_accumulate_buffer(uint16_t*crcAccum,constchar*pBuffer,uint16_t length)
  72. {
  73. constuint8_t*p =(constuint8_t*)pBuffer;
  74. while(length--){
  75. crc_accumulate(*p++, crcAccum);
  76. }
  77. }
  78. #endif/* _CHECKSUM_H_ */
  79. #ifdef __cplusplus
  80. }
  81. #endif

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

(就是这个函数MAVLINK_HELPER void mavlink_dcm_to_quaternion(constfloat dcm[3][3],float quaternion[4])
mavlink_conversions.h全代码
  1. #ifndef _MAVLINK_CONVERSIONS_H_//作者:恒久力行  qq:624668529
  2. #define _MAVLINK_CONVERSIONS_H_
  3. /* enable math defines on Windows */
  4. #ifdef _MSC_VER
  5. #ifndef _USE_MATH_DEFINES
  6. #define _USE_MATH_DEFINES
  7. #endif
  8. #endif
  9. #include<math.h>
  10. #ifndef M_PI_2
  11. #define M_PI_2 ((float)asin(1))
  12. #endif
  13. /**
  14. * @file mavlink_conversions.h
  15. *
  16. * These conversion functions follow the NASA rotation standards definition file
  17. * available online.
  18. *
  19. * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free
  20. * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free)
  21. * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the
  22. * protocol as widely as possible.
  23. *
  24. * @author James Goppert
  25. * @author Thomas Gubler <thomasgubler@gmail.com>
  26. */
  27. /**
  28. * Converts a quaternion to a rotation matrix
  29. *
  30. * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
  31. * @param dcm a 3x3 rotation matrix
  32. */
  33. MAVLINK_HELPER void mavlink_quaternion_to_dcm(constfloat quaternion[4],float dcm[3][3])
  34. {
  35. double a = quaternion[0];
  36. double b = quaternion[1];
  37. double c = quaternion[2];
  38. double d = quaternion[3];
  39. double aSq = a * a;
  40. double bSq = b * b;
  41. double cSq = c * c;
  42. double dSq = d * d;
  43. dcm[0][0]= aSq + bSq - cSq - dSq;
  44. dcm[0][1]=2*(b * c - a * d);
  45. dcm[0][2]=2*(a * c + b * d);
  46. dcm[1][0]=2*(b * c + a * d);
  47. dcm[1][1]= aSq - bSq + cSq - dSq;
  48. dcm[1][2]=2*(c * d - a * b);
  49. dcm[2][0]=2*(b * d - a * c);
  50. dcm[2][1]=2*(a * b + c * d);
  51. dcm[2][2]= aSq - bSq - cSq + dSq;
  52. }
  53. /**
  54. * Converts a rotation matrix to euler angles
  55. *
  56. * @param dcm a 3x3 rotation matrix
  57. * @param roll the roll angle in radians
  58. * @param pitch the pitch angle in radians
  59. * @param yaw the yaw angle in radians
  60. */
  61. MAVLINK_HELPER void mavlink_dcm_to_euler(constfloat dcm[3][3],float* roll,float* pitch,float* yaw)
  62. {
  63. float phi, theta, psi;
  64. theta = asin(-dcm[2][0]);
  65. if(fabsf(theta -(float)M_PI_2)<1.0e-3f){
  66. phi =0.0f;
  67. psi =(atan2f(dcm[1][2]- dcm[0][1],
  68. dcm[0][2]+ dcm[1][1])+ phi);
  69. }elseif(fabsf(theta +(float)M_PI_2)<1.0e-3f){
  70. phi =0.0f;
  71. psi = atan2f(dcm[1][2]- dcm[0][1],
  72. dcm[0][2]+ dcm[1][1]- phi);
  73. }else{
  74. phi = atan2f(dcm[2][1], dcm[2][2]);
  75. psi = atan2f(dcm[1][0], dcm[0][0]);
  76. }
  77. *roll = phi;
  78. *pitch = theta;
  79. *yaw = psi;
  80. }
  81. /**
  82. * Converts a quaternion to euler angles
  83. *
  84. * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
  85. * @param roll the roll angle in radians
  86. * @param pitch the pitch angle in radians
  87. * @param yaw the yaw angle in radians
  88. */
  89. MAVLINK_HELPER void mavlink_quaternion_to_euler(constfloat quaternion[4],float* roll,float* pitch,float* yaw)
  90. {
  91. float dcm[3][3];
  92. mavlink_quaternion_to_dcm(quaternion, dcm);
  93. mavlink_dcm_to_euler((constfloat(*)[3])dcm, roll, pitch, yaw);
  94. }
  95. /**
  96. * Converts euler angles to a quaternion
  97. *
  98. * @param roll the roll angle in radians
  99. * @param pitch the pitch angle in radians
  100. * @param yaw the yaw angle in radians
  101. * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
  102. */
  103. MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll,float pitch,float yaw,float quaternion[4])
  104. {
  105. float cosPhi_2 = cosf(roll /2);
  106. float sinPhi_2 = sinf(roll /2);
  107. float cosTheta_2 = cosf(pitch /2);
  108. float sinTheta_2 = sinf(pitch /2);
  109. float cosPsi_2 = cosf(yaw /2);
  110. float sinPsi_2 = sinf(yaw /2);
  111. quaternion[0]=(cosPhi_2 * cosTheta_2 * cosPsi_2 +
  112. sinPhi_2 * sinTheta_2 * sinPsi_2);
  113. quaternion[1]=(sinPhi_2 * cosTheta_2 * cosPsi_2 -
  114. cosPhi_2 * sinTheta_2 * sinPsi_2);
  115. quaternion[2]=(cosPhi_2 * sinTheta_2 * cosPsi_2 +
  116. sinPhi_2 * cosTheta_2 * sinPsi_2);
  117. quaternion[3]=(cosPhi_2 * cosTheta_2 * sinPsi_2 -
  118. sinPhi_2 * sinTheta_2 * cosPsi_2);
  119. }
  120. /**
  121. * Converts a rotation matrix to a quaternion
  122. * Reference:
  123. * - Shoemake, Quaternions,
  124. * http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
  125. *
  126. * @param dcm a 3x3 rotation matrix
  127. * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
  128. */
  129. MAVLINK_HELPER void mavlink_dcm_to_quaternion(constfloat dcm[3][3],float quaternion[4])
  130. {
  131. int dcm_j,dcm_k;
  132. float s;
  133. float tr = dcm[0][0]+ dcm[1][1]+ dcm[2][2];
  134. if(tr >0.0f){
  135. float s = sqrtf(tr +1.0f);
  136. quaternion[0]= s *0.5f;
  137. s =0.5f/ s;
  138. quaternion[1]=(dcm[2][1]- dcm[1][2])* s;
  139. quaternion[2]=(dcm[0][2]- dcm[2][0])* s;
  140. quaternion[3]=(dcm[1][0]- dcm[0][1])* s;
  141. }else{
  142. /* Find maximum diagonal element in dcm
  143. * store index in dcm_i */
  144. int dcm_i =0;
  145. int i;
  146. for(i =1; i <3; i++){
  147. if(dcm[i][i]> dcm[dcm_i][dcm_i]){
  148. dcm_i = i;
  149. }
  150. }
  151. dcm_j =(dcm_i +1)%3;
  152. dcm_k =(dcm_i +2)%3;
  153. s = sqrtf((dcm[dcm_i][dcm_i]- dcm[dcm_j][dcm_j]-
  154. dcm[dcm_k][dcm_k])+1.0f);
  155. quaternion[dcm_i +1]= s *0.5f;
  156. s =0.5f/ s;
  157. quaternion[dcm_j +1]=(dcm[dcm_i][dcm_j]+ dcm[dcm_j][dcm_i])* s;
  158. quaternion[dcm_k +1]=(dcm[dcm_k][dcm_i]+ dcm[dcm_i][dcm_k])* s;
  159. quaternion[0]=(dcm[dcm_k][dcm_j]- dcm[dcm_j][dcm_k])* s;
  160. }
  161. }
  162. /**
  163. * Converts euler angles to a rotation matrix
  164. *
  165. * @param roll the roll angle in radians
  166. * @param pitch the pitch angle in radians
  167. * @param yaw the yaw angle in radians
  168. * @param dcm a 3x3 rotation matrix
  169. */
  170. MAVLINK_HELPER void mavlink_euler_to_dcm(float roll,float pitch,float yaw,float dcm[3][3])
  171. {
  172. float cosPhi = cosf(roll);
  173. float sinPhi = sinf(roll);
  174. float cosThe = cosf(pitch);
  175. float sinThe = sinf(pitch);
  176. float cosPsi = cosf(yaw);
  177. float sinPsi = sinf(yaw);
  178. dcm[0][0]= cosThe * cosPsi;
  179. dcm[0][1]=-cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
  180. dcm[0][2]= sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
  181. dcm[1][0]= cosThe * sinPsi;
  182. dcm[1][1]= cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
  183. dcm[1][2]=-sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
  184. dcm[2][0]=-sinThe;
  185. dcm[2][1]= sinPhi * cosThe;
  186. dcm[2][2]= cosPhi * cosThe;
  187. }
  188. #endif

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

(关于MAVPACKED相关定义在mavlink_avoid_errors.h中)
  1. // Macro to define packed structures
  2. //#ifdef __GNUC__
  3. // #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed))
  4. //#else
  5. // #define MAVPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
  6. //#endif
mavlink_types.h全代码
  1. #ifndef MAVLINK_TYPES_H_
  2. #define MAVLINK_TYPES_H_
  3. // Visual Studio versions before 2010 don't have stdint.h, so we just error out.
  4. #if (defined _MSC_VER) && (_MSC_VER < 1600)
  5. #error"The C-MAVLink implementation requires Visual Studio 2010 or greater"
  6. #endif
  7. #include<stdint.h>
  8. // Macro to define packed structures
  9. //#ifdef __GNUC__
  10. // #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed))
  11. //#else
  12. // #define MAVPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
  13. //#endif
  14. #ifndef MAVLINK_MAX_PAYLOAD_LEN
  15. // it is possible to override this, but be careful!
  16. #define MAVLINK_MAX_PAYLOAD_LEN 255///< Maximum payload length
  17. #endif
  18. #define MAVLINK_CORE_HEADER_LEN 5///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte)
  19. #define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN +1)///< Length of all header bytes, including core and checksum
  20. #define MAVLINK_NUM_CHECKSUM_BYTES 2
  21. #define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
  22. #define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)///< Maximum packet length
  23. #define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
  24. #define MAVLINK_EXTENDED_HEADER_LEN 14
  25. #if (defined _MSC_VER) || ((defined __APPLE__) && (defined __MACH__)) || (defined __linux__)
  26. /* full fledged 32bit++ OS */
  27. #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
  28. #else
  29. /* small microcontrollers */
  30. #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048
  31. #endif
  32. #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
  33. /**
  34. * Old-style 4 byte param union
  35. *
  36. * This struct is the data format to be used when sending
  37. * parameters. The parameter should be copied to the native
  38. * type (without type conversion)
  39. * and re-instanted on the receiving side using the
  40. * native type as well.
  41. */
  42. MAVPACKED(
  43. typedefstruct param_union {
  44. union{
  45. float param_float;
  46. int32_t param_int32;
  47. uint32_t param_uint32;
  48. int16_t param_int16;
  49. uint16_t param_uint16;
  50. int8_t param_int8;
  51. uint8_t param_uint8;
  52. uint8_t bytes[4];
  53. };
  54. uint8_t type;
  55. })mavlink_param_union_t;
  56. /**
  57. * New-style 8 byte param union
  58. * mavlink_param_union_double_t will be 8 bytes long, and treated as needing 8 byte alignment for the purposes of MAVLink 1.0 field ordering.
  59. * The mavlink_param_union_double_t will be treated as a little-endian structure.
  60. *
  61. * If is_double is 1 then the type is a double, and the remaining 63 bits are the double, with the lowest bit of the mantissa zero.
  62. * The intention is that by replacing the is_double bit with 0 the type can be directly used as a double (as the is_double bit corresponds to the
  63. * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union.
  64. * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above,
  65. * as bitfield ordering isn’t consistent between platforms. The above is intended to be for gcc on x86,
  66. * which should be the same as gcc on little-endian arm. When using shifts/masks the value will be treated as a 64 bit unsigned number,
  67. * and the bits pulled out using the shifts/masks.
  68. */
  69. MAVPACKED(
  70. typedefstruct param_union_extended {
  71. union{
  72. struct{
  73. uint8_t is_double:1;
  74. uint8_t mavlink_type:7;
  75. union{
  76. char c;
  77. uint8_tuint8;
  78. int8_tint8;
  79. uint16_tuint16;
  80. int16_tint16;
  81. uint32_tuint32;
  82. int32_tint32;
  83. float f;
  84. uint8_t align[7];
  85. };
  86. };
  87. uint8_t data[8];
  88. };
  89. })mavlink_param_union_double_t;
  90. /**
  91. * This structure is required to make the mavlink_send_xxx convenience functions
  92. * work, as it tells the library what the current system and component ID are.
  93. */
  94. MAVPACKED(
  95. typedefstruct __mavlink_system {
  96. uint8_t sysid;///< Used by the MAVLink message_xx_send() convenience function
  97. uint8_t compid;///< Used by the MAVLink message_xx_send() convenience function
  98. })mavlink_system_t;
  99. MAVPACKED(
  100. typedefstruct __mavlink_message {
  101. uint16_t checksum;///< sent at end of packet
  102. uint8_t magic;///< protocol magic marker
  103. uint8_t len;///< Length of payload
  104. uint8_t seq;///< Sequence of packet
  105. uint8_t sysid;///< ID of message sender system/aircraft
  106. uint8_t compid;///< ID of the message sender component
  107. uint8_t msgid;///< ID of message in payload
  108. uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
  109. })mavlink_message_t;
  110. MAVPACKED(
  111. typedefstruct __mavlink_extended_message {
  112. mavlink_message_t base_msg;
  113. int32_t extended_payload_len;///< Length of extended payload if any
  114. uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
  115. })mavlink_extended_message_t;
  116. typedefenum{
  117. MAVLINK_TYPE_CHAR =0,
  118. MAVLINK_TYPE_UINT8_T =1,
  119. MAVLINK_TYPE_INT8_T =2,
  120. MAVLINK_TYPE_UINT16_T =3,
  121. MAVLINK_TYPE_INT16_T =4,
  122. MAVLINK_TYPE_UINT32_T =5,
  123. MAVLINK_TYPE_INT32_T =6,
  124. MAVLINK_TYPE_UINT64_T =7,
  125. MAVLINK_TYPE_INT64_T =8,
  126. MAVLINK_TYPE_FLOAT =9,
  127. MAVLINK_TYPE_DOUBLE =10
  128. }mavlink_message_type_t;
  129. #define MAVLINK_MAX_FIELDS 64
  130. typedefstruct __mavlink_field_info {
  131. constchar*name;// name of this field
  132. constchar*print_format;// printing format hint, or NULL
  133. mavlink_message_type_t type;// type of this field
  134. unsignedint array_length;// if non-zero, field is an array
  135. unsignedint wire_offset;// offset of each field in the payload
  136. unsignedint structure_offset;// offset in a C structure
  137. }mavlink_field_info_t;
  138. // note that in this structure the order of fields is the order
  139. // in the XML file, not necessary the wire order
  140. typedefstruct __mavlink_message_info {
  141. constchar*name;// name of the message
  142. unsigned num_fields;// how many fields in this message
  143. mavlink_field_info_t fields[MAVLINK_MAX_FIELDS];// field information
  144. }mavlink_message_info_t;
  145. #define _MAV_PAYLOAD(msg)((constchar*)(&((msg)->payload64[0])))
  146. #define _MAV_PAYLOAD_NON_CONST(msg)((char*)(&((msg)->payload64[0])))
  147. // checksum is immediately after the payload bytes
  148. #define mavlink_ck_a(msg)*((msg)->len +(uint8_t*)_MAV_PAYLOAD_NON_CONST(msg))
  149. #define mavlink_ck_b(msg)*(((msg)->len+(uint16_t)1)+(uint8_t*)_MAV_PAYLOAD_NON_CONST(msg))
  150. typedefenum{
  151. MAVLINK_COMM_0,
  152. MAVLINK_COMM_1,
  153. MAVLINK_COMM_2,
  154. MAVLINK_COMM_3
  155. }mavlink_channel_t;
  156. /*
  157. * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
  158. * of buffers they will use. If more are used, then the result will be
  159. * a stack overrun
  160. */
  161. #ifndef MAVLINK_COMM_NUM_BUFFERS
  162. #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
  163. # define MAVLINK_COMM_NUM_BUFFERS 16
  164. #else
  165. # define MAVLINK_COMM_NUM_BUFFERS 4
  166. #endif
  167. #endif
  168. typedefenum{
  169. MAVLINK_PARSE_STATE_UNINIT=0,
  170. MAVLINK_PARSE_STATE_IDLE,
  171. MAVLINK_PARSE_STATE_GOT_STX,
  172. MAVLINK_PARSE_STATE_GOT_SEQ,
  173. MAVLINK_PARSE_STATE_GOT_LENGTH,
  174. MAVLINK_PARSE_STATE_GOT_SYSID,
  175. MAVLINK_PARSE_STATE_GOT_COMPID,
  176. MAVLINK_PARSE_STATE_GOT_MSGID,
  177. MAVLINK_PARSE_STATE_GOT_PAYLOAD,
  178. MAVLINK_PARSE_STATE_GOT_CRC1,
  179. MAVLINK_PARSE_STATE_GOT_BAD_CRC1
  180. }mavlink_parse_state_t;///< The state machine for the comm parser
  181. typedefenum{
  182. MAVLINK_FRAMING_INCOMPLETE=0,
  183. MAVLINK_FRAMING_OK=1,
  184. MAVLINK_FRAMING_BAD_CRC=2
  185. }mavlink_framing_t;
  186. typedefstruct __mavlink_status {
  187. uint8_t msg_received;///< Number of received messages
  188. uint8_t buffer_overrun;///< Number of buffer overruns
  189. uint8_t parse_error;///< Number of parse errors
  190. mavlink_parse_state_t parse_state;///< Parsing state machine
  191. uint8_t packet_idx;///< Index in current packet
  192. uint8_t current_rx_seq;///< Sequence number of last packet received
  193. uint8_t current_tx_seq;///< Sequence number of last packet sent
  194. uint16_t packet_rx_success_count;///< Received packets
  195. uint16_t packet_rx_drop_count;///< Number of packet drops
  196. }mavlink_status_t;
  197. #define MAVLINK_BIG_ENDIAN 0
  198. #define MAVLINK_LITTLE_ENDIAN 1
  199. #endif/* MAVLINK_TYPES_H_ */

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

protocol.h全代码
  1. #ifndef _MAVLINK_PROTOCOL_H_//作者:恒久力行  qq:624668529
  2. #define _MAVLINK_PROTOCOL_H_
  3. #include"string.h"
  4. #include"mavlink_types.h"
  5. /*
  6. If you want MAVLink on a system that is native big-endian,
  7. you need to define NATIVE_BIG_ENDIAN
  8. */
  9. #ifdef NATIVE_BIG_ENDIAN
  10. # define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
  11. #else
  12. # define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
  13. #endif
  14. #ifndef MAVLINK_STACK_BUFFER
  15. #define MAVLINK_STACK_BUFFER 0
  16. #endif
  17. #ifndef MAVLINK_AVOID_GCC_STACK_BUG
  18. # define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__)
  19. #endif
  20. #ifndef MAVLINK_ASSERT
  21. #define MAVLINK_ASSERT(x)
  22. #endif
  23. #ifndef MAVLINK_START_UART_SEND
  24. #define MAVLINK_START_UART_SEND(chan, length)
  25. #endif
  26. #ifndef MAVLINK_END_UART_SEND
  27. #define MAVLINK_END_UART_SEND(chan, length)
  28. #endif
  29. /* option to provide alternative implementation of mavlink_helpers.h */
  30. #ifdef MAVLINK_SEPARATE_HELPERS
  31. #define MAVLINK_HELPER
  32. /* decls in sync with those in mavlink_helpers.h */
  33. #ifndef MAVLINK_GET_CHANNEL_STATUS
  34. MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
  35. #endif
  36. MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
  37. #if MAVLINK_CRC_EXTRA
  38. MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg,uint8_t system_id,uint8_t component_id,
  39. uint8_t chan,uint8_t min_length,uint8_t length,uint8_t crc_extra);
  40. MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg,uint8_t system_id,uint8_t component_id,
  41. uint8_t min_length,uint8_t length,uint8_t crc_extra);
  42. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  43. MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan,uint8_t msgid,constchar*packet,
  44. uint8_t min_length,uint8_t length,uint8_t crc_extra);
  45. #endif
  46. #else
  47. MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg,uint8_t system_id,uint8_t component_id,
  48. uint8_t chan,uint8_t length);
  49. MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg,uint8_t system_id,uint8_t component_id,
  50. uint8_t length);
  51. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  52. MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan,uint8_t msgid,constchar*packet,uint8_t length);
  53. #endif
  54. #endif// MAVLINK_CRC_EXTRA
  55. MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t*buffer,constmavlink_message_t*msg);
  56. MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
  57. MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg,uint8_t c);
  58. MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
  59. mavlink_status_t* status,
  60. uint8_t c,
  61. mavlink_message_t* r_message,
  62. mavlink_status_t* r_mavlink_status);
  63. MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan,uint8_t c,mavlink_message_t* r_message,mavlink_status_t* r_mavlink_status);
  64. MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan,uint8_t c,mavlink_message_t* r_message,mavlink_status_t* r_mavlink_status);
  65. MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b,uint8_t bits,uint8_t packet_index,uint8_t bit_index,
  66. uint8_t* r_bit_index,uint8_t* buffer);
  67. #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
  68. MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan,constchar*buf,uint16_t len);
  69. MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan,constmavlink_message_t*msg);
  70. #endif
  71. #else
  72. #define MAVLINK_HELPER staticinline
  73. #include"mavlink_helpers.h"
  74. #endif// MAVLINK_SEPARATE_HELPERS
  75. /**
  76. * @brief Get the required buffer size for this message
  77. */
  78. staticinlineuint16_t mavlink_msg_get_send_buffer_length(constmavlink_message_t* msg)
  79. {
  80. return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
  81. }
  82. #if MAVLINK_NEED_BYTE_SWAP
  83. staticinlinevoid byte_swap_2(char*dst,constchar*src)
  84. {
  85. dst[0]= src[1];
  86. dst[1]= src[0];
  87. }
  88. staticinlinevoid byte_swap_4(char*dst,constchar*src)
  89. {
  90. dst[0]= src[3];
  91. dst[1]= src[2];
  92. dst[2]= src[1];
  93. dst[3]= src[0];
  94. }
  95. staticinlinevoid byte_swap_8(char*dst,constchar*src)
  96. {
  97. dst[0]= src[7];
  98. dst[1]= src[6];
  99. dst[2]= src[5];
  100. dst[3]= src[4];
  101. dst[4]= src[3];
  102. dst[5]= src[2];
  103. dst[6]= src[1];
  104. dst[7]= src[0];
  105. }
  106. #elif!MAVLINK_ALIGNED_FIELDS
  107. staticinlinevoid byte_copy_2(char*dst,constchar*src)
  108. {
  109. dst[0]= src[0];
  110. dst[1]= src[1];
  111. }
  112. staticinlinevoid byte_copy_4(char*dst,constchar*src)
  113. {
  114. dst[0]= src[0];
  115. dst[1]= src[1];
  116. dst[2]= src[2];
  117. dst[3]= src[3];
  118. }
  119. staticinlinevoid byte_copy_8(char*dst,constchar*src)
  120. {
  121. memcpy(dst, src,8);
  122. }
  123. #endif
  124. #define_mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset]=(uint8_t)b
  125. #define_mav_put_int8_t(buf, wire_offset, b) buf[wire_offset]=(int8_t)b
  126. #define _mav_put_char(buf, wire_offset, b) buf[wire_offset]= b
  127. #if MAVLINK_NEED_BYTE_SWAP
  128. #define_mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset],(constchar*)&b)
  129. #define_mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset],(constchar*)&b)
  130. #define_mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset],(constchar*)&b)
  131. #define_mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset],(constchar*)&b)
  132. #define_mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset],(constchar*)&b)
  133. #define_mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset],(constchar*)&b)
  134. #define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset],(constchar*)&b)
  135. #define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset],(constchar*)&b)
  136. #elif!MAVLINK_ALIGNED_FIELDS
  137. #define_mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset],(constchar*)&b)
  138. #define_mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset],(constchar*)&b)
  139. #define_mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset],(constchar*)&b)
  140. #define_mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset],(constchar*)&b)
  141. #define_mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset],(constchar*)&b)
  142. #define_mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset],(constchar*)&b)
  143. #define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset],(constchar*)&b)
  144. #define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset],(constchar*)&b)
  145. #else
  146. #define_mav_put_uint16_t(buf, wire_offset, b)*(uint16_t*)&buf[wire_offset]= b
  147. #define_mav_put_int16_t(buf, wire_offset, b)*(int16_t*)&buf[wire_offset]= b
  148. #define_mav_put_uint32_t(buf, wire_offset, b)*(uint32_t*)&buf[wire_offset]= b
  149. #define_mav_put_int32_t(buf, wire_offset, b)*(int32_t*)&buf[wire_offset]= b
  150. #define_mav_put_uint64_t(buf, wire_offset, b)*(uint64_t*)&buf[wire_offset]= b
  151. #define_mav_put_int64_t(buf, wire_offset, b)*(int64_t*)&buf[wire_offset]= b
  152. #define _mav_put_float(buf, wire_offset, b)*(float*)&buf[wire_offset]= b
  153. #define _mav_put_double(buf, wire_offset, b)*(double*)&buf[wire_offset]= b
  154. #endif
  155. /*
  156. like memcpy(), but if src is NULL, do a memset to zero
  157. */
  158. staticinlinevoid mav_array_memcpy(void*dest,constvoid*src,size_t n)
  159. {
  160. if(src == NULL){
  161. memset(dest,0, n);
  162. }else{
  163. memcpy(dest, src, n);
  164. }
  165. }
  166. /*
  167. * Place a char array into a buffer
  168. */
  169. staticinlinevoid _mav_put_char_array(char*buf,uint8_t wire_offset,constchar*b,uint8_t array_length)
  170. {
  171. mav_array_memcpy(&buf[wire_offset], b, array_length);
  172. }
  173. /*
  174. * Place a uint8_t array into a buffer
  175. */
  176. staticinlinevoid _mav_put_uint8_t_array(char*buf,uint8_t wire_offset,constuint8_t*b,uint8_t array_length)
  177. {
  178. mav_array_memcpy(&buf[wire_offset], b, array_length);
  179. }
  180. /*
  181. * Place a int8_t array into a buffer
  182. */
  183. staticinlinevoid _mav_put_int8_t_array(char*buf,uint8_t wire_offset,constint8_t*b,uint8_t array_length)
  184. {
  185. mav_array_memcpy(&buf[wire_offset], b, array_length);
  186. }
  187. #if MAVLINK_NEED_BYTE_SWAP
  188. #define _MAV_PUT_ARRAY(TYPE, V) \
  189. staticinlinevoid _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
  190. { \
  191. if(b == NULL){ \
  192. memset(&buf[wire_offset],0, array_length*sizeof(TYPE)); \
  193. }else{ \
  194. uint16_t i; \
  195. for(i=0; i<array_length; i++){ \
  196. _mav_put_## TYPE (buf, wire_offset+(i*sizeof(TYPE)), b[i]); \
  197. } \
  198. } \
  199. }
  200. #else
  201. #define _MAV_PUT_ARRAY(TYPE, V) \
  202. staticinlinevoid _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
  203. { \
  204. mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
  205. }
  206. #endif
  207. _MAV_PUT_ARRAY(uint16_t, u16)
  208. _MAV_PUT_ARRAY(uint32_t, u32)
  209. _MAV_PUT_ARRAY(uint64_t, u64)
  210. _MAV_PUT_ARRAY(int16_t, i16)
  211. _MAV_PUT_ARRAY(int32_t, i32)
  212. _MAV_PUT_ARRAY(int64_t, i64)
  213. _MAV_PUT_ARRAY(float, f)
  214. _MAV_PUT_ARRAY(double, d)
  215. #define _MAV_RETURN_char(msg, wire_offset)(constchar)_MAV_PAYLOAD(msg)[wire_offset]
  216. #define_MAV_RETURN_int8_t(msg, wire_offset)(int8_t)_MAV_PAYLOAD(msg)[wire_offset]
  217. #define_MAV_RETURN_uint8_t(msg, wire_offset)(uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
  218. #if MAVLINK_NEED_BYTE_SWAP
  219. #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
  220. staticinline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
  221. { TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
  222. _MAV_MSG_RETURN_TYPE(uint16_t,2)
  223. _MAV_MSG_RETURN_TYPE(int16_t,2)
  224. _MAV_MSG_RETURN_TYPE(uint32_t,4)
  225. _MAV_MSG_RETURN_TYPE(int32_t,4)
  226. _MAV_MSG_RETURN_TYPE(uint64_t,8)
  227. _MAV_MSG_RETURN_TYPE(int64_t,8)
  228. _MAV_MSG_RETURN_TYPE(float,4)
  229. _MAV_MSG_RETURN_TYPE(double,8)
  230. #elif!MAVLINK_ALIGNED_FIELDS
  231. #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
  232. staticinline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
  233. { TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
  234. _MAV_MSG_RETURN_TYPE(uint16_t,2)
  235. _MAV_MSG_RETURN_TYPE(int16_t,2)
  236. _MAV_MSG_RETURN_TYPE(uint32_t,4)
  237. _MAV_MSG_RETURN_TYPE(int32_t,4)
  238. _MAV_MSG_RETURN_TYPE(uint64_t,8)
  239. _MAV_MSG_RETURN_TYPE(int64_t,8)
  240. _MAV_MSG_RETURN_TYPE(float,4)
  241. _MAV_MSG_RETURN_TYPE(double,8)
  242. #else// nicely aligned, no swap
  243. #define _MAV_MSG_RETURN_TYPE(TYPE) \
  244. staticinline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
  245. {return*(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
  246. _MAV_MSG_RETURN_TYPE(uint16_t)
  247. _MAV_MSG_RETURN_TYPE(int16_t)
  248. _MAV_MSG_RETURN_TYPE(uint32_t)
  249. _MAV_MSG_RETURN_TYPE(int32_t)
  250. _MAV_MSG_RETURN_TYPE(uint64_t)
  251. _MAV_MSG_RETURN_TYPE(int64_t)
  252. _MAV_MSG_RETURN_TYPE(float)
  253. _MAV_MSG_RETURN_TYPE(double)
  254. #endif// MAVLINK_NEED_BYTE_SWAP
  255. staticinlineuint16_t _MAV_RETURN_char_array(constmavlink_message_t*msg,char*value,
  256. uint8_t array_length,uint8_t wire_offset)
  257. {
  258. memcpy(value,&_MAV_PAYLOAD(msg)[wire_offset], array_length);
  259. return array_length;
  260. }
  261. staticinlineuint16_t _MAV_RETURN_uint8_t_array(constmavlink_message_t*msg,uint8_t*value,
  262. uint8_t array_length,uint8_t wire_offset)
  263. {
  264. memcpy(value,&_MAV_PAYLOAD(msg)[wire_offset], array_length);
  265. return array_length;
  266. }
  267. staticinlineuint16_t _MAV_RETURN_int8_t_array(constmavlink_message_t*msg,int8_t*value,
  268. uint8_t array_length,uint8_t wire_offset)
  269. {
  270. memcpy(value,&_MAV_PAYLOAD(msg)[wire_offset], array_length);
  271. return array_length;
  272. }
  273. #if MAVLINK_NEED_BYTE_SWAP
  274. #define _MAV_RETURN_ARRAY(TYPE, V) \
  275. staticinlineuint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
  276. uint8_t array_length,uint8_t wire_offset) \
  277. { \
  278. uint16_t i; \
  279. for(i=0; i<array_length; i++){ \
  280. value[i]= _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
  281. } \
  282. return array_length*sizeof(value[0]); \
  283. }
  284. #else
  285. #define _MAV_RETURN_ARRAY(TYPE, V) \
  286. staticinlineuint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
  287. uint8_t array_length,uint8_t wire_offset) \
  288. { \
  289. memcpy(value,&_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
  290. return array_length*sizeof(TYPE); \
  291. }
  292. #endif
  293. _MAV_RETURN_ARRAY(uint16_t, u16)
  294. _MAV_RETURN_ARRAY(uint32_t, u32)
  295. _MAV_RETURN_ARRAY(uint64_t, u64)
  296. _MAV_RETURN_ARRAY(int16_t, i16)
  297. _MAV_RETURN_ARRAY(int32_t, i32)
  298. _MAV_RETURN_ARRAY(int64_t, i64)
  299. _MAV_RETURN_ARRAY(float, f)
  300. _MAV_RETURN_ARRAY(double, d)
  301. #endif// _MAVLINK_PROTOCOL_H_//作者:恒久力行  qq:624668529

12.其他代码均未修改,详细移植步骤请参考教程二和教程一。

 
现在代码已经移植成功。
 

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

将代码下载到stm32f407的开发板中去。
打开串口调试助手,设置波特率115200,16进制显示接收到的数据。不细讲,看图

1.测试开发板发送功能

       然后重启或者复位开发板,会有数据返回(注意:选中16进制显示)。如图收到如图数据表示发送移植成功。复位一次返回一次同样的数据//作者:恒久力行  qq:624668529
        

 2.测试开发板接收功能

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

三:分析数据

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

  1. FE 0900000000000000000203510403855D FE 1C01000021010000000200000003000000040000000500000006000700080009002A16

2.下图是消息包的发送格式说明

 

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

 
 
 
 
 

4分析

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

附件列表

移植MAVLINK到STM32详细教程之三的更多相关文章

  1. 移植mavlink到stm32详细教程,后面附快速移植方法

    一:准备材料: mavlink源码 stm32串口程序    1.mavlink源码:        a.进入mavlink官网(http://qgroundcontrol.org/mavlink/s ...

  2. 移植mavlink协议到STM32详细教程

    1准备材料, 首先准备一个带串口的stm32程序(这里选用整点原子的官方串口例程这里自己去找不讲)作者:恒久力行 QQ:624668529,然后去mavlink官网下载mavlink源码,这里重点讲解 ...

  3. Xilinx zynq-7000系列FPGA移植Linux操作系统详细教程

    Xilinx zynq-7000系列FPGA移植Linux操作系统详细教程 一:前言 最近手上压了一块米联客的Miz7035,一块xilinx zynq-7000系列的开发板,想着正好学习一下linu ...

  4. 【OpenCV入门教程之三】 图像的载入,显示和输出 一站式完全解析(转)

    本系列文章由@浅墨_毛星云 出品,转载请注明出处. 文章链接:http://blog.csdn.net/poem_qianmo/article/details/20537737 作者:毛星云(浅墨)  ...

  5. SASS教程sass超详细教程

    SASS安装及使用(sass教程.详细教程) 采用SASS开发CSS,可以提高开发效率. SASS建立在Ruby的基础之上,所以得先安装Ruby. Ruby的安装: 安装 rubyinstaller- ...

  6. Git使用详细教程(一)

    很久不发博客,最近有兴趣想写点东西,但 Live Writer 不支持从Word复制图片,疯狂吐槽下 Git使用详细教程(一) Git使用详细教程(二) 该教程主要是Git与IntelliJ IDEA ...

  7. Win7 U盘安装Ubuntu16.04 双系统详细教程

    Win7 U盘安装Ubuntu16.04 双系统详细教程 安装主要分为以下几步: 一. 下载Ubuntu 16.04镜像软件: 二. 制作U盘启动盘使用ultraISO: 三. 安装Ubuntu系统: ...

  8. Windows7 64位系统搭建Cocos2d-x-2.2.1最新版以及Android交叉编译环境(详细教程)

    Windows7 64位系统搭建Cocos2d-x-2.2.1最新版以及Android交叉编译环境(详细教程) 声明:本教程在参考了以下博文,并经过自己的摸索后实际操作得出,本教程系本人原创,由于升级 ...

  9. Ubuntu 16.04安装QQ国际版图文详细教程

            因工作需要,我安装了Ubuntu 16.04,但是工作上的很多事情需要QQ联系,然而在Ubuntu上的WebQQ很是不好用,于是在网上搜索了好多个Linux版本的QQ,然而不是功能不全 ...

随机推荐

  1. [转发]深入理解git,从研究git目录开始

    转发学习的啦. 似乎很少有人在读某个git快速教程的时候会说:“这个关于git的快速教程太酷了!读完了用起git来超级舒服,并且我一点也不怕自己会破坏什么东西.” 对git的初学者来说,刚接触git时 ...

  2. vim加亮和自动缩进

    修改/etc/vimrc 添加 set cindent syntax on

  3. c#中Marshal.Copy()方法的使用

    c#中Marshal.Copy方法的使用 Marshal.copy()方法用来在托管对象(数组)和非托管对象(IntPtr)之间进行内容的复制 函数有很多重载,如下所示: Copy(array< ...

  4. .net core webapi +ddd(领域驱动)+nlog配置+swagger配置 学习笔记(2)

    DDD领域驱动模型设计 什么是DDD 软件开发不是一蹴而就的事情,我们不可能在不了解产品(或行业领域)的前提下进行软件开发,在开发前,通常需要进行大量的业务知识梳理,而后到达软件设计的层面,最后才是开 ...

  5. 虚拟创建失败之Dbus调试

    DBus调试命令 查询连接名 ### 查询所有连接名 # dbus-send --system --print-reply --dest=org.freedesktop.DBus / org.free ...

  6. 第三方登录---微信(使用laravel插件)

    转发: https://www.jianshu.com/p/7be757655814 TP框架: http://www.php.cn/php-weizijiaocheng-363509.html

  7. vue -- 异常处理集合

    1.npm run dev 运行出错,报错如下: > webpack-dev-server --inline --progress --config build/webpack.dev.conf ...

  8. 前端编码规范 -- css篇

    合理的避免使用ID 一般情况下ID不应该被应用于样式. ID的样式不能被复用并且每个页面中你只能使用一次ID. 使用ID唯一有效的是确定网页或整个站点中的位置. 尽管如此,你应该始终考虑使用class ...

  9. 51nod1347(简单逻辑)

    题目链接:https://www.51nod.com/onlineJudge/questionCode.html#!problemId=1347 题意:中文题诶- 思路:稍推理一下就可以发现字符串a是 ...

  10. 关于在SSM框架下使用PageHelper

    首先,如果各位在这块配置和代码有什么问题欢迎说出来,我也会尽自己最大的能力帮大家解答 这些代码我都是写在一个小项目里的,项目的github地址为:https://github.com/Albert-B ...