add speed plan log

src/decition/decition_brain_sf/decition/adc_tools/compute_00.cpp

@@ -212,7 +212,7 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
                 if (deltaphi<-PI)
                         {deltaphi=deltaphi+2*PI;}
                 distance=sqrt( pow((doubledata[i+1][0]-doubledata[i][0]),2)+pow((doubledata[i+1][1]-doubledata[i][1]),2));
-                doubledata[i][3]=-atan( 2.5* (deltaphi) / distance  );//车辆坐标系和惯导坐标系方向相反，delta变号
+                doubledata[i][3]=-atan( 4.0* (deltaphi) / distance  );//车辆坐标系和惯导坐标系方向相反，delta变号
 
                 MapTrace[i]->delta=doubledata[i][3];
     }
@@ -295,8 +295,6 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                     }else if(MapTrace[i]->roadMode==14){
                         mode0to18countSum=mode0to5flash+mode18flash;
                         mode18countSum=mode18count;
-                        mode0to5count=0;
-                        mode18count=0;
                         double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
                         double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
                         int brake_distanceLarge=(int)brake_distanceLarge_double;
@@ -315,7 +313,7 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 
                         }else if(mode0to18countSum>brake_distance0to18){
 
-                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
 
                                 for(int j=i;j>i-brake_distance0to18;j--){
                                     MapTrace[j]->plan_speed=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
@@ -325,19 +323,21 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                                 MapTrace[i]->plan_speed=Curve_LargeSpeed;
                         }
 
-
-
-
+                        mode0to5count=0;
+                        mode18count=0;
                         mode0to5flash=0;
                         mode18flash=0;
                     }
+                }
+
+                for(int i=0;i<MapTrace.size();i++){
                     //将数据写入文件开始
                     ofstream outfile;
                     outfile.open("ctr0108003.txt", ostream::app);        /*以添加模式打开文件*/
-                    outfile<<"MODE"<< ","  <<MapTrace[i]->plan_speed<< "," <<endl;
+                    outfile<<"Delta"<< ","  <<MapTrace[i]->delta<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
+                           <<"plan_speed"<< ","  << MapTrace[i]->plan_speed<< ","<<endl;
                     outfile.close();                                 /*关闭文件*/
                     //将数据写入文件结束
-
                 }
 
 }