2158 lines
74 KiB
C++
2158 lines
74 KiB
C++
#include "Parse.h"
|
||
#include <cmath>
|
||
|
||
|
||
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");
|
||
|
||
qRegisterMetaType<Parse::_100eIMU>("Parse::_100eIMU");
|
||
qRegisterMetaType<Parse::_100eIns>("Parse::_100eIns");
|
||
qRegisterMetaType<Parse::thruster>("Parse::thruster");
|
||
|
||
|
||
|
||
}
|
||
|
||
Parse::~Parse()
|
||
{
|
||
|
||
}
|
||
|
||
|
||
void Parse::run()
|
||
{
|
||
QElapsedTimer time;
|
||
time.start();
|
||
//qDebug() << "parse thread:" << QThread::currentThreadId();
|
||
|
||
//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] * 180.0 /32767;//航向角
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.pitch = src.i[0] * 180.0 /32767;//俯仰角
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
ins.roll = src.i[0] * 180.0 /32767;//滚转角
|
||
|
||
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.az = 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;//
|
||
/*
|
||
qDebug() << ins.roll_rate
|
||
<< ins.yaw_rate
|
||
<< ins.pitch_rate
|
||
<< ins.yaw
|
||
<< ins.pitch
|
||
<< ins.roll
|
||
<< ins.ax
|
||
<< ins.ay
|
||
<< ins.az;
|
||
*/
|
||
|
||
saveInsDataToFile(ins); //把数据保存到文件
|
||
emit INS_Info(ins);
|
||
}
|
||
|
||
if(raw.size() >= 79)//18~78 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 * 180.0 / 0x7FFFFFFF;//位置经度
|
||
|
||
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 * 180.0 / 0x7FFFFFFF;//位置纬度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.altitude = src.i[0] * 12000.0 / 0x7FFF;//组合高度
|
||
|
||
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;//东向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Vn = src.i[0] * 400.0 / 0x7FFF;//北向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Vu = src.i[0] * 400.0 / 0x7FFF;//天向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Gs = src.i[0] * 400.0 / 0x7FFF;//地速
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.heading = src.i[0] * 180.0 / 0x7FFF;//航迹角
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Ae = src.i[0] * 100.0 / 0x7FFF;//东向加速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.An = src.i[0] * 100.0 / 0x7FFF;//北向加速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.Au = src.i[0] * 100.0 / 0x7FFF;//天向加速度
|
||
|
||
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 * 180.0 / 0x7FFFFFFF;//卫星经度
|
||
|
||
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 * 180.0 / 0x7FFFFFFF;//卫星纬度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.satellite_altitude = src.i[0] * 12000.0 / 0x7FFF;//卫星高度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.satellite_Ve = src.i[0] * 400.0 / 0x7FFF;//卫星东向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.satellite_Vn = src.i[0] * 400.0 / 0x7FFF;//卫星北向速度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
nav.satellite_Vu = src.i[0] * 400.0 / 0x7FFF;//卫星天向速度
|
||
|
||
|
||
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++] + 8;//UTC时
|
||
if(nav.hour >= 24)
|
||
{
|
||
nav.hour -= 24;
|
||
}
|
||
|
||
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;//UTC秒
|
||
|
||
nav.starsRecNum = data[data_count++];
|
||
union
|
||
{
|
||
quint16 uint16;
|
||
quint8 uint8[2];
|
||
}u16;
|
||
|
||
u16.uint8[0] = data[data_count++];
|
||
u16.uint8[1] = data[data_count++];
|
||
nav.PDOP = u16.uint16;
|
||
|
||
u16.uint8[0] = data[data_count++];
|
||
u16.uint8[1] = data[data_count++];
|
||
nav.HDOP = u16.uint16;
|
||
|
||
switch(data[data_count++])
|
||
{
|
||
case 0x00:
|
||
nav.locateMode = tr("未定位");
|
||
break;
|
||
case 0x11:
|
||
nav.locateMode = tr("单点定位");
|
||
break;
|
||
case 0x19:
|
||
nav.locateMode = tr("单点定位");
|
||
break;
|
||
default:
|
||
nav.locateMode = tr("未知");
|
||
break;
|
||
}
|
||
|
||
//qDebug() << "state" <<state1.byte << state2.byte << state3.byte;
|
||
|
||
saveNavDataToFile(nav);;
|
||
emit NAV_Info(nav);
|
||
}
|
||
|
||
if(raw.size() >= 121)//79~120 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(79);
|
||
_gear group;
|
||
int data_count = 0;
|
||
|
||
int swidx = 0;
|
||
group.DI[0] = (uint8_t)data[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.DI[1] = (uint8_t)data[1];
|
||
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.DI[2] = (uint8_t)data[2];
|
||
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 / 5.0 * 2.5;
|
||
|
||
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 / 5.0 * 16;
|
||
|
||
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 / 5.0 * 2.5;
|
||
|
||
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 * 0.0106 - 0.0228;
|
||
|
||
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++];
|
||
|
||
|
||
saveGearDataToFile(group); //把数据保存到文件中
|
||
emit gear_Info(group);
|
||
}
|
||
|
||
if(raw.size() >= 135)//121~134 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(121);
|
||
_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];
|
||
|
||
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] * 0.1;
|
||
|
||
group.temp = data[data_count++];
|
||
|
||
group.left_brake = data[data_count++];
|
||
group.right_brake = data[data_count++];
|
||
|
||
saveActuatorDataToFile(group); //把数据保存到文件
|
||
emit actuator_info(group);
|
||
|
||
/*
|
||
QString str;
|
||
for (int var = 0; var < 14; ++var) {
|
||
str.append(QString::number((quint8)data[var],16)); str.append(' ');
|
||
}
|
||
|
||
qDebug() << "0" << str;
|
||
*/
|
||
|
||
}
|
||
|
||
if(raw.size() >= 149)//135~148 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(135);
|
||
_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++];
|
||
|
||
saveActuator1DataToFile(group); //把数据保存到文件
|
||
emit actuator1_info(group);
|
||
|
||
/*
|
||
QString str;
|
||
for (int var = 0; var < 14; ++var) {
|
||
str.append(QString::number((quint8)data[var],16)); str.append(' ');
|
||
}
|
||
|
||
qDebug() << "1" << str;
|
||
*/
|
||
|
||
}
|
||
|
||
if(raw.size() >= 163)//149~162 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(149);
|
||
_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++];
|
||
|
||
saveActuator2DataToFile(group); //把数据保存到文件
|
||
emit actuator2_info(group);
|
||
|
||
/*
|
||
QString str;
|
||
for (int var = 0; var < 14; ++var) {
|
||
str.append(QString::number((quint8)data[var],16)); str.append(' ');
|
||
}
|
||
|
||
qDebug() << "2" << str;
|
||
*/
|
||
|
||
}
|
||
|
||
if(raw.size() >= 209)//163~208 100e_IMU
|
||
{
|
||
QByteArray data = raw.mid(163);
|
||
int index = 0;
|
||
_100eIMU imu;
|
||
|
||
memcpy(&imu.GPSWeek, data.data() + index, 2);
|
||
index += 2;
|
||
|
||
//qDebug() << "imu.GPSWeek" << QString::number(imu.GPSWeek, 16);
|
||
|
||
memcpy(&imu.tow_ms, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&imu.GPSWeek1, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&imu.tow, data.data() + index, 8);
|
||
index += 8;
|
||
|
||
union
|
||
{
|
||
quint32 state;
|
||
struct
|
||
{
|
||
quint32 b0:1;
|
||
quint32 b1:1;
|
||
quint32 b2:1;
|
||
quint32 b3:1;
|
||
quint32 b4:1;
|
||
quint32 b5:1;
|
||
quint32 b6:1;
|
||
}status;
|
||
}imuState;
|
||
|
||
|
||
memcpy(&imuState.state, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
qDebug() << "imuState" << QString::number(imuState.state, 16);
|
||
|
||
|
||
imu.IMUState.XGyroState1 = imuState.status.b0;
|
||
imu.IMUState.YGyroState = imuState.status.b1;
|
||
imu.IMUState.ZGyroState = imuState.status.b2;
|
||
imu.IMUState.XAccelerometerState1 = imuState.status.b4;
|
||
imu.IMUState.YAccelerometerState = imuState.status.b5;
|
||
imu.IMUState.ZAccelerometerState = imuState.status.b6;
|
||
|
||
qint32 t;
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
imu.az = t * 0.00001826 * -1;
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
imu.ax = t * 0.00001826 * -1;
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
imu.ay = t * 0.00001826;
|
||
|
||
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
imu.r = t * 0.00006706 * -1;
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
imu.p = t * 0.00006706 * -1;
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
imu.q = t * 0.00006706;
|
||
|
||
|
||
emit _100eIMUInfo(imu);
|
||
}
|
||
|
||
if(raw.size() >= 227)//209~226 SSPC
|
||
{
|
||
_sspc sspc;
|
||
QByteArray data = raw.mid(209);
|
||
|
||
/*
|
||
qDebug() << "sspc"
|
||
<< QString::number(data[0],16) << QString::number(data[1],16)
|
||
<< QString::number(data[2],16) << QString::number(data[3],16);
|
||
*/
|
||
|
||
sspc.bus_voltage = (10 + (quint8)data[0] * 0.1);
|
||
sspc.battery_voltage = (10 + (quint8)data[1] * 0.1);
|
||
sspc.battery_current = (quint8)data[2];
|
||
sspc.main_voltage = (10 + (quint8)data[3] * 0.1);
|
||
sspc.main_current = (quint8)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;
|
||
|
||
saveSspcDataToFile(sspc); //把数据保存到文件中
|
||
emit SSPC_Info(sspc);
|
||
}
|
||
/*
|
||
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(245);
|
||
_actuator group;
|
||
int data_count = 0;
|
||
|
||
group.left_brake = data[data_count++];
|
||
group.right_brake = data[data_count++];
|
||
|
||
//变体舵机电流、方向
|
||
Variant var;
|
||
|
||
union
|
||
{
|
||
uint8_t c[2];
|
||
|
||
int t;
|
||
} test;
|
||
|
||
test.t = 0;
|
||
data = raw.mid(247);
|
||
test.c[0] = data[0];
|
||
test.c[1] = data[1];
|
||
|
||
var.l_var_current = test.t * 0.1;
|
||
|
||
test.t = 0;
|
||
test.c[0] = data[2];
|
||
test.c[1] = data[3];
|
||
|
||
var.r_var_current = test.t * 0.1;
|
||
|
||
var.l_var_dir = data[4];
|
||
var.r_var_dir = data[5];
|
||
|
||
qDebug() << "variant: " << data;
|
||
|
||
saveVariantToFile(var);
|
||
saveActuatorDataToFile(group);
|
||
emit actuator_info_brake(group);
|
||
emit variantInfo(var);
|
||
|
||
}
|
||
*/
|
||
}break;
|
||
case 0x01:
|
||
{
|
||
if(raw.size() >= 34)//0~33 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;
|
||
|
||
data_count++;
|
||
data_count++;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.hp = src.i[0] * 0.5; //气压高度
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.ps = src.I[0] * 2; //静压
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.mi = src.I[0] * 0.00004; //马赫数
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.vi = src.I[0] * 0.1; //指示空速
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.tp = src.I[0] * 3; //总压
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.attA = src.i[0] * 0.001; //迎角
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.sideA = src.i[0] * 0.001; //侧滑角
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.tt = src.i[0] * 0.01 - 273.15; //大气总温
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.attAAcq = src.i[0] * 0.001; //迎角采集值
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
eadc.sideAAcq = src.i[0] * 0.001; //侧滑角采集值
|
||
|
||
|
||
quint8 byte = data[data_count++];
|
||
eadc.datavalid.hp = byte & 0x01; //气压高度有效性
|
||
eadc.datavalid.ps = (byte >> 1) & 0x01; //静压有效性
|
||
eadc.datavalid.mi = (byte >> 2) & 0x01; //马赫数有效性
|
||
eadc.datavalid.vi = (byte >> 3) & 0x01; //指示空速有效性
|
||
eadc.datavalid.tp = (byte >> 4) & 0x01; //总压有效性
|
||
eadc.datavalid.attA = (byte >> 5) & 0x01; //迎角有效性
|
||
eadc.datavalid.sideA = (byte >> 6) & 0x01; //侧滑角有效性
|
||
eadc.datavalid.tt = (byte >> 7) & 0x01; //大气总温有效性
|
||
|
||
byte = data[data_count++];
|
||
eadc.faultword.productFault = byte & 0x01; //产品故障
|
||
eadc.faultword.memoryFault = (byte >> 1) & 0x01; //存储器故障
|
||
eadc.faultword.vmcFault = (byte >> 2) & 0x01; //VMC通讯故障
|
||
eadc.faultword.cpuFault = (byte >> 3) & 0x01; //cpu解算故障
|
||
eadc.faultword.tpFault = (byte >> 4) & 0x01; //总压传感器故障
|
||
eadc.faultword.psFault = (byte >> 5) & 0x01; //静压传感器故障
|
||
eadc.faultword.batteryFault = (byte >> 6) & 0x01; //电源故障
|
||
|
||
byte = data[data_count++];
|
||
eadc.validAndStatus.probeHeating = byte & 0x01; //探头加温状态
|
||
eadc.validAndStatus.attAAcq = (byte >> 1) & 0x01; //迎角采集值有效性
|
||
eadc.validAndStatus.sideAAcq = (byte >> 2) & 0x01; //迎角采集值有效性
|
||
|
||
data_count++;
|
||
|
||
src.B[0] = data[data_count++];
|
||
src.B[1] = data[data_count++];
|
||
src.B[2] = data[data_count++];
|
||
src.B[3] = data[data_count++];
|
||
// memcpy(&src.B[0], data.data()+data_count, 4);
|
||
// data_count+=4;
|
||
|
||
eadc.dp = 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++];
|
||
eadc.vt = src.F; //真空速
|
||
|
||
saveEadcDataToFile(eadc); //把数据保存到文件中
|
||
emit EADC_Info(eadc);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 43)//34~42 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(34);
|
||
_ecu group;
|
||
int data_count = 0;
|
||
|
||
group.rpm = data[0] * 30000.0 / 255;;
|
||
group.t1 = ((quint8)data[1] - 0x84) * (85.0 + 55.0)/85.0 - 55;
|
||
group.p2 = data[2] * 0.6 / 255;
|
||
group.servo_current = data[3] * 150.0 / 255;
|
||
|
||
//qDebug() << QString::number((quint8)data[5],16) << group.t1;
|
||
|
||
switch ((uint8_t)data[4]) {
|
||
case 0x00:
|
||
group.states = tr("转速大于600r/min");
|
||
break;
|
||
case 0xA5:
|
||
group.states = tr("收到起动指令并且判断出发动机起动正常");
|
||
break;
|
||
case 0xA1:
|
||
group.states = tr("收到起动指令，未判断出发动机起动是否异常");
|
||
break;
|
||
case 0xAF:
|
||
group.states = tr("收到起动指令并且判断出发动机起动异常");
|
||
break;
|
||
case 0xC5:
|
||
group.states = tr("收到转级指令,并判转级正常");
|
||
break;
|
||
case 0xC1:
|
||
group.states = tr("收到转级指令,未判断处转级是否正常");
|
||
break;
|
||
case 0xCF:
|
||
group.states = tr("收到转级指令,并且发动机转级异常");
|
||
break;
|
||
case 0xFF:
|
||
group.states = tr("飞控阶段指令");
|
||
break;
|
||
default:
|
||
group.states = tr("未知");
|
||
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("滑油压力正常");
|
||
}
|
||
*/
|
||
|
||
saveEcuDataToFile(group); //把数据保存到文件
|
||
emit ECU_Info(group);
|
||
}
|
||
|
||
|
||
if(raw.size() >= 162)//43~161 100e 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(43);
|
||
|
||
int index = 0;
|
||
_100eIns ins;
|
||
|
||
float nnn;
|
||
memcpy(&nnn, data.data() + 69, 4);
|
||
qDebug() << "北向加速度" << nnn;
|
||
|
||
memcpy(&ins.tick, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
union
|
||
{
|
||
uint8_t c;
|
||
struct
|
||
{
|
||
uint8_t b012:3;
|
||
uint8_t b3:1;
|
||
uint8_t b4:1;
|
||
uint8_t b5:1;
|
||
uint8_t b6:1;
|
||
uint8_t b7:1;
|
||
}s;
|
||
}state;
|
||
|
||
|
||
memcpy(&state.c, data.data() + index, 1);
|
||
++index;
|
||
ins.state.ahrs = state.s.b012;
|
||
ins.state.compassCalibration = state.s.b3;
|
||
ins.state.compass = state.s.b4;
|
||
ins.state.gyroscope = state.s.b5;
|
||
ins.state.additive = state.s.b6;
|
||
ins.state.barometer = state.s.b7;
|
||
|
||
memcpy(&ins.pitch, data.data() + index, 4);
|
||
index += 4;
|
||
ins.pitch *= 57.3;
|
||
|
||
memcpy(&ins.roll, data.data() + index, 4);
|
||
index += 4;
|
||
ins.roll *= 57.3;
|
||
|
||
memcpy(&ins.yaw, data.data() + index, 4);
|
||
index += 4;
|
||
ins.yaw *= 57.3;
|
||
|
||
memcpy(&ins.gps_yaw, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
|
||
|
||
memcpy(&ins.pitch_rate, data.data() + index, 4);
|
||
index += 4;
|
||
ins.pitch_rate *= 57.3;
|
||
|
||
memcpy(&ins.roll_rate, data.data() + index, 4);
|
||
index += 4;
|
||
ins.roll_rate *= 57.3;
|
||
|
||
memcpy(&ins.yaw_rate, data.data() + index, 4);
|
||
index += 4;
|
||
ins.yaw_rate *= 57.3;
|
||
|
||
qint32 t;
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
ins.lon = t * 0.0000001;
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
ins.lat = t * 0.0000001;
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
ins.alt_baro = t * 0.01;
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
ins.alt_gps = t * 0.01;
|
||
|
||
memcpy(&t, data.data() + index, 4);
|
||
index += 4;
|
||
ins.alt = t * 0.01;
|
||
|
||
memcpy(&ins.velocity_n, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.velocity_e, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.velocity_d, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.velocity_air, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.accel_n, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.accel_e, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.accel_d, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.satellite_num, data.data() + index, 1);
|
||
++index;
|
||
|
||
quint16 t2;
|
||
memcpy(&t2, data.data() + index, 2);
|
||
index += 2;
|
||
ins.hdop = t2 * 0.01;
|
||
|
||
memcpy(&t2, data.data() + index, 2);
|
||
index += 2;
|
||
ins.vdop = t2 * 0.01;
|
||
|
||
quint8 u8;
|
||
memcpy(&u8, data.data() + index, 1);
|
||
++index;
|
||
|
||
switch(u8)
|
||
{
|
||
case 0:
|
||
{
|
||
ins.gps_fixtype = "无GPS数据";
|
||
break;
|
||
}
|
||
|
||
case 1:
|
||
{
|
||
ins.gps_fixtype = "GPS信号失锁";
|
||
break;
|
||
}
|
||
|
||
case 2:
|
||
{
|
||
ins.gps_fixtype = "2D定位";
|
||
break;
|
||
}
|
||
|
||
case 3:
|
||
{
|
||
ins.gps_fixtype = "3D定位";
|
||
break;
|
||
}
|
||
|
||
case 4:
|
||
{
|
||
ins.gps_fixtype = "3D_DGPS";
|
||
break;
|
||
}
|
||
|
||
case 5:
|
||
{
|
||
ins.gps_fixtype = "3D RTK Float";
|
||
break;
|
||
}
|
||
|
||
case 6:
|
||
{
|
||
ins.gps_fixtype = "3D RTK Fixed";
|
||
break;
|
||
}
|
||
|
||
default:
|
||
{
|
||
ins.gps_fixtype = "未知数据";
|
||
break;
|
||
}
|
||
}
|
||
|
||
quint8 hour, minute, sec;
|
||
memcpy(&hour, data.data() + index, 1);
|
||
++index;
|
||
memcpy(&minute, data.data() + index, 1);
|
||
++index;
|
||
memcpy(&sec, data.data() + index, 1);
|
||
++index;
|
||
|
||
QString str("%1:%2.%3");
|
||
ins.time = str.arg(hour).arg(minute).arg(sec);
|
||
|
||
memcpy(&ins.temperature, data.data() + index, 1);
|
||
++index;
|
||
|
||
quint16 u16;
|
||
memcpy(&u16, data.data() + index, 2);
|
||
index += 2;
|
||
ins.gps_hdg = u16 * 0.01 / 57.3;
|
||
|
||
memcpy(&u16, data.data() + index, 2);
|
||
index += 2;
|
||
ins.gps_hdg_dev = u16 * 0.01 / 57.3;
|
||
|
||
union
|
||
{
|
||
uint8_t u8_2;
|
||
struct
|
||
{
|
||
uint8_t b01:2;
|
||
uint8_t b23:2;
|
||
uint8_t b45:2;
|
||
uint8_t b67:2;
|
||
}re;
|
||
}redu;
|
||
|
||
memcpy(&redu.u8_2, data.data() + index, 1);
|
||
++index;
|
||
|
||
switch(redu.re.b01)
|
||
{
|
||
case 0:
|
||
ins.redundancy.additive = "外部";
|
||
break;
|
||
case 1:
|
||
ins.redundancy.additive = "内部1";
|
||
break;
|
||
case 2:
|
||
ins.redundancy.additive = "内部2";
|
||
break;
|
||
}
|
||
|
||
switch(redu.re.b23)
|
||
{
|
||
case 0:
|
||
ins.redundancy.gyroscope = "外部";
|
||
break;
|
||
case 1:
|
||
ins.redundancy.gyroscope = "内部1";
|
||
break;
|
||
case 2:
|
||
ins.redundancy.gyroscope = "内部2";
|
||
break;
|
||
}
|
||
|
||
ins.redundancy.compass = redu.re.b45 ? "内部" : "外部";
|
||
ins.redundancy.gps = redu.re.b67 ? "内部" : "外部";
|
||
|
||
memcpy(&u8, data.data() + index, 1);
|
||
++index;
|
||
ins.gps0_dt = u8 * 100;
|
||
|
||
memcpy(&u8, data.data() + index, 1);
|
||
++index;
|
||
ins.gps1_dt = u8 * 100;
|
||
|
||
memcpy(&ins.gps_vn, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.gps_ve, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.gps_vd, data.data() + index, 4);
|
||
index += 4;
|
||
|
||
memcpy(&ins.gps_msec, data.data() + index, 2);
|
||
index += 2;
|
||
|
||
memcpy(&ins.gps_day, data.data() + index, 1);
|
||
++index;
|
||
|
||
memcpy(&ins.gps_week, data.data() + index, 2);
|
||
index += 2;
|
||
|
||
memcpy(&u8, data.data() + index, 1);
|
||
++index;
|
||
|
||
switch(u8)
|
||
{
|
||
case 0x00:
|
||
ins.work_state = "待机";
|
||
break;
|
||
case 0x10:
|
||
ins.work_state = "粗对准";
|
||
break;
|
||
case 0x20:
|
||
ins.work_state = "精对准";
|
||
break;
|
||
case 0x30:
|
||
ins.work_state = "组合导航";
|
||
break;
|
||
case 0x31:
|
||
ins.work_state = "惯性导航";
|
||
break;
|
||
}
|
||
|
||
emit _100eInsInfo(ins);
|
||
}
|
||
|
||
|
||
|
||
if(raw.size() >= 172)//162~171 sspc_stats sspc_errs
|
||
{
|
||
|
||
|
||
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(162);
|
||
_sspc group;
|
||
int data_count = 0;
|
||
|
||
group.source = data[data_count++];
|
||
|
||
quint8 state1 = data[data_count++];
|
||
group.state1.current_ch1 = (state1 & 0x01)?(true):(false);
|
||
group.state1.current_ch2 = (state1 & 0x02)?(true):(false);
|
||
group.state1.current_ch3 = (state1 & 0x04)?(true):(false);
|
||
group.state1.current_ch4 = (state1 & 0x08)?(true):(false);
|
||
group.state1.current_ch5 = (state1 & 0x10)?(true):(false);
|
||
group.state1.current_ch6 = (state1 & 0x20)?(true):(false);
|
||
group.state1.current_ch7 = (state1 & 0x40)?(true):(false);
|
||
group.state1.current_ch8 = (state1 & 0x80)?(true):(false);
|
||
|
||
quint8 state2 = data[data_count++];
|
||
group.state2.current_ch9 = (state2 & 0x01)?(true):(false);
|
||
group.state2.current_ch10 = (state2 & 0x02)?(true):(false);
|
||
group.state2.current_ch11 = (state2 & 0x04)?(true):(false);
|
||
group.state2.current_ch12 = (state2 & 0x08)?(true):(false);
|
||
group.state2.current_ch13 = (state2 & 0x10)?(true):(false);
|
||
group.state2.current_ch14 = (state2 & 0x20)?(true):(false);
|
||
group.state2.current_ch15 = (state2 & 0x40)?(true):(false);
|
||
group.state2.current_ch16 = (state2 & 0x80)?(true):(false);
|
||
|
||
|
||
//===========================
|
||
quint8 check = data[data_count++];
|
||
group.check.CUP_STA = (check & 0x01)?(true):(false);
|
||
group.check.current = (check & 0x02)?(true):(false);
|
||
group.check.current_10A = (check & 0x04)?(true):(false);
|
||
group.check.current_40A = (check & 0x08)?(true):(false);
|
||
|
||
quint8 err1 = data[data_count++];
|
||
group.err1.ins_sbg = (err1 & 0x01)?(true):(false);
|
||
group.err1.ins_320 = (err1 & 0x02)?(true):(false);
|
||
group.err1.dlink_l = (err1 & 0x04)?(true):(false);
|
||
group.err1.dlink_u = (err1 & 0x08)?(true):(false);
|
||
group.err1.eadc = (err1 & 0x10)?(true):(false);
|
||
group.err1.rec = (err1 & 0x20)?(true):(false);
|
||
group.err1.landinggear = (err1 & 0x40)?(true):(false);
|
||
group.err1.act1 = (err1 & 0x80)?(true):(false);
|
||
|
||
quint8 err2 = data[data_count++];
|
||
group.err2.act2 = (err2 & 0x01)?(true):(false);
|
||
group.err2.act3 = (err2 & 0x02)?(true):(false);
|
||
group.err2.computer = (err2 & 0x04)?(true):(false);
|
||
|
||
quint8 err3 = data[data_count++];
|
||
group.err3.pump = (err3 & 0x03) >> 0;
|
||
group.err3.ail = (err3 & 0x0C) >> 2;
|
||
group.err3.temp_56v = (err3 & 0x30) >> 4;
|
||
group.err3.temp_28v = (err3 & 0xC0) >> 6;
|
||
|
||
|
||
quint8 err4 = data[data_count++];
|
||
group.err4.computer = (err4 & 0x03) >> 0;
|
||
group.err4.ecu = (err4 & 0x0C) >> 2;
|
||
group.err4.eadc = (err4 & 0x30) >> 4;
|
||
group.err4.temp = (err4 & 0xC0) >> 6;
|
||
|
||
quint8 err5 = data[data_count++];
|
||
group.err5.fuel = (err5 & 0x03) >> 0;
|
||
group.err5.temp_56v2 = (err5 & 0x0C) >> 2;
|
||
group.err5.v_act = (err5 & 0x30) >> 4;
|
||
group.err5.current56V = (err5 & 0xC0) >> 6;
|
||
|
||
quint8 err6 = data[data_count++];
|
||
group.err6.oil_press = (err6 & 0x03) >> 0;
|
||
|
||
saveSspcDataToFile(group); //把数据保存到文件中
|
||
emit SSPC_Info_state(group);
|
||
}
|
||
|
||
if(raw.size() >= 186)//172~185 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(172);
|
||
//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++];
|
||
group.left_brake = data[data_count++];
|
||
group.right_brake = data[data_count++];
|
||
|
||
saveActuatorDataToFile(group); //把数据保存到文件中
|
||
emit actuator_info(group);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 200)//186~199 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(186);
|
||
_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++];
|
||
|
||
saveActuator1DataToFile(group); //把数据保存到文件中
|
||
emit actuator1_info(group);
|
||
|
||
}
|
||
|
||
if(raw.size() >= 214)//200~213 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(200);
|
||
_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++];
|
||
|
||
saveActuator2DataToFile(group); //把数据保存到文件
|
||
emit actuator2_info(group);
|
||
|
||
}
|
||
|
||
|
||
if(raw.size() >= 226) //214~225 thruster
|
||
{
|
||
QByteArray data = raw.mid(214);
|
||
thruster thr;
|
||
|
||
thr.ignitionCommand1 = data[4];
|
||
thr.ignitionStatus1 = data[5];
|
||
thr.ignitionCommand2 = data[6];
|
||
thr.ignitionStatus2 = data[7];
|
||
|
||
quint8 t = data[8];
|
||
|
||
switch(t)
|
||
{
|
||
case 0x55:
|
||
thr.relayStatus = "继电器关";
|
||
break;
|
||
case 0xAA:
|
||
thr.relayStatus = "继电器开";
|
||
break;
|
||
default:
|
||
thr.relayStatus = "未知";
|
||
break;
|
||
}
|
||
|
||
emit thrusterInfo(thr);
|
||
}
|
||
|
||
|
||
|
||
|
||
}break;
|
||
}
|
||
|
||
break;//退出，结束线程
|
||
}
|
||
//qDebug() << "thread finished, time: " << time.nsecsElapsed() << "ns";
|
||
}
|
||
|
||
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;
|
||
*/
|
||
}
|
||
|
||
void Parse::saveVariantToFile(const Variant &var)
|
||
{
|
||
QFile file(variant_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
qDebug() << u8"保存variant数据";
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
stream << var.l_var_current << ",";
|
||
stream << var.r_var_current << ",";
|
||
stream << (var.l_var_dir ? "inversion" : "foreward") << ",";
|
||
stream << (var.r_var_dir ? "inversion" : "foreward") << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveInsDataToFile(const _ins &ins)
|
||
{
|
||
QFile file(ins_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
//qDebug() << u8"保存ins数据";
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
stream << ins.roll_rate << ",";
|
||
stream << ins.yaw_rate << ",";
|
||
stream << ins.pitch_rate << ",";
|
||
stream << ins.yaw << ",";
|
||
stream << ins.pitch << ",";
|
||
stream << ins.roll << ",";
|
||
stream << ins.ax << ",";
|
||
stream << ins.ay << ",";
|
||
stream << ins.az << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveNavDataToFile(const _nav &nav)
|
||
{
|
||
QFile file(nav_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
stream << nav.state1.navStatus << ",";
|
||
stream << nav.state1.insStatus << ",";
|
||
stream << nav.state1.headingSrc << ",";
|
||
stream << nav.state1.Satellite << ",";
|
||
stream << nav.longitude << ",";
|
||
stream << nav.latitude << ",";
|
||
stream << nav.altitude << ",";
|
||
stream << nav.worktime << ",";
|
||
stream << nav.Ve << ",";
|
||
stream << nav.Vn << ",";
|
||
stream << nav.Vu << ",";
|
||
stream << nav.Gs << ",";
|
||
stream << nav.heading << ",";
|
||
stream << nav.Ae << ",";
|
||
stream << nav.An << ",";
|
||
stream << nav.Au << ",";
|
||
stream << nav.satellite_longitude << ",";
|
||
stream << nav.satellite_latitude << ",";
|
||
stream << nav.satellite_altitude << ",";
|
||
stream << nav.satellite_Ve << ",";
|
||
stream << nav.satellite_Vn << ",";
|
||
stream << nav.satellite_Vu << ",";
|
||
stream << nav.state2.ins << ",";
|
||
stream << nav.state2.altitude << ",";
|
||
stream << nav.state2.exRTK << ",";
|
||
stream << nav.state2.exSatellite << ",";
|
||
stream << nav.state2.RTK << ",";
|
||
stream << nav.state2.satellite << ",";
|
||
stream << nav.state2.heading << ",";
|
||
stream << nav.state2.communication << ",";
|
||
stream << nav.state3.back7 << ",";
|
||
stream << nav.state3.das << ",";
|
||
stream << nav.state3.install << ",";
|
||
stream << nav.state3.back4 << ",";
|
||
stream << nav.state3.altitudeInfo << ",";
|
||
stream << nav.state3.back1 << ",";
|
||
stream << nav.state3.back0 << ",";
|
||
stream << nav.year << ",";
|
||
stream << nav.month << ",";
|
||
stream << nav.day << ",";
|
||
stream << nav.hour << ",";
|
||
stream << nav.minute << ",";
|
||
stream << nav.second << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveGearDataToFile(const _gear &gear)
|
||
{
|
||
QFile file(gear_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
for(int i = 0; i < 24; ++i)
|
||
{
|
||
stream << gear.SW[i] << ",";
|
||
}
|
||
for(int i = 0; i < 3; ++i)
|
||
{
|
||
stream << gear.DI[i] << ",";
|
||
}
|
||
|
||
stream << gear.LeftWheel << ",";
|
||
stream << gear.RightWheel << ",";
|
||
stream << gear.rpm3 << ",";
|
||
stream << gear.rpm4 << ",";
|
||
stream << gear.fuel << ",";
|
||
stream << gear.LeftPressure << ",";
|
||
stream << gear.RightPressure << ",";
|
||
stream << gear.AirPressure << ",";
|
||
stream << gear.cpu_load << ",";
|
||
for(int i = 0; i < 24; ++i)
|
||
{
|
||
stream << gear.DO[i] << ",";
|
||
}
|
||
|
||
stream << gear.cmd << ",";
|
||
stream << gear.status << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveActuatorDataToFile(const _actuator &actuator)
|
||
{
|
||
QFile file(actuator_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UFT-8");
|
||
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
stream << actuator.type << ",";
|
||
stream << actuator.id << ",";
|
||
stream << actuator.wheel << ",";
|
||
stream << actuator.left_trans << ",";
|
||
stream << actuator.right_trans << ",";
|
||
stream << actuator.status << ",";
|
||
stream << actuator.current << ",";
|
||
stream << actuator.temp << ",";
|
||
stream << actuator.left_brake << ",";
|
||
stream << actuator.right_brake << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveActuator1DataToFile(const _actuator1 &actuator1)
|
||
{
|
||
QFile file(actuator1_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
|
||
stream << actuator1.type << ",";
|
||
stream << actuator1.id << ",";
|
||
stream << actuator1.left_ail << ",";
|
||
stream << actuator1.right_ail << ",";
|
||
stream << actuator1.left_rud << ",";
|
||
stream << actuator1.right_rud << ",";
|
||
stream << actuator1.status << ",";
|
||
stream << actuator1.current << ",";
|
||
stream << actuator1.temp << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveActuator2DataToFile(const _actuator2 &actuator2)
|
||
{
|
||
QFile file(actuator2_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
|
||
stream << actuator2.type << ",";
|
||
stream << actuator2.id << ",";
|
||
stream << actuator2.left_ele << ",";
|
||
stream << actuator2.right_ele << ",";
|
||
stream << actuator2.left_brake << ",";
|
||
stream << actuator2.right_brake << ",";
|
||
stream << actuator2.status << ",";
|
||
stream << actuator2.current << ",";
|
||
stream << actuator2.temp << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveImuDataToFile(const _imu &imu)
|
||
{
|
||
QFile file(imu_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
|
||
stream << imu.time_stamp << ",";
|
||
stream << imu.status.com_ok << ",";
|
||
stream << imu.status.status_bit << ",";
|
||
stream << imu.status.accel_x_bit << ",";
|
||
stream << imu.status.accel_y_bit << ",";
|
||
stream << imu.status.accel_z_bit << ",";
|
||
stream << imu.status.gyro_x_bit << ",";
|
||
stream << imu.status.gyro_y_bit << ",";
|
||
stream << imu.status.gyro_z_bit << ",";
|
||
stream << imu.status.accel_in_range << ",";
|
||
stream << imu.status.gyro_in_range << ",";
|
||
stream << imu.accel_x << ",";
|
||
stream << imu.accel_y << ",";
|
||
stream << imu.accel_z << ",";
|
||
stream << imu.gyro_x << ",";
|
||
stream << imu.gyro_y << ",";
|
||
stream << imu.gyro_z << ",";
|
||
stream << imu.temp << ",";
|
||
stream << imu.delta_vel_x << ",";
|
||
stream << imu.delta_vel_y << ",";
|
||
stream << imu.delta_vel_z << ",";
|
||
stream << imu.delta_angle_x << ",";
|
||
stream << imu.delta_angle_y << ",";
|
||
stream << imu.delta_angle_z << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveEulerDataToFile(const _euler &euler)
|
||
{
|
||
QFile file(euler_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
|
||
stream << euler.time_stamp << ",";
|
||
stream << euler.roll << ",";
|
||
stream << euler.pitch << ",";
|
||
stream << euler.yaw << ",";
|
||
stream << euler.roll_acc << ",";
|
||
stream << euler.pitch_acc << ",";
|
||
stream << euler.yaw_acc << ",";
|
||
|
||
stream << euler.solution.solutionMode << ",";
|
||
stream << euler.solution.attitude_valid << ",";
|
||
stream << euler.solution.heading_valid << ",";
|
||
stream << euler.solution.velocity_valid << ",";
|
||
stream << euler.solution.position_valid << ",";
|
||
stream << euler.solution.vert_ref_used << ",";
|
||
stream << euler.solution.mag_ref_used << ",";
|
||
stream << euler.solution.gps1_vel_used << ",";
|
||
stream << euler.solution.gps1_pos_used << ",";
|
||
stream << euler.solution.gps1_hdt_used << ",";
|
||
stream << euler.solution.gps2_vel_used << ",";
|
||
stream << euler.solution.gps2_pos_used << ",";
|
||
stream << euler.solution.gps2_hdt_used << ",";
|
||
stream << euler.solution.odo_used << ",";
|
||
stream << euler.solution.dvl_bt_used << ",";
|
||
stream << euler.solution.dvl_wt_used << ",";
|
||
stream << euler.solution.usel_used << ",";
|
||
stream << euler.solution.air_data_used << ",";
|
||
stream << euler.solution.zupt_used << ",";
|
||
stream << euler.solution.align_valid << ",";
|
||
stream << euler.solution.depth_used << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveEadcDataToFile(const _eadc &eadc)
|
||
{
|
||
QFile file(eadc_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
|
||
// stream << eadc.heatmode << ",";
|
||
// stream << eadc.heatstatus << ",";
|
||
// stream << eadc.heatmode_t << ",";
|
||
// stream << eadc.heatstatus_t << ",";
|
||
// stream << eadc.psi << ",";
|
||
stream << eadc.ps << ",";
|
||
// stream << eadc.qci << ",";
|
||
// stream << eadc.qc << ",";
|
||
stream << eadc.hp << ",";
|
||
// stream << eadc.hpr << ",";
|
||
// stream << eadc.ts << ",";
|
||
stream << eadc.tt << ",";
|
||
stream << eadc.mi << ",";
|
||
stream << eadc.vi << ",";
|
||
// stream << eadc.vt << ",";
|
||
// stream << eadc.adr << ",";
|
||
// stream << eadc.aoai1 << ",";
|
||
// stream << eadc.aoai2 << ",";
|
||
// stream << eadc.aoat1 << ",";
|
||
// stream << eadc.aoat2 << ",";
|
||
// stream << eadc.aosi1 << ",";
|
||
// stream << eadc.aosi2 << ",";
|
||
// stream << eadc.faultword.ps << ",";
|
||
// stream << eadc.faultword.qc << ",";
|
||
// stream << eadc.faultword.ts << ",";
|
||
// stream << eadc.faultword.aoa1 << ",";
|
||
// stream << eadc.faultword.aoa2 << ",";
|
||
// stream << eadc.faultword.aos1 << ",";
|
||
// stream << eadc.faultword.aos2 << ",";
|
||
// stream << eadc.faultword.heat << ",";
|
||
// stream << eadc.faultword.ex_storage << ",";
|
||
// stream <<eadc.faultword.aoa_diff << ",";
|
||
// stream << eadc.faultword.aos_diff << ",";
|
||
// stream << eadc.faultword.qc_zero << ",";
|
||
// stream << eadc.faultword.back1 << ",";
|
||
// stream << eadc.faultword.back2 << ",";
|
||
// stream << eadc.faultword.overtemp << ",";
|
||
// stream << eadc.faultword.system << ",";
|
||
// stream << eadc.datavalid.psi << ",";
|
||
stream << eadc.datavalid.ps << ",";
|
||
// stream << eadc.datavalid.qci << ",";
|
||
// stream << eadc.datavalid.qc << ",";
|
||
stream << eadc.datavalid.hp << ",";
|
||
// stream << eadc.datavalid.hpr << ",";
|
||
// stream << eadc.datavalid.ts << ",";
|
||
stream << eadc.datavalid.tt << ",";
|
||
stream << eadc.datavalid.mi << ",";
|
||
stream << eadc.datavalid.vi << ",";
|
||
// stream << eadc.datavalid.vt << ",";
|
||
// stream << eadc.datavalid.adr << ",";
|
||
// stream << eadc.datavalid.aoai1 << ",";
|
||
// stream << eadc.datavalid.aoai2 << ",";
|
||
// stream << eadc.datavalid.aoat1 << ",";
|
||
// stream << eadc.datavalid.aoat2 << ",";
|
||
// stream << eadc.datavalid.aosi1 << ",";
|
||
// stream << eadc.datavalid.aosi2 << ",";
|
||
// stream << eadc.datavalid.aost1 << ",";
|
||
// stream << eadc.datavalid.aost2 << ",";
|
||
// stream << eadc.coffpress_k0 << ",";
|
||
// stream << eadc.coffpress_k1 << ",";
|
||
// stream << eadc.coffpress_k2 << ",";
|
||
// stream << eadc.coffpress_k3 << ",";
|
||
// stream << eadc.coffangle_k0 << ",";
|
||
// stream << eadc.coffangle_k1 << ",";
|
||
// stream << eadc.coffangle_k2 << ",";
|
||
// stream << eadc.coffangle_k3 << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveEcuDataToFile(const _ecu &ecu)
|
||
{
|
||
QFile file(ecu_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
|
||
stream << ecu.rpm << ",";
|
||
stream << ecu.t1 << ",";
|
||
stream << ecu.p2 << ",";
|
||
stream << ecu.servo_current << ",";
|
||
stream << ecu.states << "\n";
|
||
|
||
stream.flush();
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
|
||
}
|
||
|
||
void Parse::saveSspcDataToFile(const _sspc &sspc)
|
||
{
|
||
QFile file(sspc_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
|
||
stream << sspc.bus_voltage << ",";
|
||
stream << sspc.battery_voltage << ",";
|
||
stream << sspc.battery_current << ",";
|
||
stream << sspc.main_voltage << ",";
|
||
stream << sspc.main_current << ",";
|
||
stream << sspc.current_ch1 << ",";
|
||
stream << sspc.current_ch2 << ",";
|
||
stream << sspc.current_ch3 << ",";
|
||
stream << sspc.current_ch4 << ",";
|
||
stream << sspc.current_ch5 << ",";
|
||
stream << sspc.current_ch6 << ",";
|
||
stream << sspc.current_ch7 << ",";
|
||
stream << sspc.current_ch8 << ",";
|
||
stream << sspc.current_ch9 << ",";
|
||
stream << sspc.current_ch10 << ",";
|
||
stream << sspc.current_ch11 << ",";
|
||
stream << sspc.current_ch12 << ",";
|
||
stream << sspc.current_ch13 << ",";
|
||
stream << sspc.source << ",";
|
||
stream << sspc.state1.current_ch1 << ",";
|
||
stream << sspc.state1.current_ch2 << ",";
|
||
stream << sspc.state1.current_ch3 << ",";
|
||
stream << sspc.state1.current_ch4 << ",";
|
||
stream << sspc.state1.current_ch5 << ",";
|
||
stream << sspc.state1.current_ch6 << ",";
|
||
stream << sspc.state1.current_ch7 << ",";
|
||
stream << sspc.state1.current_ch8 << ",";
|
||
stream << sspc.state2.current_ch9 << ",";
|
||
stream << sspc.state2.current_ch10 << ",";
|
||
stream << sspc.state2.current_ch11 << ",";
|
||
stream << sspc.state2.current_ch12 << ",";
|
||
stream << sspc.state2.current_ch13 << ",";
|
||
stream << sspc.state2.current_ch14 << ",";
|
||
stream << sspc.state2.current_ch15 << ",";
|
||
stream << sspc.state2.current_ch16 << ",";
|
||
stream << sspc.check.CUP_STA << ",";
|
||
stream << sspc.check.current << ",";
|
||
stream << sspc.check.current_10A << ",";
|
||
stream << sspc.check.current_40A << ",";
|
||
stream << sspc.err1.ins_sbg << ",";
|
||
stream << sspc.err1.ins_320 << ",";
|
||
stream << sspc.err1.dlink_l << ",";
|
||
stream << sspc.err1.eadc << ",";
|
||
stream << sspc.err1.rec << ",";
|
||
stream << sspc.err1.landinggear << ",";
|
||
stream << sspc.err1.act1 << ",";
|
||
stream << sspc.err2.act2 << ",";
|
||
stream << sspc.err2.act3 << ",";
|
||
stream << sspc.err2 .computer << ",";
|
||
stream << sspc.err3.pump << ",";
|
||
stream << sspc.err3.ail << ",";
|
||
stream << sspc.err3.temp_56v << ",";
|
||
stream << sspc.err3.temp_28v << ",";
|
||
stream << sspc.err4.computer << ",";
|
||
stream << sspc.err4.ecu << ",";
|
||
stream << sspc.err4.eadc << ",";
|
||
stream << sspc.err4.temp << ",";
|
||
stream << sspc.err5.fuel << ",";
|
||
stream << sspc.err5.temp_56v2 << ",";
|
||
stream << sspc.err5.v_act << ",";
|
||
stream << sspc.err5.current56V << ",";
|
||
stream << sspc.err6.oil_press << "\n";
|
||
|
||
stream.flush();
|
||
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::saveVelDataToFile(const _vel &vel)
|
||
{
|
||
QFile file(vel_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
|
||
stream << vel.time_stamp << ",";
|
||
stream << vel.status.status_value << ",";
|
||
stream << vel.status.type_value << ",";
|
||
stream << vel.status.vel_status << ",";
|
||
stream << vel.status.vel_type << ",";
|
||
stream << vel.tow << ",";
|
||
stream << vel.vel_n << ",";
|
||
stream << vel.vel_e << ",";
|
||
stream << vel.vel_d << ",";
|
||
stream << vel.vel_acc_n << ",";
|
||
stream << vel.vel_acc_e << ",";
|
||
stream << vel.vel_acc_d << ",";
|
||
stream << vel.course << ",";
|
||
stream << vel.course_acc << "\n";
|
||
|
||
stream.flush();
|
||
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|
||
|
||
void Parse::savePosDataToFile(const _pos &pos)
|
||
{
|
||
QFile file(pos_file_name);
|
||
if(file.open(QIODevice::Append) )
|
||
{
|
||
QTextStream stream(&file);
|
||
stream.setCodec("UTF-8");
|
||
stream << QDateTime::currentDateTime().toString("yyyyMMddHHmmss").toUtf8() << ",";
|
||
|
||
stream << pos.time_stamp << ",";
|
||
stream << pos.status.status_value << ",";
|
||
stream << pos.status.type_value << ",";
|
||
stream << pos.status.pos_status << ",";
|
||
stream << pos.status.pos_type << ",";
|
||
stream << pos.status.gps_l1_used << ",";
|
||
stream << pos.status.gps_l2_used << ",";
|
||
stream << pos.status.gps_l5_used << ",";
|
||
stream << pos.status.glo_l1_used << ",";
|
||
stream << pos.status.glo_l2_used << ",";
|
||
stream << pos.status.glo_l2_used << ",";
|
||
stream << pos.status.glo_l3_used << ",";
|
||
stream << pos.status.gal_e1_used << ",";
|
||
stream << pos.status.gal_e5a_used << ",";
|
||
stream << pos.status.gal_e5b_used << ",";
|
||
stream << pos.status.gal_e5alt_used << ",";
|
||
stream << pos.status.gal_e6_used << ",";
|
||
stream << pos.status.bds_b1_used << ",";
|
||
stream << pos.status.bds_b2_used << ",";
|
||
stream << pos.status.bds_b3_used << ",";
|
||
stream << pos.status.qzss_l1_used << ",";
|
||
stream << pos.status.qzss_l2_used << ",";
|
||
stream << pos.status.qzss_l3_used << ",";
|
||
stream << pos.tow << ",";
|
||
stream << pos.lat << ",";
|
||
stream << pos.lng << ",";
|
||
stream << pos.alt << ",";
|
||
stream << pos.undulation << ",";
|
||
stream << pos.pos_acc_lat << ",";
|
||
stream << pos.pos_acc_lng << ",";
|
||
stream << pos.pos_acc_alt << ",";
|
||
stream << pos.num_sv_used << ",";
|
||
stream << pos.base_station_id << ",";
|
||
stream << pos.diff_age << "\n";
|
||
|
||
stream.flush();
|
||
|
||
|
||
file.flush();
|
||
file.close();
|
||
}
|
||
}
|