|
|
@@ -2954,6 +2954,8 @@ else
|
|
|
else
|
|
|
obsDistance_log=obsDistance;
|
|
|
QDateTime dt2=QDateTime::currentDateTime();
|
|
|
+ double disToEndTrace = GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
|
|
|
+ double disToParkPoint = GetDistance(now_gps_ins, aim_gps_ins);
|
|
|
outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
|
|
|
<<"Decide_Dspeed" << "," <<setprecision(2)<<dSpeed<< ","
|
|
|
<<"Decide_ACC" << "," <<setprecision(2)<<gps_decition->torque<< ","
|
|
|
@@ -2961,6 +2963,8 @@ else
|
|
|
<<"Vehicle_RealSpd"<< "," <<setprecision(2)<<now_gps_ins.speed<< ","
|
|
|
// <<"readyParkMode"<< ","<<readyParkMode<< ","
|
|
|
// <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
|
|
|
+ <<"trace_disToEndTrace"<< ","<<disToEndTrace<< ","
|
|
|
+ <<"disToParkPoint"<< ","<<disToParkPoint<< ","
|
|
|
<<"OBS_Distance"<< ","<<obsDistance_log<< ","
|
|
|
<<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
|
|
|
<<"obsSpeed_fusion"<<","<<obsSpeed<<","
|
|
|
@@ -2981,6 +2985,12 @@ else
|
|
|
<<"Vehicle_GPS_lng"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
|
|
|
<<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_x<< ","
|
|
|
<<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_y<< ","
|
|
|
+
|
|
|
+ <<"aim_GPS_lat"<< ","<< setprecision(10)<<aim_gps_ins.gps_lat<< ","
|
|
|
+ <<"aim_GPS_lng"<< ","<< setprecision(10)<<aim_gps_ins.gps_lng<< ","
|
|
|
+ <<"aim_GPS_X"<< ","<< setprecision(10)<<aim_gps_ins.gps_x<< ","
|
|
|
+ <<"aim_GPS_Y"<< ","<< setprecision(10)<<aim_gps_ins.gps_y<< ","
|
|
|
+
|
|
|
<<"MAP_GPS_heading"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->ins_heading_angle<< ","
|
|
|
<<"MAP_GPS_X"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_x<< ","
|
|
|
<<"MAP_GPS_Y"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_y<< ","
|
chenxiaowei