移植MAVLINK到STM32詳細教程之三

在前面教程的基礎上繼續移植優化，以前的沒有加緩衝區，沒有接收函數功能，這裏進行統一的講解                            做者：恆久力行  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"
4. #define UART_TX_BUFFER_SIZE 255
5. #define UART_RX_BUFFER_SIZE 255
6. fifo_t uart_rx_fifo, uart_tx_fifo;
7. uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE], uart_rx_buf[UART_RX_BUFFER_SIZE];
9. /** @brief 讀FIFO
10. * @param fifo 待讀緩衝區
11. * *ch 讀到的數據
12. * @return
13. * 正確讀取，1； 無數據，0
14. */
15. uint8_t fifo_read_ch(fifo_t* fifo,uint8_t* ch)
16. {
17. if(fifo->tail == fifo->head)returnfalse;
18. *ch = fifo->buf[fifo->tail];
20. if(++fifo->tail >= fifo->length) fifo->tail =0;
21. returntrue;
22. }
24. /** @brief 寫一字節數據到FIFO
25. * @param fifo 待寫入緩衝區
26. * ch 待寫入的數據
27. * @return
28. * 正確，1； 緩衝區滿，0
29. */
30. uint8_t fifo_write_ch(fifo_t* fifo,uint8_t ch)
31. {
32. uint16_t h = fifo->head;
34. if(++h >= fifo->length) h =0;
35. if(h == fifo->tail)returnfalse;
37. fifo->buf[fifo->head]= ch;
38. fifo->head = h;
39. returntrue;
40. }
42. /** @brief 返回緩衝區剩餘字節長度
43. * @param fifo
44. * @return
45. * 剩餘空間
46. *
47. * @note 剩餘字節長度大於等於2時，纔可寫入數據
48. */
49. uint16_t fifo_free(fifo_t* fifo)
50. {
51. uint16_t free;
53. if(fifo->head >= fifo->tail) free = fifo->tail +(fifo->length - fifo->head);
54. else free = fifo->tail - fifo->head;
56. return free;
57. }
59. uint16_t fifo_used(fifo_t* fifo)
60. {
61. uint16_t used;
63. if(fifo->head >= fifo->tail) used = fifo->head - fifo->tail;
64. else used = fifo->head +(fifo->length - fifo->tail);
66. return used;
67. }
70. /** @brief 初始化緩衝區
71. * @param *fifo
72. * *buf
73. * length
74. */
75. void fifo_init(fifo_t* fifo,uint8_t* buf,uint16_t length)
76. {
77. uint16_t i;
79. fifo->buf = buf;
80. fifo->length = length;
81. fifo->head =0;
82. fifo->tail =0;
84. for(i=0; i<length; i++) fifo->buf[i]=0;
85. }
87. /** @brief 寫數據到串口，啓動發射
88. *
89. * @note 數據寫入發射緩衝區後，啓動發射中斷，在中斷程序，數據自動發出
90. */
91. uint8_t serial_write_buf(uint8_t* buf,uint16_t length){
92. uint16_t i;
94. if(length ==0)returnfalse;
95. for(i =0; length >0; length--, i++){
96. fifo_write_ch(&uart_tx_fifo, buf[i]);
97. }
98. USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
100. returntrue;
101. }
102. /** @brief 自串口讀數據
103. * @return 一字節數據
104. */
105. uint8_t serial_read_ch(void){
106. uint8_t ch;
107. fifo_read_ch(&uart_rx_fifo,&ch);
108. return ch;
109. }
110. /** @breif 檢測發射緩衝區剩餘字節長度
111. * @return 剩餘字節長度
112. */
113. uint16_t serial_free(void){
114. return fifo_free(&uart_tx_fifo);
115. }
116. uint16_t serial_available(void){
117. return fifo_used(&uart_rx_fifo);
118. }
121. void USART1_IRQHandler(void)
122. {
123. uint8_t c;
124. if(USART_GetITStatus(USART1, USART_IT_RXNE)!= RESET)//數據接收終端
125. {
126. c = USART_ReceiveData(USART1);
127. fifo_write_ch(&uart_rx_fifo, c);
128. //USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);
129. }
130. if(USART_GetITStatus(USART1, USART_IT_TXE)!= RESET)//數據發送中斷
131. {
132. if(fifo_read_ch(&uart_tx_fifo,&c))
133. USART_SendData(USART1, c);
134. else
135. USART_SendData(USART1,0x55);
136. if(fifo_used(&uart_tx_fifo)==0)// Check if all data is transmitted . if yes disable transmitter UDRE interrupt
137. {
138. // Disable the EVAL_COM1 Transmit interrupt
139. USART_ITConfig(USART1, USART_IT_TXE, DISABLE);
140. }
141. }
142. }

 

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. //////////////////////////////////////////////////////////////////////////////////
34. //////////////////////////////////////////////////////////////////
35. //加入如下代碼,支持printf函數,而不須要選擇use MicroLIB
36. #if 1
37. #pragmaimport(__use_no_semihosting)
38. //標準庫須要的支持函數
39. struct __FILE
40. {
41. int handle;
42. };
43. FILE __stdout;
44. //定義_sys_exit()以免使用半主機模式
45. _sys_exit(int x)
46. {
47. x = x;
48. }
49. //重定義fputc函數
50. int fputc(int ch,FILE*f)
51. {
52. while((USART1->SR&0X40)==0);//循環發送,直到發送完畢
53. USART1->DR =(u8) ch;
54. return ch;
55. }
56. #endif
58. #if EN_USART1_RX //若是使能了接收
59. //串口1中斷服務程序
60. //注意,讀取USARTx->SR能避免莫名其妙的錯誤
61. u8 USART_RX_BUF[USART_REC_LEN];//接收緩衝,最大USART_REC_LEN個字節.
62. //接收狀態
63. //bit15， 接收完成標誌
64. //bit14， 接收到0x0d
65. //bit13~0， 接收到的有效字節數目
66. u16 USART_RX_STA=0;//接收狀態標記
67. //初始化IO 串口1
68. //bound:波特率
69. void uart_init(u32 bound){
70. //GPIO端口設置
71. GPIO_InitTypeDef GPIO_InitStructure;
72. USART_InitTypeDef USART_InitStructure;
73. NVIC_InitTypeDef NVIC_InitStructure;
75. RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);//使能GPIOA時鐘
76. RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1時鐘
78. //串口1對應引腳複用映射
79. GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1);//GPIOA9複用爲USART1
80. GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1);//GPIOA10複用爲USART1
82. //USART1端口配置
83. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10;//GPIOA9與GPIOA10
84. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//複用功能
85. GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//速度50MHz
86. GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;//推輓複用輸出
87. GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉
88. GPIO_Init(GPIOA,&GPIO_InitStructure);//初始化PA9，PA10
89. //USART1 初始化設置
90. USART_InitStructure.USART_BaudRate = bound;//波特率設置
91. USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字長爲8位數據格式
92. USART_InitStructure.USART_StopBits = USART_StopBits_1;//一箇中止位
93. USART_InitStructure.USART_Parity = USART_Parity_No;//無奇偶校驗位
94. USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//無硬件數據流控制
95. USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;//收發模式
96. USART_Init(USART1,&USART_InitStructure);//初始化串口1
98. USART_Cmd(USART1, ENABLE);//使能串口1
100. //USART_ClearFlag(USART1, USART_FLAG_TC);
102. #if EN_USART1_RX
103. USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//開啓相關中斷
104. //Usart1 NVIC 配置
105. NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中斷通道
106. NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//搶佔優先級3
107. NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;//子優先級3
108. NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;//IRQ通道使能
109. NVIC_Init(&NVIC_InitStructure);//根據指定的參數初始化VIC寄存器、
110. #endif
112. }
113. //void USART1_IRQHandler(void) //串口1中斷服務程序
114. //{
115. // u8 Res;
116. //#if SYSTEM_SUPPORT_OS //若是SYSTEM_SUPPORT_OS爲真，則須要支持OS.
117. // OSIntEnter();
118. //#endif
119. // if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收中斷(接收到的數據必須是0x0d 0x0a結尾)
120. // {
121. // Res =USART_ReceiveData(USART1);//(USART1->DR); //讀取接收到的數據
122. //
123. // if((USART_RX_STA&0x8000)==0)//接收未完成
124. // {
125. // if(USART_RX_STA&0x4000)//接收到了0x0d
126. // {
127. // if(Res!=0x0a)USART_RX_STA=0;//接收錯誤,從新開始
128. // else USART_RX_STA|=0x8000; //接收完成了
129. // }
130. // else //還沒收到0X0D
131. // {
132. // if(Res==0x0d)USART_RX_STA|=0x4000;
133. // else
134. // {
135. // USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
136. // USART_RX_STA++;
137. // if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收數據錯誤,從新開始接收
138. // }
139. // }
140. // }
141. // }
142. //#if SYSTEM_SUPPORT_OS //若是SYSTEM_SUPPORT_OS爲真，則須要支持OS.
143. // OSIntExit();
144. //#endif
145. //}
146. #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"
10. /*解決..\MAVLINK\common\../mavlink_types.h(53): error: #20: identifier "pack" is undefined*/
11. #define MAVPACKED( __Declaration__ ) __Declaration__
13. /*解決..\MAVLINK\common\../mavlink_types.h(53): error: #3092: anonymous unions are only supported in --gnu mode, or when enabled with #pragma anon_unions*/
14. #pragma anon_unions
17. #defineinline __inline
19. //#ifndef memset//由624668529添加 2018-08-24
20. //void *memset(void *dest, int data, size_t length) {
21. // uint32_t i;
22. // int *point = dest;
23. // for(i=0; i<length; i++) point[i] = data;
24. // return dest;
25. //}
26. //#endif
27. //#ifndef memcpy//由624668529添加 2018-08-24
28. //void *memcpy(void *dest, const void *src, size_t n)
29. //{
30. // unsigned char *pout = (unsigned char*)dest;
31. // unsigned char *pin = (unsigned char*)src;
32. // while (n-- > 0) *pout++ = *pin++;
33. // return dest;
34. //}
35. //#endif
38. #include"mavlink_types.h"
39. #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
40. //#define MAVLINK_SEPARATE_HELPERS
41. //mavlink_system_t mavlink_system = {0,0};
42. //mavlink_system_t mavlink_system = {
43. // 1,
44. // 1
45. //}; // System ID, 1-255, Component/Subsystem ID, 1-255
49. //void comm_send_ch(mavlink_channel_t chan, uint8_t buf)
50. //{
51. // chan=chan;
52. // USART_SendData(USART1, buf); //向串口1發送數據
53. // while(USART_GetFlagStatus(USART1,USART_FLAG_TC)!=SET);//等待發送結束
54. //}
59. #include"mavlink.h"
60. #include"mavlink_helpers.h"
61. #endif//AVLINK_AVOID_ERRORS_H

 

4.添加open_tel_mavlink.h 和open_tel_mavlink.c 這兩個函數是測試mavlink通訊的上層代碼

open_tel_mavlink.h

2. #ifndef __OPEN_TEL_MAVLINK_H//做者：恆久力行  qq:624668529
3. #define __OPEN_TEL_MAVLINK_H
6. //#include "./minimal/minimal/minimal.h"
7. #include"define.h"
8. #include"mavlink_avoid_errors.h"
9. #include"stdint.h"
13. void mavlink_send_message(mavlink_channel_t chan,enum ap_message id,uint16_t packet_drops);
14. void update(void);
15. void handleMessage(mavlink_message_t* msg);
17. #endif/*__OPENTEL_MAVLINK_H*/

open_tel_mavlink.c

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

5.添加define.h函數，這裏是上層函數的一些結構類型定義，爲避免上層報錯就添加上（有興趣的能夠本身精簡）

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

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;
4. MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan,constchar*buf,uint16_t len)
5. {
6. serial_write_buf((uint8_t*)buf, len);
7. }

 

mavlink_helpers.h全代碼

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

8.checksum未經任何修改（爲了你們方便看附上全代碼）

checksum.h

1. #ifdef __cplusplus//做者：恆久力行  qq:624668529
2. extern"C"{
3. #endif
5. #ifndef _CHECKSUM_H_
6. #define _CHECKSUM_H_
8. // Visual Studio versions before 2010 don't have stdint.h, so we just error out.
9. #if (defined _MSC_VER) && (_MSC_VER < 1600)
10. #error"The C-MAVLink implementation requires Visual Studio 2010 or greater"
11. #endif
13. #include<stdint.h>
15. /**
16. *
17. * CALCULATE THE CHECKSUM
18. *
19. */
21. #define X25_INIT_CRC 0xffff
22. #define X25_VALIDATE_CRC 0xf0b8
24. #ifndef HAVE_CRC_ACCUMULATE
25. /**
26. * @brief Accumulate the X.25 CRC by adding one char at a time.
27. *
28. * The checksum function adds the hash of one char at a time to the
29. * 16 bit checksum (uint16_t).
30. *
31. * @param data new char to hash
32. * @param crcAccum the already accumulated checksum
33. **/
34. staticinlinevoid crc_accumulate(uint8_t data,uint16_t*crcAccum)
35. {
36. /*Accumulate one byte of data into the CRC*/
37. uint8_t tmp;
39. tmp = data ^(uint8_t)(*crcAccum &0xff);
40. tmp ^=(tmp<<4);
41. *crcAccum =(*crcAccum>>8)^(tmp<<8)^(tmp <<3)^(tmp>>4);
42. }
43. #endif
46. /**
47. * @brief Initiliaze the buffer for the X.25 CRC
48. *
49. * @param crcAccum the 16 bit X.25 CRC
50. */
51. staticinlinevoid crc_init(uint16_t* crcAccum)
52. {
53. *crcAccum = X25_INIT_CRC;
54. }
57. /**
58. * @brief Calculates the X.25 checksum on a byte buffer
59. *
60. * @param pBuffer buffer containing the byte array to hash
61. * @param length length of the byte array
62. * @return the checksum over the buffer bytes
63. **/
64. staticinlineuint16_t crc_calculate(constuint8_t* pBuffer,uint16_t length)
65. {
66. uint16_t crcTmp;
67. crc_init(&crcTmp);
68. while(length--){
69. crc_accumulate(*pBuffer++,&crcTmp);
70. }
71. return crcTmp;
72. }
75. /**
76. * @brief Accumulate the X.25 CRC by adding an array of bytes
77. *
78. * The checksum function adds the hash of one char at a time to the
79. * 16 bit checksum (uint16_t).
80. *
81. * @param data new bytes to hash
82. * @param crcAccum the already accumulated checksum
83. **/
84. staticinlinevoid crc_accumulate_buffer(uint16_t*crcAccum,constchar*pBuffer,uint16_t length)
85. {
86. constuint8_t*p =(constuint8_t*)pBuffer;
87. while(length--){
88. crc_accumulate(*p++, crcAccum);
89. }
90. }
92. #endif/* _CHECKSUM_H_ */
94. #ifdef __cplusplus
95. }
96. #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_
4. /* enable math defines on Windows */
5. #ifdef _MSC_VER
6. #ifndef _USE_MATH_DEFINES
7. #define _USE_MATH_DEFINES
8. #endif
9. #endif
10. #include<math.h>
12. #ifndef M_PI_2
13. #define M_PI_2 ((float)asin(1))
14. #endif
16. /**
17. * @file mavlink_conversions.h
18. *
19. * These conversion functions follow the NASA rotation standards definition file
20. * available online.
21. *
22. * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free
23. * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free)
24. * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the
25. * protocol as widely as possible.
26. *
27. * @author James Goppert
28. * @author Thomas Gubler <thomasgubler@gmail.com>
29. */
32. /**
33. * Converts a quaternion to a rotation matrix
34. *
35. * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
36. * @param dcm a 3x3 rotation matrix
37. */
38. MAVLINK_HELPER void mavlink_quaternion_to_dcm(constfloat quaternion[4],float dcm[3][3])
39. {
40. double a = quaternion[0];
41. double b = quaternion[1];
42. double c = quaternion[2];
43. double d = quaternion[3];
44. double aSq = a * a;
45. double bSq = b * b;
46. double cSq = c * c;
47. double dSq = d * d;
48. dcm[0][0]= aSq + bSq - cSq - dSq;
49. dcm[0][1]=2*(b * c - a * d);
50. dcm[0][2]=2*(a * c + b * d);
51. dcm[1][0]=2*(b * c + a * d);
52. dcm[1][1]= aSq - bSq + cSq - dSq;
53. dcm[1][2]=2*(c * d - a * b);
54. dcm[2][0]=2*(b * d - a * c);
55. dcm[2][1]=2*(a * b + c * d);
56. dcm[2][2]= aSq - bSq - cSq + dSq;
57. }
60. /**
61. * Converts a rotation matrix to euler angles
62. *
63. * @param dcm a 3x3 rotation matrix
64. * @param roll the roll angle in radians
65. * @param pitch the pitch angle in radians
66. * @param yaw the yaw angle in radians
67. */
68. MAVLINK_HELPER void mavlink_dcm_to_euler(constfloat dcm[3][3],float* roll,float* pitch,float* yaw)
69. {
70. float phi, theta, psi;
71. theta = asin(-dcm[2][0]);
73. if(fabsf(theta -(float)M_PI_2)<1.0e-3f){
74. phi =0.0f;
75. psi =(atan2f(dcm[1][2]- dcm[0][1],
76. dcm[0][2]+ dcm[1][1])+ phi);
78. }elseif(fabsf(theta +(float)M_PI_2)<1.0e-3f){
79. phi =0.0f;
80. psi = atan2f(dcm[1][2]- dcm[0][1],
81. dcm[0][2]+ dcm[1][1]- phi);
83. }else{
84. phi = atan2f(dcm[2][1], dcm[2][2]);
85. psi = atan2f(dcm[1][0], dcm[0][0]);
86. }
88. *roll = phi;
89. *pitch = theta;
90. *yaw = psi;
91. }
94. /**
95. * Converts a quaternion to euler angles
96. *
97. * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
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. */
102. MAVLINK_HELPER void mavlink_quaternion_to_euler(constfloat quaternion[4],float* roll,float* pitch,float* yaw)
103. {
104. float dcm[3][3];
105. mavlink_quaternion_to_dcm(quaternion, dcm);
106. mavlink_dcm_to_euler((constfloat(*)[3])dcm, roll, pitch, yaw);
107. }
110. /**
111. * Converts euler angles to a quaternion
112. *
113. * @param roll the roll angle in radians
114. * @param pitch the pitch angle in radians
115. * @param yaw the yaw angle in radians
116. * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
117. */
118. MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll,float pitch,float yaw,float quaternion[4])
119. {
120. float cosPhi_2 = cosf(roll /2);
121. float sinPhi_2 = sinf(roll /2);
122. float cosTheta_2 = cosf(pitch /2);
123. float sinTheta_2 = sinf(pitch /2);
124. float cosPsi_2 = cosf(yaw /2);
125. float sinPsi_2 = sinf(yaw /2);
126. quaternion[0]=(cosPhi_2 * cosTheta_2 * cosPsi_2 +
127. sinPhi_2 * sinTheta_2 * sinPsi_2);
128. quaternion[1]=(sinPhi_2 * cosTheta_2 * cosPsi_2 -
129. cosPhi_2 * sinTheta_2 * sinPsi_2);
130. quaternion[2]=(cosPhi_2 * sinTheta_2 * cosPsi_2 +
131. sinPhi_2 * cosTheta_2 * sinPsi_2);
132. quaternion[3]=(cosPhi_2 * cosTheta_2 * sinPsi_2 -
133. sinPhi_2 * sinTheta_2 * cosPsi_2);
134. }
137. /**
138. * Converts a rotation matrix to a quaternion
139. * Reference:
140. * - Shoemake, Quaternions,
141. * http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
142. *
143. * @param dcm a 3x3 rotation matrix
144. * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
145. */
146. MAVLINK_HELPER void mavlink_dcm_to_quaternion(constfloat dcm[3][3],float quaternion[4])
147. {
148. int dcm_j,dcm_k;
149. float s;
150. float tr = dcm[0][0]+ dcm[1][1]+ dcm[2][2];
151. if(tr >0.0f){
152. float s = sqrtf(tr +1.0f);
153. quaternion[0]= s *0.5f;
154. s =0.5f/ s;
155. quaternion[1]=(dcm[2][1]- dcm[1][2])* s;
156. quaternion[2]=(dcm[0][2]- dcm[2][0])* s;
157. quaternion[3]=(dcm[1][0]- dcm[0][1])* s;
158. }else{
159. /* Find maximum diagonal element in dcm
160. * store index in dcm_i */
161. int dcm_i =0;
162. int i;
163. for(i =1; i <3; i++){
164. if(dcm[i][i]> dcm[dcm_i][dcm_i]){
165. dcm_i = i;
166. }
167. }
168. dcm_j =(dcm_i +1)%3;
169. dcm_k =(dcm_i +2)%3;
170. s = sqrtf((dcm[dcm_i][dcm_i]- dcm[dcm_j][dcm_j]-
171. dcm[dcm_k][dcm_k])+1.0f);
172. quaternion[dcm_i +1]= s *0.5f;
173. s =0.5f/ s;
174. quaternion[dcm_j +1]=(dcm[dcm_i][dcm_j]+ dcm[dcm_j][dcm_i])* s;
175. quaternion[dcm_k +1]=(dcm[dcm_k][dcm_i]+ dcm[dcm_i][dcm_k])* s;
176. quaternion[0]=(dcm[dcm_k][dcm_j]- dcm[dcm_j][dcm_k])* s;
177. }
178. }
181. /**
182. * Converts euler angles to a rotation matrix
183. *
184. * @param roll the roll angle in radians
185. * @param pitch the pitch angle in radians
186. * @param yaw the yaw angle in radians
187. * @param dcm a 3x3 rotation matrix
188. */
189. MAVLINK_HELPER void mavlink_euler_to_dcm(float roll,float pitch,float yaw,float dcm[3][3])
190. {
191. float cosPhi = cosf(roll);
192. float sinPhi = sinf(roll);
193. float cosThe = cosf(pitch);
194. float sinThe = sinf(pitch);
195. float cosPsi = cosf(yaw);
196. float sinPsi = sinf(yaw);
198. dcm[0][0]= cosThe * cosPsi;
199. dcm[0][1]=-cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
200. dcm[0][2]= sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
202. dcm[1][0]= cosThe * sinPsi;
203. dcm[1][1]= cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
204. dcm[1][2]=-sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
206. dcm[2][0]=-sinThe;
207. dcm[2][1]= sinPhi * cosThe;
208. dcm[2][2]= cosPhi * cosThe;
209. }
211. #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_
4. // Visual Studio versions before 2010 don't have stdint.h, so we just error out.
5. #if (defined _MSC_VER) && (_MSC_VER < 1600)
6. #error"The C-MAVLink implementation requires Visual Studio 2010 or greater"
7. #endif
9. #include<stdint.h>
11. // Macro to define packed structures
12. //#ifdef __GNUC__
13. // #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed))
14. //#else
15. // #define MAVPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
16. //#endif
18. #ifndef MAVLINK_MAX_PAYLOAD_LEN
19. // it is possible to override this, but be careful!
20. #define MAVLINK_MAX_PAYLOAD_LEN 255///< Maximum payload length
21. #endif
23. #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)
24. #define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN +1)///< Length of all header bytes, including core and checksum
25. #define MAVLINK_NUM_CHECKSUM_BYTES 2
26. #define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
28. #define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)///< Maximum packet length
30. #define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
31. #define MAVLINK_EXTENDED_HEADER_LEN 14
33. #if (defined _MSC_VER) || ((defined __APPLE__) && (defined __MACH__)) || (defined __linux__)
34. /* full fledged 32bit++ OS */
35. #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
36. #else
37. /* small microcontrollers */
38. #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048
39. #endif
41. #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
44. /**
45. * Old-style 4 byte param union
46. *
47. * This struct is the data format to be used when sending
48. * parameters. The parameter should be copied to the native
49. * type (without type conversion)
50. * and re-instanted on the receiving side using the
51. * native type as well.
52. */
53. MAVPACKED(
54. typedefstruct param_union {
55. union{
56. float param_float;
57. int32_t param_int32;
58. uint32_t param_uint32;
59. int16_t param_int16;
60. uint16_t param_uint16;
61. int8_t param_int8;
62. uint8_t param_uint8;
63. uint8_t bytes[4];
64. };
65. uint8_t type;
66. })mavlink_param_union_t;
69. /**
70. * New-style 8 byte param union
71. * 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.
72. * The mavlink_param_union_double_t will be treated as a little-endian structure.
73. *
74. * 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.
75. * 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
76. * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union.
77. * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above,
78. * as bitfield ordering isn’t consistent between platforms. The above is intended to be for gcc on x86,
79. * 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,
80. * and the bits pulled out using the shifts/masks.
81. */
82. MAVPACKED(
83. typedefstruct param_union_extended {
84. union{
85. struct{
86. uint8_t is_double:1;
87. uint8_t mavlink_type:7;
88. union{
89. char c;
90. uint8_tuint8;
91. int8_tint8;
92. uint16_tuint16;
93. int16_tint16;
94. uint32_tuint32;
95. int32_tint32;
96. float f;
97. uint8_t align[7];
98. };
99. };
100. uint8_t data[8];
101. };
102. })mavlink_param_union_double_t;
104. /**
105. * This structure is required to make the mavlink_send_xxx convenience functions
106. * work, as it tells the library what the current system and component ID are.
107. */
108. MAVPACKED(
109. typedefstruct __mavlink_system {
110. uint8_t sysid;///< Used by the MAVLink message_xx_send() convenience function
111. uint8_t compid;///< Used by the MAVLink message_xx_send() convenience function
112. })mavlink_system_t;
114. MAVPACKED(
115. typedefstruct __mavlink_message {
116. uint16_t checksum;///< sent at end of packet
117. uint8_t magic;///< protocol magic marker
118. uint8_t len;///< Length of payload
119. uint8_t seq;///< Sequence of packet
120. uint8_t sysid;///< ID of message sender system/aircraft
121. uint8_t compid;///< ID of the message sender component
122. uint8_t msgid;///< ID of message in payload
123. uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
124. })mavlink_message_t;
126. MAVPACKED(
127. typedefstruct __mavlink_extended_message {
128. mavlink_message_t base_msg;
129. int32_t extended_payload_len;///< Length of extended payload if any
130. uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
131. })mavlink_extended_message_t;
133. typedefenum{
134. MAVLINK_TYPE_CHAR =0,
135. MAVLINK_TYPE_UINT8_T =1,
136. MAVLINK_TYPE_INT8_T =2,
137. MAVLINK_TYPE_UINT16_T =3,
138. MAVLINK_TYPE_INT16_T =4,
139. MAVLINK_TYPE_UINT32_T =5,
140. MAVLINK_TYPE_INT32_T =6,
141. MAVLINK_TYPE_UINT64_T =7,
142. MAVLINK_TYPE_INT64_T =8,
143. MAVLINK_TYPE_FLOAT =9,
144. MAVLINK_TYPE_DOUBLE =10
145. }mavlink_message_type_t;
147. #define MAVLINK_MAX_FIELDS 64
149. typedefstruct __mavlink_field_info {
150. constchar*name;// name of this field
151. constchar*print_format;// printing format hint, or NULL
152. mavlink_message_type_t type;// type of this field
153. unsignedint array_length;// if non-zero, field is an array
154. unsignedint wire_offset;// offset of each field in the payload
155. unsignedint structure_offset;// offset in a C structure
156. }mavlink_field_info_t;
158. // note that in this structure the order of fields is the order
159. // in the XML file, not necessary the wire order
160. typedefstruct __mavlink_message_info {
161. constchar*name;// name of the message
162. unsigned num_fields;// how many fields in this message
163. mavlink_field_info_t fields[MAVLINK_MAX_FIELDS];// field information
164. }mavlink_message_info_t;
166. #define _MAV_PAYLOAD(msg)((constchar*)(&((msg)->payload64[0])))
167. #define _MAV_PAYLOAD_NON_CONST(msg)((char*)(&((msg)->payload64[0])))
169. // checksum is immediately after the payload bytes
170. #define mavlink_ck_a(msg)*((msg)->len +(uint8_t*)_MAV_PAYLOAD_NON_CONST(msg))
171. #define mavlink_ck_b(msg)*(((msg)->len+(uint16_t)1)+(uint8_t*)_MAV_PAYLOAD_NON_CONST(msg))
173. typedefenum{
174. MAVLINK_COMM_0,
175. MAVLINK_COMM_1,
176. MAVLINK_COMM_2,
177. MAVLINK_COMM_3
178. }mavlink_channel_t;
180. /*
181. * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
182. * of buffers they will use. If more are used, then the result will be
183. * a stack overrun
184. */
185. #ifndef MAVLINK_COMM_NUM_BUFFERS
186. #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
187. # define MAVLINK_COMM_NUM_BUFFERS 16
188. #else
189. # define MAVLINK_COMM_NUM_BUFFERS 4
190. #endif
191. #endif
193. typedefenum{
194. MAVLINK_PARSE_STATE_UNINIT=0,
195. MAVLINK_PARSE_STATE_IDLE,
196. MAVLINK_PARSE_STATE_GOT_STX,
197. MAVLINK_PARSE_STATE_GOT_SEQ,
198. MAVLINK_PARSE_STATE_GOT_LENGTH,
199. MAVLINK_PARSE_STATE_GOT_SYSID,
200. MAVLINK_PARSE_STATE_GOT_COMPID,
201. MAVLINK_PARSE_STATE_GOT_MSGID,
202. MAVLINK_PARSE_STATE_GOT_PAYLOAD,
203. MAVLINK_PARSE_STATE_GOT_CRC1,
204. MAVLINK_PARSE_STATE_GOT_BAD_CRC1
205. }mavlink_parse_state_t;///< The state machine for the comm parser
207. typedefenum{
208. MAVLINK_FRAMING_INCOMPLETE=0,
209. MAVLINK_FRAMING_OK=1,
210. MAVLINK_FRAMING_BAD_CRC=2
211. }mavlink_framing_t;
213. typedefstruct __mavlink_status {
214. uint8_t msg_received;///< Number of received messages
215. uint8_t buffer_overrun;///< Number of buffer overruns
216. uint8_t parse_error;///< Number of parse errors
217. mavlink_parse_state_t parse_state;///< Parsing state machine
218. uint8_t packet_idx;///< Index in current packet
219. uint8_t current_rx_seq;///< Sequence number of last packet received
220. uint8_t current_tx_seq;///< Sequence number of last packet sent
221. uint16_t packet_rx_success_count;///< Received packets
222. uint16_t packet_rx_drop_count;///< Number of packet drops
223. }mavlink_status_t;
225. #define MAVLINK_BIG_ENDIAN 0
226. #define MAVLINK_LITTLE_ENDIAN 1
228. #endif/* MAVLINK_TYPES_H_ */

11.protocol.h未作任何修改，附上全代碼

protocol.h全代碼

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

 

  

附件列表