【QGroundControl二次开发】七.QGC自定义MAVLink消息MavLink通信协议 C++应用

news2024/9/23 7:29:56

1. 接收解析源码分析

通过接收串口或UDP发来的的字节流buffer，长度length=buffer.size()，通过下列脚本解析，每解析出一个mavlink数据包就执行onMavLinkMessage函数

for(int i = 0 ; i < length ; ++i){
      msgReceived = mavlink_parse_char(MAVLINK_COMM_1, (uint8_t)buffer[i], &message, &status);
if(msgReceived){
      emit onMavLinkMessage(message);
 }
}

其中： void onMavLinkMessage(mavlink_message_t message);是得到一个MAVLink消息包后的处理函数，需要根据这个消息的ID来识别当前包的用途（心跳包，GPS位置，姿态等），并提取出感兴趣的数据。

解析函数实现如下，根据message.msgid跳到对应的_decode函数，解码出数据

void onMavLinkMessage(mavlink_message_t message){
  switch (message.msgid){
    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:{
      mavlink_global_position_int_t position;
      mavlink_msg_global_position_int_decode(&message, &position);
      ui->label->setText("经度:"+QString::number(position.lat* 1e-7)// 将整数经纬度转换为度
                      + "\n 纬度:"+QString::number(position.lon* 1e-7)
                      + "\n 高度:"+QString::number(position.alt)// 以米为单位的高度
               );
      break;
     }
    } 
  }

2. 发送源码解析 — 发送一条mavlink_hil_actuator_controls消息

在这里插入图片描述

void sendHILCtrlMessage(uint8_t modes, uint64_t flags, float ctrl[])

{
	mavlink_hil_actuator_controls_t hilctrl;
	hilctrl.mode = modes;
	hilctrl.flags = flags;
	
	for(int i=0;i<16;i++){
		hilctrl.controls[i]=ctrl[i];
	}
	mavlink_message_t mess;
	mavlink_msg_hil_actuator_controls_encode(SystemID, TargetCompID, &mess, &hilctrl);
	
	char buffer[500];
	memset(buffer,0,500);
	unsigned int length = mavlink_msg_to_send_buffer((uint8_t*)buffer, & mess);
	udp.writeDatagram(buffer,length);//通过UDP或者串口将buffer发送出去即可
}