1、修改PCICAN和PCI模拟量通信相关代码

2、修改系统学习请求的报文

src/canbus/canbus.cpp

@@ -38,9 +38,9 @@ int Canbus::Init() {
   node_handle_.reset(new ros::NodeHandle());
   //20240111 by zhaowei: add tcptocan mode
   if (!ros::param::get("rcu_canbus_mode", canbus_mode))
-    canbus_mode = "tcp";
+    canbus_mode = "pcitcp";
   LOG(INFO)<< Name()<<": read param rcu_canbus_mode = " << canbus_mode;
-  if (canbus_mode == "tcp") {
+  if (canbus_mode == "tcp" || canbus_mode == "pcitcp") {
     for(int i=0; i < sizeof(s_g_pBufSet); ++i ) {
       s_g_pBufSet[i] = 0x00;
       s_g_pBufGet[i] = 0x00;
@@ -145,6 +145,11 @@ int Canbus::Start() {
     {
       LOG(INFO)<<Name()<<"socket can init failed!";
     }
+else
+{
+LOG(INFO)<<Name()<<"socket can init OK OK OK!";
+cout << "can_fd = " << can_fd << endl;
+}
 
   }
   ros::Rate loop_rate(1/rate);
@@ -3161,20 +3166,20 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 int Canbus::can_init_for_linux(int *fd)
 {
     //0、设置can通信波特率
-    system("ip link set can0 down");
+    system("sudo ip link set can0 down");
     //波特率设置为500Kbps
-    system("ip link set can0 type can bitrate 500000");
-    system("ip link set can0 up");
+    system("sudo ip link set can0 type can bitrate 500000 triple-sampling on");
+    system("sudo ip link set can0 up");
 
     int ret = 0;
     //1、创建socket can套接字
     *fd = socket(AF_CAN, SOCK_RAW, CAN_RAW);
     if(*fd == -1)
     {
-        LOG(ERROR) << Name() <<"create socket can failed";
+        LOG(ERROR) << Name() <<" create socket can failed";
         return -1;
     }
-
+cout << "*fd = " << *fd <<endl;
     //2、套接子绑定到can0端口
     struct sockaddr_can addr;
     struct ifreq ifr;
@@ -3186,7 +3191,7 @@ int Canbus::can_init_for_linux(int *fd)
     ret = bind(*fd, (struct sockaddr *)&addr, sizeof(addr));
     if(ret < 0)
     {
-        LOG(ERROR) << Name() <<"socket can bind can0 error";
+        LOG(ERROR) << Name() <<" socket can bind can0 error";
         return -1;
     }
 
@@ -3232,7 +3237,7 @@ int Canbus::can_write_data(int fd, int can_id,char *data,int data_len)
     struct can_frame send_data;
     if(fd == -1)
     {
-        LOG(ERROR) << Name() <<"socket can write data failed!";
+        LOG(ERROR) << Name() <<" socket can write data failed!";
         return -1;
     }
     send_data.can_dlc = data_len;
@@ -3241,10 +3246,10 @@ int Canbus::can_write_data(int fd, int can_id,char *data,int data_len)
     ret = write(fd, &send_data, sizeof(struct can_frame));
     if(ret == -1)
     {
-        LOG(ERROR) << Name() <<"socket can write data failed!";
+        LOG(ERROR) << Name() <<" socket can write data failed!";
         return -1;
     }else{
-        printf("ID=%03X, DLC=%d, data=%02X %02X %02X %02X %02X %02X %02X %02X \n",  \
+       /* printf("SEND ID=%03X, DLC=%d, data=%02X %02X %02X %02X %02X %02X %02X %02X \n",  \
         send_data.can_id, send_data.can_dlc,  \
         send_data.data[0],\
         send_data.data[1],\
@@ -3253,7 +3258,7 @@ int Canbus::can_write_data(int fd, int can_id,char *data,int data_len)
         send_data.data[4],\
         send_data.data[5],\
         send_data.data[6],\
-        send_data.data[7] );
+        send_data.data[7] );*/
     }
 
     return 0;
@@ -3270,28 +3275,23 @@ int Canbus::can_read_data(int fd, int *can_id, char *data)
 {
     if(fd == -1)
     {
-        LOG(ERROR) << Name() <<"socket can read data failed!";
+        LOG(ERROR) << Name() <<" socket can read data failed!";
         return -1;
     }
     int ret = 0;
     struct can_frame recv_data;
-    recv_data.can_id = *can_id;
+   // recv_data.can_id = *can_id;
     recv_data.can_dlc = 8;
 
     ret = read(fd, &recv_data,sizeof(struct can_frame));
     if(ret == -1)
     {
-        LOG(ERROR) << Name() <<"socket can read data failed!";
+      //  LOG(ERROR) << Name() <<" socket can read data failed!";
         return -1;
     }else{
         memcpy(data, &recv_data.data[0], recv_data.can_dlc);
-        if(recv_data.can_id != *can_id)
-        {
-            LOG(ERROR) << Name() <<"socket can read recv other can id data";
-
-            return -1;
-        }
-    printf("ID=%03X, DLC=%d, data=%02X %02X %02X %02X %02X %02X %02X %02X \n",  \
+        *can_id = recv_data.can_id;
+    /*printf("RECV ID=%03X, DLC=%d, data=%02X %02X %02X %02X %02X %02X %02X %02X \n",  \
       recv_data.can_id, recv_data.can_dlc,  \
       recv_data.data[0],\
       recv_data.data[1],\
@@ -3300,7 +3300,8 @@ int Canbus::can_read_data(int fd, int *can_id, char *data)
       recv_data.data[4],\
       recv_data.data[5],\
       recv_data.data[6],\
-      recv_data.data[7] );
+      recv_data.data[7] );*/
+     // LOG(INFO) << Name() <<" socket can read data success!";
     }
 
     return 0;

src/canbus/candatatype.h

@@ -299,8 +299,8 @@ typedef struct
   unsigned int :3;
   unsigned int uiPsngLrnReq:3;		//位置学习请求确认
   unsigned int :5;
-  unsigned int uiSysLrnReq:3;			//系统学习请求
-  unsigned int :5;
+  unsigned int uiSysLrnReq:4;			//系统学习请求
+  unsigned int :4;
 }T_CONTROL_CMD_7F7;
 
 typedef struct
@@ -323,6 +323,9 @@ typedef struct
 #define	SYS_LRN_REQ_STR			4	//转向学习
 #define	SYS_LRN_REQ_AUTO_SFT	5	//自动档学习
 #define	SYS_LRN_REQ_HAND_SFT	6	//手动档学习
+#define	SYS_LRN_REQ_ACC_AUTO			7	//油门自动学习(脱机)
+#define	SYS_LRN_REQ_BRK_AUTO			8	//制动踏板自动学习(脱机)
+#define	SYS_LRN_REQ_STR_AUTO			9	//转向自动学习(脱机)
 
 #define	PSNG_LRN_REQ_ZERO		1	//位置学习-零位学习
 #define	PSNG_LRN_REQ_MAX		2	//位置学习-最大值学习

src/localization/CMakeLists.txt

@@ -223,3 +223,4 @@ target_link_libraries(localization
     glog
     gflags
     acts2110)
+

src/localization/localization.cpp

@@ -69,70 +69,102 @@ int Localization::Init() {
   }
   else if(car_input_mode == "pci")
   {
-    // 第一步 创建设备对象
-     hDevice = ACTS2110_DEV_Create(0, 0);
-     if (hDevice == NULL)
-     {
-       LOG(ERROR)<<Name()<< "PCI DEV_Create Error";
-       return ROBOT_FAILTURE;
-     }
-     memset(&AIParam, 0, sizeof(ACTS2110_AI_PARAM));
-     // 通道参数
-     AIParam.nSampChanCount = 1;
-     for (nChannel = 0; nChannel < MainInfo.nAIChannelCount; nChannel++)
-     {
-         AIParam.CHParam[nChannel].nChannel = nChannel;
-         AIParam.CHParam[nChannel].nSampleRange = ACTS2110_AI_SAMPRANGE_N10_P10V;
-         AIParam.CHParam[nChannel].nRefGround = ACTS2110_AI_REFGND_RSE;
-         AIParam.CHParam[nChannel].nReserved0 = 0;
-         AIParam.CHParam[nChannel].nReserved1 = 0;
-         AIParam.CHParam[nChannel].nReserved2 = 0;
-     }
-     AIParam.nSampleSignal = ACTS2110_AI_SAMPSIGNAL_2D5V;	// ACTS2110_AI_SAMPSIGNAL_0V
-
-     // 时钟参数
-     AIParam.fSampleRate = fSampleRate;
-     AIParam.nSampleMode = ACTS2110_AI_SAMPMODE_CONTINUOUS;
-     AIParam.nSampsPerChan = 1024;
-     AIParam.nSampClkSource = ACTS2110_AIO_SAMPCLKSRC_LOCAL;
-     AIParam.nClockOutput = ACTS2110_AIO_CLKOUT_DISABLE;
-     AIParam.StartTrig.nSyncTSOut = ACTS2110_AIO_STSO_DISABLE;
-
-     // 触发参数
-     AIParam.StartTrig.nTriggerType = ACTS2110_AI_START_TRIGTYPE_NONE;
-     AIParam.StartTrig.nTriggerSource = ACTS2110_AI_TRIG_SRC_FIRST;
-     AIParam.StartTrig.nTriggerDir = ACTS2110_AI_TRIGDIR_FALLING;
-     AIParam.StartTrig.fTriggerLevelTop = 1.0;
-     AIParam.StartTrig.fTriggerLevelBtm = 0.0;
-     AIParam.StartTrig.nTriggerSens = 0;
-     AIParam.StartTrig.nDelaySamps = 0;
-     AIParam.StartTrig.nReTrigger = 0;
-
-     AIParam.PauseTrig.nTriggerType = ACTS2110_AI_START_TRIGTYPE_NONE;
-     AIParam.PauseTrig.nTriggerSource = ACTS2110_AI_TRIG_SRC_FIRST;
-     AIParam.PauseTrig.nTriggerDir = ACTS2110_AI_TRIGDIR_FALLING;
-     AIParam.PauseTrig.fTriggerLevelTop = 1.0;
-     AIParam.PauseTrig.fTriggerLevelBtm = 0.0;
-     AIParam.PauseTrig.nTriggerSens = 0;
-
-
-     // 其他参数
-     AIParam.nReserved1 = 0;
-     AIParam.nReserved2 = 0;
-     AIParam.nReserved3 = 0;
-
-     if (!ACTS2110_AI_VerifyParam(hDevice, &AIParam))
-     {
-       LOG(ERROR)<<Name()<< "PCI Wrong parameter, it has been adjusted to legal value，Please check the log file acts2110.log";
-       return ROBOT_FAILTURE;
-     }
-
-     // 第三步 开始AI采集任务
-      if (!ACTS2110_AI_StartTask(hDevice))
-      {
-          LOG(ERROR)<<Name()<< "PCI AI_StartTask Error";
-          return ROBOT_FAILTURE;
-      }
+system("sudo chmod 666 /dev/acts2110-0");
+   // 第一步 创建设备对象
+    hDevice = ACTS2110_DEV_Create(0, 0);
+    if (hDevice == NULL)
+    {
+        LOG(ERROR)<<Name()<< "PCI DEV_Create Error";
+        return ROBOT_FAILTURE;
+    }
+
+    ACTS2110_GetMainInfo(hDevice, &MainInfo);			// DDR2的长度(单位：MB)
+    switch (MainInfo.nDeviceType >> 16)
+    {
+    case 0x2012:
+        printf("PXIE%04X\n", MainInfo.nDeviceType & 0xFFFF);
+        break;
+    case 0x2011:
+        printf("PCIE%04X\n", MainInfo.nDeviceType & 0xFFFF);
+        break;
+    default:
+        printf("ACTS2110-%04X\n", MainInfo.nDeviceType);
+    }
+
+    memset(&AIParam, 0, sizeof(ACTS2110_AI_PARAM));
+
+    // 通道参数
+    AIParam.nSampChanCount = 1;
+    for (nChannel = 0; nChannel < MainInfo.nAIChannelCount; nChannel++)
+    {
+        AIParam.CHParam[nChannel].nChannel = nChannel;
+        AIParam.CHParam[nChannel].nSampleRange = ACTS2110_AI_SAMPRANGE_N10_P10V;
+        AIParam.CHParam[nChannel].nRefGround = ACTS2110_AI_REFGND_RSE;
+        AIParam.CHParam[nChannel].nReserved0 = 0;
+        AIParam.CHParam[nChannel].nReserved1 = 0;
+        AIParam.CHParam[nChannel].nReserved2 = 0;
+    }
+    AIParam.nSampleSignal = ACTS2110_AI_SAMPSIGNAL_AI;	// ACTS2110_AI_SAMPSIGNAL_0V
+
+    // 时钟参数
+    AIParam.fSampleRate = fSampleRate;
+    AIParam.nSampleMode = ACTS2110_AI_SAMPMODE_CONTINUOUS;
+    AIParam.nSampsPerChan = 1024;
+    AIParam.nSampClkSource = ACTS2110_AIO_SAMPCLKSRC_LOCAL;
+    AIParam.nClockOutput = ACTS2110_AIO_CLKOUT_DISABLE;
+    AIParam.StartTrig.nSyncTSOut = ACTS2110_AIO_STSO_DISABLE;
+
+    // 触发参数
+    AIParam.StartTrig.nTriggerType = ACTS2110_AI_START_TRIGTYPE_NONE;
+    AIParam.StartTrig.nTriggerSource = ACTS2110_AI_TRIG_SRC_FIRST;
+    AIParam.StartTrig.nTriggerDir = ACTS2110_AI_TRIGDIR_FALLING;
+    AIParam.StartTrig.fTriggerLevelTop = 1.0;
+    AIParam.StartTrig.fTriggerLevelBtm = 0.0;
+    AIParam.StartTrig.nTriggerSens = 0;
+    AIParam.StartTrig.nDelaySamps = 0;
+    AIParam.StartTrig.nReTrigger = 0;
+
+    AIParam.PauseTrig.nTriggerType = ACTS2110_AI_START_TRIGTYPE_NONE;
+    AIParam.PauseTrig.nTriggerSource = ACTS2110_AI_TRIG_SRC_FIRST;
+    AIParam.PauseTrig.nTriggerDir = ACTS2110_AI_TRIGDIR_FALLING;
+    AIParam.PauseTrig.fTriggerLevelTop = 1.0;
+    AIParam.PauseTrig.fTriggerLevelBtm = 0.0;
+    AIParam.PauseTrig.nTriggerSens = 0;
+
+
+    // 其他参数
+    AIParam.nReserved1 = 0;
+    AIParam.nReserved2 = 0;
+    AIParam.nReserved3 = 0;
+
+    if (!ACTS2110_AI_VerifyParam(hDevice, &AIParam))
+    {
+      LOG(ERROR)<<Name()<< "PCI Wrong parameter, it has been adjusted to legal value，Please check the log file acts2110.log";
+      return ROBOT_FAILTURE;
+    }
+
+    // 第二步 初始化AI采集任务
+    if (!ACTS2110_AI_InitTask(hDevice, &AIParam))
+    {
+        LOG(ERROR)<<Name()<< "PCI AI_InitTask Error,Please refer to log file";
+        ACTS2110_DEV_Release(hDevice);
+        return ROBOT_FAILTURE;
+    }
+
+    // 第三步 开始AI采集任务
+    if (!ACTS2110_AI_StartTask(hDevice))
+    {
+      LOG(ERROR)<<Name()<< "PCI AI_StartTask Error";
+      return ROBOT_FAILTURE;
+    }
+
+    // 第四步 发送软件触发事件(硬件外触发时不需要)
+    if (!ACTS2110_AI_SendSoftTrig(hDevice))
+    {
+        printf("AI_SendSoftTrig Error\n");
+    }
+
+    printf("Wait...\n");
   }
     //TODO
   node_handle_.reset(new ros::NodeHandle());
@@ -153,7 +185,7 @@ int Localization::Start() {
   //TODO dakai
 if (car_input_mode == "modbus") {
     if (!ros::param::get("car_input_volspeed_max", car_input_volspeed_max))
-        car_input_volspeed_max = 0;
+        car_input_volspeed_max = 100;
 
     ros::Rate loop_rate(rate);
     while(ros::ok())
@@ -242,6 +274,8 @@ if (car_input_mode == "modbus") {
     }
   }
 else if(car_input_mode == "pci"){
+    if (!ros::param::get("car_input_volspeed_max", car_input_volspeed_max))
+        car_input_volspeed_max = 100;
   ros::Rate loop_rate(rate);
   while(ros::ok())
   {
@@ -255,7 +289,7 @@ else if(car_input_mode == "pci"){
       {
           continue;
       }
-      LOG(INFO) << Name() << "PCI nSampsPerChanRead= " << nSampsPerChanRead;
+      //LOG(INFO) << Name() << "PCI nSampsPerChanRead= " << nSampsPerChanRead;
       double voltage = 0;
       for (U32 nIndex = 0; nIndex < 1; nIndex++)
       {
@@ -265,13 +299,15 @@ else if(car_input_mode == "pci"){
             printf("AI%02d=%.4f\t\n", nChannel, fVoltArray[nChannel + nIndex * AIParam.nSampChanCount]);
           }
       }
+      LOG(INFO) << Name() << "PCI voltage= " << voltage;
       try{
           localization::localMsg localization;
           localization.Longitude =  0;
           localization.Latitude = 0;
           localization.Height = 0;
           localization.AngleDeviated = 0;
-          localization.Speed = voltage;
+          int max = car_input_volspeed_max;
+          localization.Speed = max / 10 * voltage;
           if(localization.Speed < 0.4)
           {
           localization.Speed = 0;

src/localization/localization.h

@@ -156,7 +156,7 @@ private:
 
   //----------pci
   #define CH_PERCHAN 102400
-  F64 fVoltArray[ACTS2110_AI_MAX_CHANNELS * CH_PERCHAN];
+  F64 fVoltArray[CH_PERCHAN];
   ACTS2110_AI_PARAM AIParam;
   U32 nReadSampsPerChan = CH_PERCHAN;	// 每通道读取点数
   U32 nSampsPerChanRead = 0;
@@ -165,7 +165,7 @@ private:
   HANDLE hDevice = NULL;
   ACTS2110_MAIN_INFO MainInfo;
   U32 nChannel = 0;
-  F64 fSampleRate = 100.0;  //100sps = 100Hz
+  F64 fSampleRate = 50000.0;  //100sps = 100Hz
 
 };