raw_sensor_scan sensor_read_raw() {
// Initialize parameters for laser scanning
int start_step = 44;
int end_step = 725;
int step_cluster = 1;
int scan_interval = 0;
int scan_num = 1;
int step_num;
int sleep_time = poll_time - (utime() - last_poll);
if (sleep_time > 0)
usleep(sleep_time);
else printf("sleepless\n");
buffer = scip2MeasureScan(port, start_step, end_step, step_cluster,
scan_interval, scan_num, ENC_3BYTE, &step_num);
last_poll = utime();
int i, distance;
for (i = 0 ; i < step_num ; i++) {
distance = buffer[0][i];
if ((distance >= SENSOR_MIN) && (distance <= SENSOR_MAX))
// Copy the data into the static variable
lidar_data.distances[i] = distance;
}
return lidar_data;
}
raw_sensor_scan sensor_read_raw() {
// Initialize parameters for laser scanning
int start_step = 44;
int end_step = 725;
int step_cluster = 1;
int scan_interval = 0;
int scan_num = 1;
int step_num;
long timestamp;
int sleep_time = poll_time_usb - (utime() - last_poll_usb);
if (sleep_time > 0)
usleep(sleep_time);
else printf("sleepless for %g seconds\n", -sleep_time/1000000.0);
if (eth) {
step_num = urg_get_distance(&connection_eth, buffer_eth, ×tamp);
last_poll_eth = utime();
}
if (usb) {
buffer_usb = scip2MeasureScan(connection_usb, start_step, end_step, step_cluster,
scan_interval, scan_num, ENC_3BYTE, &step_num);
last_poll_usb = utime();
}
int i, distance;
for (i = 0 ; i < step_num ; i++) {
distance = buffer_usb[0][i];
if ((distance >= SENSOR_MIN) && (distance <= SENSOR_MAX_USB))
// Copy the data into the static variable
lidar_data.distances[i] = distance;
}
// free usb_buffer, it is malloc'd every time we call scip2MeasureScan
for (i = 0; i < scan_num; i++)
free(buffer_usb[i]);
free(buffer_usb);
return lidar_data;
}
