|
@@ -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;
|