diff --git a/MavLinkNode/mavlinknode.cpp b/MavLinkNode/mavlinknode.cpp index e27d487..01ae2eb 100644 --- a/MavLinkNode/mavlinknode.cpp +++ b/MavLinkNode/mavlinknode.cpp @@ -551,33 +551,32 @@ void MavLinkNode::process()//线程函数 { QThread::msleep(5000);//5s后再发送 - setFileData(autopilot_version_file,QByteArray("capabilities,flight_sw_version,middleware_sw_version,os_sw_version,board_version,uid,vendor_id,product_id,flight_custom_version[8],middleware_custom_version[8],os_custom_version[8],uid2[18]\n")); - setFileData(sys_status_file, QByteArray("present,enabled,health,load,voltage,current,remaining,drop_rate_comm,errors_comm,errors_count1,errors_count2,errors_count3,errors_count4\n")); - setFileData(heartbeat_file, QByteArray("type,autopilot,base_mode,custom_mode,system_status,mavlink\n")); - setFileData(ping_file, QByteArray("time_usec,seq,target_system,target_component\n")); - setFileData(attitude_file, QByteArray("time_boot_ms,roll,pitch,yaw,rollspeed,pitchspeed,yawspeed\n")); - setFileData(ins1_file, QByteArray("time_boot_ms,pitch,roll,yaw,lon,lat,alt,v_north,v_up,v_east,gx,gy,gz,ax,ay,az,time,sys,com,gps,bit,seq,eph,epv,svn\n")); - setFileData(ins2_file, QByteArray("time_boot_ms,pitch,roll,yaw,lon,lat,alt,v_north,v_up,v_east,gx,gy,gz,ax,ay,az,time,sys,com,gps,bit,seq,eph,epv,svn\n")); - setFileData(gps_raw_int_file, QByteArray("time_usec,lat,lon,alt,eph,epv,vel,cog,fix_type,satellites_visible,alt_ellipsoid,h_acc,v_acc,vel_acc,hdg_acc,yaw\n")); - setFileData(global_position_int_file,QByteArray("time_boot_ms,lat,lon,alt,relative_alt,vx,vy,vz,hdg\n")); - setFileData(servo_output_raw_file, QByteArray("time_usec,port,ch1,ch2,ch3,ch4,ch5,ch6,ch7,ch8,ch9,ch10,ch11,ch12,ch13,ch14,ch15,ch16\n")); - setFileData(rc_channels_raw_file, QByteArray("time_boot_ms,port,rssi,chan1_raw,chan2_raw,chan3_raw,chan4_raw,chan5_raw,chan6_raw,chan7_raw,chan8_raw,chan9_raw,chan10_raw,chan11_raw,chan12_raw,chan13_raw,chan14_raw,chan15_raw,chan16_raw\n")); - setFileData(nav_controller_output_file,QByteArray("nav_roll,nav_pitch,nav_bearing,target_bearing,wp_dist,alt_err,as_err,xtrack_err\n")); - setFileData(airspeed_autocal_file, QByteArray("vx,vy,vz,diff_pressure,EAS2TAS,ratio,state_x,state_y,state_z,Pax,Pby,Pcz\n")); - setFileData(rpm_file, QByteArray("rpm1,rpm2,rpm3,rpm4,rpm5\n")); - setFileData(scaled_pressure_file, QByteArray("time_boot_ms,press_abs,press_diff,temperature,temperature_diff\n")); - setFileData(extended_sys_state_file,QByteArray("vtol_state,landed_state\n")); - setFileData(battery_status_file, QByteArray("current_consumed,energy_consumed,temperature,voltages[0],voltages[1],voltages[2],voltages[3],voltages[4],voltages[5],voltages[6],voltages[7],voltages[8],voltages[9],current_battery,id,battery_function,type,battery_remaining,time_remaining,charge_state,voltages_ext[0],voltages_ext[1],voltages_ext[2],voltages_ext[3]\n")); - setFileData(vibration_file, QByteArray("")); - setFileData(enginestate_file, QByteArray("time_boot_ms,ChokeFlag,Ignition2Flag,AmbientTemperatur,AirPressure,ActualFuelPressure,FuelPumpDutyCycle,ActualJet1DutyCycle,ActualRPM,CHTemperature1,counts\n")); - setFileData(vfr_hud_file, QByteArray("airspeed,groundspeed,alt,climb,heading,throttle\n")); - setFileData(aoa_ssa_file, QByteArray("")); - setFileData(emb_atom_com_file, QByteArray("time_boot_ms,airspeed,beta,alpha,ps,qbar,seq,mach\n")); - setFileData(turbinstate_file, QByteArray("time_boot_ms,RPM_mea,T5,Kfuel,RPM_des,RPM_des_ap,RPM_bak,IOState,SysState,Fault,stage_ap,temp_ap,tas_ap,asl_ap,KabMain,KabFire,KDj,T1t,P1t,P3t,P5t,DJS,Vcc,Tbak,rev,CFuelMode,Cmd\n")); - setFileData(bmustate_file, QByteArray("time_boot_ms,BAT1_group_voltage_mv,BAT1_group_current_dA,BAT1_remain_perc,BAT1_low_temp_degC,AT1_hi_temp_degC,BAT1_voltages_mv[7],BAT1_hi_voltage_mv,BAT1_low_voltage_mv,BAT2_group_voltage_mv,BAT2_group_current_dA,BAT2_remain_perc,BAT2_low_temp_degC,BAT2_hi_temp_degC,BAT2_voltages_mv[14],BAT2_hi_voltage_mv,BAT2_low_voltage_mv,BAT1_STA1,BAT1_STA2,BAT2_STA1,BAT2_STA2,p500w_enabled\n")); - setFileData(ccmstate_file, QByteArray("time_boot_ms,fuel_level,temp[0],temp[1],temp[2],temp[3],volts[0],volts[1],volts[2],volts[3],echo_seq\n")); - setFileData(serial_control_file, QByteArray("baudrate,timeout,device,flags,count,data\n")); - + setFileData(autopilot_version_file, QByteArray("year,mon,day,hour,min,sec,ms,capabilities,flight_sw_version,middleware_sw_version,os_sw_version,board_version,uid,vendor_id,product_id,flight_custom_version[8],middleware_custom_version[8],os_custom_version[8],uid2[18]\n")); + setFileData(sys_status_file, QByteArray("year,mon,day,hour,min,sec,ms,present,enabled,health,load,voltage,current,remaining,drop_rate_comm,errors_comm,errors_count1,errors_count2,errors_count3,errors_count4\n")); + setFileData(heartbeat_file, QByteArray("year,mon,day,hour,min,sec,ms,type,autopilot,base_mode,custom_mode,system_status,mavlink\n")); + setFileData(ping_file, QByteArray("year,mon,day,hour,min,sec,ms,time_usec,seq,target_system,target_component\n")); + setFileData(attitude_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,roll,pitch,yaw,rollspeed,pitchspeed,yawspeed\n")); + setFileData(ins1_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,pitch,roll,yaw,lon,lat,alt,v_north,v_up,v_east,gx,gy,gz,ax,ay,az,time,sys,com,gps,bit,seq,eph,epv,svn\n")); + setFileData(ins2_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,pitch,roll,yaw,lon,lat,alt,v_north,v_up,v_east,gx,gy,gz,ax,ay,az,time,sys,com,gps,bit,seq,eph,epv,svn\n")); + setFileData(gps_raw_int_file, QByteArray("year,mon,day,hour,min,sec,ms,time_usec,lat,lon,alt,eph,epv,vel,cog,fix_type,satellites_visible,alt_ellipsoid,h_acc,v_acc,vel_acc,hdg_acc,yaw\n")); + setFileData(global_position_int_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,lat,lon,alt,relative_alt,vx,vy,vz,hdg\n")); + setFileData(servo_output_raw_file, QByteArray("year,mon,day,hour,min,sec,ms,time_usec,port,ch1,ch2,ch3,ch4,ch5,ch6,ch7,ch8,ch9,ch10,ch11,ch12,ch13,ch14,ch15,ch16\n")); + setFileData(rc_channels_raw_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,port,rssi,chan1_raw,chan2_raw,chan3_raw,chan4_raw,chan5_raw,chan6_raw,chan7_raw,chan8_raw,chan9_raw,chan10_raw,chan11_raw,chan12_raw,chan13_raw,chan14_raw,chan15_raw,chan16_raw\n")); + setFileData(nav_controller_output_file, QByteArray("year,mon,day,hour,min,sec,ms,nav_roll,nav_pitch,nav_bearing,target_bearing,wp_dist,alt_err,as_err,xtrack_err\n")); + setFileData(airspeed_autocal_file, QByteArray("year,mon,day,hour,min,sec,ms,vx,vy,vz,diff_pressure,EAS2TAS,ratio,state_x,state_y,state_z,Pax,Pby,Pcz\n")); + setFileData(rpm_file, QByteArray("year,mon,day,hour,min,sec,ms,rpm1,rpm2,rpm3,rpm4,rpm5\n")); + setFileData(scaled_pressure_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,press_abs,press_diff,temperature,temperature_diff\n")); + setFileData(extended_sys_state_file, QByteArray("year,mon,day,hour,min,sec,ms,vtol_state,landed_state\n")); + setFileData(battery_status_file, QByteArray("year,mon,day,hour,min,sec,ms,current_consumed,energy_consumed,temperature,voltages[0],voltages[1],voltages[2],voltages[3],voltages[4],voltages[5],voltages[6],voltages[7],voltages[8],voltages[9],current_battery,id,battery_function,type,battery_remaining,time_remaining,charge_state,voltages_ext[0],voltages_ext[1],voltages_ext[2],voltages_ext[3]\n")); + setFileData(vibration_file, QByteArray("year,mon,day,hour,min,sec,ms,time_usec,vibration_x,vibration_y,vibration_z,clipping_0,clipping_1,clipping_2\n")); + setFileData(enginestate_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,ChokeFlag,Ignition2Flag,AmbientTemperatur,AirPressure,ActualFuelPressure,FuelPumpDutyCycle,ActualJet1DutyCycle,ActualRPM,CHTemperature1,counts\n")); + setFileData(vfr_hud_file, QByteArray("year,mon,day,hour,min,sec,ms,airspeed,groundspeed,alt,climb,heading,throttle\n")); + setFileData(aoa_ssa_file, QByteArray("year,mon,day,hour,min,sec,ms,time_usec,AOA,SSA\n")); + setFileData(emb_atom_com_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,airspeed,beta,alpha,ps,qbar,seq,mach\n")); + setFileData(turbinstate_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,RPM_mea,T5,Kfuel,RPM_des,RPM_des_ap,RPM_bak,IOState,SysState,Fault,stage_ap,temp_ap,tas_ap,asl_ap,KabMain,KabFire,KDj,T1t,P1t,P3t,P5t,DJS,Vcc,Tbak,rev,CFuelMode,Cmd\n")); + setFileData(bmustate_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,BAT1_group_voltage_mv,BAT1_group_current_dA,BAT1_remain_perc,BAT1_low_temp_degC,AT1_hi_temp_degC,BAT1_voltages_mv[7],BAT1_hi_voltage_mv,BAT1_low_voltage_mv,BAT2_group_voltage_mv,BAT2_group_current_dA,BAT2_remain_perc,BAT2_low_temp_degC,BAT2_hi_temp_degC,BAT2_voltages_mv[14],BAT2_hi_voltage_mv,BAT2_low_voltage_mv,BAT1_STA1,BAT1_STA2,BAT2_STA1,BAT2_STA2,p500w_enabled\n")); + setFileData(ccmstate_file, QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,fuel_level,temp[0],temp[1],temp[2],temp[3],volts[0],volts[1],volts[2],volts[3],echo_seq\n")); + setFileData(serial_control_file, QByteArray("year,mon,day,hour,min,sec,ms,baudrate,timeout,device,flags,count,data\n")); qDebug() << "mavlinknode" << QThread::currentThreadId(); @@ -701,7 +700,7 @@ bool MavLinkNode::setLogData(mavlink_message_t msg) uint16_t len = mavlink_msg_to_send_buffer(buff+sizeof(quint64), &msg); if (mavLogFile) { - auto size = Packer2_pack(&packer,1,buff,len); + auto size = Packer2_pack(&packer,1,buff,len+sizeof(quint64)); QByteArray data; @@ -943,13 +942,24 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) vehicle.sysid = msg.sysid; vehicle.compid = msg.compid; + //qDebug() << LocationTime.date() << LocationTime.time(); + switch (msg.msgid) { case MAVLINK_MSG_ID_AUTOPILOT_VERSION: { mavlink_msg_autopilot_version_decode(&msg,&vehicle.autopilot_version); + //gpsTimer.ms = QDateTime::currentDateTimeUtc().; + QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.autopilot_version.capabilities)); csvdata.append(','); csvdata.append(QString::number(vehicle.autopilot_version.flight_sw_version)); csvdata.append(','); csvdata.append(QString::number(vehicle.autopilot_version.middleware_sw_version)); csvdata.append(','); @@ -973,6 +983,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.sys_status.onboard_control_sensors_present)); csvdata.append(','); csvdata.append(QString::number(vehicle.sys_status.onboard_control_sensors_enabled)); csvdata.append(','); csvdata.append(QString::number(vehicle.sys_status.onboard_control_sensors_health)); csvdata.append(','); @@ -1004,6 +1021,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.heartbeat.type)); csvdata.append(','); csvdata.append(QString::number(vehicle.heartbeat.autopilot)); csvdata.append(','); csvdata.append(QString::number(vehicle.heartbeat.base_mode)); csvdata.append(','); @@ -1022,10 +1046,17 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.ping.time_usec)); csvdata.append(','); csvdata.append(QString::number(vehicle.ping.seq)); csvdata.append(','); csvdata.append(QString::number(vehicle.ping.target_system)); csvdata.append(','); - csvdata.append(QString::number(vehicle.ping.target_component)); csvdata.append(','); + csvdata.append(QString::number(vehicle.ping.target_component)); csvdata.append('\n'); setFileData(ping_file,csvdata); @@ -1036,6 +1067,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.attitude.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.attitude.roll)); csvdata.append(','); csvdata.append(QString::number(vehicle.attitude.pitch)); csvdata.append(','); @@ -1053,6 +1091,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.ins1.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.ins1.pitch)); csvdata.append(','); csvdata.append(QString::number(vehicle.ins1.roll)); csvdata.append(','); @@ -1090,6 +1135,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.ins2.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.ins2.pitch)); csvdata.append(','); csvdata.append(QString::number(vehicle.ins2.roll)); csvdata.append(','); @@ -1120,6 +1172,43 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) emit signal_ins2(vehicle.ins2); + + uint64_t time = ((uint64_t)vehicle.ins2.time)% 1000000; + uint64_t date = ((uint64_t)vehicle.ins2.time)/ 1000000; + + + + uint16_t year = date / 10000; + uint8_t mon = (date % 10000)/100; + uint8_t day = date % 100; + + + + uint8_t hour = time / 10000; + + if(hour >= 24){ + hour -= 24; + } + + uint8_t min = (time % 10000)/100; + uint8_t sec = time % 100; + + gpsTimer.year = year; + gpsTimer.mon = mon; + gpsTimer.day = day; + + gpsTimer.hour = hour; + gpsTimer.min = min; + gpsTimer.sec = sec; + + + LocationTime.addYears(year); + LocationTime.addMonths(mon); + LocationTime.addDays(day); + //LocationTime.setTime(QTime(hour,min,sec)); + + + }break; case MAVLINK_MSG_ID_GPS_RAW_INT: { mavlink_msg_gps_raw_int_decode(&msg,&vehicle.gps_raw_int); @@ -1127,6 +1216,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.gps_raw_int.time_usec)); csvdata.append(','); csvdata.append(QString::number(vehicle.gps_raw_int.lat)); csvdata.append(','); csvdata.append(QString::number(vehicle.gps_raw_int.lon)); csvdata.append(','); @@ -1153,6 +1249,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.global_position_int.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.global_position_int.lat)); csvdata.append(','); csvdata.append(QString::number(vehicle.global_position_int.lon)); csvdata.append(','); @@ -1172,6 +1275,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.servo_output_raw.time_usec)); csvdata.append(','); csvdata.append(QString::number(vehicle.servo_output_raw.port)); csvdata.append(','); csvdata.append(QString::number(vehicle.servo_output_raw.servo1_raw)); csvdata.append(','); @@ -1204,6 +1314,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.rc_channels_raw.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.rc_channels_raw.port)); csvdata.append(','); csvdata.append(QString::number(vehicle.rc_channels_raw.rssi)); csvdata.append(','); @@ -1233,6 +1350,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.nav_controller_output.nav_roll)); csvdata.append(','); csvdata.append(QString::number(vehicle.nav_controller_output.nav_pitch)); csvdata.append(','); csvdata.append(QString::number(vehicle.nav_controller_output.nav_bearing)); csvdata.append(','); @@ -1249,6 +1373,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.airspeed_autocal.vx)); csvdata.append(','); csvdata.append(QString::number(vehicle.airspeed_autocal.vy)); csvdata.append(','); csvdata.append(QString::number(vehicle.airspeed_autocal.vz)); csvdata.append(','); @@ -1260,7 +1391,7 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) csvdata.append(QString::number(vehicle.airspeed_autocal.state_z)); csvdata.append(','); csvdata.append(QString::number(vehicle.airspeed_autocal.Pax)); csvdata.append(','); csvdata.append(QString::number(vehicle.airspeed_autocal.Pby)); csvdata.append(','); - csvdata.append(QString::number(vehicle.airspeed_autocal.Pcz)); csvdata.append(','); + csvdata.append(QString::number(vehicle.airspeed_autocal.Pcz)); csvdata.append('\n'); setFileData(airspeed_autocal_file,csvdata); }break; @@ -1270,6 +1401,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.rpm.rpm1)); csvdata.append(','); csvdata.append(QString::number(vehicle.rpm.rpm2)); csvdata.append(','); csvdata.append(QString::number(vehicle.rpm.rpm3)); csvdata.append(','); @@ -1284,6 +1422,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.scaled_pressure.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.scaled_pressure.press_abs)); csvdata.append(','); csvdata.append(QString::number(vehicle.scaled_pressure.press_diff)); csvdata.append(','); @@ -1298,6 +1443,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.extended_sys_state.vtol_state)); csvdata.append(','); csvdata.append(QString::number(vehicle.extended_sys_state.landed_state)); csvdata.append('\n'); @@ -1309,6 +1461,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.battery_status.current_consumed)); csvdata.append(','); csvdata.append(QString::number(vehicle.battery_status.energy_consumed)); csvdata.append(','); csvdata.append(QString::number(vehicle.battery_status.temperature)); csvdata.append(','); @@ -1352,6 +1511,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.enginestate.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.enginestate.ChokeFlag)); csvdata.append(','); csvdata.append(QString::number(vehicle.enginestate.Ignition2Flag)); csvdata.append(','); @@ -1372,6 +1538,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.vfr_hud.airspeed)); csvdata.append(','); csvdata.append(QString::number(vehicle.vfr_hud.groundspeed)); csvdata.append(','); csvdata.append(QString::number(vehicle.vfr_hud.alt)); csvdata.append(','); @@ -1387,6 +1560,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.emb_atom_com.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.emb_atom_com.Airspeed)); csvdata.append(','); csvdata.append(QString::number(vehicle.emb_atom_com.beta)); csvdata.append(','); @@ -1405,6 +1585,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.turbinstate.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.turbinstate.RPM_mea)); csvdata.append(','); csvdata.append(QString::number(vehicle.turbinstate.T5)); csvdata.append(','); @@ -1441,6 +1628,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.bmustate.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.bmustate.BAT1_group_voltage_mv)); csvdata.append(','); csvdata.append(QString::number(vehicle.bmustate.BAT1_group_current_dA)); csvdata.append(','); @@ -1471,6 +1665,13 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) QByteArray csvdata; csvdata.clear(); + csvdata.append(QString::number(gpsTimer.year)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.mon)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.day)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.hour)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.min)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.sec)); csvdata.append(','); + csvdata.append(QString::number(gpsTimer.ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.ccmstate.time_boot_ms)); csvdata.append(','); csvdata.append(QString::number(vehicle.ccmstate.fuel_level)); csvdata.append(','); csvdata.append(QString::number(vehicle.ccmstate.temp[0])); csvdata.append(','); diff --git a/MavLinkNode/mavlinknode.h b/MavLinkNode/mavlinknode.h index 58434e9..1a4a5bc 100644 --- a/MavLinkNode/mavlinknode.h +++ b/MavLinkNode/mavlinknode.h @@ -34,6 +34,19 @@ class MavLinkNode : public ThreadTemplet Q_OBJECT public: + typedef struct + { + quint16 year; + quint8 mon; + quint8 day; + + quint8 hour; + quint8 min; + quint8 sec; + quint16 ms; + }_gpsTimer; + + typedef struct { int sysid = 0; /* ID of message sender system/aircraft */ int compid = 0; /* ID of the message sender component */ @@ -73,6 +86,7 @@ public: explicit MavLinkNode(QObject *parent = nullptr); ~MavLinkNode(); + _gpsTimer gpsTimer; _vehicle vehicle;//没有初始化，所以会有野值 QHash vehicleList; @@ -253,6 +267,8 @@ protected: QDateTime startuptime; + QDateTime LocationTime; + QTimer *gdt_timer = nullptr; QTimer *timer = nullptr; @@ -278,6 +294,10 @@ protected: mutable QReadWriteLock RWlock; + + + + }; #endif // MAVLINKNODE_H diff --git a/MavLinkNode/replay.cpp b/MavLinkNode/replay.cpp index 8ea0876..452d914 100644 --- a/MavLinkNode/replay.cpp +++ b/MavLinkNode/replay.cpp @@ -36,34 +36,31 @@ void Replay::process()//线程函数 if(position <= file->size()) { QByteArray data; - QByteArray time; + file->seek(position); - //file->seek(position); - - data = file->readAll(); - //data = file->read(MAVLINK_MAX_PACKET_LEN + sizeof(quint64)); + //data = file->readAll(); + data = file->read(255); for (QByteArray::const_iterator i = data.cbegin(); i != data.cend(); ++i) { + position ++; + if(0x01 == Parser2_char(&parser,*i)) { QByteArray raw; + QByteArray time; + raw.setRawData((const char*)parser.buff,parser.len); - if(raw.indexOf(0xFD) >= 8) - { - time = raw.mid(raw.indexOf(0xFD) - 8,8); + /* + QString hex; + for (int var = 0; var < raw.size(); ++var) { + hex.append(QString::number((uint8_t)raw[var],16).toUpper() + " "); } - else - { - position -= 8 - raw.indexOf(0xFD); + qDebug() << hex << parser.len << endl; + */ - file->seek(position); - - raw = file->read(MAVLINK_MAX_PACKET_LEN + sizeof(quint64)); - - time = raw.mid(raw.indexOf(0xFD) - 8,8); - } + time = raw.left(8); union {uint8_t B[8];uint64_t DW;}src; @@ -107,10 +104,12 @@ void Replay::process()//线程函数 } - position += (uint8_t)raw.at(raw.indexOf(0xFD)+1) + 20; + buff.clear(); - buff = raw.mid(raw.indexOf(0xFD),(uint8_t)raw.at(raw.indexOf(0xFD) +1) + 12); + buff = raw.mid(8); + + //qDebug() << QString::number((uint8_t)buff[0],16).toUpper(); //读取一帧数据 //将百分比增加，或者位置增加 @@ -120,7 +119,6 @@ void Replay::process()//线程函数 lastTimestamp = currentTimestamp; - break; } }