yu controller

src/controller/controller_Geely_xingyueL/controller_Geely_xingyueL.pro

@@ -19,7 +19,9 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += $$PWD/main.cpp \
     ../../include/msgtype/decition.pb.cc\
-    ../../include/msgtype/chassis.pb.cc
+    ../../include/msgtype/chassis.pb.cc \
+    ../../include/msgtype/remotectrl.pb.cc \
+    ../../include/msgtype/gpsimu.pb.cc
 
 
 !include(../../../include/common.pri ) {
@@ -44,7 +46,9 @@ HEADERS += \
     include/socket_udp/socket_udp.h \
     include/basic_types.h \
     include/car_control.h \
-    include/status.h
+    include/status.h \
+    ../../include/msgtype/remotectrl.pb.h \
+    ../../include/msgtype/gpsimu.pb.h
 
 LIBS += $$PWD/lib/libcar_control.a
 

src/controller/controller_Geely_xingyueL/main.cpp

@@ -10,6 +10,8 @@
 #include "ivversion.h"
 #include "decition.pb.h"
 #include "chassis.pb.h"
+#include "remotectrl.pb.h"
+#include "gpsimu.pb.h"
 #include <thread>
 #include <iostream>
 #include <fstream>
@@ -58,6 +60,12 @@ int communicate_cnt=0; //判断decition中是否有决策，超过设定的数
 bool communicate_state=0;
 int  case_state=0;//记录switch  case的状态，调试用
 
+static bool gbAutoDriving = true;
+static iv::brain::decition gdecition_remote;
+static qint64 gLastRemoteTime = 0;
+static qint64 gLastGPSIMUTime = 0;
+static double gfVehSpeed = 0; // km/h
+const  double gfMaxRemoteVehSpeed = 10; //Max Speed in Remote Mode
 
 void quit_ctrl()  //退出车辆控制，cxw,20220622
 {
@@ -181,6 +189,96 @@ void executeDecition(const iv::brain::decition decition)
 }
 
 
+void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+//        ggpsimu->UpdateGPSIMU(strdata,nSize);
+
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRaw Parse error."<<std::endl;
+        return;
+    }
+
+    double fSpeed = xgpsimu.speed() *3.6;
+//    std::cout<<" speed is "<<fSpeed<<std::endl;
+    gfVehSpeed = fSpeed;
+    gLastGPSIMUTime = QDateTime::currentMSecsSinceEpoch();
+//        qDebug(" gps time is %ld",QDateTime::currentMSecsSinceEpoch());
+
+}
+
+
+void ListenRemotectrl(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+//    qint64 oldtime;
+    iv::brain::decition xdecition;
+
+    iv::remotectrl xrc;
+
+    if(!xrc.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRemotectrl parse error."<<std::endl;
+        return;
+    }
+
+    std::cout<<" recv remote."<<std::endl;
+
+    if(xrc.ntype() == iv::remotectrl_CtrlType_AUTO)
+    {
+        gbAutoDriving = true;
+    }
+    else
+    {
+        gnDecitionNum = gnDecitionNumMax;
+        gbAutoDriving = false;
+
+        if(gfVehSpeed>=(gfMaxRemoteVehSpeed*1.1))
+        {
+          xdecition.set_accelerator(0.0);
+        }
+        else
+        {
+            if(gfVehSpeed>gfMaxRemoteVehSpeed)
+            {
+                xdecition.set_accelerator(xrc.acc()*0.01*(gfMaxRemoteVehSpeed*1.1-gfVehSpeed)/(gfMaxRemoteVehSpeed*0.1));
+            }
+            else
+            {
+                if(abs(QDateTime::currentMSecsSinceEpoch() - gLastGPSIMUTime)<3000)
+                {
+                    xdecition.set_accelerator(xrc.acc()*0.01);
+                }
+                else
+                {
+                    xdecition.set_accelerator(0.0);
+                }
+            }
+        }
+ //       xdecition.set_accelerator(xrc.acc()*0.01);
+        if(xrc.brake()>0.01)
+        {
+            xdecition.set_accelerator(xrc.brake()*(-0.03));
+        }
+//        xdecition.set_brake(0);  //not use brake
+        xdecition.set_wheelangle(xrc.wheel() *5.5);
+        xdecition.set_gear(3);
+        gMutex.lock();
+        gdecition_remote.CopyFrom(xdecition);
+        gMutex.unlock();
+        gLastRemoteTime = QDateTime::currentMSecsSinceEpoch();
+
+    }
+
+//    if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)qDebug("dection time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
+//    oldtime = QDateTime::currentMSecsSinceEpoch();
+//    gMutex.lock();
+//    gdecition.CopyFrom(xdecition);
+//    gMutex.unlock();
+//    gnDecitionNum = gnDecitionNumMax;
+//    gbChassisEPS = false;
+
+}
 
 
 void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -233,10 +331,24 @@ void sendthread()
         }
         else
         {
-            gMutex.lock();
-            xdecition.CopyFrom(gdecition);
-//            std::cout<<"copy from ADAS gdecition"<<std::endl;
-            gMutex.unlock();
+            if(gbAutoDriving == false)
+            {
+                gMutex.lock();
+                xdecition.CopyFrom(gdecition_remote);
+    //            std::cout<<"copy from ADAS gdecition"<<std::endl;
+                gMutex.unlock();
+                if((QDateTime::currentMSecsSinceEpoch() - gLastRemoteTime)> 3000)
+                {
+                    gbAutoDriving = true;
+                }
+            }
+            else
+            {
+                gMutex.lock();
+                xdecition.CopyFrom(gdecition);
+                //            std::cout<<"copy from ADAS gdecition"<<std::endl;
+                gMutex.unlock();
+            }
             gnDecitionNum--;
             communicate_cnt=0;
             communicate_state=1;
@@ -436,6 +548,9 @@ int main(int argc, char *argv[])
     gpa = iv::modulecomm::RegisterSend(strchassismsgname.data(),1000,1);
     gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
 
+    void * pa1 = iv::modulecomm::RegisterRecv("hcp2_gpsimu",Listengpsimu);
+    void * pa2 = iv::modulecomm::RegisterRecv("remotectrl",ListenRemotectrl);
+
     std::thread xthread(sendthread);
 //    std::thread myxthread(recvthread);