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

src/test/testsideparkcalc/.gitignore

@@ -0,0 +1,74 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+CMakeLists.txt.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/test/testsideparkcalc/main.cpp

@@ -0,0 +1,11 @@
+#include "mainwindow.h"
+
+#include <QApplication>
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+    MainWindow w;
+    w.show();
+    return a.exec();
+}

src/test/testsideparkcalc/mainwindow.cpp

@@ -0,0 +1,32 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+
+#include "sideparkcalc.h"
+
+MainWindow::MainWindow(QWidget *parent)
+    : QMainWindow(parent)
+    , ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+
+    ui->lineEdit_NowX->setText("9.0");
+    ui->lineEdit_NowY->setText("3.0");
+    ui->lineEdit_NowHdg->setText("0.0");
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+}
+
+
+void MainWindow::on_pushButton_Calc_clicked()
+{
+    double x = ui->lineEdit_NowX->text().toDouble();
+    double y = ui->lineEdit_NowY->text().toDouble();
+    double hdg = ui->lineEdit_NowHdg->text().toDouble();
+
+    SideParkCalc xPark(x,y,hdg);
+    xPark.CalcPark();
+}
+

src/test/testsideparkcalc/mainwindow.h

@@ -0,0 +1,26 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+
+QT_BEGIN_NAMESPACE
+namespace Ui { class MainWindow; }
+QT_END_NAMESPACE
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    MainWindow(QWidget *parent = nullptr);
+    ~MainWindow();
+
+    void CalcSidePark(double x,double y,double hdg);
+
+private slots:
+    void on_pushButton_Calc_clicked();
+
+private:
+    Ui::MainWindow *ui;
+};
+#endif // MAINWINDOW_H

src/test/testsideparkcalc/mainwindow.ui

@@ -0,0 +1,120 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>800</width>
+    <height>600</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <widget class="QWidget" name="centralwidget">
+   <widget class="QPushButton" name="pushButton_Calc">
+    <property name="geometry">
+     <rect>
+      <x>270</x>
+      <y>120</y>
+      <width>171</width>
+      <height>41</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Calc</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_NowX">
+    <property name="geometry">
+     <rect>
+      <x>123</x>
+      <y>40</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>40</y>
+      <width>91</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Now X:</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_NowY">
+    <property name="geometry">
+     <rect>
+      <x>392</x>
+      <y>40</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string/>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_2">
+    <property name="geometry">
+     <rect>
+      <x>301</x>
+      <y>40</y>
+      <width>91</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Now Y:</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_3">
+    <property name="geometry">
+     <rect>
+      <x>555</x>
+      <y>40</y>
+      <width>91</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Now HDG:</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_NowHdg">
+    <property name="geometry">
+     <rect>
+      <x>647</x>
+      <y>40</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string/>
+    </property>
+   </widget>
+  </widget>
+  <widget class="QMenuBar" name="menubar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>800</width>
+     <height>27</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QStatusBar" name="statusbar"/>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/test/testsideparkcalc/sideparkcalc.cpp

@@ -0,0 +1,342 @@
+#include "sideparkcalc.h"
+
+#include "math.h"
+#include <iostream>
+
+SideParkCalc::SideParkCalc(double x,double y,double hdg)
+{
+    mx = x;
+    my = y;
+    mhdg =  hdg;
+}
+
+
+void SideParkCalc::CalcPark()
+{
+    if(mx< 0.5)
+    {
+        return;
+    }
+    SideParkMode parkmode = CalcParkMode();
+
+    if(parkmode == SideParkMode::ParkAtRight)
+    {
+        ParkAtRightCalc();
+    }
+    else
+        ParkAtLeftCalc();
+}
+
+SideParkMode SideParkCalc::CalcParkMode()
+{
+
+
+    if(my>=0)return SideParkMode::ParkAtRight;
+
+    return SideParkMode::ParkAtLeft;
+}
+
+void SideParkCalc::ParkAtRightCalc()
+{
+    if(mhdg<=0.0)
+    {
+        ParkAtRightCalc_Model1();
+    }
+    else
+    {
+        ParkAtRightCalc_Model2();
+    }
+}
+
+void SideParkCalc::ParkAtRightCalc_Model1()
+{
+    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
+    dy6 = 0;
+    dx6 = mfLastDirectDis;
+    //    double fMaxdx5 = mfRaidus * sin(mfMaxAngle );
+    double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));
+
+    //    double fMaxdx3 = fMaxdx5;
+    //    double fMaxdy3 = fMaxdy5;
+
+    dx2 = mfRaidus * sin(fabs(mhdg));
+    dy2 = mfRaidus * (1 - cos(fabs(mhdg)));
+
+
+    double fang ;//= mfMaxAngle;
+    if((my+dy2) >= (2*fMaxdy5) )
+    {
+        fang = mfMaxAngle;
+    }
+    else
+    {
+        fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
+    }
+
+    dx5 = mfRaidus * sin(fang);
+    dy5 = mfRaidus * (1.0 - cos(fang));
+
+    dx3 = dx5;
+    dy3 = dy5;
+
+    double thetax = mx - dx6 - dx3 - dx5 - dx2;
+
+    if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
+    {
+        std::cout<<" divide error."<<std::endl;
+        return;
+    }
+    dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
+    dx1 = thetax - dx4;
+    dy1 = dx1 * tan(fabs(mhdg));
+    dy4 = my + dy1 +dy2 -dy3-dy5;
+
+    std::cout<<" compute.  "<<std::endl;
+
+
+    //    if((my+dy2) > (2*fMaxdy3) )
+    //    {
+    //        dy3 = fMaxdy3;
+    //        dx3 = fMaxdx3;
+    //        dx5 = fMaxdx5;
+    //        dy5 = fMaxdy5;
+    //        double thetax = mx - dx6 - fMaxdx3 - fMaxdx5 - dx2;
+    //        dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - fMaxdy3 - fMaxdy5)/(tan(fabs(mhdg ) + tan(mfMaxAngle )));
+    //        dx1 = thetax - dx4;
+    //        dy1 = dx1 * tan(fabs(mhdg));
+    //        dy4 = my + dy1 + dy2 -dy3-dy5;
+
+    //        std::cout<<" compute.  "<<std::endl;
+    //    }
+    //    else
+    //    {
+    //        double fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
+    //        dx5 = mfRaidus * sin(fang);
+    //        dy5 = mfRaidus * (1.0 - cos(fang));
+
+    //        dx3 = dx5;
+    //        dy3 = dy5;
+
+    //        double thetax = mx - dx6 - dx3 - dx5 - dx2;
+    //        dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
+    //        dx1 = thetax - dx4;
+    //        dy1 = dx1 * tan(fabs(mhdg));
+    //        dy4 = my + dy1 +dy2 -dy3-dy5;
+
+    //        std::cout<<" compute.  "<<std::endl;
+    //     }
+}
+
+void SideParkCalc::ParkAtRightCalc_Model2()
+{
+    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
+    dy5 = 0;
+    dx5 = mfLastDirectDis;
+
+    double dx2fix = mfRaidus * sin(mhdg);
+    double dy2fix = mfRaidus * (1.0 - cos(mhdg));
+
+    double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));
+
+    double fang ;
+    if((my+dy2fix) >= (2*fMaxdy4) )
+    {
+        fang = mfMaxAngle;
+    }
+    else
+    {
+        fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
+    }
+
+    dx4 = mfRaidus * sin(fang);
+    dy4 = mfRaidus * (1.0 - cos(fang));
+
+    dx2 = dx4 - dx2fix;
+    dy2 = dy4 - dy2fix;
+
+    double thetax = mx - dx2 -dx4 -dx5;
+
+    double k1 = tan(mhdg);
+    double k3 = tan(fang);
+
+    if(fabs(k1-k3)<0.001)
+    {
+        dy1 = 0;
+        dx1 = 0;
+        dy3 = my - dy2 - dy4;
+        if(k3 > 0.001)
+        {
+            dx3 = dy3/k3;
+        }
+
+        if(fabs(mx - dx1 - dx2 -dx3 -dx4 - dx5) > 0.1)
+        {
+            std::cout<<" no solve"<<std::endl;
+            return;
+        }
+    }
+    else
+    {
+        dx3 = (my - dy2 - dy4 - k1 * thetax)/(k3 - k1);
+        dy3 = k3 * dx3;
+        dx1 = thetax - dx3;
+        dy1 = k1 * dx1;
+    }
+
+    if((dx3<-0.1) || (dx1<-0.1))
+    {
+        if(cos(mhdg)> 0.001)
+        {
+            double fR = my/(1 - cos(mhdg));
+            double Rx = mx - fR*sin(mhdg);
+            if((Rx>0.3) && (fR >= mfRaidus))
+            {
+                std::cout<<" Have One Step soluton."<<std::endl;
+            }
+        }
+    }
+    else
+    {
+        std::cout<<" have compute."<<std::endl;
+    }
+
+    std::cout<<" compute. "<<std::endl;
+}
+
+void SideParkCalc::ParkAtLeftCalc()
+{
+    if(mhdg>=0.0)
+    {
+        ParkAtLeftCalc_Model1();
+    }
+    else
+    {
+        ParkAtLeftCalc_Model2();
+    }
+}
+
+void SideParkCalc::ParkAtLeftCalc_Model1()
+{
+    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
+    dy6 = 0;
+    dx6 = mfLastDirectDis;
+
+    double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));
+
+
+    dx2 = mfRaidus * sin(fabs(mhdg));
+    dy2 = mfRaidus * (1 - cos(fabs(mhdg)));
+
+ //   double x = mx;
+    double y = fabs(my);
+
+    double fang ;//= mfMaxAngle;
+    if((y+dy2) >= (2*fMaxdy5) )
+    {
+        fang = mfMaxAngle;
+    }
+    else
+    {
+        fang = acos(1.0 - (y+dy2)/(2.0*mfRaidus)) ;
+    }
+
+    dx5 = mfRaidus * sin(fang);
+    dy5 = mfRaidus * (1.0 - cos(fang));
+
+    dx3 = dx5;
+    dy3 = dy5;
+
+    double thetax = mx - dx6 - dx3 - dx5 - dx2;
+
+    if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
+    {
+        std::cout<<" divide error."<<std::endl;
+        return;
+    }
+    dx4 = (y + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
+    dx1 = thetax - dx4;
+    dy1 = dx1 * tan(fabs(mhdg));
+    dy4 = y + dy1 +dy2 -dy3-dy5;
+
+    std::cout<<" compute.  "<<std::endl;
+
+
+
+}
+
+void SideParkCalc::ParkAtLeftCalc_Model2()
+{
+    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
+    dy5 = 0;
+    dx5 = mfLastDirectDis;
+
+    double dx2fix = mfRaidus * sin(fabs(mhdg));
+    double dy2fix = mfRaidus * (1.0 - cos(fabs(mhdg)));
+
+    double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));
+
+    double fang ;
+    double y = fabs(my);
+    if((y+dy2fix) >= (2*fMaxdy4) )
+    {
+        fang = mfMaxAngle;
+    }
+    else
+    {
+        fang = acos(1.0 - (y+dy2)/(2.0*mfRaidus)) ;
+    }
+
+    dx4 = mfRaidus * sin(fang);
+    dy4 = mfRaidus * (1.0 - cos(fang));
+
+    dx2 = dx4 - dx2fix;
+    dy2 = dy4 - dy2fix;
+
+    double thetax = mx - dx2 -dx4 -dx5;
+
+    double k1 = tan(mhdg);
+    double k3 = tan(fang);
+
+    if(fabs(k1-k3)<0.001)
+    {
+        dy1 = 0;
+        dx1 = 0;
+        dy3 = y - dy2 - dy4;
+        if(k3 > 0.001)
+        {
+            dx3 = dy3/k3;
+        }
+
+        if(fabs(mx - dx1 - dx2 -dx3 -dx4 - dx5) > 0.1)
+        {
+            std::cout<<" no solve"<<std::endl;
+            return;
+        }
+    }
+    else
+    {
+        dx3 = (y - dy2 - dy4 - k1 * thetax)/(k3 - k1);
+        dy3 = k3 * dx3;
+        dx1 = thetax - dx3;
+        dy1 = k1 * dx1;
+    }
+
+    if((dx3<-0.1) || (dx1<-0.1))
+    {
+        if(cos(fabs(mhdg))> 0.001)
+        {
+            double fR = y/(1 - cos(fabs(mhdg)));
+            double Rx = mx - fR*sin(fabs(mhdg));
+            if((Rx>0.3) && (fR >= mfRaidus))
+            {
+                std::cout<<" Have One Step soluton."<<std::endl;
+            }
+        }
+    }
+    else
+    {
+        std::cout<<" have compute."<<std::endl;
+    }
+
+    std::cout<<" compute. "<<std::endl;
+}

src/test/testsideparkcalc/sideparkcalc.h

@@ -0,0 +1,44 @@
+#ifndef SIDEPARKCALC_H
+#define SIDEPARKCALC_H
+
+
+#include <math.h>
+
+
+enum SideParkMode
+{
+    ParkAtLeft =1,
+    ParkAtRight =2
+};
+
+class SideParkCalc
+{
+public:
+    SideParkCalc(double x,double y,double hdg);
+
+public:
+    void CalcPark();
+
+private:
+    double mfLastDirectDis = 0.3;
+    double mfRaidus = 5.0;
+    double mfMaxAngle = 45.0 *M_PI/180.0;
+
+private:
+    double mx,my,mhdg;
+
+    bool mbPark = false;
+
+private:
+    SideParkMode CalcParkMode();
+
+    void ParkAtRightCalc();
+    void ParkAtRightCalc_Model1();
+    void ParkAtRightCalc_Model2();
+
+    void ParkAtLeftCalc();
+    void ParkAtLeftCalc_Model1();
+    void ParkAtLeftCalc_Model2();
+};
+
+#endif // SIDEPARKCALC_H

src/test/testsideparkcalc/testsideparkcalc.pro

@@ -0,0 +1,26 @@
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+CONFIG += c++17
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    main.cpp \
+    mainwindow.cpp \
+    sideparkcalc.cpp
+
+HEADERS += \
+    mainwindow.h \
+    sideparkcalc.h
+
+FORMS += \
+    mainwindow.ui
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target