int sick_wait_packet_ts(sick_laser_t* sick, unsigned char* reply, struct timeval *tv ){
unsigned char tempBuf[8192];
unsigned char*t=tempBuf;
unsigned int packet_size=0;
unsigned char checksum[3];
unsigned short s_checksum=0;
unsigned short computed_checksum=0;
int header_found=0;
int val=0;
int maxchars=2048;
int chars=0;
while (! header_found && chars<maxchars){
t=tempBuf;
val=serial_readn(sick->fd, t, 1);
//printf("%02x \n", *t);
if (val<=0){
return 0;
}
if (*t==STX){
t++;
header_found=1;
if (tv){
gettimeofday(tv, NULL);
}
//fprintf(stderr,"H");
}
chars++;
}
val=serial_readn(sick->fd, t, 3);
if (val<=0)
return 0;
t+=val;
packet_size=sick_parse_uint16(tempBuf+2);
//fprintf (stderr,"[%d]",(int)packet_size);
if (packet_size>1024){
//fprintf(stderr,"A");
return 0;
}
val=serial_readn(sick->fd, t, packet_size);
if (val<=0)
return 0;
//printf ("val=%d\n",val);
t+=packet_size;
val=serial_readn(sick->fd, checksum, 2);
if (val<=0)
return 0;
s_checksum=sick_parse_uint16(checksum);
computed_checksum=sick_compute_checksum(tempBuf,t-tempBuf);
//printf("buffer= %d\n",t-tempBuf);
if (computed_checksum==s_checksum){
memcpy(reply,tempBuf,t-tempBuf);
//printf ("size=%d, returned=%d\n",(int)packet_size, t-tempBuf);
gettimeofday(&sick->last_packet_time, NULL);
return t-tempBuf;
}
fprintf (stderr, " checksum error ");
return 0;
}
int m600_read_card(unsigned int count, m600_cardfn_t on_card, void* ctx)
{
static const m600_request_t req = M600_REQ_READ_CARD;
m600_reply_t rep;
size_t size;
int error;
for (; count; --count)
{
error = serial_write(&handle, &req, sizeof(req), &size);
if (error == -1 || size != sizeof(req))
return -1;
if (wait_for_read() == -1)
return -1;
error = serial_readn(&handle, (void*)&rep, sizeof(rep));
if (error == -1)
return -1;
if (on_card(rep.card_data, rep.alarms, ctx) == -1)
break;
}
return 0;
}
//tries to synchronize with a packet
int sick_wait_ack(sick_laser_t* sick, int max_retries){
int j=0;
unsigned char c;
; //printf("Wait for ack... ");
while (j<max_retries){
int val=serial_readn(sick->fd, &c, 1);
if(val>0){
if (c==ACK){
//printf("OK\n");
return 1;
}
if (c==NAK){
fprintf(stderr, "ERROR, NAK received\n");
return 1;
}
}
j++;
}
; //printf(" Timeout \n");
return 0;
}
int m600_read_alarms(m600_alarms_t* alarms)
{
static const m600_request_t req = M600_REQ_READ_ALARMS;
m600_reply_t rep;
size_t size;
int error;
error = serial_write(&handle, &req, sizeof(req), &size);
if (error == -1 || size != sizeof(req))
return -1;
if (wait_for_read() == -1)
return -1;
error = serial_readn(&handle, (void*)&rep, sizeof(rep));
if (error == -1)
return -1;
*alarms = rep.alarms;
return 0;
}
