//-------------------------------------------
bool CanPeakSys::transmitMsg(CanMsg CMsg, bool bBlocking)
{
TPCANMsg TPCMsg;
bool bRet = true;
if (m_bInitialized == false) return false;
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(int i=0; i<8; i++)
TPCMsg.DATA[i] = CMsg.getAt(i);
// write msg
int iRet;
iRet = CAN_Write(m_handle, &TPCMsg);
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
std::cout << "CanPeakSys::transmitMsg, errorcode= " << nGetLastError() << std::endl;
bRet = false;
}
return bRet;
}
// read from CAN forever - until manual break
int read_loop(bool display_on)
{
// read in endless loop until Ctrl-C
while (1) {
TPCANRdMsg m;
__u32 status;
if (LINUX_CAN_Read(h, &m)) {
perror("receivetest: LINUX_CAN_Read()");
return errno;
}
if (display_on)
print_message_ex(&m);
// check if a CAN status is pending
if (m.Msg.MSGTYPE & MSGTYPE_STATUS) {
status = CAN_Status(h);
if ((int)status < 0) {
errno = nGetLastError();
perror("receivetest: CAN_Status()");
return errno;
}
printf("receivetest: pending CAN status 0x%04x read.\n",
(__u16)status);
}
}
return 0;
}
//-------------------------------------------
bool CANPeakSys2PCI::transmitMsg(CanMsg& CMsg)
{
TPCANMsg TPCMsg;
bool bRet = true;
if (m_bInitialized == false) return false;
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(int i=0; i<8; i++)
TPCMsg.DATA[i] = CMsg.getAt(i);
// write msg
int iRet;
iRet = CAN_Write(m_handle, &TPCMsg);
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
LOGERROR( "transmitMsg() : errorcode= " << nGetLastError() );
bRet = false;
}
return bRet;
}
//-------------------------------------------
bool CANPeakSys2PCI::receiveMsg(CanMsg* pCMsg)
{
TPCANRdMsg TPCMsg;
TPCMsg.Msg.LEN = 8;
TPCMsg.Msg.MSGTYPE = 0;
TPCMsg.Msg.ID = 0;
int iRet = CAN_ERR_OK;
bool bRet = false;
if (m_bInitialized == false) return false;
iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0);
if (iRet == CAN_ERR_OK)
{
pCMsg->m_iID = TPCMsg.Msg.ID;
pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3],
TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]);
bRet = true;
}
else if (CAN_Status(m_handle) != CAN_ERR_QRCVEMPTY)
{
// Error (Don't output error of empty queue)
LOGERROR( "receiveMsg() : errorcode= " << nGetLastError() );
pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
}
return bRet;
}
//-------------------------------------------
bool CANPeakSysDongle::transmitMsg(CanMsg& CMsg)
{
bool bRet;
int i, iRet;
TPCANMsg TPCMsg;
if(!m_bInitialized) { return false; }
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(i = 0; i < 8; i++)
{
TPCMsg.DATA[i] = CMsg.getAt(i);
}
// write msg
bRet = true;
// std::cout<<"sending command: "<<std::endl;
CAN_Write(m_handle, &TPCMsg);
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
std::cout<<"transmitMsg() : errorcode= " << nGetLastError()<<std::endl;
bRet = false;
}
return bRet;
}
//-------------------------------------------
bool CanPeakSys::receiveMsg(CanMsg* pCMsg)
{
TPCANRdMsg TPCMsg;
TPCMsg.Msg.LEN = 8;
TPCMsg.Msg.MSGTYPE = 0;
TPCMsg.Msg.ID = 0;
int iRet = CAN_ERR_OK;
bool bRet = false;
if (m_bInitialized == false) return false;
iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0);
if (iRet == CAN_ERR_OK)
{
pCMsg->m_iID = TPCMsg.Msg.ID;
pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3],
TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]);
bRet = true;
}
else if (CAN_Status(m_handle) != CAN_ERR_QRCVEMPTY)
{
std::cout << "CanPeakSys::receiveMsg ERROR: iRet = " << iRet << std::endl;
pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
}
else
{
// make sure there's never an undefined state (even when can drivers fail)
pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
}
return bRet;
}
// read from CAN forever - until manual break
int read_loop()
{
// read in endless loop until Ctrl-C
while (1)
{
TPCANMsg m;
__u32 status;
if ((errno = CAN_Read(h, &m)))
{
perror("receivetest: CAN_Read()");
return errno;
}
else
{
print_message(&m);
// check if a CAN status is pending
if (m.MSGTYPE & MSGTYPE_STATUS)
{
status = CAN_Status(h);
if ((int)status < 0)
{
errno = nGetLastError();
perror("receivetest: CAN_Status()");
return errno;
}
else
printf("receivetest: pending CAN status 0x%04x read.\n", (__u16)status);
}
}
}
return 0;
}
//-------------------------------------------
bool CANPeakSysUSB::transmitMsg(CanMsg CMsg, bool bBlocking)
{
TPCANMsg TPCMsg;
bool bRet = true;
if (m_bInitialized == false) return false;
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(int i=0; i<8; i++)
TPCMsg.DATA[i] = CMsg.getAt(i);
//TODO Hier stürtzt die Base ab.. verwende libpcan.h pcan.h um Fehler auszulesen, diagnostizieren, ausgeben und CAN_INIT erneut aufzurufen = neustart can-hardware.
int iRet;
//iRet = CAN_Write(m_handle, &TPCMsg);
iRet = LINUX_CAN_Write_Timeout(m_handle, &TPCMsg, 25); //Timeout in micrsoseconds
if(iRet != CAN_ERR_OK) {
#ifdef __DEBUG__
std::cout << "CANPeakSysUSB::transmitMsg An error occured while sending..." << iRet << std::endl;
outputDetailedStatus();
#endif
bRet = false;
}
#ifdef __DEBUG__
//is this necessary? try iRet==CAN_Status(m_handle) ?
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
std::cout << "CANPeakSysUSB::transmitMsg, system error: " << iRet << std::endl;
bRet = false;
} else if((iRet & CAN_ERR_BUSOFF) != 0) {
std::cout << "CANPeakSysUSB::transmitMsg, BUSOFF detected" << std::endl;
//Try to restart CAN-Device
std::cout << "Trying to re-init Hardware..." << std::endl;
bRet = initCAN();
} else if((iRet & CAN_ERR_ANYBUSERR) != 0) {
std::cout << "CANPeakSysUSB::transmitMsg, ANYBUSERR" << std::endl;
} else if( (iRet & (~CAN_ERR_QRCVEMPTY)) != 0) {
std::cout << "CANPeakSysUSB::transmitMsg, CAN_STATUS: " << iRet << std::endl;
bRet = false;
}
#endif
return bRet;
}
int main(int argc, char *argv[])
{
int i, err;
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
mlockall(MCL_CURRENT | MCL_FUTURE);
can_handle = CAN_Open(HW_PCI, 1);
CAN_Init(can_handle, CAN_BAUD_1M, CAN_INIT_TYPE_EX);
printf("Status = %i\n", CAN_Status(can_handle));
for (i = 0; i < 5; i++) {
can_msg[i].MSGTYPE = MSGTYPE_EXTENDED;
can_msg[i].LEN = 8;
printf("Frame = %08lx %02x %02x %02x %02x %02x %02x %02x %02x, "
"Time diff = %llu ns\n",
(unsigned long) (can_msg[i].ID = i*13),
can_msg[i].DATA[0] = i*6,
can_msg[i].DATA[1] = i*7,
can_msg[i].DATA[2] = i*8,
can_msg[i].DATA[3] = i*9,
can_msg[i].DATA[4] = i*10,
can_msg[i].DATA[5] = i*11,
can_msg[i].DATA[6] = i*12,
can_msg[i].DATA[7] = i*13,
i == 0 ? 0 : (unsigned long long) TIME_DIFF
);
}
err = rt_task_create(&test_task, "test", 0, 99, T_JOINABLE);
if (err) {
printf("receivetest: Failed to create rt task, code %d\n",
errno);
return err;
}
rt_task_start(&test_task, test, NULL);
if (err) {
printf("receivetest: Failed to start rt task, code %d\n",
errno);
return errno;
}
rt_task_join(&test_task);
CAN_Close(can_handle);
return 0;
}
int main()
{
int i=0;
int err=0;
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
mlockall(MCL_CURRENT | MCL_FUTURE);
can_handle = CAN_Open(HW_PCI, 1);
CAN_Init(can_handle, CAN_BAUD_1M, CAN_INIT_TYPE_EX);
printf("Status = %i\n", CAN_Status(can_handle));
err=rt_task_create(&test_task, "test", 0, 99, T_JOINABLE);
if (err) {
printf("receivetest: Failed to create rt task, code %d\n",errno);
return err;
}
rt_task_start(&test_task, test, NULL);
if (err) {
printf("receivetest: Failed to start rt task, code %d\n",errno);
return errno;
}
rt_task_join(&test_task);
for (i = 0; i < 5; i++)
{
printf(
"Frame = %08lx %02x %02x %02x %02x %02x %02x %02x %02x, Time diff = %llu ns\n",
(unsigned long) can_msg[i].Msg.ID,
can_msg[i].Msg.DATA[0],
can_msg[i].Msg.DATA[1],
can_msg[i].Msg.DATA[2],
can_msg[i].Msg.DATA[3],
can_msg[i].Msg.DATA[4],
can_msg[i].Msg.DATA[5],
can_msg[i].Msg.DATA[6],
can_msg[i].Msg.DATA[7],
i == 0 ? 0 :
(can_msg[i].dwTime*(unsigned long long)1000+can_msg[i].wUsec)*1000-
(can_msg[i-1].dwTime*(unsigned long long)1000+can_msg[i-1].wUsec)*1000
);
}
CAN_Close(can_handle);
return 0;
}
//-------------------------------------------
bool CANPeakSysDongle::receiveMsg(CanMsg* pCMsg)
{
TPCANRdMsg TPCMsg;
TPCMsg.Msg.LEN = 8;
TPCMsg.Msg.MSGTYPE = 0;
TPCMsg.Msg.ID = 0;
int iRet = CAN_ERR_OK;
bool bRet = false;
if(!m_bInitialized) { return false; }
iRet = LINUX_CAN_Read_Timeout(m_handle, &TPCMsg, 0);
if (iRet == CAN_ERR_OK)
{
pCMsg->m_iID = TPCMsg.Msg.ID;
pCMsg->set(TPCMsg.Msg.DATA[0], TPCMsg.Msg.DATA[1], TPCMsg.Msg.DATA[2], TPCMsg.Msg.DATA[3],
TPCMsg.Msg.DATA[4], TPCMsg.Msg.DATA[5], TPCMsg.Msg.DATA[6], TPCMsg.Msg.DATA[7]);
bRet = true;
}
else if(TPCMsg.Msg.MSGTYPE & MSGTYPE_STATUS)
{
iRet = CAN_Status(m_handle);
if (iRet < 0)
{
errno = nGetLastError();
perror("receivetest: CAN_Status()");
return false;
}
else
{
printf("receivetest: pending CAN status 0x%04x read.\n", (__u16)iRet);
}
pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
if(iRet != CAN_ERR_QRCVEMPTY)
{
// Error (Don't output error of empty queue)
std::cout<< "receiveMsg() : errorcode= " << nGetLastError()<<std::endl;
pCMsg->set(0, 0, 0, 0, 0, 0, 0, 0);
}
}
return bRet;
}
int BVCan::read( void *param ){
// IMPORTANT : msg should be a pointer to a string
int count=0;
#ifdef USE_CAN
TPCANMsg canmsg;
errno = CAN_Read( h, &canmsg);
if (errno){
BV_WARNING("BVCan::read XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
perror("BVCan::read CAN_Read() return ERROR, means that I did not read anything at all");
BV_WARNING("BVCan::read XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXxXXXXXXXXX");
BV_ASSERT(0);
}
else{
if ( (int)(canmsg.MSGTYPE) == MSGTYPE_STATUS){
__u32 status = CAN_Status(h);
BV_WARNING("BVCan::read E R R O R : peak insert a ERROR message in Q with messageType=0x80 CAN_Status(h)=[" << status <<"]");
}
count = (sizeof( canmsg.ID )) +
sizeof( canmsg.MSGTYPE )+
sizeof( canmsg.LEN )+
canmsg.LEN * sizeof( canmsg.DATA[0] );
BVMessageTPCANMsg mes(0,0,canmsg.ID ,canmsg.MSGTYPE,canmsg.LEN,
canmsg.DATA[0],canmsg.DATA[1],canmsg.DATA[2],
canmsg.DATA[3],canmsg.DATA[4],canmsg.DATA[5],
canmsg.DATA[6],canmsg.DATA[7]);
*((string*)param) = mes.objectToString();
}
#endif
return count;
}
int main(int argc, char *argv[])
{
char *ptr;
int i;
int nType = HW_PCI;
__u32 dwPort = 0;
__u16 wIrq = 0;
__u16 wBTR0BTR1 = 0;
__u32 dwMaxTimeInterval = 0, dwMaxLoop = 0;
char *filename = NULL;
const char *szDevNode = DEFAULT_NODE;
bool bDevNodeGiven = false;
bool bTypeGiven = false;
parser MyParser;
char txt[VERSIONSTRING_LEN];
errno = 0;
current_release = CURRENT_RELEASE;
disclaimer("transmitest");
init();
// decode command line arguments
for (i = 1; i < argc; i++)
{
char c;
ptr = argv[i];
if (*ptr == '-')
{
while (*ptr == '-')
ptr++;
c = *ptr;
ptr++;
if (*ptr == '=')
ptr++;
switch(tolower(c))
{
case 'f':
szDevNode = ptr;
bDevNodeGiven = true;
break;
case 't':
nType = getTypeOfInterface(ptr);
if (!nType)
{
errno = EINVAL;
printf("transmitest: unknown type of interface\n");
goto error;
}
bTypeGiven = true;
break;
case 'p':
dwPort = strtoul(ptr, NULL, 16);
break;
case 'i':
wIrq = (__u16)strtoul(ptr, NULL, 10);
break;
case 'e':
nExtended = CAN_INIT_TYPE_EX;
break;
case '?':
case 'h':
hlpMsg();
goto error;
break;
case 'b':
wBTR0BTR1 = (__u16)strtoul(ptr, NULL, 16);
break;
case 'r':
dwMaxTimeInterval = strtoul(ptr, NULL, 10);
break;
case 'n':
dwMaxLoop = strtoul(ptr, NULL, 10);
break;
default:
errno = EINVAL;
printf("transmitest: unknown command line argument\n");
goto error;
break;
}
}
else
filename = ptr;
}
// test for filename
if (filename == NULL)
{
errno = EINVAL;
perror("transmitest: no filename given");
goto error;
}
// test device node and type
if (bDevNodeGiven && bTypeGiven)
{
errno = EINVAL;
perror("transmitest: device node and type together is useless");
goto error;
}
// give the filename to my parser
MyParser.setFileName(filename);
// tell some information to the user
if (!bTypeGiven)
{
printf("transmitest: device node=\"%s\"\n", szDevNode);
}
else {
printf("transmitest: type=%s", getNameOfInterface(nType));
if (nType == HW_USB)
printf(", Serial Number=default, Device Number=%d\n", dwPort);
else {
if (dwPort)
{
if (nType == HW_PCI)
printf(", %d. PCI device", dwPort);
else
printf(", port=0x%08x", dwPort);
}
else
printf(", port=default");
if ((wIrq) && !(nType == HW_PCI))
printf(" irq=0x%04x\n", wIrq);
else
printf(", irq=default\n");
}
}
if (nExtended == CAN_INIT_TYPE_EX)
printf(" Extended frames are sent");
else
printf(" Only standard frames are sent");
if (wBTR0BTR1)
printf(", init with BTR0BTR1=0x%04x\n", wBTR0BTR1);
else
printf(", init with 500 kbit/sec.\n");
printf(" Data will be read from \"%s\".\n", filename);
if (dwMaxTimeInterval)
printf(" Messages are send in random time intervalls with a max. gap time of %d msec.\n", dwMaxTimeInterval);
/* get the list of data from parser */
List = MyParser.Messages();
if (!List)
{
errno = MyParser.nGetLastError();
perror("transmitest: error at file read");
goto error;
}
/* open CAN port */
if ((bDevNodeGiven) || (!bDevNodeGiven && !bTypeGiven))
{
h = LINUX_CAN_Open(szDevNode, O_RDWR);
if (!h)
{
printf("transmitest: can't open %s\n", szDevNode);
goto error;
}
}
else {
// please use what is appropriate
// HW_DONGLE_SJA
// HW_DONGLE_SJA_EPP
// HW_ISA_SJA
// HW_PCI
h = CAN_Open(nType, dwPort, wIrq);
if (!h)
{
printf("transmitest: can't open %s device.\n", getNameOfInterface(nType));
goto error;
}
}
/* clear status */
CAN_Status(h);
// get version info
errno = CAN_VersionInfo(h, txt);
if (!errno)
printf("transmitest: driver version = %s\n", txt);
else {
perror("transmitest: CAN_VersionInfo()");
goto error;
}
// init to a user defined bit rate
if (wBTR0BTR1)
{
errno = CAN_Init(h, wBTR0BTR1, nExtended);
if (errno)
{
perror("transmitest: CAN_Init()");
goto error;
}
}
// enter in the write loop
errno = write_loop(dwMaxTimeInterval, dwMaxLoop);
if (!errno)
return 0;
error:
do_exit(errno);
return errno;
}
void BVCan::initializeCan( ){
#ifdef USE_CAN
WORD awBTR0BTR1=0x0014;
int aCANMsgType=0;
h = NULL;
errno = 0;
device = string("/dev/pcan32");
// open the device
// // O_RDWR which request opening the file read/write
h = LINUX_CAN_Open((char*)(device.c_str()), O_RDWR);
if (h==0)
{
device = string("/dev/pcan0");
h = LINUX_CAN_Open((char*)(device.c_str()), O_RDWR);
}
if(h==0){
cerr<< "PCan_Device nicht gefunden !!!"<<endl;
exit(37);
}
if(h){
PCAN_DESCRIPTOR *desc= (PCAN_DESCRIPTOR *)h;
BV_ASSERT( desc->nFileNo>0 );
BV_DEBUGINFO4NNL("BVCan::ctor device=[" << desc->szDevicePath << "]");
BV_DEBUGINFO4C(" fd=[" << desc->nFileNo << "]" << endl);
setfd(desc->nFileNo);
CAN_Status(h);
// -----------------------------------------------------------
// returns a text string with driver version info
errno = CAN_VersionInfo(h, info);
if (!errno){
string infostr( string(info) );
BV_DEBUGINFO4("BVCan::ctor driver version=[" << infostr << "]");
}
else
perror("BVCan::ctor CAN_VersionInfo()");
wBTR0BTR1 = 0x401c;
if(LINUX_CAN_BTR0BTR1(h,wBTR0BTR1)==0)
BV_WARNING("BVCan::ctor 0x" << hex << wBTR0BTR1 << dec << " is not possible" );
else
BV_DEBUGINFO4("BVCan::ctor baudrate changes to 0x" << hex << wBTR0BTR1 << dec );
}
else
BV_DEBUGINFO4("BVCan::ctor cannot open the can device");
BV_WARNING("BVCan::initializeCan .....................");
if(h){
// init to a user defined bit rate
//nExtended = CAN_INIT_TYPE_ST;
//wBTR0BTR1 = CAN_BAUD_500K;
canMsgType = aCANMsgType;
wBTR0BTR1 = awBTR0BTR1;
errno = CAN_Init(h, wBTR0BTR1, canMsgType);
if (errno)
perror("BVCan::initializeCan CAN_Init failed");
else
BV_WARNING("BVCan::initializeCan baudrate=0x" << hex << wBTR0BTR1 << dec << " msgType=" << canMsgType );
}
else
BV_WARNING("BVCan::initializeCan can HANDLE is NULL");
#endif
}
void _publish()
{
ROS_INFO("_publish()");
printf("yaw: %f\n", yaw_);
printf("throttle: %f\n" , throttle_);
yaw_IEEE754 = pack754(yaw_, 32, 8);
throttle_IEEE754 = pack754(throttle_, 32 , 8 );
// std::cout << "yaw ieee754: " << "0x" <<std::hex << yaw_IEEE754 << std::endl;
// std::cout << "throttle ieee754: " << "0x" <<std::hex << throttle_IEEE754 << std::endl;
// std::cout << "zero ieee754: " << "0x" <<std::hex << zero_IEEE754 << std::endl;
// std::cout << "one ieee754: " << "0x" <<std::hex << one_IEEE754 << std::endl;
std::cout << "forward: " << forward << std::endl;
std::cout << "reverse: " << reverse << std::endl;
std::cout << "reverse_on: " << reverse_on << std::endl;
if(forward) {
if(reverse_on) {
ROS_WARN_NAMED("test","writing forward throttle msg");
if(!CAN_Write(h_, &normal_throttle_msg_)) {
ROS_INFO("Success");
reverse_on=false;
usleep(250000);
}
else
ROS_ERROR("ERROR");
}
}
if(reverse) {
if(!reverse_on) {
ROS_ERROR("writing reverse throttle msg...\n");
if(!CAN_Write(h_, &reverse_throttle_msg_)) {
ROS_INFO("Success");
reverse_on=true;
usleep(250000);
}
else
ROS_ERROR("ERROR");
}
}
//steering msg
can_steering_msg_.ID = 2;
can_steering_msg_.MSGTYPE = MSGTYPE_STANDARD;
can_steering_msg_.LEN = sizeof(yaw_IEEE754);
for(int i=0 ; i<can_steering_msg_.LEN; i++ )
{
can_steering_msg_.DATA[i] = (yaw_IEEE754 >> (8*i)) & 0xff;
}
//throttle msg
can_throttle_msg_.ID = 1;
can_throttle_msg_.MSGTYPE = MSGTYPE_STANDARD;
can_throttle_msg_.LEN = sizeof(throttle_IEEE754);
for(int i=0 ; i<can_throttle_msg_.LEN; i++ )
{
can_throttle_msg_.DATA[i] = (throttle_IEEE754 >> (8*i)) & 0xff;
}
//clear CAN status
CAN_Status(h_);
std::cout << "writing steering msg..." << std::endl;
if(!CAN_Write(h_, &can_steering_msg_))
ROS_INFO("Success");
else
ROS_ERROR("ERROR");
std::cout << "writing throttle msg..." << std::endl;
if(!CAN_Write(h_, &can_throttle_msg_))
ROS_INFO("Success");
else
ROS_ERROR("ERROR");
std::cout << "i'm finished" << std::endl;
}
//constructor
CmdVelToCAN() :
yaw_(0.0),
throttle_(2000.0),
reverse_on(false),
forward(true),
reverse(false)
{
nh_.param<std::string>("cmd_vel_topic", cmd_vel_topic_, "/cmd_vel");
nh_.param<std::string>("can_device", can_device_, "/dev/pcanusb0");
cmd_vel_sub_ = nh_.subscribe(cmd_vel_topic_, 1, &CmdVelToCAN::cmd_vel_callback, this);
errno=0;
char baudrate[7] = "0x0014"; //1 Mbit/s
//char baudrate[7] = "0x001C"; //500 kbit/s
//char baudrate[7] = "0x011C"; //250 kbit/s
std::cout << "here scheppert dat" << std::endl;
int wBTR0BTR1 = (int)strtoul(baudrate, NULL, 16);
int nExtended = 0;
std::cout << wBTR0BTR1 << std::endl;
std::cout << nExtended << std::endl;
h_ = LINUX_CAN_Open(can_device_.c_str(), O_RDWR);
if (!h_) {
ROS_ERROR("CMD_VEL_TO_CAN: can't open %s\n", can_device_.c_str());
return;
}
else {
ROS_INFO("CMD_VEL_TO_CAN: succesfully opened %s\n", can_device_.c_str());
}
/* clear status */
CAN_Status(h_);
errno = CAN_Init(h_, wBTR0BTR1, 0);
std::cout << "bla bla" << std::endl;
//prepare reverse / normal throttle msgs
one_IEEE754 = 1;
zero_IEEE754 = 0;
reverse_throttle_msg_.ID = 3;
reverse_throttle_msg_.MSGTYPE = MSGTYPE_STANDARD;
reverse_throttle_msg_.LEN = 1;
reverse_throttle_msg_.DATA[0] = one_IEEE754;
normal_throttle_msg_.ID = 3;
normal_throttle_msg_.MSGTYPE = MSGTYPE_STANDARD;
normal_throttle_msg_.LEN = 1;
normal_throttle_msg_.DATA[0] = zero_IEEE754;
ros::Rate r(20);
while(ros::ok()) {
_publish();
ros::spinOnce();
r.sleep();
}
}
void pcan_transmit::init(int argc, char **argv)
{
/*!
*\brief The init() function is based on the middle part of the main() function of the transmitest program.
*
* Compared to the transmitest program a lot of cases have been removed.
* Therefore, only the PCAN-USB adapter is, and standard messages are, supported.
*
* */
int nExtended = CAN_INIT_TYPE_ST;
int nType;
__u32 dwPort;
__u16 wIrq;
char *ptr;
__u16 wBTR0BTR1 = 0x011C;
// parameter wBTR0BTR1
// bitrate codes of BTR0/BTR1 registers
//#define CAN_BAUD_1M 0x0014 // 1 MBit/s
//#define CAN_BAUD_500K 0x001C // 500 kBit/s
//#define CAN_BAUD_250K 0x011C // 250 kBit/s
//#define CAN_BAUD_125K 0x031C // 125 kBit/s
//#define CAN_BAUD_100K 0x432F // 100 kBit/s
//#define CAN_BAUD_50K 0x472F // 50 kBit/s
//#define CAN_BAUD_20K 0x532F // 20 kBit/s
//#define CAN_BAUD_10K 0x672F // 10 kBit/s
//#define CAN_BAUD_5K 0x7F7F // 5 kBit/s
const char *szDevNode = DEFAULT_NODE;
bool bDevNodeGiven = false;
bool bTypeGiven = false;
char txt[VERSIONSTRING_LEN];
/* QApplication a(argc, argv);
QWidget *mainWindow=new QWidget;
mainWindow->setWindowTitle("Change the Data Value");
QSlider *slider= new QSlider(Qt::Horizontal);
QSpinBox *spinner= new QSpinBox;
slider->setRange(0,100);
spinner->setRange(0,100);
connect(ui->slider, SIGNAL(valueChanged(double)), SLOT(onSliderValueChanged(double)));
double slidValue= ui->Slider->value();
//string msg[]= "ID, LEN, ss";*/
// decode command line arguments
for (int i = 1; i < argc; i++)
{
char c;
ptr = argv[i];
while (*ptr == '-')
ptr++;
c = *ptr;
ptr++;
if (*ptr == '=')
ptr++;
switch(tolower(c))
{
case '?':
case 'h':
hlpMsg();
do_exit(errno, h);
break;
case 'f':
szDevNode = ptr;
bDevNodeGiven = true;
break;
case 'b':
wBTR0BTR1 = (__u16)strtoul(ptr, NULL, 16);
break;
default:
errno = EINVAL;
do_exit(errno, h);;
break;
}
}
/* open CAN port */
if ((bDevNodeGiven) || (!bDevNodeGiven && !bTypeGiven))
{
h = LINUX_CAN_Open(szDevNode, O_RDWR);
if (!h)
{
printf("pcan_transmit: can't open %s\n", szDevNode);
do_exit(errno, h);;
}
}
else {
// please use what is appropriate
// HW_DONGLE_SJA
// HW_DONGLE_SJA_EPP
// HW_ISA_SJA
// HW_PCI
h = CAN_Open(nType, dwPort, wIrq);
if (!h)
{
printf("pcan_transmit: can't open %s device.\n", getNameOfInterface(nType));
do_exit(errno, h);;
}
}
/* clear status */
CAN_Status(h);
// get version info
errno = CAN_VersionInfo(h, txt);
if (!errno)
printf("pcan_transmit: driver version = %s\n", txt);
else {
perror("pcan_transmit: CAN_VersionInfo()");
do_exit(errno, h);;
}
// init to a user defined bit rate
if (wBTR0BTR1)
{
errno = CAN_Init(h, wBTR0BTR1, nExtended);
if (errno)
{
perror("pcan_transmit: CAN_Init()");
do_exit(errno, h);;
}
}
}
int PcanPci::getStatus()
{
return CAN_Status(pcanHandle);
}
