printf some log

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -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<< ","