hm/gcs-nf
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加消息输出

Browse Source
This commit is contained in:
hm-thinkbook14p
2023-02-13 21:55:03 +08:00
parent f94d45b08c
commit 97d077aef0
14 changed files with 907 additions and 254 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/mavlinknode.cpp
+182
View File
@@ -1276,7 +1276,26 @@ void MavLinkNode::StatusParse(mavlink_message_t msg)
gpsTimer.min = min;
gpsTimer.sec = sec;
_showinfo info;
info.flag = 0x23F8;
info.id = infoExportID;//1
info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000
info.lng = vehicle.ins1.lon * 10000000;//*10000000
info.lat = vehicle.ins1.lat * 10000000;//*10000000
info.alt = vehicle.ins1.alt * 100;//*100
info.ve = vehicle.ins1.v_east * 100;//*100
info.vn = vehicle.ins1.v_north * 100;//*100
info.vu = vehicle.ins1.v_up * 100;//*100
info.v = sqrt(vehicle.ins1.v_east * vehicle.ins1.v_east +
vehicle.ins1.v_north * vehicle.ins1.v_north +
vehicle.ins1.v_up * vehicle.ins1.v_up) * 100;//*100
info.course = vehicle.ins1.yaw * 100;//*100
//qDebug() << "infoExportID" << infoExportID;
infoExport(info);
@@ -1938,3 +1957,166 @@ void MavLinkNode::serial_control(void)
}
void MavLinkNode::infoExport(_showinfo info)
{
uint8_t buff[250];
int dataCount = 0;
union {uint8_t B[4];uint16_t D[2];int32_t H;}src;
#if 0
src.D[0] = info.flag;
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.D[0] = info.id;
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = info.time;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = info.lng;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = info.lat;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = info.alt;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = info.ve;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = info.vn;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = info.vu;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = info.v;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = info.course;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = 0;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
src.H = 0;
buff[dataCount++] = src.B[3];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[0];
#else
src.D[0] = info.flag;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
src.D[0] = info.id;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
src.H = info.time;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = info.lng;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = info.lat;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = info.alt;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = info.ve;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = info.vn;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = info.vu;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = info.v;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = info.course;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = 0;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
src.H = 0;
buff[dataCount++] = src.B[0];
buff[dataCount++] = src.B[1];
buff[dataCount++] = src.B[2];
buff[dataCount++] = src.B[3];
#endif
emit SendMessageToExport(0,buff,dataCount);
}