Merge branch 'master' of http://116.63.46.168:3000/ADPilot/modularization

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.cpp

@@ -2193,7 +2193,7 @@ double iv::decition::Compute00::bocheComputeNew(GPS_INS nowGps, GPS_INS aimGps)
 
     double hdgrel = hdgnow - hdgaim;
 
-    VerticalParkCalc xCalc(pt.y,pt.x*(-1),hdgrel,ServiceCarStatus.mfMinRadius * 1.1,ServiceCarStatus.mfMaxWheel,45,ServiceCarStatus.mfParkLastStraightDis);
+    VerticalParkCalc xCalc(pt.y,pt.x*(-1),hdgrel,ServiceCarStatus.mfMinRadius ,ServiceCarStatus.mfMaxWheel,45,ServiceCarStatus.mfParkLastStraightDis);
     xCalc.CalcPark();
 
     std::vector<iv::VerticalParkPoint> xvectorverticalparkpoint;

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -531,7 +531,7 @@ void iv::decition::BrainDecition::run() {
                     decition_gps->brake = -2;//保证在进入自动驾驶的时候车辆能够泄压,否则进入自动驾驶车辆无法加速
                 }
 
-   //             decition_gps->brake = 0;
+//                decition_gps->brake = 0;
                 decition_gps->torque=0;
                 decition_gps->acc_active = 0;//ACC扭矩请求激活（为1激活，为0不激活），不激活后不执行扭矩请求，对于深蓝没什么用，因为控制模块直接给的1
                 decition_gps->brake_active = 0;//ACC减速激活（为1制动激活，为0制动不激活）
@@ -542,7 +542,7 @@ void iv::decition::BrainDecition::run() {
                // ServiceCarStatus.ang_debug=ServiceCarStatus.ang_debug*15.9;//如果xml中配置的是车轮转角
                 ServiceCarStatus.ang_debug=max((double)-430.0,ServiceCarStatus.ang_debug);
                 ServiceCarStatus.ang_debug=min((double)430.0,ServiceCarStatus.ang_debug);
-                decition_gps->wheel_angle = ServiceCarStatus.ang_debug;//= -430;// ServiceCarStatus.ang_debug;//0;
+                decition_gps->wheel_angle =ServiceCarStatus.ang_debug;//= -430;// ServiceCarStatus.ang_debug;//0;
 
 
                 double dSpeed=200;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -597,7 +597,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
         iv::Point2D ptfar = iv::decition::Coordinate_Transfer(Compute00().farTpoint.gps_x,Compute00().farTpoint.gps_y,aim_gps_ins);
         double xx = pt.y - ptfar.y;
-        if((((angdis<3)||(angdis>357)))||(xx<(-0.3)))
+        if((((angdis<3)||(angdis>357)))||(xx<(-0.5)))
             //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
         {
             vehState = reverseDirect;
@@ -731,7 +731,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
         Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint0.gps_x,Compute00().dTpoint0.gps_y, aim_gps_ins);
         double xx= (pt1.y-pt2.y);
-        if(xx < 0.1) //(dis<2.0)
+        double fsd = 0.6* secSpeed * secSpeed/(2*1.5);
+        if((xx < 0.01) ||(xx<fsd))//(dis<2.0)
         {
             vehState = dRever1;
             if(Compute00().mSideParkType == SideParkType::TwoStep)vehState = dRever4;

src/test/testsideparkcalc/mainwindow.cpp

@@ -1,6 +1,7 @@
 #include "mainwindow.h"
 #include "ui_mainwindow.h"
 
+#include  "math/gnss_coordinate_convert.h"
 
 
 #define VIEW_WIDTH 1300
@@ -42,6 +43,13 @@ MainWindow::MainWindow(QWidget *parent)
     QGraphicsLineItem * pliney = new QGraphicsLineItem(RATIO * X_MOVE,0,RATIO * X_MOVE,VIEW_HEIGHT);
     mpscene->addItem(pliney);
 
+    ui->lineEdit_ParkLon->setText("116.0");
+    ui->lineEdit_ParkLat->setText("39.0");
+    ui->lineEdit_ParkHeading->setText("90.0");
+    ui->lineEdit_NowLon->setText("116.00006");
+    ui->lineEdit_NowLat->setText("39.00005");
+    ui->lineEdit_NowHeading->setText("360.0");
+
     setWindowTitle("Calc Park   (if HDG>0.5236  Vertical Park , Else Side Park)");
 }
 
@@ -602,3 +610,57 @@ void MainWindow::on_pushButton_Calc_clicked()
     }
 }
 
+
+void MainWindow::on_pushButton_CalcXY_clicked()
+{
+    double lon_base,lat_base,heading_base;
+    double lon_test,lat_test,heading_test;
+
+    double x_base,y_base,hdg_base;
+    double x_test,y_test,hdg_test;
+
+    double x_rel,y_rel,hdg_rel;
+    double x_rel_raw,y_rel_raw;
+
+    double fDis;
+
+    lon_base = ui->lineEdit_ParkLon->text().toDouble();
+    lat_base = ui->lineEdit_ParkLat->text().toDouble();
+    heading_base = ui->lineEdit_ParkHeading->text().toDouble();
+
+    lon_test = ui->lineEdit_NowLon->text().toDouble();
+    lat_test = ui->lineEdit_NowLat->text().toDouble();
+    heading_test = ui->lineEdit_NowHeading->text().toDouble();
+
+
+    GaussProjCal(lon_base,lat_base,&x_base,&y_base);
+    GaussProjCal(lon_test,lat_test,&x_test,&y_test);
+
+
+    x_rel_raw = x_test - x_base;
+    y_rel_raw = y_test - y_base;
+
+    hdg_base = (90-heading_base)*M_PI/180.0;
+    normalhdg(hdg_base);
+
+    hdg_test = (90-heading_test)*M_PI/180.0;
+    normalhdg(hdg_test);
+
+    hdg_rel = hdg_test - hdg_base;
+    normalhdg(hdg_rel);
+
+    x_rel = x_rel_raw * cos(-hdg_base) - y_rel_raw * sin(-hdg_base);
+    y_rel = x_rel_raw * sin(-hdg_base) + y_rel_raw * cos(-hdg_base);
+
+    ui->lineEdit_NowX->setText(QString::number(x_rel,'f',3));
+    ui->lineEdit_NowY->setText(QString::number(y_rel,'f',3));
+    ui->lineEdit_NowHdg->setText(QString::number(hdg_rel,'f',3));
+
+}
+
+void MainWindow::normalhdg(double & hdg)
+{
+    while(hdg >= (2.0*M_PI))hdg = hdg - M_PI * 2.0;
+    while(hdg < 0)hdg  = hdg  + M_PI * 2.0;
+}
+

src/test/testsideparkcalc/mainwindow.h

@@ -25,6 +25,8 @@ public:
 private slots:
     void on_pushButton_Calc_clicked();
 
+    void on_pushButton_CalcXY_clicked();
+
 private:
     double mfRadius = 5.0;
     double mfMaxWheel = 430.0;
@@ -49,5 +51,8 @@ private:
     void SidePark(double x,double y ,double hdg, double fMinRadius,double fMaxWheel,double fStraightDis);
     void VerticalPark(double x,double y ,double hdg, double fMinRadius,double fMaxWheel,double fStraightDis);
 
+private:
+    void normalhdg(double & hdg);
+
 };
 #endif // MAINWINDOW_H

src/test/testsideparkcalc/mainwindow.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1280</width>
+    <width>1920</width>
     <height>800</height>
    </rect>
   </property>
@@ -17,14 +17,14 @@
    <widget class="QPushButton" name="pushButton_Calc">
     <property name="geometry">
      <rect>
-      <x>1010</x>
+      <x>760</x>
       <y>30</y>
-      <width>171</width>
+      <width>101</width>
       <height>41</height>
      </rect>
     </property>
     <property name="text">
-     <string>Calc</string>
+     <string>Calc Park</string>
     </property>
    </widget>
    <widget class="QLineEdit" name="lineEdit_NowX">
@@ -53,7 +53,7 @@
    <widget class="QLineEdit" name="lineEdit_NowY">
     <property name="geometry">
      <rect>
-      <x>440</x>
+      <x>371</x>
       <y>17</y>
       <width>121</width>
       <height>31</height>
@@ -66,7 +66,7 @@
    <widget class="QLabel" name="label_2">
     <property name="geometry">
      <rect>
-      <x>350</x>
+      <x>281</x>
       <y>12</y>
       <width>91</width>
       <height>31</height>
@@ -79,7 +79,7 @@
    <widget class="QLabel" name="label_3">
     <property name="geometry">
      <rect>
-      <x>650</x>
+      <x>509</x>
       <y>10</y>
       <width>91</width>
       <height>31</height>
@@ -92,7 +92,7 @@
    <widget class="QLineEdit" name="lineEdit_NowHdg">
     <property name="geometry">
      <rect>
-      <x>740</x>
+      <x>613</x>
       <y>13</y>
       <width>121</width>
       <height>31</height>
@@ -125,7 +125,7 @@
    <widget class="QLineEdit" name="lineEdit_MaxWheel">
     <property name="geometry">
      <rect>
-      <x>440</x>
+      <x>371</x>
       <y>60</y>
       <width>121</width>
       <height>31</height>
@@ -135,7 +135,7 @@
    <widget class="QLineEdit" name="lineEdit_StraightDis">
     <property name="geometry">
      <rect>
-      <x>741</x>
+      <x>614</x>
       <y>60</y>
       <width>121</width>
       <height>31</height>
@@ -152,33 +152,190 @@
      </rect>
     </property>
     <property name="text">
-     <string>转弯半径：</string>
+     <string>Radius:</string>
     </property>
    </widget>
    <widget class="QLabel" name="label_5">
     <property name="geometry">
      <rect>
-      <x>350</x>
+      <x>281</x>
       <y>60</y>
       <width>91</width>
       <height>31</height>
      </rect>
     </property>
     <property name="text">
-     <string>转向角度：</string>
+     <string>Wheel:</string>
     </property>
    </widget>
    <widget class="QLabel" name="label_6">
     <property name="geometry">
      <rect>
-      <x>650</x>
+      <x>507</x>
       <y>60</y>
+      <width>101</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Last Straight:</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_7">
+    <property name="geometry">
+     <rect>
+      <x>927</x>
+      <y>16</y>
+      <width>91</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Park Lon:</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_ParkHeading">
+    <property name="geometry">
+     <rect>
+      <x>1520</x>
+      <y>14</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string/>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_8">
+    <property name="geometry">
+     <rect>
+      <x>1178</x>
+      <y>61</y>
+      <width>91</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Now Lat:</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_9">
+    <property name="geometry">
+     <rect>
+      <x>1396</x>
+      <y>61</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Now Heading:</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_NowLat">
+    <property name="geometry">
+     <rect>
+      <x>1262</x>
+      <y>61</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_10">
+    <property name="geometry">
+     <rect>
+      <x>1178</x>
+      <y>13</y>
+      <width>91</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Park Lat:</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_NowLon">
+    <property name="geometry">
+     <rect>
+      <x>1020</x>
+      <y>61</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_ParkLon">
+    <property name="geometry">
+     <rect>
+      <x>1020</x>
+      <y>13</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_11">
+    <property name="geometry">
+     <rect>
+      <x>927</x>
+      <y>61</y>
       <width>91</width>
       <height>31</height>
      </rect>
     </property>
     <property name="text">
-     <string>直线距离：</string>
+     <string>Now Lon:</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_12">
+    <property name="geometry">
+     <rect>
+      <x>1396</x>
+      <y>11</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Park Heading:</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_ParkLat">
+    <property name="geometry">
+     <rect>
+      <x>1261</x>
+      <y>18</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string/>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_NowHeading">
+    <property name="geometry">
+     <rect>
+      <x>1521</x>
+      <y>61</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="pushButton_CalcXY">
+    <property name="geometry">
+     <rect>
+      <x>1670</x>
+      <y>35</y>
+      <width>121</width>
+      <height>41</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Calc XY</string>
     </property>
    </widget>
   </widget>
@@ -187,13 +344,30 @@
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>1280</width>
+     <width>1920</width>
      <height>27</height>
     </rect>
    </property>
   </widget>
   <widget class="QStatusBar" name="statusbar"/>
  </widget>
+ <tabstops>
+  <tabstop>lineEdit_NowX</tabstop>
+  <tabstop>lineEdit_NowY</tabstop>
+  <tabstop>lineEdit_NowHdg</tabstop>
+  <tabstop>lineEdit_MinRadius</tabstop>
+  <tabstop>lineEdit_MaxWheel</tabstop>
+  <tabstop>lineEdit_StraightDis</tabstop>
+  <tabstop>pushButton_Calc</tabstop>
+  <tabstop>lineEdit_ParkLon</tabstop>
+  <tabstop>lineEdit_ParkLat</tabstop>
+  <tabstop>lineEdit_ParkHeading</tabstop>
+  <tabstop>lineEdit_NowLon</tabstop>
+  <tabstop>lineEdit_NowLat</tabstop>
+  <tabstop>lineEdit_NowHeading</tabstop>
+  <tabstop>pushButton_CalcXY</tabstop>
+  <tabstop>plainTextEdit</tabstop>
+ </tabstops>
  <resources/>
  <connections/>
 </ui>

src/test/testsideparkcalc/testsideparkcalc.pro

@@ -12,12 +12,14 @@ SOURCES += \
     main.cpp \
     mainwindow.cpp \
     myview.cpp \
+    ../../common/common/math/gnss_coordinate_convert.cpp \
     sideparkcalc.cpp \
     verticalparkcalc.cpp
 
 HEADERS += \
     mainwindow.h \
     myview.h \
+    ../../common/common/math/gnss_coordinate_convert.h \
     sideparkcalc.h \
     verticalparkcalc.h
 
@@ -28,3 +30,5 @@ FORMS += \
 qnx: target.path = /tmp/$${TARGET}/bin
 else: unix:!android: target.path = /opt/$${TARGET}/bin
 !isEmpty(target.path): INSTALLS += target
+
+INCLUDEPATH += $$PWD/../../common/common