1x 源码支持yuhesenfr09pro,在hunter的controller中添加了对应的逻辑，其余没变

src/controller/controller_hunter/control/control.pri

@@ -3,7 +3,8 @@ HEADERS += \
     $$PWD/control_status.h \
     $$PWD/vv7.h \
     $$PWD/hunter.h \
-    $$PWD/yuhesenfr09_1x.h
+    $$PWD/yuhesenfr09_1x.h \
+    $$PWD/yuhesenfr09pro_1x.h
 
 SOURCES += \
     $$PWD/controller.cpp \

src/controller/controller_hunter/control/control_status.cpp

@@ -49,6 +49,11 @@ void iv::control::ControlStatus::set_wheel_angle(float angle)
       ang=-570;
     command111.bit.ads_ang_h = GET_HIGH_BYTE(ang);
     command111.bit.ads_ang_l = GET_LOW_BYTE(ang);
+
+    int fr09ang = angle *100* 0.047;  //0.01的比例因子，40.96 870
+    cmd2d0pro.bit.ads_steerl4bit = fr09ang& 0x0F;
+    cmd2d0pro.bit.ads_steerm8bit = (fr09ang>>4)&0xFF;
+    cmd2d0pro.bit.ads_steerh4bit = (fr09ang>>12)&0x0F;
 }
 
 void iv::control::ControlStatus::set_wheel_speed(float speed)
@@ -96,11 +101,22 @@ void iv::control::ControlStatus::set_torque(float torque)
         spd=-1500;
     command111.bit.ads_spd_h=GET_HIGH_BYTE(spd);
     command111.bit.ads_spd_l= GET_LOW_BYTE(spd);
+
+    spd = int((torque/3.6)*1000);
+
+    cmd2d0pro.bit.ads_spdl4bit = spd& 0x0F;
+    cmd2d0pro.bit.ads_spdm8bit = (spd>>4)&0xFF;
+    cmd2d0pro.bit.ads_spdh4bit = (spd>>12)&0x0F;
 }
 
 void iv::control::ControlStatus::set_brake(float brake)
 {
    // command11.bit.brake=(unsigned)brake;
+    unsigned char brakephy = brake;
+    if(brakephy>=100)
+        brakephy=100;
+    cmd2d0pro.bit.ads_brakel4bit = brakephy&0x0f;
+    cmd2d0pro.bit.ads_brakeh4bit = (brakephy>>4)&0x0f;
 }
 
 
@@ -108,6 +124,7 @@ void iv::control::ControlStatus::set_brake(float brake)
 void iv::control::ControlStatus::set_gear(int gear)
 {
     //command11.bit.gear = (unsigned)gear;
+    cmd2d0pro.bit.ads_gear = gear;
 }
 
 void iv::control::ControlStatus::set_handBrake(bool handbrake)
@@ -237,22 +254,19 @@ void iv::control::ControlStatus::set_door(char enable)
 
 void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right)
 {
-//    if ( (left == true) && (right == false) )
-//    {
-//        command10.bit.turn_light = 0x01;
-//    }
-//    else if ( (left == false) && (right == true) )
-//    {
-//        command10.bit.turn_light = 0x02;
-//    }
-//    else if ((left == false) && (right == false))
-//    {
-//        command10.bit.turn_light = 0x00;
-//    }
-//    else
-//    {
-//        command10.bit.turn_light = 0x03;
-//    }
+    cmd7d0pro.bit.ads_ioen=1;
+    if ( (left == true) && (right == false) )
+    {
+        cmd7d0pro.bit.ads_turnlamp = 0x01;
+    }
+    else if ( (left == false) && (right == true) )
+    {
+        cmd7d0pro.bit.ads_turnlamp= 0x02;
+    }
+    else
+    {
+       cmd7d0pro.bit.ads_turnlamp = 0x00;
+    }
 }
 
 

src/controller/controller_hunter/control/control_status.h

@@ -7,7 +7,7 @@
 //#include <control/vv7.h>
 #include <control/hunter.h>
 #include <control/yuhesenfr09_1x.h>
-
+#include <control/yuhesenfr09pro_1x.h>
 namespace iv {
 namespace control {
 class ControlStatus : public boost::noncopyable
@@ -44,6 +44,10 @@ public:
     Command3d0 command3d0 ;
     Command4d0 command4d0 ;
     Command5d0 command5d0 ;
+    Command2d0pro cmd2d0pro;
+    Command7d0pro cmd7d0pro;
+    int cmd2d0pro_ID= 0x18C4D2D0;
+    int cmd7d0pro_ID= 0x18C4D7D0;
 
     int command_ID1D0= 0x18C4D1D0;
     int command_ID2D0= 0x18C4D2D0;

src/controller/controller_hunter/control/controller.cpp

@@ -57,15 +57,15 @@ void iv::control::Controller::control_aeb(float aeb){
 //    ServiceControlStatus.set_aeb(aeb);
 }
 void iv::control::Controller::control_aeb_en(bool enable){
-    ServiceControlStatus.set_aeb_enable(enable);
+//    ServiceControlStatus.set_aeb_enable(enable);
 }
 //刹车控制
 void iv::control::Controller::control_brake(float brake){
-//    ServiceControlStatus.set_brake(brake);
+    ServiceControlStatus.set_brake(brake);
 }
 //档位控制
 void iv::control::Controller::control_gear(float gear){
-//    ServiceControlStatus.set_gear(gear);
+    ServiceControlStatus.set_gear(gear);
 }
 void iv::control::Controller::control_gear_en(bool enable){
     ServiceControlStatus.set_gear_enable(enable);

src/controller/controller_hunter/control/yuhesenfr09_1x.h

@@ -170,24 +170,24 @@ union Command5d0
 
 
 
-struct Command_Response_Bit
-{
-    unsigned char head;
+//struct Command_Response_Bit
+//{
+//    unsigned char head;
 
 
-    unsigned char grade : 2;
-    unsigned char driveMode : 2;
-    unsigned char epb : 1;
-    unsigned char epsMode : 2;
-    unsigned char  obligate: 1;
+//    unsigned char grade : 2;
+//    unsigned char driveMode : 2;
+//    unsigned char epb : 1;
+//    unsigned char epsMode : 2;
+//    unsigned char  obligate: 1;
 
 
-};
+//};
 
-union Command_Response
-{
-    Command_Response_Bit bit;
+//union Command_Response
+//{
+//    Command_Response_Bit bit;
 
-    unsigned char byte[2];
-};
+//    unsigned char byte[2];
+//};
 #endif // YUHESENFR09_H

src/controller/controller_hunter/control/yuhesenfr09pro_1x.h

@@ -0,0 +1,74 @@
+#ifndef YUHESENFR09PRO_1X_H
+#define YUHESENFR09PRO_1X_H
+struct Command_Bit2d0_pro
+{
+
+    unsigned char ads_gear:4 ;
+    unsigned char ads_spdl4bit:4;
+
+    unsigned char ads_spdm8bit;
+
+    unsigned char ads_spdh4bit:4;
+    unsigned char ads_steerl4bit:4;
+
+    unsigned char ads_steerm8bit;
+
+    unsigned char ads_steerh4bit:4;
+    unsigned char ads_brakel4bit:4;
+
+    unsigned char ads_brakeh4bit:4;
+    unsigned char reserved0:4;
+
+
+    unsigned char reserved6:4;
+    unsigned char ads_cnt:4; //
+
+    unsigned char ads_check;
+};
+
+
+union Command2d0pro
+{
+    Command_Bit2d0_pro bit;
+
+    unsigned char byte[8];
+};
+
+struct Command_Bit7d0_pro
+{
+
+    unsigned char ads_ioen:1 ;
+    unsigned char reserved0:7;
+
+    unsigned char reserved1:2;
+    unsigned char ads_turnlamp:2;
+    unsigned char reserved2:1;
+    unsigned char ads_clearlamp:1;
+    unsigned char reserved3:2;
+
+
+    unsigned char ads_speaker:1;
+    unsigned char reserved4:7;
+
+    unsigned char reserved5;
+
+    unsigned char reserved6;
+
+    unsigned char reserved7;
+
+
+    unsigned char reserved8:4;
+    unsigned char ads_cnt:4; //
+
+    unsigned char ads_check;
+};
+
+
+union Command7d0pro
+{
+    Command_Bit7d0_pro bit;
+
+    unsigned char byte[8];
+};
+
+#endif // YUHESENFR09PRO_1X_H

src/controller/controller_hunter/main.cpp

@@ -41,6 +41,8 @@ int gnDecitionNum = 0; //when is zero,send default;
 const int gnDecitionNumMax = 100;
 int gnIndex = 0;
 
+
+std::string vehtype;  //1x 支持hunter 和 yuhesenfr09pro
 boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器
 
 
@@ -142,7 +144,7 @@ void executeDecition(const iv::brain::decition decition)
 
 //    gcontroller->cmd_checksum(0x10);
 //    gcontroller->cmd_checksum(0x11);
-//    gcontroller->cmd_checksum(0x12);
+//    gcontroller->cmd_checksum(2d0);
 
     //    qDebug("dangwei is %d mode is %d",decition.gear(),decition.mode());
 }
@@ -197,41 +199,87 @@ void ExecSend()
 //    unsigned char * strp = (unsigned char *)&(ServiceControlStatus.command10.byte[0]);
     //    qDebug("%02x %02x %02x %02x %02x %02x %02x %02x",strp[0]
     //            ,strp[1],strp[2],strp[3],strp[4],strp[5],strp[6],strp[7]);
-    xraw.set_id(ServiceControlStatus.command_ID111);
-    xraw.set_data(ServiceControlStatus.command111.byte,8);
-    xraw.set_bext(false);
-    xraw.set_bremote(false);
-    xraw.set_len(8);
-    iv::can::canraw * pxraw = xmsg.add_rawmsg();
-    pxraw->CopyFrom(xraw);
-    xmsg.set_channel(0);
-    xmsg.set_index(gnIndex);
-
-    xraw.set_id(ServiceControlStatus.command_ID421);
-    xraw.set_data(ServiceControlStatus.command421.byte,8);
-//    int a = ServiceControlStatus.command11.byte[5]&0x0f;
-//    if(a != 0x04)
-//    {
-//        qDebug("not D.");
-//    }
-    xraw.set_bext(false);
-    xraw.set_bremote(false);
-    xraw.set_len(8);
-    iv::can::canraw * pxraw1 = xmsg.add_rawmsg();
-    pxraw1->CopyFrom(xraw);
-    xmsg.set_channel(0);
-    xmsg.set_index(gnIndex);
-
-    xraw.set_id(ServiceControlStatus.command_ID131);
-    xraw.set_data(ServiceControlStatus.command131.byte,8);
-    xraw.set_bext(false);
-    xraw.set_bremote(false);
-    xraw.set_len(8);
-    iv::can::canraw * pxraw2 = xmsg.add_rawmsg();
-    pxraw2->CopyFrom(xraw);
-    xmsg.set_channel(0);
-    xmsg.set_index(gnIndex);
+    if(vehtype=="hunter")
+    {
+        xraw.set_id(ServiceControlStatus.command_ID111);
+        xraw.set_data(ServiceControlStatus.command111.byte,8);
+        xraw.set_bext(false);
+        xraw.set_bremote(false);
+        xraw.set_len(8);
+        iv::can::canraw * pxraw = xmsg.add_rawmsg();
+        pxraw->CopyFrom(xraw);
+        xmsg.set_channel(0);
+        xmsg.set_index(gnIndex);
+
+        xraw.set_id(ServiceControlStatus.command_ID421);
+        xraw.set_data(ServiceControlStatus.command421.byte,8);
+    //    int a = ServiceControlStatus.command11.byte[5]&0x0f;
+    //    if(a != 0x04)
+    //    {
+    //        qDebug("not D.");
+    //    }
+        xraw.set_bext(false);
+        xraw.set_bremote(false);
+        xraw.set_len(8);
+        iv::can::canraw * pxraw1 = xmsg.add_rawmsg();
+        pxraw1->CopyFrom(xraw);
+        xmsg.set_channel(0);
+        xmsg.set_index(gnIndex);
+
+        xraw.set_id(ServiceControlStatus.command_ID131);
+        xraw.set_data(ServiceControlStatus.command131.byte,8);
+        xraw.set_bext(false);
+        xraw.set_bremote(false);
+        xraw.set_len(8);
+        iv::can::canraw * pxraw2 = xmsg.add_rawmsg();
+        pxraw2->CopyFrom(xraw);
+        xmsg.set_channel(0);
+        xmsg.set_index(gnIndex);
+    }
+    if(vehtype=="yuhesenfr09pro")
+    {
+        xraw.set_id(ServiceControlStatus.cmd2d0pro_ID);
+        static unsigned char cnt=0;
+        if(cnt<0xff)
+        {
+            ServiceControlStatus.cmd2d0pro.bit.ads_cnt =cnt;
+            ServiceControlStatus.cmd7d0pro.bit.ads_cnt =cnt;
+            cnt=cnt+1;
+        }
 
+        unsigned int check_sum=0;
+        for(int i=0;i<7;i++)
+        {
+            check_sum^=(ServiceControlStatus.cmd2d0pro.byte[i]);
+        }
+        ServiceControlStatus.cmd2d0pro.byte[7]=check_sum;
+
+        xraw.set_data(ServiceControlStatus.cmd2d0pro.byte,8);
+        xraw.set_bext(true);
+        xraw.set_bremote(false);
+        xraw.set_len(8);
+        iv::can::canraw * pxraw2d0 = xmsg.add_rawmsg();
+        pxraw2d0->CopyFrom(xraw);
+        xmsg.set_channel(0);
+        xmsg.set_index(gnIndex);
+
+        xraw.set_id(ServiceControlStatus.cmd7d0pro_ID);
+        unsigned int check_7d0sum=0;
+        for(int i=0;i<7;i++)
+        {
+            check_7d0sum^=(ServiceControlStatus.cmd7d0pro.byte[i]);
+        }
+        ServiceControlStatus.cmd7d0pro.byte[7]=check_7d0sum;
+
+        xraw.set_data(ServiceControlStatus.cmd7d0pro.byte,8);
+        xraw.set_bext(true);
+        xraw.set_bremote(false);
+        xraw.set_len(8);
+        iv::can::canraw * pxraw7d0 = xmsg.add_rawmsg();
+        pxraw7d0->CopyFrom(xraw);
+        xmsg.set_channel(0);
+        xmsg.set_index(gnIndex);
+    }
     gnIndex++;
     xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
     int ndatasize = xmsg.ByteSize();
@@ -292,7 +340,7 @@ int main(int argc, char *argv[])
     QString strpath = QCoreApplication::applicationDirPath();
 
     if(argc < 2)
-        strpath = strpath + "/controller_vv7.xml";
+        strpath = strpath + "/controller.xml";
     else
         strpath = argv[1];
     std::cout<<strpath.toStdString()<<std::endl;
@@ -319,6 +367,9 @@ int main(int argc, char *argv[])
     gstrmemdecition = xp.GetParam("dection","deciton");
     gstrmemchassis = xp.GetParam("chassismsgname","chassis");
 
+    vehtype = xp.GetParam("vehType","hunter");
+     std::cout<<vehtype<<std::endl;
+
     gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
     gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
     gpachassis = iv::modulecomm::RegisterRecv(gstrmemchassis.data(),UpdateChassis);