int CanBus::writeWaitReadMessage(VSCAN_MSG* msg)
{
DWORD written,read;
VSCAN_MSG sended;
int retries=0, right_response = 0;
// printf("------------------------------------\n");
// printf("Write messages\n");
// printMessageData(*msg);
int status = VSCAN_Write(devHandler,msg,1,&written);
VSCAN_Flush(devHandler);
if(status != 0)
{
printf("writeWaitReadMessage() ERROR: El comando no se escribio correctamente\n");
return -1;
}
if(msg->Id == 0x0)
return 1;
usleep(2000);
memcpy(&sended, msg, sizeof(VSCAN_MSG));
// printf("received\n");
do{
if(VSCAN_Read(devHandler, msg, 1, &read) == 0 and read != 0)
{
// printMessageData(*msg);
right_response = checkMessage(&sended, msg);
}
usleep(2000);
retries++;
}while(right_response != 1 and retries < MAX_READ_RETRIES);
if(retries >= MAX_READ_RETRIES)
{
printf("writeWaitReadMessage() ERROR: No se pudo leer la respuesta al comando enviado (retries = %d)\n", retries);
printMessageData(sended);
return -1;
}
else
{
// qDebug()<<"tryed: "<<retries<<endl;
// qDebug()<<"good response";
return 1;
}
}
UNS8 canSend_driver(CAN_HANDLE fd0, Message const *m)
{
VSCAN_MSG Msg[1];
UNS8 i;
DWORD dwWritten; /* number of written frames */
/* identifier of the CAN frame */
Msg[0].Id = m->cob_id;
/* CAN frame type */
if(m->rtr == 0)
Msg[0].Flags = VSCAN_FLAGS_STANDARD;
else
Msg[0].Flags = VSCAN_FLAGS_REMOTE;
/* width of the data bytes */
Msg[0].Size = m->len;
/* copy data bytes to the CAN frame, up to 8 */
for(i = 0 ; i < m->len; i++)
Msg[0].Data[i] = m->data[i];
/* copy CAN frame to the output buffer */
if (!(VSCAN_Write((VSCAN_HANDLE)fd0, Msg, (DWORD)1, &dwWritten) == VSCAN_ERR_OK && dwWritten))
{
perror("canSend_driver (VScom): error writing to output buffer.\n");
return 1;
}
/* really send CAN frame */
if(VSCAN_Flush((VSCAN_HANDLE)fd0) != VSCAN_ERR_OK)
{
perror("canSend_driver (VScom): error flushing.\n");
return 1;
}
return 0;
}
int CanBus::multiWriteWaitReadMessage(VSCAN_MSG* msgs, int msg_count)
{
int status, retries=0;
uint8_t rec_count = 0, count_readed = 0;
DWORD written,read;
VSCAN_MSG sended_msgs[msg_count] , readed_msgs[msg_count];
memcpy(sended_msgs, msgs, sizeof(VSCAN_MSG)*msg_count);
QVector<VSCAN_MSG> cmds_tocheck;
for(int x=0;x<msg_count;x++)
cmds_tocheck.push_back(msgs[x] );
// printf("------------------------------------\n");
// printf("Initial messages:\n");
// for(mcount=0;mcount<msg_count;mcount++)
// printMessageData(msgs[mcount]);
status = VSCAN_Write(devHandler,msgs,msg_count,&written);
VSCAN_Flush(devHandler);
if(msgs[0].Id / 0x100 == 3)
return 1;
if(status != 0)
{
printf("writeWaitReadMessage() ERROR: El comando no se escribio correctamente\n");
return -1;
}
do{
if(VSCAN_Read(devHandler, readed_msgs, msg_count, &read) == 0 and read != 0)
{
for( int i=0;i< msg_count;i++)
{
QVector<VSCAN_MSG>::iterator it;
for(int j = 0;j < cmds_tocheck.size();j++) //sended messages
{
if(checkMessage(&cmds_tocheck[j],&readed_msgs[i]) == 1)
{
memcpy(&msgs[i], &readed_msgs[i], sizeof(VSCAN_MSG));
cmds_tocheck.remove(j);
break;
}
}
}
}
usleep(20000);
retries++;
}while(cmds_tocheck.size() > 0 and retries < MAX_READ_RETRIES);
if(retries >= MAX_READ_RETRIES)
{
printf("writeWaitReadMessage() ERROR: No se pudo leer la respuesta al comando enviado (retries = %d)\n", retries);
while(cmds_tocheck.size() > 0)
{
printMessageData(cmds_tocheck.front());
cmds_tocheck.pop_front();
}
return -1;
}
else
{
// qDebug()<<"good response";
return 1;
}
}
