Commit 193420d0 authored by Nic Arnold's avatar Nic Arnold
Browse files

update to get_distance.c

parent 45060751
// to run use ./get_disctance -e
#include "urg_sensor.h"
#include "urg_utils.h"
#include "open_urg_sensor.h"
#include <stdlib.h>
#include <stdio.h>
#include <sys/select.h>
#include <math.h>
#include <stdio.h>
#include <sys/time.h>
#include "urg_sensor.h"
#include "urg_utils.h"
#include "open_urg_sensor.h"
// Exits the program on enter
int kbhit()
......@@ -18,14 +18,21 @@ int kbhit()
return select(1, &fds, NULL, NULL, &tv);
}
static void print_data(urg_t *urg, long data[], long time_stamp, unsigned short intensity[], int data_n) {
static void print_data(urg_t *urg, long data[], long time_stamp,
unsigned short intensity[], int data_n) {
(void)urg;
(void)time_stamp;
int i;
long min_distance;
long max_distance;
long time_stamp_offset;
long t;
// realtime timer
char buffer[26];
struct timeval tp;
gettimeofday(&tp, 0);
time_t timer = tp.tv_sec;
struct tm *tm_info = localtime(&timer);
// Prints the X-Y coordinates for all the measurement points
urg_distance_min_max(urg, &min_distance, &max_distance);
for (i = 0; i < data_n; ++i) {
......@@ -33,7 +40,10 @@ static void print_data(urg_t *urg, long data[], long time_stamp, unsigned short
double radian;
long x;
long y;
float k; // count of rotations
if (radian == 0) {
k++;
}
if ((l <= min_distance) || (l >= max_distance)) {
continue;
}
......@@ -41,7 +51,9 @@ static void print_data(urg_t *urg, long data[], long time_stamp, unsigned short
// once GPS coordinates are availible, need to switch to new function
x = (long)(l * cos(radian));
y = (long)(l * sin(radian));
printf("%ld, %ld, %ld, %d\n", x, y, time_stamp, intensity[i]);
printf("%ld %ld %ld %d %ld %f %f %s ", x, y, time_stamp, intensity[i], l, radian, k, buffer);
printf("%02d.%02d.%02d.%03d\n", tm_info->tm_hour, tm_info->tm_min, tm_info->tm_sec, tp.tv_usec/1000);
}
printf("\n");
//#endif
......@@ -49,13 +61,14 @@ static void print_data(urg_t *urg, long data[], long time_stamp, unsigned short
int main(int argc, char *argv[]) {
urg_t urg;
long *data = NULL;
urg_t urg;
unsigned short *intensity = NULL;
long time_stamp;
int n;
long *data = NULL;
long set_angle = 90;
int i;
int n;
if (open_urg_sensor(&urg, argc, argv) < 0) {
return 1;
}
......@@ -75,13 +88,16 @@ int main(int argc, char *argv[]) {
// Gets measurement data
// Case where the measurement range (start/end steps) is defined
//set_angle = strtol(argv[2], NULL, 3);
//if (argc < 2) printf("Set print angle. ex: 90 = +90deg,-90deg view \n");
/*
urg_set_scanning_parameter(&urg,
urg_deg2step(&urg, -45),
urg_deg2step(&urg, +45), 0);
urg_deg2step(&urg, -1 * set_angle),
urg_deg2step(&urg, +1 * set_angle), 0);
*/
urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, 0);
printf("x,y,time,i\n");
printf("x,y,urg_time,i,real_time,pc,sensor\n");
while (!kbhit()) {
n = urg_get_distance_intensity(&urg, data, intensity, &time_stamp);
if (n <= 0) {
......@@ -94,8 +110,6 @@ int main(int argc, char *argv[]) {
}
//printf("Process Stopped.\n");
//if (argc >= 2) printf("Data was written to: %s \n", argv[2]);
//else printf("Data was written to: LiDAR_o.csv\n");
// Disconnects
free(data);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment