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

sbg惯导换成100e

Browse Source
This commit is contained in:
zhangxusong
2024-07-26 14:29:17 +08:00
parent 464d2765e9
commit 9077c81d53
6 changed files with 3109 additions and 2328 deletions
Download Patch File Download Diff File Expand all files Collapse all files
App/ToolsUI/INS/INS.cpp
+410 -243
View File
@@ -77,115 +77,115 @@ INS::INS(QWidget *parent) :
setColor(ui->chkInstall, inital);
setColor(ui->chkAltitudeInfo, inital);
_100eUIInit();
//=============sbg===============
//imu
setColor(ui->label_imu_timestamp, inital);
setColor(ui->label_imu_ax, inital);
setColor(ui->label_imu_ay, inital);
setColor(ui->label_imu_az, inital);
setColor(ui->label_imu_gx, inital);
setColor(ui->label_imu_gy, inital);
setColor(ui->label_imu_gz, inital);
setColor(ui->label_imu_temp, inital);
setColor(ui->label_imu_velx, inital);
setColor(ui->label_imu_vely, inital);
setColor(ui->label_imu_velz, inital);
setColor(ui->label_imu_anglex, inital);
setColor(ui->label_imu_angley, inital);
setColor(ui->label_imu_anglez, inital);
// setColor(ui->label_imu_timestamp, inital);
// setColor(ui->label_imu_ax, inital);
// setColor(ui->label_imu_ay, inital);
// setColor(ui->label_imu_az, inital);
// setColor(ui->label_imu_gx, inital);
// setColor(ui->label_imu_gy, inital);
// setColor(ui->label_imu_gz, inital);
// setColor(ui->label_imu_temp, inital);
// setColor(ui->label_imu_velx, inital);
// setColor(ui->label_imu_vely, inital);
// setColor(ui->label_imu_velz, inital);
// setColor(ui->label_imu_anglex, inital);
// setColor(ui->label_imu_angley, inital);
// setColor(ui->label_imu_anglez, inital);
setColor(ui->checkBox_imu_com_ok, inital);
setColor(ui->checkBox_imu_status, inital);
setColor(ui->checkBox_imu_ax_bit, inital);
setColor(ui->checkBox_imu_ay_bit, inital);
setColor(ui->checkBox_imu_az_bit, inital);
setColor(ui->checkBox_imu_gx_bit, inital);
setColor(ui->checkBox_imu_gy_bit, inital);
setColor(ui->checkBox_imu_gz_bit, inital);
setColor(ui->checkBox_imu_a_inrange, inital);
setColor(ui->checkBox_imu_g_inrange, inital);
//euler
setColor(ui->label_euler_timestamp, inital);
setColor(ui->label_euler_roll, inital);
setColor(ui->label_euler_pitch, inital);
setColor(ui->label_euler_yaw, inital);
setColor(ui->label_euler_acc_roll, inital);
setColor(ui->label_euler_acc_pitch, inital);
setColor(ui->label_euler_acc_yaw, inital);
setColor(ui->label_euler_solutionMode, inital);
// setColor(ui->checkBox_imu_com_ok, inital);
// setColor(ui->checkBox_imu_status, inital);
// setColor(ui->checkBox_imu_ax_bit, inital);
// setColor(ui->checkBox_imu_ay_bit, inital);
// setColor(ui->checkBox_imu_az_bit, inital);
// setColor(ui->checkBox_imu_gx_bit, inital);
// setColor(ui->checkBox_imu_gy_bit, inital);
// setColor(ui->checkBox_imu_gz_bit, inital);
// setColor(ui->checkBox_imu_a_inrange, inital);
// setColor(ui->checkBox_imu_g_inrange, inital);
// //euler
// setColor(ui->label_euler_timestamp, inital);
// setColor(ui->label_euler_roll, inital);
// setColor(ui->label_euler_pitch, inital);
// setColor(ui->label_euler_yaw, inital);
// setColor(ui->label_euler_acc_roll, inital);
// setColor(ui->label_euler_acc_pitch, inital);
// setColor(ui->label_euler_acc_yaw, inital);
// setColor(ui->label_euler_solutionMode, inital);
setColor(ui->checkBox_euler_attitude, inital);
setColor(ui->checkBox_euler_heading, inital);
setColor(ui->checkBox_euler_velocity, inital);
setColor(ui->checkBox_euler_position, inital);
setColor(ui->checkBox_euler_vertref, inital);
setColor(ui->checkBox_euler_magref, inital);
setColor(ui->checkBox_euler_gps1_vel, inital);
setColor(ui->checkBox_euler_gps1_pos, inital);
setColor(ui->checkBox_euler_gps1_hdt, inital);
setColor(ui->checkBox_euler_align, inital);
setColor(ui->checkBox_euler_gps2_vel, inital);
setColor(ui->checkBox_euler_gps2_pos, inital);
setColor(ui->checkBox_euler_gps2_hdt, inital);
setColor(ui->checkBox_euler_odo, inital);
setColor(ui->checkBox_euler_bt, inital);
setColor(ui->checkBox_euler_wt, inital);
setColor(ui->checkBox_euler_usel, inital);
setColor(ui->checkBox_euler_airdata, inital);
setColor(ui->checkBox_euler_zupt, inital);
setColor(ui->checkBox_euler_depth, inital);
// setColor(ui->checkBox_euler_attitude, inital);
// setColor(ui->checkBox_euler_heading, inital);
// setColor(ui->checkBox_euler_velocity, inital);
// setColor(ui->checkBox_euler_position, inital);
// setColor(ui->checkBox_euler_vertref, inital);
// setColor(ui->checkBox_euler_magref, inital);
// setColor(ui->checkBox_euler_gps1_vel, inital);
// setColor(ui->checkBox_euler_gps1_pos, inital);
// setColor(ui->checkBox_euler_gps1_hdt, inital);
// setColor(ui->checkBox_euler_align, inital);
// setColor(ui->checkBox_euler_gps2_vel, inital);
// setColor(ui->checkBox_euler_gps2_pos, inital);
// setColor(ui->checkBox_euler_gps2_hdt, inital);
// setColor(ui->checkBox_euler_odo, inital);
// setColor(ui->checkBox_euler_bt, inital);
// setColor(ui->checkBox_euler_wt, inital);
// setColor(ui->checkBox_euler_usel, inital);
// setColor(ui->checkBox_euler_airdata, inital);
// setColor(ui->checkBox_euler_zupt, inital);
// setColor(ui->checkBox_euler_depth, inital);
//vel
setColor(ui->label_vel_timestamp, inital);
setColor(ui->label_vel_tow, inital);
setColor(ui->label_vel_vn, inital);
setColor(ui->label_vel_ve, inital);
setColor(ui->label_vel_vd, inital);
setColor(ui->label_vel_acc_vn, inital);
setColor(ui->label_vel_acc_ve, inital);
setColor(ui->label_vel_acc_vd, inital);
setColor(ui->label_vel_course, inital);
setColor(ui->label_vel_acc_course, inital);
// //vel
// setColor(ui->label_vel_timestamp, inital);
// setColor(ui->label_vel_tow, inital);
// setColor(ui->label_vel_vn, inital);
// setColor(ui->label_vel_ve, inital);
// setColor(ui->label_vel_vd, inital);
// setColor(ui->label_vel_acc_vn, inital);
// setColor(ui->label_vel_acc_ve, inital);
// setColor(ui->label_vel_acc_vd, inital);
// setColor(ui->label_vel_course, inital);
// setColor(ui->label_vel_acc_course, inital);
setColor(ui->label_vel_status, inital);
setColor(ui->label_vel_type, inital);
// setColor(ui->label_vel_status, inital);
// setColor(ui->label_vel_type, inital);
//pos
setColor(ui->label_pos_timestamp, inital);
setColor(ui->label_pos_tow, inital);
setColor(ui->label_pos_lat, inital);
setColor(ui->label_pos_lng, inital);
setColor(ui->label_pos_alt, inital);
setColor(ui->label_pos_undelation, inital);
setColor(ui->label_pos_acc_lat, inital);
setColor(ui->label_pos_acc_lng, inital);
setColor(ui->label_pos_acc_alt, inital);
setColor(ui->label_pos_svn, inital);
setColor(ui->label_pos_base_id, inital);
setColor(ui->label_pos_diff_age, inital);
// //pos
// setColor(ui->label_pos_timestamp, inital);
// setColor(ui->label_pos_tow, inital);
// setColor(ui->label_pos_lat, inital);
// setColor(ui->label_pos_lng, inital);
// setColor(ui->label_pos_alt, inital);
// setColor(ui->label_pos_undelation, inital);
// setColor(ui->label_pos_acc_lat, inital);
// setColor(ui->label_pos_acc_lng, inital);
// setColor(ui->label_pos_acc_alt, inital);
// setColor(ui->label_pos_svn, inital);
// setColor(ui->label_pos_base_id, inital);
// setColor(ui->label_pos_diff_age, inital);
setColor(ui->checkBox_pos_status, inital);
setColor(ui->checkBox_pos_type, inital);
setColor(ui->checkBox_pos_GPS_L1, inital);
setColor(ui->checkBox_pos_GPS_L2, inital);
setColor(ui->checkBox_pos_GPS_L5, inital);
setColor(ui->checkBox_pos_GLO_L1, inital);
setColor(ui->checkBox_pos_GLO_L2, inital);
setColor(ui->checkBox_pos_GLO_L3, inital);
setColor(ui->checkBox_pos_GAL_E1, inital);
setColor(ui->checkBox_pos_GAL_E5A, inital);
setColor(ui->checkBox_pos_GAL_E5ALT, inital);
setColor(ui->checkBox_pos_GAL_E5B, inital);
setColor(ui->checkBox_pos_GAL_E6, inital);
setColor(ui->checkBox_pos_BDS_B1, inital);
setColor(ui->checkBox_pos_BDS_B2, inital);
setColor(ui->checkBox_pos_BDS_B3, inital);
setColor(ui->checkBox_pos_QZSS_L1, inital);
setColor(ui->checkBox_pos_QZSS_L2, inital);
setColor(ui->checkBox_pos_QZSS_L3, inital);
// setColor(ui->checkBox_pos_status, inital);
// setColor(ui->checkBox_pos_type, inital);
// setColor(ui->checkBox_pos_GPS_L1, inital);
// setColor(ui->checkBox_pos_GPS_L2, inital);
// setColor(ui->checkBox_pos_GPS_L5, inital);
// setColor(ui->checkBox_pos_GLO_L1, inital);
// setColor(ui->checkBox_pos_GLO_L2, inital);
// setColor(ui->checkBox_pos_GLO_L3, inital);
// setColor(ui->checkBox_pos_GAL_E1, inital);
// setColor(ui->checkBox_pos_GAL_E5A, inital);
// setColor(ui->checkBox_pos_GAL_E5ALT, inital);
// setColor(ui->checkBox_pos_GAL_E5B, inital);
// setColor(ui->checkBox_pos_GAL_E6, inital);
// setColor(ui->checkBox_pos_BDS_B1, inital);
// setColor(ui->checkBox_pos_BDS_B2, inital);
// setColor(ui->checkBox_pos_BDS_B3, inital);
// setColor(ui->checkBox_pos_QZSS_L1, inital);
// setColor(ui->checkBox_pos_QZSS_L2, inital);
// setColor(ui->checkBox_pos_QZSS_L3, inital);
}
@@ -217,12 +217,12 @@ void INS::recieveData(const int &id, const QByteArray &data)
//sbg imu
connect(parse,&Parse::imu_info,
this,&INS::imu);
// connect(parse,&Parse::imu_info,
// this,&INS::imu);
//sbg euler
connect(parse,&Parse::euler_info,
this,&INS::euler);
// connect(parse,&Parse::euler_info,
// this,&INS::euler);
QThreadPool::globalInstance()->start(parse);
@@ -241,8 +241,8 @@ void INS::recieveData(const int &id, const QByteArray &data)
parse,&Parse::parseData);
//sbg vel
connect(parse,&Parse::vel_info,
this,&INS::vel);
// connect(parse,&Parse::vel_info,
// this,&INS::vel);
//sbg pos
connect(parse,&Parse::pos_info,
this,&INS::pos);
@@ -338,157 +338,324 @@ void INS::NAV_Info(Parse::_nav info)
ui->labPDOP->setText(QString::number(info.PDOP) );
}
void INS::imu(Parse::_imu info)
void INS::_100eIMUInfo(Parse::_100eIMU info)
{
ui->label_imu_timestamp->setText(QString::number(info.time_stamp));
ui->label_imu_ax->setText(QString::number(info.accel_x, 'f', 2));
ui->label_imu_ay->setText(QString::number(info.accel_y, 'f', 2));
ui->label_imu_az->setText(QString::number(info.accel_z, 'f', 2));
ui->label_imu_gx->setText(QString::number(info.gyro_x, 'f', 2));
ui->label_imu_gy->setText(QString::number(info.gyro_y, 'f', 2));
ui->label_imu_gz->setText(QString::number(info.gyro_z, 'f', 2));
ui->label_imu_temp->setText(QString::number(info.temp, 'f', 2));
ui->label_imu_velx->setText(QString::number(info.delta_vel_x, 'f', 2));
ui->label_imu_vely->setText(QString::number(info.delta_vel_y, 'f', 2));
ui->label_imu_velz->setText(QString::number(info.delta_vel_z, 'f', 2));
ui->label_imu_anglex->setText(QString::number(info.delta_angle_x, 'f', 2));
ui->label_imu_angley->setText(QString::number(info.delta_angle_y, 'f', 2));
ui->label_imu_anglez->setText(QString::number(info.delta_angle_z, 'f', 2));
ui->lab100eGPSWeek->setText(QString::number(info.GPSWeek) );
ui->lab100eGPSWeek1->setText(QString::number(info.GPSWeek1) );
ui->lab100eTow->setText(QString::number(info.tow) );
ui->lab100eTowMs->setText(QString::number(info.tow_ms) );
ui->lab100eAx->setText(QString::number(info.ax, 'f', 1) );
ui->lab100eAy->setText(QString::number(info.ay, 'f', 1) );
ui->lab100eAz->setText(QString::number(info.az, 'f', 1) );
ui->lab100eP->setText(QString::number(info.p, 'f', 1) );
ui->lab100eQ->setText(QString::number(info.q, 'f', 1) );
ui->lab100eR->setText(QString::number(info.r, 'f', 1) );
setColor(ui->chkXGyroState, info.IMUState.XGyroState1 ? success : failure);
setColor(ui->chkYGyroState, info.IMUState.YGyroState ? success : failure);
setColor(ui->chkZGyroState, info.IMUState.ZGyroState ? success : failure);
setColor(ui->chkXAccState, info.IMUState.XAccelerometerState1 ? success : failure);
setColor(ui->chkYAccState, info.IMUState.YAccelerometerState ? success : failure);
setColor(ui->chkZAccState, info.IMUState.ZAccelerometerState ? success : failure);
setColor(ui->checkBox_imu_com_ok, (info.status.com_ok)?(success):(failure));
setColor(ui->checkBox_imu_status, (info.status.status_bit)?(success):(failure));
setColor(ui->checkBox_imu_ax_bit, (info.status.accel_x_bit)?(success):(failure));
setColor(ui->checkBox_imu_ay_bit, (info.status.accel_y_bit)?(success):(failure));
setColor(ui->checkBox_imu_az_bit, (info.status.accel_z_bit)?(success):(failure));
setColor(ui->checkBox_imu_gx_bit, (info.status.gyro_x_bit)?(success):(failure));
setColor(ui->checkBox_imu_gy_bit, (info.status.gyro_y_bit)?(success):(failure));
setColor(ui->checkBox_imu_gz_bit, (info.status.gyro_z_bit)?(success):(failure));
setColor(ui->checkBox_imu_a_inrange, (info.status.accel_in_range)?(success):(failure));
setColor(ui->checkBox_imu_g_inrange, (info.status.gyro_in_range)?(success):(failure));
}
void INS::euler(Parse::_euler info)
void INS::_100eINSInfo(Parse::_100eIns info)
{
ui->label_euler_timestamp->setText(QString::number(info.time_stamp));
ui->label_euler_roll->setText(QString::number(info.roll, 'f', 2));
ui->label_euler_pitch->setText(QString::number(info.pitch, 'f', 2));
ui->label_euler_yaw->setText(QString::number(info.yaw, 'f', 2));
ui->label_euler_acc_roll->setText(QString::number(info.roll_acc, 'f', 6));
ui->label_euler_acc_pitch->setText(QString::number(info.pitch_acc, 'f', 6));
ui->label_euler_acc_yaw->setText(QString::number(info.yaw_acc, 'f', 6));
ui->label_euler_solutionMode->setText(info.solution.solutionMode);
//gps
ui->lab100eGPSTime->setText(info.time);
ui->lab100eGPSTemp->setText(QString::number(info.temperature, 'f', 1) );
ui->labGPSFixtype->setText(info.gps_fixtype);
ui->lab100eGPSHdg->setText(QString::number(info.gps_hdg, 'f', 1) );
ui->lab100eGPSHdgDev->setText(QString::number(info.gps_hdg_dev, 'f', 1) );
ui->lab100eGPS0Dt->setText(QString::number(info.gps0_dt) );
ui->lab100eGPS1Dt->setText(QString::number(info.gps1_dt) );
ui->lab100eGPSVN->setText(QString::number(info.gps_vn, 'f', 1) );
ui->lab100eGPSVE->setText(QString::number(info.gps_ve, 'f', 1) );
ui->lab100eGPSVD->setText(QString::number(info.gps_vd, 'f', 1) );
ui->lab100eGPSAddState->setText(info.redundancy.additive );
ui->lab100eGPSGyroState->setText(info.redundancy.gyroscope );
ui->lab100eGPSCompassState->setText(info.redundancy.compass );
ui->lab100eGPSState->setText(info.redundancy.gps );
setColor(ui->checkBox_euler_attitude, (info.solution.attitude_valid)?(success):(failure));
setColor(ui->checkBox_euler_heading, (info.solution.heading_valid)?(success):(failure));
setColor(ui->checkBox_euler_velocity, (info.solution.velocity_valid)?(success):(failure));
setColor(ui->checkBox_euler_position, (info.solution.position_valid)?(success):(failure));
setColor(ui->checkBox_euler_vertref, (info.solution.vert_ref_used)?(success):(failure));
setColor(ui->checkBox_euler_magref, (info.solution.mag_ref_used)?(success):(failure));
setColor(ui->checkBox_euler_gps1_vel, (info.solution.gps1_vel_used)?(success):(failure));
setColor(ui->checkBox_euler_gps1_pos, (info.solution.gps1_pos_used)?(success):(failure));
setColor(ui->checkBox_euler_gps1_hdt, (info.solution.gps1_hdt_used)?(success):(failure));
setColor(ui->checkBox_euler_align, (info.solution.align_valid)?(success):(failure));
setColor(ui->checkBox_euler_gps2_vel, (info.solution.gps2_vel_used)?(success):(failure));
setColor(ui->checkBox_euler_gps2_pos, (info.solution.gps2_pos_used)?(success):(failure));
setColor(ui->checkBox_euler_gps2_hdt, (info.solution.gps2_hdt_used)?(success):(failure));
setColor(ui->checkBox_euler_odo, (info.solution.odo_used)?(success):(failure));
setColor(ui->checkBox_euler_bt, (info.solution.dvl_bt_used)?(success):(failure));
setColor(ui->checkBox_euler_wt, (info.solution.dvl_wt_used)?(success):(failure));
setColor(ui->checkBox_euler_usel, (info.solution.usel_used)?(success):(failure));
setColor(ui->checkBox_euler_airdata, (info.solution.air_data_used)?(success):(failure));
setColor(ui->checkBox_euler_zupt, (info.solution.zupt_used)?(success):(failure));
setColor(ui->checkBox_euler_depth, (info.solution.depth_used)?(success):(failure));
}
void INS::vel(Parse::_vel info)
{
ui->label_vel_timestamp->setText(QString::number(info.time_stamp));
ui->label_vel_tow->setText(QString::number(info.tow));
ui->label_vel_vn->setText(QString::number(info.vel_n, 'f', 2));
ui->label_vel_ve->setText(QString::number(info.vel_e, 'f', 2));
ui->label_vel_vd->setText(QString::number(info.vel_d, 'f', 2));
ui->label_vel_acc_vn->setText(QString::number(info.vel_acc_n, 'f', 6));
ui->label_vel_acc_ve->setText(QString::number(info.vel_acc_e, 'f', 6));
ui->label_vel_acc_vd->setText(QString::number(info.vel_acc_d, 'f', 6));
ui->label_vel_course->setText(QString::number(info.course, 'f', 2));
ui->label_vel_acc_course->setText(QString::number(info.course_acc, 'f', 2));
//ins
ui->lab100eINSPitch->setText(QString::number(info.pitch, 'f', 1) );
ui->lab100eINSRoll->setText(QString::number(info.roll, 'f', 1) );
ui->lab100eINSYaw->setText(QString::number(info.yaw, 'f', 1) );
ui->lab100eINSGPSYaw->setText(QString::number(info.gps_yaw, 'f', 1) );
ui->lab100eINSPitchRate->setText(QString::number(info.pitch_rate, 'f', 1) );
ui->lab100eINSRollRate->setText(QString::number(info.roll_rate, 'f', 1) );
ui->lab100eINSYawRate->setText(QString::number(info.yaw_rate, 'f', 1) );
ui->lab100eINSLon->setText(QString::number(info.lon, 'f', 7) );
ui->lab100eINSLat->setText(QString::number(info.lat, 'f', 7) );
ui->lab100eINSAltBaro->setText(QString::number(info.alt_baro, 'f', 1) );
ui->lab100eINSAltGPS->setText(QString::number(info.alt_gps, 'f', 1) );
ui->lab100eINSAlt->setText(QString::number(info.alt, 'f', 1) );
ui->lab100eINSVN->setText(QString::number(info.velocity_n, 'f', 1) );
ui->lab100eINSVE->setText(QString::number(info.velocity_e, 'f', 1) );
ui->lab100eINSVD->setText(QString::number(info.velocity_d, 'f', 1) );
ui->lab100eINSVAir->setText(QString::number(info.velocity_air, 'f', 1) );
ui->lab100eINSAccelN->setText(QString::number(info.accel_n, 'f', 1) );
ui->lab100eINSAccelE->setText(QString::number(info.accel_e, 'f', 1) );
ui->lab100eINSAccelD->setText(QString::number(info.accel_d, 'f', 1) );
ui->lab100eINSSatelliteNum->setText(QString::number(info.satellite_num) );
ui->lab100eINSHdop->setText(QString::number(info.hdop, 'f', 2) );
ui->lab100eINSVdop->setText(QString::number(info.vdop, 'f', 2) );
ui->label_vel_status->setText(info.status.vel_status);
ui->label_vel_type->setText(info.status.vel_type);
if(info.status.vel_status == 0)
switch(info.state.ahrs)
{
setColor(ui->label_vel_status, failure);
}
else
{
setColor(ui->label_vel_status, success);
case 0:
setColor(ui->chkINSAhrsState, inital);
break;
case 1:
setColor(ui->chkINSAhrsState, success);
break;
case 2:
setColor(ui->chkINSAhrsState, failure);
break;
}
//setColor(ui->checkBox_vel_status, inital);
//setColor(ui->checkBox_vel_type, inital);
}
void INS::pos(Parse::_pos info)
{
ui->label_pos_timestamp->setText(QString::number(info.time_stamp));
ui->label_pos_tow->setText(QString::number(info.tow));
ui->label_pos_lat->setText(QString::number(info.lat, 'f', 8));
ui->label_pos_lng->setText(QString::number(info.lng, 'f', 8));
ui->label_pos_alt->setText(QString::number(info.alt, 'f', 2));
ui->label_pos_undelation->setText(QString::number(info.undulation, 'f', 2));
ui->label_pos_acc_lat->setText(QString::number(info.pos_acc_lat, 'f', 6));
ui->label_pos_acc_lng->setText(QString::number(info.pos_acc_lng, 'f', 6));
ui->label_pos_acc_alt->setText(QString::number(info.pos_acc_alt, 'f', 6));
ui->label_pos_svn->setText(QString::number(info.num_sv_used));
ui->label_pos_base_id->setText(QString::number(info.base_station_id));
ui->label_pos_diff_age->setText(QString::number(info.diff_age));
ui->checkBox_pos_status->setText(info.status.pos_status);
ui->checkBox_pos_type->setText(info.status.pos_type);
//setColor(ui->checkBox_pos_status, (info.solution_status.attitude_valid)?(success):(failure));
//setColor(ui->checkBox_pos_type, (info.solution_status.attitude_valid)?(success):(failure));
setColor(ui->checkBox_pos_GPS_L1, (info.status.gps_l1_used)?(success):(failure));
setColor(ui->checkBox_pos_GPS_L2, (info.status.gps_l2_used)?(success):(failure));
setColor(ui->checkBox_pos_GPS_L5, (info.status.gps_l5_used)?(success):(failure));
setColor(ui->checkBox_pos_GLO_L1, (info.status.glo_l1_used)?(success):(failure));
setColor(ui->checkBox_pos_GLO_L2, (info.status.glo_l2_used)?(success):(failure));
setColor(ui->checkBox_pos_GLO_L3, (info.status.glo_l3_used)?(success):(failure));
setColor(ui->checkBox_pos_GAL_E1, (info.status.gal_e1_used)?(success):(failure));
setColor(ui->checkBox_pos_GAL_E5A, (info.status.gal_e5a_used)?(success):(failure));
setColor(ui->checkBox_pos_GAL_E5ALT, (info.status.gal_e5alt_used)?(success):(failure));
setColor(ui->checkBox_pos_GAL_E5B, (info.status.gal_e5b_used)?(success):(failure));
setColor(ui->checkBox_pos_GAL_E6, (info.status.gal_e6_used)?(success):(failure));
setColor(ui->checkBox_pos_BDS_B1, (info.status.bds_b1_used)?(success):(failure));
setColor(ui->checkBox_pos_BDS_B2, (info.status.bds_b2_used)?(success):(failure));
setColor(ui->checkBox_pos_BDS_B3, (info.status.bds_b3_used)?(success):(failure));
setColor(ui->checkBox_pos_QZSS_L1, (info.status.qzss_l1_used)?(success):(failure));
setColor(ui->checkBox_pos_QZSS_L2, (info.status.qzss_l2_used)?(success):(failure));
setColor(ui->checkBox_pos_QZSS_L3, (info.status.qzss_l3_used)?(success):(failure));
if(info.status.status_value == 0)
{
setColor(ui->checkBox_pos_status, success);
}
else
{
setColor(ui->checkBox_pos_status, failure);
}
if(info.status.type_value >= 2)
{
setColor(ui->checkBox_pos_type, success);
}
else
{
setColor(ui->checkBox_pos_type, failure);
}
setColor(ui->chkINSCompassState, info.state.compass ? failure : success);
setColor(ui->chkINSCompassCalibration, info.state.compassCalibration ? failure : success);
setColor(ui->chkINSAddState, info.state.additive ? failure : success);
setColor(ui->chkINSGyroState, info.state.gyroscope ? failure : success);
setColor(ui->chkINSBaroState, info.state.barometer ? failure : success);
}
void INS::_100eUIInit()
{
setColor(ui->lab100eAx, inital);
setColor(ui->lab100eAy, inital);
setColor(ui->lab100eAz, inital);
setColor(ui->lab100eGPS0Dt, inital);
setColor(ui->lab100eGPS1Dt, inital);
setColor(ui->lab100eGPSAddState, inital);
setColor(ui->lab100eGPSState, inital);
setColor(ui->lab100eGPSAddState, inital);
setColor(ui->lab100eGPSCompassState, inital);
setColor(ui->lab100eGPSGyroState, inital);
setColor(ui->chkINSCompassState, inital);
setColor(ui->lab100eGPSHdg, inital);
setColor(ui->lab100eGPSHdgDev, inital);
setColor(ui->lab100eGPSState, inital);
setColor(ui->lab100eGPSTemp, inital);
setColor(ui->lab100eGPSTime, inital);
setColor(ui->lab100eGPSVD, inital);
setColor(ui->lab100eGPSVN, inital);
setColor(ui->lab100eGPSVE, inital);
setColor(ui->lab100eGPSWeek, inital);
setColor(ui->lab100eINSAccelD, inital);
setColor(ui->lab100eINSAlt, inital);
setColor(ui->lab100eINSAltBaro, inital);
setColor(ui->lab100eINSAccelD, inital);
setColor(ui->lab100eGPSWeek1, inital);
setColor(ui->lab100eINSAltGPS, inital);
setColor(ui->lab100eINSGPSYaw, inital);
setColor(ui->lab100eINSHdop, inital);
setColor(ui->lab100eINSLat, inital);
setColor(ui->lab100eINSLon, inital);
setColor(ui->lab100eINSPitch, inital);
setColor(ui->lab100eINSRoll, inital);
setColor(ui->lab100eINSSatelliteNum, inital);
setColor(ui->lab100eP, inital);
setColor(ui->lab100eQ, inital);
setColor(ui->lab100eR, inital);
setColor(ui->lab100eTow, inital);
setColor(ui->lab100eTowMs, inital);
setColor(ui->lab100eINSYaw, inital);
setColor(ui->lab100eINSPitchRate, inital);
setColor(ui->lab100eINSRollRate, inital);
setColor(ui->lab100eINSYawRate, inital);
setColor(ui->lab100eINSVN, inital);
setColor(ui->lab100eINSVE, inital);
setColor(ui->lab100eINSVD, inital);
setColor(ui->lab100eINSVAir, inital);
setColor(ui->lab100eINSAccelN, inital);
setColor(ui->lab100eINSAccelE, inital);
setColor(ui->lab100eINSAccelD, inital);
setColor(ui->lab100eINSVdop, inital);
setColor(ui->labGPSFixtype, inital);
setColor(ui->chkINSAddState, inital);
setColor(ui->chkINSBaroState, inital);
setColor(ui->chkINSAhrsState, inital);
setColor(ui->chkIns, inital);
setColor(ui->chkINSCompassState, inital);
setColor(ui->chkINSCompassCalibration, inital);
setColor(ui->chkINSGyroState, inital);
setColor(ui->chkInsStatus, inital);
setColor(ui->chkInstall, inital);
setColor(ui->chkXGyroState, inital);
setColor(ui->chkXAccState, inital);
setColor(ui->chkYAccState, inital);
setColor(ui->chkZAccState, inital);
setColor(ui->chkYGyroState, inital);
setColor(ui->chkZGyroState, inital);
}
//void INS::imu(Parse::_imu info)
//{
// ui->label_imu_timestamp->setText(QString::number(info.time_stamp));
// ui->label_imu_ax->setText(QString::number(info.accel_x, 'f', 2));
// ui->label_imu_ay->setText(QString::number(info.accel_y, 'f', 2));
// ui->label_imu_az->setText(QString::number(info.accel_z, 'f', 2));
// ui->label_imu_gx->setText(QString::number(info.gyro_x, 'f', 2));
// ui->label_imu_gy->setText(QString::number(info.gyro_y, 'f', 2));
// ui->label_imu_gz->setText(QString::number(info.gyro_z, 'f', 2));
// ui->label_imu_temp->setText(QString::number(info.temp, 'f', 2));
// ui->label_imu_velx->setText(QString::number(info.delta_vel_x, 'f', 2));
// ui->label_imu_vely->setText(QString::number(info.delta_vel_y, 'f', 2));
// ui->label_imu_velz->setText(QString::number(info.delta_vel_z, 'f', 2));
// ui->label_imu_anglex->setText(QString::number(info.delta_angle_x, 'f', 2));
// ui->label_imu_angley->setText(QString::number(info.delta_angle_y, 'f', 2));
// ui->label_imu_anglez->setText(QString::number(info.delta_angle_z, 'f', 2));
// setColor(ui->checkBox_imu_com_ok, (info.status.com_ok)?(success):(failure));
// setColor(ui->checkBox_imu_status, (info.status.status_bit)?(success):(failure));
// setColor(ui->checkBox_imu_ax_bit, (info.status.accel_x_bit)?(success):(failure));
// setColor(ui->checkBox_imu_ay_bit, (info.status.accel_y_bit)?(success):(failure));
// setColor(ui->checkBox_imu_az_bit, (info.status.accel_z_bit)?(success):(failure));
// setColor(ui->checkBox_imu_gx_bit, (info.status.gyro_x_bit)?(success):(failure));
// setColor(ui->checkBox_imu_gy_bit, (info.status.gyro_y_bit)?(success):(failure));
// setColor(ui->checkBox_imu_gz_bit, (info.status.gyro_z_bit)?(success):(failure));
// setColor(ui->checkBox_imu_a_inrange, (info.status.accel_in_range)?(success):(failure));
// setColor(ui->checkBox_imu_g_inrange, (info.status.gyro_in_range)?(success):(failure));
//}
//void INS::euler(Parse::_euler info)
//{
// ui->label_euler_timestamp->setText(QString::number(info.time_stamp));
// ui->label_euler_roll->setText(QString::number(info.roll, 'f', 2));
// ui->label_euler_pitch->setText(QString::number(info.pitch, 'f', 2));
// ui->label_euler_yaw->setText(QString::number(info.yaw, 'f', 2));
// ui->label_euler_acc_roll->setText(QString::number(info.roll_acc, 'f', 6));
// ui->label_euler_acc_pitch->setText(QString::number(info.pitch_acc, 'f', 6));
// ui->label_euler_acc_yaw->setText(QString::number(info.yaw_acc, 'f', 6));
// ui->label_euler_solutionMode->setText(info.solution.solutionMode);
// setColor(ui->checkBox_euler_attitude, (info.solution.attitude_valid)?(success):(failure));
// setColor(ui->checkBox_euler_heading, (info.solution.heading_valid)?(success):(failure));
// setColor(ui->checkBox_euler_velocity, (info.solution.velocity_valid)?(success):(failure));
// setColor(ui->checkBox_euler_position, (info.solution.position_valid)?(success):(failure));
// setColor(ui->checkBox_euler_vertref, (info.solution.vert_ref_used)?(success):(failure));
// setColor(ui->checkBox_euler_magref, (info.solution.mag_ref_used)?(success):(failure));
// setColor(ui->checkBox_euler_gps1_vel, (info.solution.gps1_vel_used)?(success):(failure));
// setColor(ui->checkBox_euler_gps1_pos, (info.solution.gps1_pos_used)?(success):(failure));
// setColor(ui->checkBox_euler_gps1_hdt, (info.solution.gps1_hdt_used)?(success):(failure));
// setColor(ui->checkBox_euler_align, (info.solution.align_valid)?(success):(failure));
// setColor(ui->checkBox_euler_gps2_vel, (info.solution.gps2_vel_used)?(success):(failure));
// setColor(ui->checkBox_euler_gps2_pos, (info.solution.gps2_pos_used)?(success):(failure));
// setColor(ui->checkBox_euler_gps2_hdt, (info.solution.gps2_hdt_used)?(success):(failure));
// setColor(ui->checkBox_euler_odo, (info.solution.odo_used)?(success):(failure));
// setColor(ui->checkBox_euler_bt, (info.solution.dvl_bt_used)?(success):(failure));
// setColor(ui->checkBox_euler_wt, (info.solution.dvl_wt_used)?(success):(failure));
// setColor(ui->checkBox_euler_usel, (info.solution.usel_used)?(success):(failure));
// setColor(ui->checkBox_euler_airdata, (info.solution.air_data_used)?(success):(failure));
// setColor(ui->checkBox_euler_zupt, (info.solution.zupt_used)?(success):(failure));
// setColor(ui->checkBox_euler_depth, (info.solution.depth_used)?(success):(failure));
//}
//void INS::vel(Parse::_vel info)
//{
// ui->label_vel_timestamp->setText(QString::number(info.time_stamp));
// ui->label_vel_tow->setText(QString::number(info.tow));
// ui->label_vel_vn->setText(QString::number(info.vel_n, 'f', 2));
// ui->label_vel_ve->setText(QString::number(info.vel_e, 'f', 2));
// ui->label_vel_vd->setText(QString::number(info.vel_d, 'f', 2));
// ui->label_vel_acc_vn->setText(QString::number(info.vel_acc_n, 'f', 6));
// ui->label_vel_acc_ve->setText(QString::number(info.vel_acc_e, 'f', 6));
// ui->label_vel_acc_vd->setText(QString::number(info.vel_acc_d, 'f', 6));
// ui->label_vel_course->setText(QString::number(info.course, 'f', 2));
// ui->label_vel_acc_course->setText(QString::number(info.course_acc, 'f', 2));
// ui->label_vel_status->setText(info.status.vel_status);
// ui->label_vel_type->setText(info.status.vel_type);
// if(info.status.vel_status == 0)
// {
// setColor(ui->label_vel_status, failure);
// }
// else
// {
// setColor(ui->label_vel_status, success);
// }
// //setColor(ui->checkBox_vel_status, inital);
// //setColor(ui->checkBox_vel_type, inital);
//}
//void INS::pos(Parse::_pos info)
//{
// ui->label_pos_timestamp->setText(QString::number(info.time_stamp));
// ui->label_pos_tow->setText(QString::number(info.tow));
// ui->label_pos_lat->setText(QString::number(info.lat, 'f', 8));
// ui->label_pos_lng->setText(QString::number(info.lng, 'f', 8));
// ui->label_pos_alt->setText(QString::number(info.alt, 'f', 2));
// ui->label_pos_undelation->setText(QString::number(info.undulation, 'f', 2));
// ui->label_pos_acc_lat->setText(QString::number(info.pos_acc_lat, 'f', 6));
// ui->label_pos_acc_lng->setText(QString::number(info.pos_acc_lng, 'f', 6));
// ui->label_pos_acc_alt->setText(QString::number(info.pos_acc_alt, 'f', 6));
// ui->label_pos_svn->setText(QString::number(info.num_sv_used));
// ui->label_pos_base_id->setText(QString::number(info.base_station_id));
// ui->label_pos_diff_age->setText(QString::number(info.diff_age));
// ui->checkBox_pos_status->setText(info.status.pos_status);
// ui->checkBox_pos_type->setText(info.status.pos_type);
// //setColor(ui->checkBox_pos_status, (info.solution_status.attitude_valid)?(success):(failure));
// //setColor(ui->checkBox_pos_type, (info.solution_status.attitude_valid)?(success):(failure));
// setColor(ui->checkBox_pos_GPS_L1, (info.status.gps_l1_used)?(success):(failure));
// setColor(ui->checkBox_pos_GPS_L2, (info.status.gps_l2_used)?(success):(failure));
// setColor(ui->checkBox_pos_GPS_L5, (info.status.gps_l5_used)?(success):(failure));
// setColor(ui->checkBox_pos_GLO_L1, (info.status.glo_l1_used)?(success):(failure));
// setColor(ui->checkBox_pos_GLO_L2, (info.status.glo_l2_used)?(success):(failure));
// setColor(ui->checkBox_pos_GLO_L3, (info.status.glo_l3_used)?(success):(failure));
// setColor(ui->checkBox_pos_GAL_E1, (info.status.gal_e1_used)?(success):(failure));
// setColor(ui->checkBox_pos_GAL_E5A, (info.status.gal_e5a_used)?(success):(failure));
// setColor(ui->checkBox_pos_GAL_E5ALT, (info.status.gal_e5alt_used)?(success):(failure));
// setColor(ui->checkBox_pos_GAL_E5B, (info.status.gal_e5b_used)?(success):(failure));
// setColor(ui->checkBox_pos_GAL_E6, (info.status.gal_e6_used)?(success):(failure));
// setColor(ui->checkBox_pos_BDS_B1, (info.status.bds_b1_used)?(success):(failure));
// setColor(ui->checkBox_pos_BDS_B2, (info.status.bds_b2_used)?(success):(failure));
// setColor(ui->checkBox_pos_BDS_B3, (info.status.bds_b3_used)?(success):(failure));
// setColor(ui->checkBox_pos_QZSS_L1, (info.status.qzss_l1_used)?(success):(failure));
// setColor(ui->checkBox_pos_QZSS_L2, (info.status.qzss_l2_used)?(success):(failure));
// setColor(ui->checkBox_pos_QZSS_L3, (info.status.qzss_l3_used)?(success):(failure));
// if(info.status.status_value == 0)
// {
// setColor(ui->checkBox_pos_status, success);
// }
// else
// {
// setColor(ui->checkBox_pos_status, failure);
// }
// if(info.status.type_value >= 2)
// {
// setColor(ui->checkBox_pos_type, success);
// }
// else
// {
// setColor(ui->checkBox_pos_type, failure);
// }
//}
App/ToolsUI/INS/INS.h
+10 -5
View File
@@ -1,4 +1,4 @@
#ifndef INS_H
#ifndef INS_H
#define INS_H
#include <QWidget>
@@ -23,18 +23,23 @@ public slots:
void INS_Info(Parse::_ins info);
void NAV_Info(Parse::_nav info);
void _100eIMUInfo(Parse::_100eIMU info);
void _100eINSInfo(Parse::_100eIns info);
void imu(Parse::_imu info);
void euler(Parse::_euler info);
void vel(Parse::_vel info);
void pos(Parse::_pos info);
// void imu(Parse::_imu info);
// void euler(Parse::_euler info);
// void vel(Parse::_vel info);
// void pos(Parse::_pos info);
signals:
void parseData(const int &id,const QByteArray &data);
private:
void _100eUIInit();
Ui::INS *ui;
};
App/ToolsUI/INS/INS.ui
+2225 -1473
View File
File diff suppressed because it is too large Load Diff
App/ToolsUI/Parse.cpp
+363 -601
View File
@@ -1,4 +1,6 @@
#include "Parse.h"
#include <cmath>
Parse::Parse(QObject *parent) : QObject(parent)
{
@@ -670,269 +672,113 @@ void Parse::run()
}
if(raw.size() >= 221)//163~220 IMU
if(raw.size() >= 209)//163~208 100e_IMU
{
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
QByteArray data = data.mid(163);
int index = 0;
_100eIMU imu;
memcpy(&imu.GPSWeek, data.data() + index, 2);
index += 2;
union//低在前，高位在后
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
{
quint16 byte;
quint32 state;
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;
quint32 b0:1;
quint32 b1:1;
quint32 b2:1;
quint32 b3:1;
quint32 b4:1;
quint32 b5:1;
quint32 b6:1;
}status;
}imuState;
QByteArray data = raw.mid(163);
_imu group;
int data_count = 0;
memcpy(&imuState.state, data.data() + index, 4);
index += 4;
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;
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;
src.B[0] = data[data_count++];
src.B[1] = data[data_count++];
status.byte = src.I[0];
qint32 t;
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
memcpy(&t, data.data() + index, 4);
index += 4;
imu.az = t * 200 * 200 * std::pow(2, -31);
memcpy(&t, data.data() + index, 4);
index += 4;
imu.ax = t * 200 * 200 * std::pow(2, -31);
memcpy(&t, data.data() + index, 4);
index += 4;
imu.ay = t * 200 * 200 * std::pow(2, -31);
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;
memcpy(&t, data.data() + index, 4);
index += 4;
imu.r = t * 200 * 200 * std::pow(2, -31);
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;
memcpy(&t, data.data() + index, 4);
index += 4;
imu.p = t * 200 * 200 * std::pow(2, -31);
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;
memcpy(&t, data.data() + index, 4);
index += 4;
imu.q = t * 200 * 200 * std::pow(2, -31);
saveImuDataToFile(group); //把数据保存到文件
emit imu_info(group);
emit _100eIMUInfo(imu);
}
if(raw.size() >= 253)//221~252 EULER
if(raw.size() >= 227)//209~226 SSPC
{
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
_sspc sspc;
QByteArray data = raw.mid(209);
union//低在前，高位在后
{
quint32 byte;
struct
{
uint32_t solutionMode:4;//0-3
uint32_t attitude_valid:1;//4
uint32_t heading_valid:1;//5
uint32_t velocity_valid:1;//6
uint32_t position_valid:1;//7
uint32_t vert_ref_used:1;//8
uint32_t mag_ref_used:1;//9
uint32_t gps1_vel_used:1;//10
uint32_t gps1_pos_used:1;//11
uint32_t none1:1;
uint32_t gps1_hdt_used:1;//13
uint32_t gps2_vel_used:1;//14
uint32_t gps2_pos_used:1;//15
uint32_t none2:1;
uint32_t gps2_hdt_used:1;//17
uint32_t odo_used:1;//18
uint32_t dvl_bt_used:1;//19
uint32_t dvl_wt_used:1;//20
uint32_t none3:3;
uint32_t usel_used:1;//24
uint32_t air_data_used:1;//25
uint32_t zupt_used:1;//26
uint32_t align_valid:1;//27
uint32_t depth_used:1;//28
};
}status;
/*
qDebug() << "sspc"
<< QString::number(data[0],16) << QString::number(data[1],16)
<< QString::number(data[2],16) << QString::number(data[3],16);
*/
QByteArray data = raw.mid(221);
_euler group;
int data_count = 0;
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;
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;
saveEulerDataToFile(group); //把数据保存到文件中
emit euler_info(group);
saveSspcDataToFile(sspc); //把数据保存到文件中
emit SSPC_Info(sspc);
}
/*
if(raw.size() >= 253)//245~252 zero
@@ -1145,385 +991,301 @@ void Parse::run()
emit ECU_Info(group);
}
if(raw.size() >= 61)//43~60 SSPC
if(raw.size() >= 162)//43~161 100e ins
{
_sspc sspc;
QByteArray data = raw.mid(43);
union{uint8_t B[4];uint16_t I[2]; int16_t i[2]; uint32_t H; int32_t h;float F;}src;
/*
qDebug() << "sspc"
<< QString::number(data[0],16) << QString::number(data[1],16)
<< QString::number(data[2],16) << QString::number(data[3],16);
*/
QByteArray data = raw.mid(43);
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;
int index = 0;
_100eIns ins;
saveSspcDataToFile(sspc); //把数据保存到文件中
emit SSPC_Info(sspc);
}
memcpy(&ins.tick, data.data() + index, 4);
index += 4;
if(raw.size() >= 105)//61~104 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
{
quint32 stats:6;
quint32 type:6;
quint32 none1:20;
};
}status;
QByteArray data = raw.mid(61);
_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& 0x3F) {
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& 0x3F) {
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;
saveVelDataToFile(group); // 把数据保存到文件中
emit vel_info(group);
}
if(raw.size() >= 162)//105~161 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//低在前，高位在后
{
uint32_t byte;
struct
{
uint32_t stats:6;
uint32_t type:6;
uint32_t GPS_L1:1;
uint32_t GPS_L2:1;
uint32_t GPS_L5:1;
uint32_t GLO_L1:1;
uint32_t GLO_L2:1;
uint32_t GLO_L3:1;
uint32_t GLA_E1:1;
uint32_t GLA_E5A:1;
uint32_t GLA_E5B:1;
uint32_t GLA_E5ALT:1;
uint32_t GLA_E6:1;
uint32_t BDS_B1:1;
uint32_t BDS_B2:1;
uint32_t BDS_B3:1;
uint32_t QZSS_L1:1;
uint32_t QZSS_L2:1;
uint32_t QZSS_L3:1;
uint32_t NONE:3;
};
}status;
QByteArray data = raw.mid(105);
QString str;
QString str3 = data.toHex().data();//以十六进制显示
str3 = str3.toUpper ();//转换为大写
for(int i = 0;i<str3.length ();i+=2)//填加空格
union
{
QString st = str3.mid (i,2);
str += st;
str += " ";
}
//qDebug() << "MSG:" << str;
_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& 0x3F) {
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 (status.type & 0x3F) {
case 0:
group.status.pos_type = tr("NO_SOLUTION");
break;
case 1:
group.status.pos_type = tr("UNKNOWN_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 type");
}
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;
group.status.qzss_l1_used = status.QZSS_L1;
group.status.qzss_l2_used = status.QZSS_L2;
group.status.qzss_l3_used = status.QZSS_L3;
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;
//qDebug() << "stats,type" << group.status.status_value << group.status.type_value << status.byte;
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;
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];
memcpy(&ins.pitch, data.data() + index, 4);
index += 4;
ins.pitch *= 57.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++];
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;
memcpy(&ins.roll, data.data() + index, 4);
index += 4;
ins.roll *= 57.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++];
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;
memcpy(&ins.yaw, data.data() + index, 4);
index += 4;
ins.yaw *= 57.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++];
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;
memcpy(&ins.gps_yaw, data.data() + index, 4);
index += 4;
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];
memcpy(&ins.pitch_rate, data.data() + index, 4);
index += 4;
ins.pitch_rate *= 57.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.pos_acc_alt = src.F[0];
memcpy(&ins.roll_rate, data.data() + index, 4);
index += 4;
ins.roll_rate *= 57.3;
group.num_sv_used = data[data_count++];
memcpy(&ins.yaw_rate, data.data() + index, 4);
index += 4;
ins.yaw_rate *= 57.3;
src.B[0] = data[data_count++];
src.B[1] = data[data_count++];
group.base_station_id = src.I[0];
qint32 t;
memcpy(&t, data.data() + index, 4);
index += 4;
ins.lon = t * 0.0000001;
src.B[0] = data[data_count++];
src.B[1] = data[data_count++];
group.diff_age = src.I[0];
memcpy(&t, data.data() + index, 4);
index += 4;
ins.lat = t * 0.0000001;
savePosDataToFile(group); //把数据保存到文件中
emit pos_info(group);
memcpy(&t, data.data() + index, 4);
index += 4;
ins.alt_baro = t * 0.1;
memcpy(&t, data.data() + index, 4);
index += 4;
ins.alt_gps = t * 0.1;
memcpy(&t, data.data() + index, 4);
index += 4;
ins.alt = t * 0.1;
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
{
@@ -1735,10 +1497,10 @@ void Parse::run()
QByteArray data = raw.mid(214);
thruster thr;
thr.ignitionCommand1 = data[4];
thr.ignitionStatus1 = data[5];
thr.ignitionCommand2 = data[6];
thr.ignitionStatus2 = data[7];
thr.ignitionCommand1 = data[3];
thr.ignitionStatus1 = data[4];
thr.ignitionCommand2 = data[5];
thr.ignitionStatus2 = data[6];
emit thrusterInfo(thr);
}
App/ToolsUI/Parse.h
+87
View File
@@ -472,6 +472,91 @@ public:
quint16 diff_age;
}_pos;
typedef struct
{
quint16 GPSWeek;
quint32 tow_ms;
quint32 GPSWeek1;
qreal tow;
struct
{
bool XGyroState1;
bool YGyroState;
bool ZGyroState;
bool XAccelerometerState1;
bool YAccelerometerState;
bool ZAccelerometerState;
}IMUState;
qreal az;
qreal ax;
qreal ay;
qreal q;
qreal p;
qreal r;
}_100eIMU;
typedef struct
{
quint32 tick;
struct
{
int ahrs;
bool compassCalibration;
bool compass;
bool gyroscope;
bool additive;
bool barometer;
}state;
float pitch;
float roll;
float yaw;
float gps_yaw;
float pitch_rate;
float roll_rate;
float yaw_rate;
qreal lon;
qreal lat;
qreal alt_baro;
qreal alt_gps;
qreal alt;
float velocity_n;
float velocity_e;
float velocity_d;
float velocity_air;
float accel_n;
float accel_e;
float accel_d;
quint8 satellite_num;
float hdop;
float vdop;
QString gps_fixtype;
QString time;
qint8 temperature;
float gps_hdg;
float gps_hdg_dev;
struct
{
QString additive;
QString gyroscope;
QString compass;
QString gps;
}redundancy;
qint32 gps0_dt;
qint32 gps1_dt;
float gps_vn;
float gps_ve;
float gps_vd;
quint16 gps_msec;
quint8 gps_day;
quint16 gps_week;
QString work_state;
}_100eIns;
//记录数据文件的文件名
QString ecu_file_name;
QString ins_file_name;
@@ -518,6 +603,8 @@ signals:
void actuator2_info(Parse::_actuator2 info);
void imu_info(Parse::_imu info);
void _100eIMUInfo(Parse::_100eIMU info);
void _100eInsInfo(Parse::_100eIns info);
void euler_info(Parse::_euler info);
void vel_info(Parse::_vel info);
void pos_info(Parse::_pos info);
App/ToolsUI/ToolsUI.cpp
+14 -6
View File
@@ -236,11 +236,16 @@ void ToolsUI::recieveDataSlot(const int &id, const QByteArray &data)
connect(parse,&Parse::NAV_Info,
ins,&INS::NAV_Info);
connect(parse,&Parse::imu_info,
ins,&INS::imu);
// connect(parse,&Parse::imu_info,
// ins,&INS::imu);
// connect(parse,&Parse::euler_info,
// ins,&INS::euler);
connect(parse, &Parse::_100eIMUInfo,
ins, &INS::_100eIMUInfo);
connect(parse,&Parse::euler_info,
ins,&INS::euler);
connect(parse,&Parse::gear_Info,
gear,&landinggear::gear_Info);
@@ -303,12 +308,15 @@ void ToolsUI::recieveDataSlot(const int &id, const QByteArray &data)
connect(parse,&Parse::SSPC_Info_state,
ecu,&ECU::SSPC_Info_state);
connect(parse,&Parse::vel_info,
ins,&INS::vel);
// connect(parse,&Parse::vel_info,
// ins,&INS::vel);
connect(parse,&Parse::pos_info,
ins,&INS::pos);
connect(parse, &Parse::_100eInsInfo,
ins, &INS::_100eINSInfo);
connect(parse,&Parse::actuator_info,
gear,&landinggear::actuator_info);