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;
}
static void make_telegram(uint8_t *payload, int payloadlen, uint8_t *data)
{
data[0] = 0x02;
data[1] = 0x00;
data[2] = payloadlen&0xff;
data[3] = payloadlen>>8;
for (int i = 0; i < payloadlen; i++)
data[4+i] = payload[i];
int chk = sick_compute_checksum(data);
data[4+payloadlen] = chk&0xff;
data[5+payloadlen] = chk>>8;
}
int sick_send_packet_noanswer(sick_laser_t* sick, const unsigned char* command, unsigned short size){
//Here we build up the packet
unsigned char buf[1024];
unsigned char* b=buf+2;
unsigned short checksum=0;
buf[0]=STX; buf[1]=0;
b=sick_format_uint16(b, size);
memcpy(b, command, size);
b+=size;
checksum=sick_compute_checksum(buf, b-buf);
b=sick_format_uint16(b, checksum);
serial_writen(sick->fd, buf, b-buf);
return 1;
}
int sick_send_packet(sick_laser_t* sick, unsigned char* reply, const unsigned char* command, unsigned short size){
//Here we build up the packet
unsigned char buf[1024];
unsigned char* b=buf+2;
unsigned short checksum=0;
buf[0]=STX; buf[1]=0;
b=sick_format_uint16(b, size);
memcpy(b, command, size);
b+=size;
checksum=sick_compute_checksum(buf, b-buf);
b=sick_format_uint16(b, checksum);
serial_writen(sick->fd, buf, b-buf);
if (! sick_wait_ack(sick,1))
return 0;
return sick_wait_packet(sick, reply);
}
int sick_send_packet_noack(sick_laser_t* sick, unsigned char* reply, const unsigned char* command, unsigned short size){
int j;
//Here we build up the packet
unsigned char buf[1024];
unsigned char* b=buf+2;
unsigned short checksum=0;
buf[0]=STX; buf[1]=0;
b=sick_format_uint16(b, size);
memcpy(b, command, size);
b+=size;
checksum=sick_compute_checksum(buf, b-buf);
b=sick_format_uint16(b, checksum);
//serial_writen(sick->fd, b, b-buf);
serial_writen(sick->fd, buf, b-buf);
; //printf("packet: ");
for (j=0; j<b-buf; j++){
; //printf("%02x ", (unsigned int)buf[j]);
}
; //printf("\n");
// if (! sick_wait_ack(sick,1))
// return 0;
return sick_wait_packet(sick, reply);
}
int sick_read_packet(sick_t *s, uint8_t *data)
{
int res;
int datalen = 0; // how many bytes of data[] are valid?
int chk, chk2;
int want;
readmore:
// printf("%i\n", rxlen);
want = 4 - datalen;
if (want > 0) {
// we're willing to wait forever for these bytes
// (this prevents us from spinning in a poll loop
res = read_fully_timeout(s->serialfd, &data[datalen], want, -1);
rxlen += res;
if (res <= 0)
return -1;
datalen += want;
}
// two cases: either the 4 bytes consistute a good header, or
// we skip along to the next occurence of an STX and try
// again.
// is this header good?
int payloadlen = payload_length(data);
if (data[0] != 0x02 || data[1] != 0x80 || payloadlen >= (SICK_MAX_MSG_LENGTH - 6)) {
goto resync;
}
// this header is good. read the message (plus checksum bytes)
want = payloadlen + 6 - datalen;
if (want > 0) {
res = read_fully_timeout(s->serialfd, &data[datalen], want, SICK_RX_TIMEOUT_MS);
rxlen+=res;
if (res <= 0)
return -2;
datalen += want;
}
// is the checksum good?
chk = data[4+payloadlen] + (data[5+payloadlen]<<8);
chk2 = sick_compute_checksum(data);
if (chk != chk2) {
printf("bad chk: %04X != %04X\n", chk, chk2);
goto resync;
}
// good packet received!
if (s->log_packets_file) {
for (int i = 0; i < datalen; i++) {
if (i%16 == 0)
fprintf(s->log_packets_file, "RX %04x : ", i);
fprintf(s->log_packets_file, "%02x ", data[i]);
if ((i%16) == 15 || i+1 == datalen)
fprintf(s->log_packets_file, "\n");
}
}
// is this the response to a request?
pthread_mutex_lock(&s->writelock);
if (s->writedata != NULL && data[4] == s->writereqid) {
memcpy(s->writedata, data, packet_length(data));
s->writevalid = 1;
pthread_cond_signal(&s->writecond);
}
pthread_mutex_unlock(&s->writelock);
// is it a laser scan?
if (data[4] == 0xb0) {
sick_handle_scan_b0(s, data);
} else if (data[4] == 0xf5) {
sick_handle_scan_f5(s, data);
}
return 0;
resync:
for (int i = 1; i < datalen; i++) {
if (data[i] == 0x02) {
memmove(data, &data[i], datalen - i);
datalen = datalen - i;
goto readmore;
}
}
// no STX found, start from scratch
datalen = 0;
goto readmore;
}
