yuhesen dipanche debug

src/controller/controller_yuhesen/control/control_status.cpp

@@ -14,7 +14,7 @@ void iv::control::ControlStatus::set_wheel_angle(float angle)
 
 void iv::control::ControlStatus::set_brake(bool enable)
 {
-    command.bit.brake=(unsigned)true;
+    command.bit.brake=(unsigned)enable;
 }
 
 void iv::control::ControlStatus::set_velocity(float vel)
@@ -54,3 +54,10 @@ void iv::control::ControlStatus::set_horn(bool enable)
 {
     command.bit.horn = (unsigned char)enable;
 }
+
+void iv::control::ControlStatus::set_mode(bool enable)
+{
+    command.bit.drive_mode = (unsigned char)enable;
+}
+
+

src/controller/controller_yuhesen/control/control_status.h

@@ -24,6 +24,7 @@ namespace iv {
             void set_highbeam(bool enable);
             void set_dangwei(int dangwei);
             void set_horn(bool enable);
+            void set_mode(bool enable);
         };
         typedef boost::serialization::singleton<iv::control::ControlStatus> ControlStatusSingleton;
     }

src/controller/controller_yuhesen/control/controller.cpp

@@ -19,9 +19,13 @@ void iv::control::Controller::control_wheel(float angle) {
     ServiceControlStatus.set_wheel_angle(angle);
 }
 
-void iv::control::Controller::control_brake(bool enable)
+void iv::control::Controller::control_brake(float enable)
 {
-    ServiceControlStatus.set_brake(enable);
+    if(enable>0){
+    ServiceControlStatus.set_brake(true);
+    }else{
+       ServiceControlStatus.set_brake(false);
+    }
 }
 
 void iv::control::Controller::control_velocity(float vel)
@@ -54,7 +58,10 @@ void iv::control::Controller::control_horn(bool enable)
     ServiceControlStatus.set_horn(enable);
 }
 
-
+void iv::control::Controller::control_mode(bool enable)
+{
+    ServiceControlStatus.set_mode(enable);
+}
 
 
 

src/controller/controller_yuhesen/control/controller.h

@@ -6,7 +6,6 @@
 #define _IV_CONTROL_CONTROLLER_
 
 #include <boost.h>
-#include <control/control_status.h>
 
 namespace iv {
     namespace control {
@@ -18,13 +17,14 @@ namespace iv {
             void inialize();// 初始化
 
             void control_wheel(float angle);		//方向盘控制
-            void control_brake(bool enable);
+            void control_brake(float enable);
             void control_velocity(float vel);
             void control_left_light(bool enable);
             void control_right_light(bool enable);
             void control_highbeam(bool enable);
             void control_dangwei(int dangwei);
             void control_horn(bool enable);
+            void control_mode(bool enable);
 
         private:
         };

src/controller/controller_yuhesen/main.cpp

@@ -54,10 +54,11 @@ boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器
 
 void executeDecition(const iv::brain::decition decition)
 {
-    std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
-    std::cout<<"limitspeed is "<<decition.speed()<<" brake is "<<decition.brake()<<std::endl;
-    std::cout<<"drive_mode is "<<decition.mode()<<" elec_brake is "<<decition.handbrake()<<std::endl;
-    std::cout<<"brake_light is "<<decition.brakelamp()<<" dangwei is "<<decition.gear()<<std::endl;
+    std::cout<<" ang is "<<decition.wheelangle()<<std::endl;
+    std::cout<<" brake is "<<decition.brake()<<std::endl;
+    std::cout<<"drive_mode is "<<decition.mode()<<std::endl;
+    std::cout<<" dangwei is "<<decition.gear()<<std::endl;
+    std::cout<<"dspeed is" << decition.speed()<<std::endl;
     gcontroller->inialize();
     gcontroller->control_wheel(decition.wheelangle());
     gcontroller->control_brake(decition.brake());
@@ -66,6 +67,7 @@ void executeDecition(const iv::brain::decition decition)
     gcontroller->control_right_light(decition.rightlamp());
     gcontroller->control_highbeam(decition.highbeam());
     gcontroller->control_dangwei(decition.gear());
+    gcontroller->control_mode(decition.mode());
 }
 
 
@@ -90,18 +92,18 @@ void ListenRemotectrl(const char * strdata,const unsigned int nSize,const unsign
     else
     {
         gbAutoDriving = false;
-        xdecition.set_torque(xrc.acc());
+        //xdecition.set_torque(xrc.acc());
         xdecition.set_brake(xrc.brake());
         xdecition.set_wheelangle(xrc.wheel()*5.5f);
         xdecition.set_speed(5);
         xdecition.set_gear(1);
-        xdecition.set_handbrake(0);
-        xdecition.set_grade(1);
+        //xdecition.set_handbrake(0);
+       // xdecition.set_grade(1);
         xdecition.set_mode(1);
-        xdecition.set_speak(0);
-        xdecition.set_headlight(false);
-        xdecition.set_engine(0);
-        xdecition.set_taillight(false);
+        //xdecition.set_speak(0);
+        //xdecition.set_headlight(false);
+       // xdecition.set_engine(0);
+       // xdecition.set_taillight(false);
         gMutex.lock();
         gdecition_remote.CopyFrom(xdecition);
         gMutex.unlock();

src/decition/decition_brain/decition/adc_adapter/yuhesen_adapter.cpp

@@ -76,16 +76,11 @@ iv::decition::Decition iv::decition::YuHeSenAdapter::getAdapterDeciton(GPS_INS n
 
     (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
 
-    (*decition)->wheel_angle=max((float)-500.0,(*decition)->wheel_angle);
-     (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);
+    (*decition)->wheel_angle=max((float)-40.0,(*decition)->wheel_angle);
+     (*decition)->wheel_angle=min((float)40.0,(*decition)->wheel_angle);
+
 
-    if((*decition)->accelerator>=0){
-        (*decition)->torque= (*decition)->accelerator;
         (*decition)->brake=0;
-    }else{
-        (*decition)->torque=0;
-        (*decition)->brake=0-(*decition)->accelerator;
-    }
 
 
 
@@ -94,13 +89,18 @@ std::cout<<"==========================================="<<std::endl;
      std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
     <<std::endl;
 std::cout<<"========================================="<<std::endl;
-
+(*decition)->speed=5/3.6;
+(*decition)->wheel_angle=(*decition)->wheel_angle*0.1;
+(*decition)->wheel_angle=max((float)-40.0,(*decition)->wheel_angle);
+(*decition)->wheel_angle=min((float)40.0,(*decition)->wheel_angle);
+(*decition)->mode=1;
+(*decition)->dangWei=1;
     return *decition;
 }
 
 
 
-float iv::decition::VV7Adapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+float iv::decition::YuHeSenAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
 
     //刹车限制
 
@@ -140,7 +140,7 @@ float iv::decition::VV7Adapter::limitBrake(float realSpeed, float controlBrake,f
 
 }
 
-float iv::decition::VV7Adapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+float iv::decition::YuHeSenAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
 
     controlSpeed=min((float)5.0,controlSpeed);
     controlSpeed=max((float)-7.0,controlSpeed);

src/decition/decition_brain/decition/adc_adapter/yuhesen_adapter.h

@@ -1,5 +1,5 @@
-#ifndef VV7_ADAPTER_H
-#define VV7_ADAPTER_H
+#ifndef YUHESEN_ADAPTER_H
+#define YUHESEN_ADAPTER_H
 
 
 
@@ -15,7 +15,7 @@
 
 namespace iv {
      namespace decition {
-         class VV7Adapter: public BaseAdapter {
+         class YuHeSenAdapter: public BaseAdapter {
                     public:
 
                         float lastTorque;
@@ -23,8 +23,8 @@ namespace iv {
                         int lastDangWei=0;
                         int seizingCount=0;
 
-                        VV7Adapter();
-                        ~VV7Adapter();
+                        YuHeSenAdapter();
+                        ~YuHeSenAdapter();
 
                         iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
                                                                                  float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);

src/decition/decition_brain/decition/adc_adapter/zhongche_adapter.cpp

@@ -1,3 +1,4 @@
+
 #include <decition/adc_adapter/zhongche_adapter.h>
 #include <common/constants.h>
 #include <common/car_status.h>
@@ -5,7 +6,6 @@
 #include <iostream>
 #include <fstream>
 
-
 using namespace std;
 
 
@@ -18,7 +18,6 @@ iv::decition::ZhongcheAdapter::~ZhongcheAdapter(){
 }
 
 
-
 iv::decition::Decition iv::decition::ZhongcheAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
                                                                         float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
 

src/decition/decition_brain/decition/brain.cpp

@@ -272,7 +272,7 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.msysparam.mstrvin = xp.GetParam("vin","10000000000000001");
     ServiceCarStatus.msysparam.mstrid = xp.GetParam("id","1");
     ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
-    ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","ge3");
+    ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","yuhesen");
 
     ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
     ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -2057,15 +2057,13 @@ void iv::decition::DecideGps00::initMethods(){
     zhongche_Adapter = new ZhongcheAdapter();
     bus_Adapter = new BusAdapter();
     hapo_Adapter = new HapoAdapter();
+    yuhesen_Adapter = new YuHeSenAdapter();
 
 
     mpid_Controller.reset(pid_Controller);
-    mge3_Adapter.reset(ge3_Adapter);
-    mqingyuan_Adapter.reset(qingyuan_Adapter);
-    mvv7_Adapter.reset(vv7_Adapter);
-    mzhongche_Adapter.reset(zhongche_Adapter);
     mbus_Adapter.reset(bus_Adapter);
     mhapo_Adapter.reset(hapo_Adapter);
+    myuhesen_Adapter.reset(yuhesen_Adapter);
 
     frenetPlanner = new FrenetPlanner();
     //    laneChangePlanner = new LaneChangePlanner();
@@ -2093,6 +2091,9 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
     }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
         hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }
+    else if(ServiceCarStatus.msysparam.mvehtype=="yuhesen"){
+        yuhesen_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
 }
 
 

src/decition/decition_brain/decition/decide_gps_00.h

@@ -15,6 +15,8 @@
 #include <decition/adc_adapter/zhongche_adapter.h>
 #include <decition/adc_adapter/bus_adapter.h>
 #include <decition/adc_adapter/hapo_adapter.h>
+#include <decition/adc_adapter/zhongche_adapter.h>
+#include <decition/adc_adapter/yuhesen_adapter.h>
 #include "perception/perceptionoutput.h"
 #include "ivlog.h"
 #include <memory>
@@ -125,6 +127,7 @@ public:
     BaseAdapter *zhongche_Adapter;
     BaseAdapter *bus_Adapter;
     BaseAdapter *hapo_Adapter;
+    BaseAdapter *yuhesen_Adapter;
 
     std::shared_ptr<BaseController> mpid_Controller;
     std::shared_ptr<BaseAdapter> mge3_Adapter;
@@ -133,7 +136,7 @@ public:
     std::shared_ptr<BaseAdapter> mzhongche_Adapter;
     std::shared_ptr<BaseAdapter> mbus_Adapter;
     std::shared_ptr<BaseAdapter> mhapo_Adapter;
-
+    std::shared_ptr<BaseAdapter> myuhesen_Adapter;
 
 	FrenetPlanner *frenetPlanner;
 //    BasePlanner *laneChangePlanner;

src/decition/decition_brain/decition/decition.pri

@@ -24,7 +24,8 @@ HEADERS += \
     $$PWD/adc_tools/dubins.h \
     $$PWD/adc_adapter/bus_adapter.h \
     $$PWD/fanyaapi.h \
-    $$PWD/adc_adapter/hapo_adapter.h
+    $$PWD/adc_adapter/hapo_adapter.h \
+    $$PWD/adc_adapter/yuhesen_adapter.h
 
 SOURCES += \
     $$PWD/decide_gps_00.cpp \
@@ -44,9 +45,10 @@ SOURCES += \
     $$PWD/adc_adapter/base_adapter.cpp \
     $$PWD/adc_adapter/qingyuan_adapter.cpp \
     $$PWD/adc_adapter/vv7_adapter.cpp \
-    $$PWD/adc_adapter/zhongche_adapter.cpp \
     $$PWD/adc_planner/dubins_planner.cpp \
     $$PWD/adc_tools/dubins.cpp \
     $$PWD/adc_adapter/bus_adapter.cpp \
     $$PWD/fanyaapi.cpp \
-    $$PWD/adc_adapter/hapo_adapter.cpp
+    $$PWD/adc_adapter/hapo_adapter.cpp \
+    $$PWD/adc_adapter/zhongche_adapter.cpp \
+    $$PWD/adc_adapter/yuhesen_adapter.cpp

src/detection/detection_chassis/decodechassis.cpp

@@ -369,3 +369,14 @@ int ProcHAPOChassis(void * pa, iv::can::canmsg * pmsg)
     return nRtn;
 
 }
+
+
+int ProcYUHESENChassis(void * pa, iv::can::canmsg * pmsg)
+{
+    int i;
+    int nRtn = 0;
+
+
+    return nRtn;
+
+}

src/detection/detection_chassis/decodechassis.h

@@ -19,5 +19,6 @@ int ProcMIDBUSChassis(void * pa, iv::can::canmsg * pmsg);
 
 int ProcHAPOChassis(void * pa, iv::can::canmsg * pmsg);
 
+int ProcYUHESENChassis(void * pa, iv::can::canmsg * pmsg);
 
 #endif // DECODECHASSIS_H

src/detection/detection_chassis/main.cpp

@@ -26,7 +26,7 @@ QTime gTime;
 
 namespace  iv {
 
-enum VehicleTypeDef {GE3,VV7,MIDCAR,PROBLUE,MIDBUS,HAPO,UNKNOWN} gVehicleType;  //车辆类型
+enum VehicleTypeDef {GE3,VV7,MIDCAR,PROBLUE,MIDBUS,HAPO,UNKNOWN, YUHESEN} gVehicleType;  //车辆类型
 
 
 }
@@ -77,6 +77,9 @@ void Listencan0(const char * strdata,const unsigned int nSize,const unsigned int
     case iv::HAPO:
         nRtn = ProcHAPOChassis(gpa,&xmsg);
         break;
+    case iv::YUHESEN:
+        nRtn = ProcYUHESENChassis(gpa,&xmsg);
+        break;
     default:
         break;
     }
@@ -172,6 +175,10 @@ int main(int argc, char *argv[])
     {
         iv::gVehicleType = iv::HAPO;
     }
+ if(strncmp(strvehicletype.data(),"YUHESEN",255) == 0)
+    {
+        iv::gVehicleType = iv::YUHESEN;
+    }
 
 //iv::gVehicleType = iv::MIDBUS;