/*********functions which permit to communicate with the board****************/
UNS8 canReceive_driver(CAN_HANDLE fd0, Message *m)
{
UNS8 data;
TPCANMsg peakMsg;
if ((errno = CAN_Read(fd0, & peakMsg))) { // Blocks until no new message or error.
if(errno != -EIDRM && errno != -EPERM) // error is not "Can Port closed while reading"
{
perror("canReceive_driver (Peak_Linux) : error of reading.\n");
}
return 1;
}
m->cob_id = peakMsg.ID;
if (peakMsg.MSGTYPE == CAN_INIT_TYPE_ST) /* bits of MSGTYPE_*/
m->rtr = 0;
else
m->rtr = 1;
m->len = peakMsg.LEN; /* count of data bytes (0..8) */
for(data = 0 ; data < peakMsg.LEN ; data++)
m->data[data] = peakMsg.DATA[data]; /* data bytes, up to 8 */
#if defined DEBUG_MSG_CONSOLE_ON
MSG("in : ");
print_message(m);
#endif
return 0;
}
int arm_recvack(uint16_t joint, uint8_t cmd, uint8_t index){
TPCANMsg rxmsg;
//TODO: becareful the thread
memset(&rxmsg,0,sizeof(TPCANMsg));
/* Read the RX message */
if(CAN_Read(h,&rxmsg))
{
printf("ERROR: read ack error returned 1\n");
return -1;
}
if(rxmsg.ID != (u_int32_t)(joint + (uint16_t)0x100)){
printf("ERROR: read ack ID error\n");
return -1;
}
if(rxmsg.LEN != 0x03){
printf("ERROR: read ack DLC error\n");
return -1;
}
if(rxmsg.DATA[0] != cmd){
printf("ERROR: read ack cmd error\n");
return -1;
}
if(rxmsg.DATA[1] != index){
printf("ERROR: read ack index error\n");
return -1;
}
return 0;
}
// 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;
}
static void odometry_query_position(void)
{
uint8_t buffer[8];
buffer[0] = 'P';
while(CAN_Write(buffer, DRIVER_TX_IDENTIFICATOR))
_delay_ms(50);
CAN_Read(buffer, DRIVER_RX_IDENTIFICATOR);
position.state = buffer[0];
position.x = (buffer[1] << 8) | buffer[2];
position.y = (buffer[3] << 8) | buffer[4];
position.angle = (buffer[5] << 8) | buffer[6];
}
int arm_readmsg(int joint, uint8_t addr, int8_t length, uint8_t *data){
TPCANMsg rxmsg;
ssize_t nbytes;
uint8_t *temp = data;
int i;
if(arm_sendmsg(joint, CMDTYPE_RD, addr, NULL, length)!=0){
printf("send read cmd is error!\n");
return -1;
}
for(; length>0; ){
//TODO: becareful the thread
memset(&rxmsg,0,sizeof(TPCANMsg));
/* Read the RX message */
nbytes = CAN_Read(h,&rxmsg);
if (nbytes)
{
printf("ERROR: read msg error returned %d\n", nbytes);
return -1;
}
//check the CAN-ID and cmd and index is OK? if no ,return -1 ;
if(rxmsg.ID != (u_int32_t)(joint + (uint16_t)0x100)){
printf("ERROR: read msg ID error\n");
return -1;
}
if(rxmsg.DATA[0] != CMDTYPE_RD){
printf("ERROR: read msg cmd error\n");
return -1;
}
if(nbytes == 0){
printf("read Data :");
for (i = 0; i < rxmsg.LEN; i++)
printf("%x ",rxmsg.DATA[i]);
printf("\n");
memcpy(temp, rxmsg.DATA+2, rxmsg.LEN-2);
temp += (rxmsg.LEN-2);
}
length = length - (rxmsg.LEN - 2);
}
return 0;
}
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;
}
bool PcanPci::readFrame(TPCANMsg &msg)
{
//while((getStatus() & 0x20)==0x20) std::cerr << "0";
//std::cerr << std::endl;
//std::cerr << "Status: " << std::hex << getStatus() << std::endl;
//std::cerr << "Error: " << std::hex << getError() << std::endl;
#ifdef WIN32
int errorcode = CAN_Read(pcanHandle, &msg);
#else
TPCANRdMsg rmsg;
int errorcode = LINUX_CAN_Read_Timeout(pcanHandle, &rmsg, 100000);
memcpy(&msg, &rmsg.Msg, sizeof(TPCANMsg));
#endif
if (errorcode != 0)
{
std::cerr << "readFrame error, code: " << std::hex << errorcode << std::endl;
return false;
}
else
{
//std::cerr << "Received "; printMsg(msg);
return true;
}
}
void PcanPci::emptyReadQueue()
{
TPCANMsg msg;
while ((getStatus() & 0x20) != 0x20)
CAN_Read(pcanHandle, &msg);
}
