MavLink協議-編碼與解碼（STM32源程序）

ID:235201 · 發表于 2019-9-1 18:49

MavLink協議(開發筆記)

本例程在探索者串口實驗基礎上面修改。如果您的硬件平臺是探索者，可以直接下載使用。

一.MavLink編碼（數據封包）

數據封包：將所需要發送的數據包裝成MavLink協議字節流，通過串口發送出去。

下面是發送心跳包的Demo

void Mavlink_TX_Heartbeat(void){ //消息包結構
mavlink_message_t MSG; //調用mavlink_msg_heartbeat_pack函數將數據封包,傳入到消息結構中。
uint16_t len = mavlink_msg_heartbeat_pack(
0x01, //system_id                      0x01, //component_id                      &MSG, //msg                      0x02, //type                      0x03, //autopilot                      0x51, //base_mode                      0x00, //custom_mode                      0x03);//system_status //臨時緩存區
uint8_t  MAVLink_Buf[6+9+2];       //調用mavlink_msg_to_send_buffer函數將消息結構里面的數據以字節流的方式保存到緩存區
len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MSG); //調用MAV_USART_SendNByte函數發送緩存區里面的數據 MAV_USART_SendNByte(MAVLink_Buf, len);//（需要自己編寫）}

如果正常的話，就如下圖所示。

圖中就是心跳包數據，一幀數據有17位。

在ardupilotmega文件里面還有大量的數據封包函數，

比如：

mavlink_msg_sys_status_pack 系統狀態

mavlink_msg_battery_status_pack 電池狀態

mavlink_msg_highres_imu_pack IMU

mavlink_msg_global_position_int_pack 定位

等等。

二.MavLink解碼（數據解析）

數據解碼：將接收到的數據轉換成字符的形式顯示出來。

下面是解碼心跳包的Demo

//狀態uint8_t ret = 0;//消息結構mavlink_message_t msg;void USART1_IRQHandler(void)                //串口1中斷服務程序{ if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中斷
{       uint8_t data = USART_ReceiveData(USART1);//(USART1->DR);  //讀取接收到的數據       //調用mavlink_parse_char函數來判斷數據，數據接收完成，返回MAVLINK_FRAMING_OK       ret = mavlink_parse_char(MAVLINK_COMM_3, data, &msg, &status); }}
void MAVLinkRcv_Handler(mavlink_message_t MAVLinkMsg){  //心跳包結構  mavlink_heartbeat_t current_messages; if(MAVLinkMsg.msgid==MAVLINK_MSG_ID_HEARTBEAT)//消息ID為心跳包
  { //調用mavlink_msg_heartbeat_decode函數解碼心跳包 mavlink_msg_heartbeat_decode(&MAVLinkMsg, ¤t_messages); //顯示心跳包數據（需要自己編寫） SHOW_HEARTBEAT(MAVLinkMsg, current_messages);  }}
int main(void){
init();
while(1) {       if( MAVLINK_FRAMING_OK == ret )
  {          //接收成功          //解碼
MAVLinkRcv_Handler(msg);
ret = 0;       }
else /*TODO*/ ;       }}

如果正常的話，就如下圖所示。