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; }