1685 lines
64 KiB
C++
1685 lines
64 KiB
C++
#include "Parse.h"
|
||
|
||
Parse::Parse(QObject *parent) : QObject(parent)
|
||
{
|
||
setAutoDelete(true);
|
||
|
||
qRegisterMetaType<Parse::_ins>("Parse::_ins");
|
||
qRegisterMetaType<Parse::_nav>("Parse::_nav");
|
||
qRegisterMetaType<Parse::_eadc>("Parse::_eadc");
|
||
qRegisterMetaType<Parse::_ecu>("Parse::_ecu");
|
||
qRegisterMetaType<Parse::_sspc>("Parse::_sspc");
|
||
|
||
qRegisterMetaType<Parse::_gear>("Parse::_gear");
|
||
qRegisterMetaType<Parse::_actuator>("Parse::_actuator");
|
||
qRegisterMetaType<Parse::_actuator1>("Parse::_actuator1");
|
||
qRegisterMetaType<Parse::_actuator2>("Parse::_actuator2");
|
||
qRegisterMetaType<Parse::_imu>("Parse::_imu");
|
||
qRegisterMetaType<Parse::_euler>("Parse::_euler");
|
||
qRegisterMetaType<Parse::_vel>("Parse::_vel");
|
||
qRegisterMetaType<Parse::_pos>("Parse::_pos");
|
||
|
||
}
|
||
|
||
Parse::~Parse()
|
||
{
|
||
|
||
}
|
||
|
||
|
||
void Parse::run()
|
||
{
|
||
QElapsedTimer time;
|
||
time.start();
|
||
//qDebug() << "current thread is:" << QThread::currentThread();
|
||
while (raw.size() > 0) {
|
||
|
||
//qDebug() << "size" <<raw.size();
|
||
|
||
switch (index) {
|
||
case 0x00:
|
||
{
|
||
if(raw.size() >= 18)//0~17 INS
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(0);
|
||
_ins ins;
|
||
int data_count = 0;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.roll_rate = src.i[0] * 400.0/32767;//滚转角速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.yaw_rate = src.i[0] * 400.0/32767;//偏航角速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.pitch_rate = src.i[0] * 400.0/32767;//俯仰角速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.yaw = src.i[0] * M_PI/32767 * 57.29;//航向角
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.pitch = src.i[0] * M_PI/32767 * 57.29;//俯仰角
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.roll = src.i[0] * M_PI/32767 * 57.29;//滚转角
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.ax = src.i[0] * 100.0/32767;//机体纵向加速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.ay = src.i[0] * 100.0/32767;//机体法向加速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.az = src.i[0] * 100.0/32767;//机体侧向加速度
|
||
/*
|
||
qDebug() << ins.roll_rate
|
||
<< ins.yaw_rate
|
||
<< ins.pitch_rate
|
||
<< ins.yaw
|
||
<< ins.pitch
|
||
<< ins.roll
|
||
<< ins.ax
|
||
<< ins.ay
|
||
<< ins.az;
|
||
*/
|
||
|
||
|
||
emit INS_Info(ins);
|
||
}
|
||
|
||
if(raw.size() >= 73)//18~72 NAV
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(18);
|
||
_nav nav;
|
||
int data_count = 0;
|
||
|
||
|
||
union//低在前，高位在后
|
||
{
|
||
quint8 byte;
|
||
struct
|
||
{
|
||
quint8 Satellite:2;
|
||
quint8 heading:2;
|
||
quint8 ins:2;
|
||
quint8 nav:2;
|
||
};
|
||
}state1;
|
||
|
||
state1.byte = data[data_count++];
|
||
|
||
//D7D6 导航状态
|
||
switch (state1.nav) {
|
||
case 0x01:
|
||
nav.state1.navStatus = tr("准备");
|
||
break;
|
||
case 0x02:
|
||
nav.state1.navStatus = tr("对准");
|
||
break;
|
||
case 0x03:
|
||
nav.state1.navStatus = tr("导航");
|
||
break;
|
||
}
|
||
|
||
//D5D4 组合状态
|
||
switch (state1.ins) {
|
||
case 0x00:
|
||
nav.state1.insStatus = tr("纯惯导");
|
||
break;
|
||
case 0x01:
|
||
nav.state1.insStatus = tr("惯性/卫星组合");
|
||
break;
|
||
case 0x02:
|
||
nav.state1.insStatus = tr("惯性/差分卫星组合");
|
||
break;
|
||
case 0x03:
|
||
nav.state1.insStatus = tr("预留");
|
||
break;
|
||
}
|
||
|
||
//D3D2 航向信号源
|
||
switch (state1.heading) {
|
||
case 0x00:
|
||
nav.state1.headingSrc = tr("未使用外部航向");
|
||
break;
|
||
case 0x01:
|
||
nav.state1.headingSrc = tr("使用双天线航向");
|
||
break;
|
||
case 0x02:
|
||
nav.state1.headingSrc = tr("使用磁航向");
|
||
break;
|
||
}
|
||
|
||
//D1D0 卫星信号源
|
||
switch (state1.Satellite) {
|
||
case 0x01:
|
||
nav.state1.Satellite = tr("未使用卫星");
|
||
break;
|
||
case 0x02:
|
||
nav.state1.Satellite = tr("使用内部卫星");
|
||
break;
|
||
case 0x03:
|
||
nav.state1.Satellite = tr("使用外部卫星");
|
||
break;
|
||
}
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
nav.longitude = src.h * M_PI / (0x7FFFFFFF - 1) * 57.29;//位置经度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
nav.latitude = src.h * M_PI / (0x7FFFFFFF - 1) * 57.29;//位置纬度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.altitude = src.i[0] * 12000.0 / (0x7FFF - 1);//组合高度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.worktime = src.I[0];//本状态工作时间
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Ve = src.i[0] * 400.0 / (0x7FFF - 1);//东向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Vn = src.i[0] * 400.0 / (0x7FFF - 1);//北向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Vu = src.i[0] * 400.0 / (0x7FFF - 1);//天向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Gs = src.i[0] * 400.0 / (0x7FFF - 1);//地速
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.heading = src.i[0] * M_PI / (0x7FFF - 1) * 57.29;//航迹角
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Ae = src.i[0] * 100.0 / (0x7FFF - 1);//东向加速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.An = src.i[0] * 100.0 / (0x7FFF - 1);//北向加速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Au = src.i[0] * 100.0 / (0x7FFF - 1);//天向加速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
nav.satellite_longitude = src.h * M_PI / (0x7FFFFFFF - 1) * 57.29;//卫星经度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
nav.satellite_latitude = src.h * M_PI / (0x7FFFFFFF - 1) * 57.29;//卫星纬度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.satellite_altitude = src.i[0] * 12000.0 / (0x7FFF - 1);//卫星高度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.satellite_Ve = src.i[0] * 400.0 / (0x7FFF - 1);//卫星东向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.satellite_Vn = src.i[0] * 400.0 / (0x7FFF - 1);//卫星北向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.satellite_Vu = src.i[0] * 400.0 / (0x7FFF - 1);//卫星天向速度
|
||
|
||
|
||
union//低在前，高位在后
|
||
{
|
||
quint8 byte;
|
||
struct
|
||
{
|
||
bool communication:1;//通讯状态
|
||
bool heading: 1;//双天线航向有效位
|
||
bool satellite: 1;//内置卫星数据有效位
|
||
bool RTK: 1;//内置差分数据有效位
|
||
bool exSatellite: 1;//外置卫星数据有效位
|
||
bool exRTK: 1;//外置差分数据有效位
|
||
bool altitude: 1;//组合高度有效位
|
||
bool ins: 1;//惯导数据有效位
|
||
};
|
||
}state2;
|
||
|
||
state2.byte = data[data_count++];
|
||
nav.state2.ins = state2.ins;//惯导数据有效位
|
||
nav.state2.altitude = state2.altitude;//组合高度有效位
|
||
nav.state2.exRTK = state2.exRTK;//外置差分数据有效位
|
||
nav.state2.exSatellite = state2.exSatellite;//外置卫星数据有效位
|
||
nav.state2.RTK = state2.RTK;//内置差分数据有效位
|
||
nav.state2.satellite = state2.satellite;//内置卫星数据有效位
|
||
nav.state2.heading = state2.heading;//双天线航向有效位
|
||
nav.state2.communication = state2.communication;//通讯状态
|
||
|
||
|
||
union//低在前，高位在后
|
||
{
|
||
quint8 byte;
|
||
struct
|
||
{
|
||
bool back0: 1;//
|
||
bool back1: 1;//
|
||
quint8 altitudeInfo: 2;//D3D2 高度阻尼信号
|
||
bool back4: 1;//
|
||
bool install: 1;//外部状态偏角有效位
|
||
bool das: 1;//大气数据有效位
|
||
bool back7: 1;//
|
||
};
|
||
}state3;
|
||
|
||
state3.byte = data[data_count++];
|
||
nav.state3.back7 = state3.back7;//
|
||
nav.state3.das = state3.das;//大气数据有效位
|
||
nav.state3.install = state3.install;//外部状态偏角有效位
|
||
nav.state3.back4 = state3.back4;//
|
||
|
||
switch (state3.altitudeInfo) {
|
||
case 0x00:
|
||
nav.state3.altitudeInfo = tr("无");
|
||
break;
|
||
case 0x01:
|
||
nav.state3.altitudeInfo = tr("卫星");
|
||
break;
|
||
case 0x02:
|
||
nav.state3.altitudeInfo = tr("气压");
|
||
break;
|
||
}
|
||
|
||
nav.state3.back1 = state3.back1;//
|
||
nav.state3.back0 = state3.back0;//
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.year = src.I[0];//UTC年
|
||
|
||
nav.month = data[data_count++];;//UTC月
|
||
nav.day = data[data_count++];;//UTC日
|
||
nav.hour = data[data_count++];;//UTC时
|
||
nav.minute = data[data_count++];;//UTC分
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.second = src.I[0] * 60.0 / (0xFFFF - 1);//UTC秒
|
||
|
||
emit NAV_Info(nav);
|
||
}
|
||
|
||
if(raw.size() >= 115)//73~114 GEAR
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(73);
|
||
_gear group;
|
||
int data_count = 0;
|
||
|
||
int swidx = 0;
|
||
|
||
group.SW[swidx++] = (((uint8_t)data[0] & 0x80) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[0] & 0x40) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[0] & 0x20) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[0] & 0x10) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[0] & 0x08) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[0] & 0x04) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[0] & 0x02) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[0] & 0x01) > 0)?(true):(false);
|
||
|
||
group.SW[swidx++] = (((uint8_t)data[1] & 0x80) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[1] & 0x40) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[1] & 0x20) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[1] & 0x10) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[1] & 0x08) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[1] & 0x04) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[1] & 0x02) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[1] & 0x01) > 0)?(true):(false);
|
||
|
||
group.SW[swidx++] = (((uint8_t)data[2] & 0x80) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[2] & 0x40) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[2] & 0x20) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[2] & 0x10) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[2] & 0x08) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[2] & 0x04) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[2] & 0x02) > 0)?(true):(false);
|
||
group.SW[swidx++] = (((uint8_t)data[2] & 0x01) > 0)?(true):(false);
|
||
|
||
|
||
data_count = 3;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.LeftPressure = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.RightPressure = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.AirPressure = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.LeftWheel = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.RightWheel = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.rpm3 = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.rpm4 = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.fuel = src.F;
|
||
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.cpu_load = src.I[0];
|
||
|
||
|
||
swidx = 0;
|
||
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x80) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x40) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x20) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x10) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x08) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x04) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x02) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x01) > 0)?(true):(false);
|
||
data_count++;
|
||
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x80) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x40) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x20) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x10) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x08) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x04) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x02) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x01) > 0)?(true):(false);
|
||
data_count++;
|
||
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x80) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x40) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x20) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x10) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x08) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x04) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x02) > 0)?(true):(false);
|
||
group.DO[swidx++] = (((uint8_t)data[data_count] & 0x01) > 0)?(true):(false);
|
||
data_count++;
|
||
|
||
group.cmd = (data[data_count++])?(true):(false);
|
||
|
||
group.status = data[data_count++];
|
||
|
||
|
||
|
||
emit gear_Info(group);
|
||
}
|
||
|
||
if(raw.size() >= 127)//115~126 ACT
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(110);
|
||
_actuator group;
|
||
int data_count = 0;
|
||
|
||
//qDebug() << "index 0:" << data.size() << data;
|
||
|
||
group.type = data[data_count++];
|
||
|
||
group.id = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.wheel = src.i[0] * 60.0 / 0x7FF;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_trans = src.i[0] * 55.0 / 0x7FF;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_trans = src.i[0] * 55.0 / 0x7FF;
|
||
|
||
group.status = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.current = src.i[0] * 0.1;
|
||
|
||
group.temp = data[data_count++];
|
||
|
||
emit actuator_info(group);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 141)//127~140 ACT1
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(122);
|
||
_actuator1 group;
|
||
int data_count = 0;
|
||
|
||
group.type = data[data_count++];
|
||
|
||
group.id = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_ail = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_ail = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_rud = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_rud = src.i[0];
|
||
|
||
group.status = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.current = src.i[0];
|
||
|
||
group.temp = data[data_count++];
|
||
|
||
emit actuator1_info(group);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 155)//141~154 ACT2
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(136);
|
||
_actuator2 group;
|
||
int data_count = 0;
|
||
|
||
group.type = data[data_count++];
|
||
|
||
group.id = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_ele = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_ele = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_brake = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_brake = src.i[0];
|
||
|
||
group.status = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.current = src.i[0];
|
||
|
||
group.temp = data[data_count++];
|
||
|
||
emit actuator2_info(group);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 213)//155~212 IMU
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
|
||
union//低在前，高位在后
|
||
{
|
||
quint16 byte;
|
||
struct
|
||
{
|
||
bool com_ok:1;//0
|
||
bool status_bit:1;
|
||
bool accel_x_bit:1;
|
||
bool accel_y_bit:1;
|
||
bool accel_z_bit:1;
|
||
bool gyro_x_bit:1;
|
||
bool gyro_y_bit:1;
|
||
bool gyro_z_bit:1;
|
||
bool accel_in_range:1;
|
||
bool gyro_in_range:1;//9
|
||
};
|
||
}status;
|
||
|
||
QByteArray data = raw.mid(150);
|
||
_imu group;
|
||
int data_count = 0;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.time_stamp = src.H;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
status.byte = src.I[0];
|
||
|
||
group.status.com_ok = status.com_ok;//0
|
||
group.status.status_bit = status.status_bit;
|
||
group.status.accel_x_bit = status.accel_x_bit;
|
||
group.status.accel_y_bit = status.accel_y_bit;
|
||
group.status.accel_z_bit = status.accel_z_bit;
|
||
group.status.gyro_x_bit = status.gyro_x_bit;
|
||
group.status.gyro_y_bit = status.gyro_y_bit;
|
||
group.status.gyro_z_bit = status.gyro_z_bit;
|
||
group.status.accel_in_range = status.accel_in_range;
|
||
group.status.gyro_in_range = status.gyro_in_range;//9
|
||
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.accel_x = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.accel_y = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.accel_z = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.gyro_x = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.gyro_y = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.gyro_z = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.temp = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.delta_vel_x = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.delta_vel_y = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.delta_vel_z = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.delta_angle_x = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.delta_angle_y = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.delta_angle_z = src.F * 57.29;
|
||
|
||
|
||
|
||
emit imu_info(group);
|
||
}
|
||
|
||
if(raw.size() >= 245)//213~244 EULER
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
union//低在前，高位在后
|
||
{
|
||
quint32 byte;
|
||
struct
|
||
{
|
||
quint8 solutionMode:4;//0-3
|
||
bool attitude_valid:1;//4
|
||
bool heading_valid:1;//5
|
||
bool velocity_valid:1;//6
|
||
bool position_valid:1;//7
|
||
bool vert_ref_used:1;//8
|
||
bool mag_ref_used:1;//9
|
||
bool gps1_vel_used:1;//10
|
||
bool gps1_pos_used:1;//11
|
||
bool none1:1;
|
||
bool gps1_hdt_used:1;//13
|
||
bool gps2_vel_used:1;//14
|
||
bool gps2_pos_used:1;//15
|
||
bool none2:1;
|
||
bool gps2_hdt_used:1;//17
|
||
bool odo_used:1;//18
|
||
bool dvl_bt_used:1;//19
|
||
bool dvl_wt_used:1;//20
|
||
quint8 none3:3;
|
||
bool usel_used:1;//24
|
||
bool air_data_used:1;//25
|
||
bool zupt_used:1;//26
|
||
bool align_valid:1;//27
|
||
bool depth_used:1;//28
|
||
};
|
||
}status;
|
||
|
||
QByteArray data = raw.mid(208);
|
||
_euler group;
|
||
int data_count = 0;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.time_stamp = src.H;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.roll = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.pitch = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.yaw = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.roll_acc = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.pitch_acc = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.yaw_acc = src.F * 57.29;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
status.byte = src.H;
|
||
|
||
|
||
switch (status.solutionMode& 0x07) {
|
||
case 0:
|
||
group.solution.solutionMode = tr("UNINITIALIZED");
|
||
break;
|
||
case 1:
|
||
group.solution.solutionMode = tr("VERTICAL_GYRO");
|
||
break;
|
||
case 2:
|
||
group.solution.solutionMode = tr("AHRS");
|
||
break;
|
||
case 3:
|
||
group.solution.solutionMode = tr("NAV_VELOCITY");
|
||
break;
|
||
case 4:
|
||
group.solution.solutionMode = tr("NAV_POSTION");
|
||
break;
|
||
}
|
||
|
||
group.solution.attitude_valid = status.attitude_valid;
|
||
group.solution.heading_valid = status.heading_valid;
|
||
group.solution.velocity_valid = status.velocity_valid;
|
||
group.solution.position_valid = status.position_valid;
|
||
group.solution.vert_ref_used = status.vert_ref_used;
|
||
group.solution.mag_ref_used = status.mag_ref_used;
|
||
group.solution.gps1_vel_used = status.gps1_vel_used;
|
||
group.solution.gps1_pos_used = status.gps1_pos_used;
|
||
group.solution.gps1_hdt_used = status.gps1_hdt_used;
|
||
group.solution.gps2_vel_used = status.gps2_vel_used;
|
||
group.solution.gps2_pos_used = status.gps2_pos_used;
|
||
group.solution.gps2_hdt_used = status.gps2_hdt_used;
|
||
group.solution.odo_used = status.odo_used;
|
||
group.solution.dvl_bt_used = status.dvl_bt_used;
|
||
group.solution.dvl_wt_used = status.dvl_wt_used;
|
||
group.solution.usel_used = status.usel_used;
|
||
group.solution.air_data_used = status.air_data_used;
|
||
group.solution.zupt_used = status.zupt_used;
|
||
group.solution.align_valid = status.align_valid;
|
||
group.solution.depth_used = status.depth_used;
|
||
|
||
|
||
emit euler_info(group);
|
||
}
|
||
|
||
if(raw.size() >= 253)//245~252 zero
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(240);
|
||
_euler group;
|
||
int data_count = 0;
|
||
|
||
}
|
||
|
||
}break;
|
||
case 0x01:
|
||
{
|
||
if(raw.size() >= 74)//0~73 EADC
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(0);
|
||
_eadc eadc;
|
||
int data_count = 0;
|
||
|
||
|
||
|
||
|
||
//加温模式
|
||
switch ((uint8_t)data[data_count++]) {
|
||
case 0x33:
|
||
eadc.heatmode = tr("强制加温");
|
||
break;
|
||
case 0x66:
|
||
eadc.heatmode = tr("强制不加温");
|
||
break;
|
||
case 0x99:
|
||
eadc.heatmode = tr("自动加温控制");
|
||
break;
|
||
}
|
||
|
||
//加温状态
|
||
//qDebug() << QString::number(data[data_count],16).toUpper();
|
||
|
||
switch ((uint8_t)data[data_count++]) {
|
||
case 0x55:
|
||
eadc.heatstatus = tr("正在加温");
|
||
break;
|
||
case 0xAA:
|
||
eadc.heatstatus = tr("停止加温");
|
||
break;
|
||
}
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
eadc.psi = src.H * 1000.0/1024;//指示静压
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
eadc.ps = src.H * 1000.0/1024;//静压
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
eadc.qci = src.h * 1000.0/1024;//指示动压
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
eadc.qc = src.h * 1000.0/1024;//真实动压
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
eadc.hp = src.h * 1.0/16;//气压高度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.hpr = src.i[0] * 1.0/16;//升降速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.ts = src.i[0] * 1.0/16;//静温
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.tt = src.i[0] * 1.0/16;//总温
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.mi = src.I[0] * 1.0/1024;//马赫数
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.vi = src.I[0] * 1.0/57.6;//指示空速
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.vt = src.I[0] * 1.0/57.6;//真空速
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.adr = src.I[0] * 1.0/256;//大气密度比
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.aoai1 = src.i[0] * 1.0/128;//局部攻角1
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.aoai2 = src.i[0] * 1.0/128;//局部攻角2
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.aoat1 = src.i[0] * 1.0/128;//真攻角1
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.aoat2 = src.i[0] * 1.0/128;//真攻角2
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.aosi1 = src.i[0] * 1.0/128;//局部侧滑角1
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.aosi2 = src.i[0] * 1.0/128;//局部侧滑角2
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.aost1 = src.i[0] * 1.0/128;//真侧滑角1
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.aost2 = src.i[0] * 1.0/128;//真侧滑角2
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
quint16 faultword = src.I[0];//故障字
|
||
|
||
eadc.faultword.ps = (faultword & 0x0001)?(true):(false);//静压传感器故障
|
||
eadc.faultword.qc = (faultword & 0x0002)?(true):(false);//动压传感器故障
|
||
eadc.faultword.ts = (faultword & 0x0004)?(true):(false);//静温传感器故障
|
||
eadc.faultword.aoa1 = (faultword & 0x0008)?(true):(false);//攻角1故障
|
||
eadc.faultword.aoa2 = (faultword & 0x0010)?(true):(false);//攻角2故障
|
||
eadc.faultword.aos1 = (faultword & 0x0020)?(true):(false);//侧滑角1故障
|
||
eadc.faultword.aos2 = (faultword & 0x0040)?(true):(false);//侧滑角2故障
|
||
eadc.faultword.heat = (faultword & 0x0080)?(true):(false);//加热故障
|
||
eadc.faultword.ex_storage = (faultword & 0x0100)?(true):(false);//外部存储故障
|
||
eadc.faultword.aoa_diff = (faultword & 0x0200)?(true):(false);//双余度攻角不跟随故障
|
||
eadc.faultword.aos_diff = (faultword & 0x0400)?(true):(false);//双余度侧滑角不跟随故障
|
||
eadc.faultword.qc_zero = (faultword & 0x0800)?(true):(false);//动压零位过度修正
|
||
eadc.faultword.back1 = (faultword & 0x1000)?(true):(false);
|
||
eadc.faultword.back2 = (faultword & 0x2000)?(true):(false);
|
||
eadc.faultword.overtemp = (faultword & 0x4000)?(true):(false);//管头过热故障
|
||
eadc.faultword.system = (faultword & 0x8000)?(true):(false);//大气数据计算机故障
|
||
|
||
|
||
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
quint32 datavalid = src.H;//数据有效性
|
||
|
||
eadc.datavalid.psi = (datavalid & 0x00000001)?(true):(false);//指示静压
|
||
eadc.datavalid.ps = (datavalid & 0x00000002)?(true):(false);//真实静压
|
||
eadc.datavalid.qci = (datavalid & 0x00000004)?(true):(false);//指示动压
|
||
eadc.datavalid.qc = (datavalid & 0x00000008)?(true):(false);//真实动压
|
||
eadc.datavalid.hp = (datavalid & 0x00000010)?(true):(false);//气压高度
|
||
eadc.datavalid.hpr = (datavalid & 0x00000020)?(true):(false);//升降速度
|
||
eadc.datavalid.ts = (datavalid & 0x00000040)?(true):(false);//静温
|
||
eadc.datavalid.tt = (datavalid & 0x00000080)?(true):(false);//总温
|
||
eadc.datavalid.mi = (datavalid & 0x00000100)?(true):(false);//马赫数
|
||
eadc.datavalid.vi = (datavalid & 0x00000200)?(true):(false);//指示空速
|
||
eadc.datavalid.vt = (datavalid & 0x00000400)?(true):(false);//真空速
|
||
eadc.datavalid.adr = (datavalid & 0x00000800)?(true):(false);//大气密度比
|
||
eadc.datavalid.aoai1 = (datavalid & 0x00001000)?(true):(false);//局部攻角1
|
||
eadc.datavalid.aoai2 = (datavalid & 0x00002000)?(true):(false);//局部攻角2
|
||
eadc.datavalid.aoat1 = (datavalid & 0x00004000)?(true):(false);//真攻角1
|
||
eadc.datavalid.aoat2 = (datavalid & 0x00008000)?(true):(false);//真攻角2
|
||
eadc.datavalid.aosi1 = (datavalid & 0x00010000)?(true):(false);//局部侧滑角1
|
||
eadc.datavalid.aosi2 = (datavalid & 0x00020000)?(true):(false);//局部侧滑角2
|
||
eadc.datavalid.aost1 = (datavalid & 0x00040000)?(true):(false);//真侧滑角1
|
||
eadc.datavalid.aost2 = (datavalid & 0x00080000)?(true):(false);//真侧滑角2
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.coffpress_k0 = src.i[0] * 1.0/256;//压力校准系数K0
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.coffpress_k1 = src.i[0] * 1.0/256;//压力校准系数K1
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.coffpress_k2 = src.i[0] * 1.0/256;//压力校准系数K2
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.coffpress_k3 = src.i[0] * 1.0/256;//压力校准系数K3
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.coffangle_k0 = src.i[0] * 1.0/1024;//攻角侧滑角校准系数K0
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.coffangle_k1 = src.i[0] * 1.0/1024;//攻角侧滑角校准系数K1
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.coffangle_k2 = src.i[0] * 1.0/1024;//攻角侧滑角校准系数K2
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.coffangle_k3 = src.i[0] * 1.0/1024;//攻角侧滑角校准系数K3
|
||
|
||
emit EADC_Info(eadc);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 83)//74~82 ECU
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(74);
|
||
_ecu group;
|
||
int data_count = 0;
|
||
|
||
group.rpm_c = data[0] * (29500.0*0.39) / 255 + 17995;
|
||
|
||
src.B[0] = data[1];
|
||
src.B[1] = data[2];
|
||
group.rpm = src.I[0] * 30000.0 / 65535;
|
||
|
||
src.B[0] = data[3];
|
||
src.B[1] = data[4];
|
||
group.rpm_t = src.I[0] * 30000.0 / 65535;
|
||
|
||
group.t1 = ((quint8)data[5] - 0x84) * (85.0 + 55.0)/85.0 - 55;
|
||
group.p2 = data[6] * 0.6 / 255;
|
||
group.servo_current = data[7] * 150.0 / 255;
|
||
|
||
//qDebug() << QString::number((quint8)data[5],16) << group.t1;
|
||
|
||
switch ((uint8_t)data[8]) {
|
||
case 0x00:
|
||
group.states = tr("转速大于600r/min,未收到转速指令");
|
||
break;
|
||
case 0xA0:
|
||
group.states = tr("转速大于600r/min,转速未到达设定值");
|
||
break;
|
||
case 0xA1:
|
||
group.states = tr("收到转速指令，转速未到达设定值");
|
||
break;
|
||
case 0xA4:
|
||
group.states = tr("转速大于600r/min,并且判断出发动机起动正常");
|
||
break;
|
||
case 0xA5:
|
||
group.states = tr("收到起动指令并且判断出发动机起动正常");
|
||
break;
|
||
case 0xAE:
|
||
group.states = tr("未收到启动指令且判断出发动机启动异常");
|
||
break;
|
||
case 0xAF:
|
||
group.states = tr("收到起动指令并且判断出发动机起动异常");
|
||
break;
|
||
case 0xA6:
|
||
group.states = tr("飞行阶段受最小物理转速22650rpm,限制未达到指令合折转速");
|
||
break;
|
||
case 0xA7:
|
||
group.states = tr("飞行阶段受最小物理转速29000rpm,限制未达到指令合折转速");
|
||
break;
|
||
case 0xC5:
|
||
group.states = tr("收到降落A指令，判断出发动机降落A阶段正常");
|
||
break;
|
||
case 0xC6:
|
||
group.states = tr("在降落A阶段，受最小物理转速18000rpm限制未达到指令合折转速");
|
||
break;
|
||
case 0xD5:
|
||
group.states = tr("收到降落B指令，判断出发动机降落B阶段正常");
|
||
break;
|
||
case 0xD6:
|
||
group.states = tr("在降落B阶段，受最小物理转速20650rpm限制未达到指令合折转速");
|
||
break;
|
||
case 0xD7:
|
||
group.states = tr("在降落B阶段，受最大物理转速29000rpm限制未达到指令合折转速");
|
||
break;
|
||
case 0xE1:
|
||
group.states = tr("在飞行和降落阶段发动机物理转速小于14000rpm，判定为发动机异常停车");
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
/*
|
||
if(ecu.p2 < 0.15)
|
||
{
|
||
ecu.p2_high = tr("滑油压力过低");
|
||
}
|
||
else if(ecu.p2 > 0.28)
|
||
{
|
||
ecu.p2_high = tr("滑油压力过高");
|
||
}
|
||
else{
|
||
ecu.p2_high = tr("滑油压力正常");
|
||
}
|
||
*/
|
||
|
||
emit ECU_Info(group);
|
||
}
|
||
|
||
if(raw.size() >= 101)//83~100 SSPC
|
||
{
|
||
_sspc sspc;
|
||
QByteArray data = raw.mid(83);
|
||
|
||
|
||
sspc.bus_voltage = data[0];
|
||
sspc.battery_voltage = 10 + data[1] * 0.1;
|
||
sspc.battery_current = data[2];
|
||
sspc.main_voltage = 10 + data[3] * 0.1;
|
||
sspc.main_current = data[4];
|
||
sspc.current_ch1 = data[5] * 0.2;
|
||
sspc.current_ch2 = data[6] * 0.2;
|
||
sspc.current_ch3 = data[7] * 0.1;
|
||
sspc.current_ch4 = data[8] * 0.1;
|
||
sspc.current_ch5 = data[9] * 0.1;
|
||
sspc.current_ch6 = data[10] * 0.1;
|
||
sspc.current_ch7 = data[11] * 0.1;
|
||
sspc.current_ch8 = data[12] * 0.1;
|
||
sspc.current_ch9 = data[13] * 0.1;
|
||
sspc.current_ch10 = data[14] * 0.1;
|
||
sspc.current_ch11 = data[15] * 0.1;
|
||
sspc.current_ch12 = data[16] * 0.1;
|
||
sspc.current_ch13 = data[17] * 0.1;
|
||
|
||
emit SSPC_Info(sspc);
|
||
}
|
||
|
||
if(raw.size() >= 113)//101~112 ACT
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(101);
|
||
//QByteArray data = raw.mid(110);
|
||
_actuator group;
|
||
int data_count = 0;
|
||
|
||
//qDebug() << "index 1:" << data.size() << data;
|
||
|
||
group.type = data[data_count++];
|
||
|
||
group.id = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.wheel = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_trans = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_trans = src.i[0];
|
||
|
||
group.status = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.current = src.i[0];
|
||
|
||
group.temp = data[data_count++];
|
||
|
||
emit actuator_info(group);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 127)//113~126 ACT1
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(113);
|
||
_actuator1 group;
|
||
int data_count = 0;
|
||
|
||
group.type = data[data_count++];
|
||
|
||
group.id = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_ail = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_ail = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_rud = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_rud = src.i[0];
|
||
|
||
group.status = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.current = src.i[0];
|
||
|
||
group.temp = data[data_count++];
|
||
|
||
emit actuator1_info(group);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 141)//127~140 ACT2
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(127);
|
||
_actuator2 group;
|
||
int data_count = 0;
|
||
|
||
group.type = data[data_count++];
|
||
|
||
group.id = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_ele = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_ele = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.left_brake = src.i[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.right_brake = src.i[0];
|
||
|
||
group.status = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.current = src.i[0];
|
||
|
||
group.temp = data[data_count++];
|
||
|
||
emit actuator2_info(group);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 185)//141~184 GPS_VEL
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
union//低在前，高位在后
|
||
{
|
||
quint32 byte;
|
||
struct
|
||
{
|
||
quint8 stats:6;
|
||
quint8 type:6;
|
||
quint32 none1:20;
|
||
};
|
||
}status;
|
||
|
||
QByteArray data = raw.mid(141);
|
||
_vel group;
|
||
int data_count = 0;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.time_stamp = src.H;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
status.byte = src.H;
|
||
|
||
switch (status.stats& 0x03) {
|
||
case 0:
|
||
group.status.vel_status = tr("SOL_COMPUTED");
|
||
break;
|
||
case 1:
|
||
group.status.vel_status = tr("INSUFFICIENT_OBS");
|
||
break;
|
||
case 2:
|
||
group.status.vel_status = tr("INTERNAL_ERROR");
|
||
break;
|
||
case 3:
|
||
group.status.vel_status = tr("LIMIT");
|
||
break;
|
||
default:
|
||
group.status.vel_status = tr("undefined status");
|
||
}
|
||
|
||
group.status.status_value = status.stats;
|
||
|
||
switch (status.type& 0x03) {
|
||
case 0:
|
||
group.status.vel_type = tr("NO_SOLUTION");
|
||
break;
|
||
case 1:
|
||
group.status.vel_type = tr("UNKNOWN_TYPE");
|
||
break;
|
||
case 2:
|
||
group.status.vel_type = tr("DOPPLER");
|
||
break;
|
||
case 3:
|
||
group.status.vel_type = tr("DIFFERENTIAL");
|
||
break;
|
||
default:
|
||
group.status.vel_type = tr("undefined status");
|
||
}
|
||
|
||
group.status.type_value = status.type;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.tow = src.H;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.vel_n = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.vel_e = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.vel_d = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.vel_acc_n = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.vel_acc_e = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.vel_acc_d = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.course = src.F;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.course_acc = src.F;
|
||
|
||
|
||
emit vel_info(group);
|
||
}
|
||
|
||
if(raw.size() >= 242)//185~241 GPS_POS
|
||
{
|
||
union{uint8_t B[8];uint16_t I[4]; int16_t i[4]; uint32_t H[2]; int32_t h[2];float F[2];double D;}src;
|
||
|
||
union//低在前，高位在后
|
||
{
|
||
quint32 byte;
|
||
struct
|
||
{
|
||
quint8 stats:6;
|
||
quint8 type:6;
|
||
quint8 GPS_L1:1;
|
||
quint8 GPS_L2:1;
|
||
quint8 GPS_L5:1;
|
||
quint8 GLO_L1:1;
|
||
quint8 GLO_L2:1;
|
||
quint8 GLO_L3:1;
|
||
quint8 GLA_E1:1;
|
||
quint8 GLA_E5A:1;
|
||
quint8 GLA_E5B:1;
|
||
quint8 GLA_E5ALT:1;
|
||
quint8 GLA_E6:1;
|
||
quint8 BDS_B1:1;
|
||
quint8 BDS_B2:1;
|
||
quint8 BDS_B3:1;
|
||
quint8 NONE:6;
|
||
};
|
||
}status;
|
||
|
||
QByteArray data = raw.mid(185);
|
||
_pos group;
|
||
int data_count = 0;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.time_stamp = src.H[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
status.byte = src.H[0];
|
||
|
||
switch (status.stats& 0x03) {
|
||
case 0:
|
||
group.status.pos_status = tr("SOL_COMPUTED");
|
||
break;
|
||
case 1:
|
||
group.status.pos_status = tr("INSUFFICIENT_OBS");
|
||
break;
|
||
case 2:
|
||
group.status.pos_status = tr("INTERNAL_ERROR");
|
||
break;
|
||
case 3:
|
||
group.status.pos_status = tr("HEIGHT_LIMIT");
|
||
break;
|
||
default:
|
||
group.status.pos_status = tr("undefined status");
|
||
}
|
||
|
||
group.status.status_value = status.stats;
|
||
|
||
switch ((quint8)status.type& 0x0F) {
|
||
case 0:
|
||
group.status.pos_type = tr("NO_SOLUTION");
|
||
break;
|
||
case 1:
|
||
group.status.pos_type = tr("PUNKNOWN_TYPE");
|
||
break;
|
||
case 2:
|
||
group.status.pos_type = tr("SINGLE");
|
||
break;
|
||
case 3:
|
||
group.status.pos_type = tr("PSRDIFF");
|
||
break;
|
||
case 4:
|
||
group.status.pos_type = tr("SBAS");
|
||
break;
|
||
case 5:
|
||
group.status.pos_type = tr("OMNISTAR");
|
||
break;
|
||
case 6:
|
||
group.status.pos_type = tr("RTK_FLOAT");
|
||
break;
|
||
case 7:
|
||
group.status.pos_type = tr("RTK_INT");
|
||
break;
|
||
case 8:
|
||
group.status.pos_type = tr("PPP_FLOAT");
|
||
break;
|
||
case 9:
|
||
group.status.pos_type = tr("PPP_INT");
|
||
break;
|
||
case 10:
|
||
group.status.pos_type = tr("FIXED");
|
||
break;
|
||
default:
|
||
group.status.pos_type = tr("undefined status");
|
||
}
|
||
|
||
|
||
group.status.type_value = status.type;
|
||
|
||
group.status.gps_l1_used = status.GPS_L1;
|
||
group.status.gps_l2_used = status.GPS_L2;
|
||
group.status.gps_l5_used = status.GPS_L5;
|
||
group.status.glo_l1_used = status.GLO_L1;
|
||
group.status.glo_l2_used = status.GLO_L2;
|
||
group.status.glo_l3_used = status.GLO_L3;
|
||
group.status.gal_e1_used = status.GLA_E1;
|
||
group.status.gal_e5a_used = status.GLA_E5A;
|
||
group.status.gal_e5b_used = status.GLA_E5B;
|
||
group.status.gal_e5alt_used = status.GLA_E5ALT;
|
||
group.status.gal_e6_used = status.GLA_E6;
|
||
group.status.bds_b1_used = status.BDS_B1;
|
||
group.status.bds_b2_used = status.BDS_B2;
|
||
group.status.bds_b3_used = status.BDS_B3;
|
||
|
||
|
||
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.tow = src.H[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
src.B[4] = data[data_count++];
|
||
src.B[5] = data[data_count++];
|
||
src.B[6] = data[data_count++];
|
||
src.B[7] = data[data_count++];
|
||
group.lat = src.D;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
src.B[4] = data[data_count++];
|
||
src.B[5] = data[data_count++];
|
||
src.B[6] = data[data_count++];
|
||
src.B[7] = data[data_count++];
|
||
group.lng = src.D;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
src.B[4] = data[data_count++];
|
||
src.B[5] = data[data_count++];
|
||
src.B[6] = data[data_count++];
|
||
src.B[7] = data[data_count++];
|
||
group.alt = src.D;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.undulation = src.F[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.pos_acc_lat = src.F[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.pos_acc_lng = src.F[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
group.pos_acc_alt = src.F[0];
|
||
|
||
group.num_sv_used = data[data_count++];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.base_station_id = src.I[0];
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
group.diff_age = src.I[0];
|
||
|
||
emit pos_info(group);
|
||
}
|
||
|
||
if(raw.size() >= 253)//242~252 zero
|
||
{
|
||
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
|
||
|
||
QByteArray data = raw.mid(242);
|
||
_euler group;
|
||
int data_count = 0;
|
||
|
||
//qDebug() << data.size() << data;
|
||
|
||
}
|
||
|
||
|
||
}break;
|
||
}
|
||
|
||
break;//退出，结束线程
|
||
}
|
||
//qDebug() << "thread finished, time: " << time.elapsed() << "ms";
|
||
}
|
||
|
||
void Parse::parseData(const int id, const QByteArray data)
|
||
{
|
||
index = id;
|
||
//raw.push_back(data);
|
||
raw = data;
|
||
|
||
/*
|
||
QString str;
|
||
|
||
QString str3 = data.toHex().data();//以十六进制显示
|
||
str3 = str3.toUpper ();//转换为大写
|
||
for(int i = 0;i<str3.length ();i+=2)//填加空格
|
||
{
|
||
QString st = str3.mid (i,2);
|
||
str += st;
|
||
str += " ";
|
||
}
|
||
|
||
qDebug() << "id" << id << ":" << str;
|
||
*/
|
||
}
|