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
2022-09-23 00:36:28 +08:00
parent 0634421ecd
commit d6b98e93f7
23 changed files with 912 additions and 475 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/MAVLINKNODE_zh_CN.ts
+1 -1
View File
@@ -4,7 +4,7 @@
<context>
<name>MavLinkNode</name>
<message>
<location filename="mavlinknode.cpp" line="1293"/>
<location filename="mavlinknode.cpp" line="1274"/>
<source>new vehicle be found,id %1</source>
<translation>发现新的无人机，编号为 %1</translation>
</message>
MavLinkNode/MavLinkNode.pro
+2
View File
@@ -48,6 +48,7 @@ DEFINES += QtMavlinkNode
HEADERS += \
ParsePack.h \
Terminal.h \
ThreadTemplet.h \
commandprocess.h \
@@ -62,6 +63,7 @@ HEADERS += \
statusprocess.h
SOURCES += \
ParsePack.c \
Terminal.cpp \
ThreadTemplet.cpp \
commandprocess.cpp \
MavLinkNode/ParsePack.c
+230
View File
@@ -0,0 +1,230 @@
/*
* ParserPack.c
*
* Created on: Jun 24, 2020
* Author: matth
*/
#include "ParsePack.h"
static uint16_t CRC_CheckSum (uint8_t *pBuffer, uint16_t len)
{
uint16_t poly = 0x8408;
uint16_t crc = 0;
uint8_t carry;
uint8_t i_bits;
uint16_t j;
for (j = 0; j < len; j++) {
crc = (uint16_t) (crc ^ (uint8_t) pBuffer[j]);
for (i_bits = 0; i_bits < 8; i_bits++) {
carry = (uint8_t) (crc & 1);
crc = (uint16_t) (crc / 2);
if (carry) {
crc = (uint16_t) (crc ^ poly);
}
}
}
return crc;
}
void Packer_init(Packer_t *pack)
{
pack->buff[0] = 0xEB;
pack->buff[1] = 0x90;
pack->seq = 0;
}
size_t Packer_pack(Packer_t *pack, uint8_t id, uint8_t pkg[], uint16_t len)
{
uint16_t j;
size_t i;
pack->buff[2] = pack->seq++;
pack->buff[3] = id;
pack->buff[4] = (len >> 8) & 0xFF;
pack->buff[5] = len & 0xFF;
for (i=6,j=0u;j<len;++i,++j)
{
pack->buff[i] = pkg[j];
}
uint16_t crc16 = CRC_CheckSum(&pack->buff[6], len);
pack->buff[i++] = (crc16 & 0xFF);
pack->buff[i++] = (crc16 >> 8)&0xFF;
return i;
}
void Parser_init(Parser_t *parser)
{
parser->stage = 0;
parser->crc_err_cnt = 0;
parser->head_err_cnt = 0;
}
int Parser_char(Parser_t *parser, uint8_t c)
{
int rslt = -1;
switch (parser->stage)
{
case 0:
if (c == 0xEB)
{
parser->stage++;
}
else
{
parser->head_err_cnt++;
}
break;
case 1:
if (c == 0x90)
{
parser->stage++;
}
else
{
parser->stage = 0;
parser->head_err_cnt++;
}
break;
case 2:
parser->seq = c;
parser->stage++;
break;
case 3:
parser->id = c;
parser->stage++;
break;
case 4:
parser->len = c;
parser->stage++;
break;
case 5:
parser->len = (parser->len << 8) + c;
if (parser->len <=PARSER_BUFF_LEN)
{
parser->idx = 0;
parser->stage++;
}
else
{
parser->stage = 0;
}
break;
case 6:
parser->buff[parser->idx++] = c;
if (parser->idx == parser->len)
{
parser->stage++;
}
break;
case 7:
parser->crc16 = c;
parser->stage++;
break;
case 8:
parser->crc16 += (c<<8);
if (parser->crc16 == CRC_CheckSum(parser->buff, parser->len))
{
rslt = parser->id;
}
else
{
parser->crc_err_cnt++;
}
parser->stage = 0;
break;
default:
parser->stage = 0;
break;
}
return rslt;
}
void Packer2_init(Packer2_t *pack)
{
pack->buff[0] = 0xEB;
pack->buff[1] = 0x90;
}
size_t Packer2_pack(Packer2_t *pack, uint8_t id, uint8_t pkg[], uint8_t len)
{
uint8_t j;
size_t i;
uint8_t sum;
pack->buff[2] = id;
pack->buff[3] = len;
sum = 0u;
for (i=4,j=0u;j<len;++i,++j)
{
pack->buff[i] = pkg[j];
sum += pkg[j];
}
pack->buff[i++] = sum;
return i;
}
void Parser2_init(Parser2_t *parser)
{
parser->stage = 0;
}
int Parser2_char(Parser2_t *parser, uint8_t c)
{
int rslt = -1;
switch (parser->stage)
{
case 0:
if (c == 0xEB)
{
parser->stage++;
}
break;
case 1:
if (c == 0x90)
{
parser->stage++;
}
else
{
parser->stage = 0;
}
break;
case 2:
parser->id = c;
parser->stage++;
break;
case 3:
parser->len = c;
parser->stage++;
parser->idx = 0;
parser->sum = 0;
break;
case 4:
parser->buff[parser->idx++] = c;
parser->sum += c;
if (parser->idx == parser->len)
{
parser->stage++;
}
break;
case 5:
if (parser->sum == c)
{
rslt = parser->id;
}
parser->stage = 0;
break;
default:
parser->stage = 0;
break;
}
return rslt;
}
MavLinkNode/ParsePack.h
+72
View File
@@ -0,0 +1,72 @@
/*
* ParserPack.h
*
* Created on: Jun 24, 2020
* Author: matth
*/
#ifndef MATT_PARSEPACK_H_
#define MATT_PARSEPACK_H_
#include <stdint.h>
#include <stddef.h>
#ifdef __cplusplus
extern "C" {
#endif
#ifndef PARSER_BUFF_LEN
#define PARSER_BUFF_LEN (512)
#endif
#ifndef PACKER_BUFF_LEN
#define PACKER_BUFF_LEN (512+6)
#endif
typedef struct{
int stage;
uint16_t idx;
uint16_t len;
uint8_t seq;
uint16_t id;
uint16_t crc16;
uint32_t crc_err_cnt;
uint32_t head_err_cnt;
uint8_t buff[PARSER_BUFF_LEN];
} Parser_t;
typedef struct{
uint8_t seq;
uint8_t buff[PARSER_BUFF_LEN];
} Packer_t;
void Parser_init(Parser_t *parser);
void Packer_init(Packer_t *pack);
int Parser_char(Parser_t *parser, uint8_t c);
size_t Packer_pack(Packer_t *pack, uint8_t id, uint8_t pkg[], uint16_t len);
typedef struct{
int stage;
uint8_t idx;
uint8_t len;
uint16_t id;
uint8_t sum;
uint8_t buff[255];
} Parser2_t;
typedef struct{
uint8_t buff[260];
} Packer2_t;
void Parser2_init(Parser2_t *parser);
void Packer2_init(Packer2_t *pack);
int Parser2_char(Parser2_t *parser, uint8_t c);
size_t Packer2_pack(Packer2_t *pack, uint8_t id, uint8_t pkg[], uint8_t len);
#ifdef __cplusplus
}
#endif
#endif /* MATT_PARSEPACK_H_ */
MavLinkNode/mavlinknode.cpp
+34 -1
View File
@@ -163,6 +163,11 @@ MavLinkNode::MavLinkNode(QObject *parent) : ThreadTemplet(parent)
qDebug() << "mavlink" << QThread::currentThreadId();
Parser2_init(&parser);
Packer2_init(&packer);
}
MavLinkNode::~MavLinkNode()
@@ -282,7 +287,7 @@ void MavLinkNode::setGCSID(int id)
void MavLinkNode::CreateCSV(void)
{
QString filetime = QString("/log/csv/%1").arg(startuptime.toString("yyyyMMddHHmmss"));
QString filetime = QString("./log/csv/%1").arg(startuptime.toString("yyyyMMddHHmmss"));
QDir *temp = new QDir;
@@ -559,6 +564,34 @@ void MavLinkNode::LogTimerOut(void)
*/
}
bool MavLinkNode::setLogData(mavlink_message_t msg)
{
uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
quint64 currentTimestamp = ((quint64)QDateTime::currentMSecsSinceEpoch()) * 1000;
qToBigEndian(currentTimestamp, buff);
uint16_t len = mavlink_msg_to_send_buffer(buff+sizeof(quint64), &msg);
if (mavLogFile)
{
auto size = Packer2_pack(&packer,1,buff,len+sizeof(quint64));
QByteArray data;
data.clear();
data.append((const char *)packer.buff,size);
QDataStream stream(mavLogFile);
stream << data;
return mavLogFile->flush();
}
return false;
}
void MavLinkNode::timer_1s_Out(void)
{
static qint64 last = QDateTime::currentMSecsSinceEpoch();
MavLinkNode/mavlinknode.h
+8 -1
View File
@@ -21,6 +21,8 @@
#include "ThreadTemplet.h"
#include "ParsePack.h"
#ifdef QtMavlinkNode
#include <mavlinknodeglobal.h>
class MAVLINKNODESHARED_EXPORT MavLinkNode : public ThreadTemplet {
@@ -269,6 +271,10 @@ private slots:
void TimerOut(void);
void LogTimerOut(void);
void timer_1s_Out(void);
bool setLogData(mavlink_message_t msg);
//解析
void Mavlinkparse(quint32 src,QByteArray datagram);
@@ -316,7 +322,8 @@ protected:
QTimer *logTimer = nullptr;
Parser2_t parser;
Packer2_t packer;
QTimer *timer_1s = nullptr;