change driver_lidar_rs16 for add offset. change controller_hapo for use shenlan decison, can comment in .pro file.

src/controller/controller_hapo/controller_hapo.pro

@@ -42,6 +42,8 @@ INCLUDEPATH += $$PWD/../controllercommon
 
 DEFINES += TORQUEBRAKETEST
 
+DEFINES += USE_SHENLANDECISION
+
 #unix:!macx: LIBS += -L/home/yuchuli/qt/lib -lncomgnss -ladcmemshare
 
 unix:!macx: INCLUDEPATH += $$PWD/.

src/controller/controller_hapo/main.cpp

@@ -78,6 +78,7 @@ void executeDecition(const iv::brain::decition decition)
     gcontroller->control_angle_speed(decition.wheelspeed());
     gcontroller->control_torque(decition.torque());
 
+
     gcontroller->control_brake(decition.brake());
     gcontroller->control_gear(decition.gear());
 
@@ -280,6 +281,38 @@ void sendthread()
             gMutex.lock();
             xdecition.CopyFrom(gdecition);
             gMutex.unlock();
+
+#ifdef USE_SHENLANDECISION
+            double fbrake;
+            if(xdecition.brake()<0)
+            {
+                fbrake = xdecition.brake()*(-1)/0.07;
+            }
+            else
+                fbrake = 0.0;
+            xdecition.set_brake(fbrake);
+
+            double ftorue = 0;
+            if(xdecition.torque()>0)
+            {
+                double fVehWeight = 1800;
+                double fRollForce = 50;
+                const double fRatio = 2.5;
+                double accAim = (xdecition.torque() * fRatio - fRollForce)/fVehWeight;
+                if(accAim>=0)
+                {
+                    ftorue = accAim/0.05;
+                }
+            }
+            xdecition.set_torque(ftorue);
+
+            xdecition.set_mode(1);
+            xdecition.set_grade(1);
+            xdecition.set_gear(4);
+            xdecition.set_engine(0);
+#endif
+
+
             gnDecitionNum--;
         }
         executeDecition(xdecition);

src/driver/driver_lidar_rs16/lidar_driver_rs16.cpp

@@ -54,9 +54,9 @@ extern char gstr_inclinationang_xaxis[256];  //from x axis
 extern char gstr_hostip[256];
 extern char gstr_port[256];
 
-extern double gfx_off = 0.0;
-extern double gfy_off = 0.0;
-extern double gfz_off = 0.0;
+extern double gfx_off ;
+extern double gfy_off ;
+extern double gfz_off ;
 extern bool gboffset;
 
 class rs16_Buf

src/driver/driver_lidar_rs16/main.cpp

@@ -179,7 +179,7 @@ void decodeyaml(const char * stryaml)
         gfz_off = atof(stroff);
     }
 
-    if((fabs(gfx_off>0.001))||(fabs(gfy_off>0.001))||(fabs(gfz_off>0.001)))
+    if((fabs(gfx_off)>0.001)||(fabs(gfy_off)>0.001)||(fabs(gfz_off)>0.001))
     {
         gboffset = true;
     }