From 966de3036e08cc513bb87c1691a1c391e3962bcc Mon Sep 17 00:00:00 2001 From: Antti Alhonen Date: Tue, 27 Feb 2018 16:43:20 +0200 Subject: [PATCH] pulutof development --- makefile | 16 ++++++---------- pulutof.c | 35 ++++++++++++++++++++++++++++++----- pulutof.h | 2 ++ rn1host.c | 37 +++++++++++++++++++++---------------- uart.c | 10 +++++++++- 5 files changed, 68 insertions(+), 32 deletions(-) diff --git a/makefile b/makefile index 67f58a4..d0dede7 100755 --- a/makefile +++ b/makefile @@ -3,22 +3,18 @@ MODEL=DELIVERY_BOY CFLAGS = -D$(MODEL) -DMAP_DIR=\"/home/hrst/rn1-host\" -DSERIAL_DEV=\"/dev/ttyUSB0\" -Wall -Winline -std=c99 -g LDFLAGS = -DEPS = mapping.h uart.h map_memdisk.h datatypes.h hwdata.h tcp_comm.h tcp_parser.h routing.h map_opers.h -OBJ = rn1host.o mapping.o map_memdisk.o uart.o hwdata.o tcp_comm.o tcp_parser.o routing.o map_opers.o +DEPS = mapping.h uart.h map_memdisk.h datatypes.h hwdata.h tcp_comm.h tcp_parser.h routing.h map_opers.h pulutof.h +OBJ = rn1host.o mapping.o map_memdisk.o uart.o hwdata.o tcp_comm.o tcp_parser.o routing.o map_opers.o pulutof.o -all: rn1host_without_depthsense +all: rn1host -#CFLAGS += -DDEPTHSENSE -tof3d.o: tof3d.cpp tof3d.h - c++ -c -o tof3d.o -Wall -I/opt/softkinetic/DepthSenseSDK/include tof3d.cpp -pthread -D$(MODEL) +CFLAGS += -DSIMULATE_SERIAL +CFLAGS += -DPULUTOF1 %.o: %.c $(DEPS) gcc -c -o $@ $< $(CFLAGS) -pthread -rn1host_with_depthsense: $(OBJ) tof3d.o - c++ $(LDFLAGS) -o rn1host $^ -lm -L/opt/softkinetic/DepthSenseSDK/lib -lDepthSense -pthread - -rn1host_without_depthsense: $(OBJ) +rn1host: $(OBJ) gcc $(LDFLAGS) -o rn1host $^ -lm -pthread e: diff --git a/pulutof.c b/pulutof.c index c58f007..ae304f2 100644 --- a/pulutof.c +++ b/pulutof.c @@ -31,6 +31,8 @@ */ +#define _BSD_SOURCE // glibc backwards incompatibility workaround to bring usleep back. + #include #include #include @@ -186,12 +188,34 @@ static int read_frame() return -1; } + printf("Frame read ok, timing:\n"); + for(int i=0; i<24; i++) + { + printf("[%d]:%.1fms ", i, (float)pulutof_ringbuf[pulutof_ringbuf_wr].timestamps[i]/10.0); + } + printf("\n"); + printf("Time deltas:\n"); + for(int i=1; i<24; i++) + { + printf("[%d->%d]:%.1fms ", i-1, i, (float)(pulutof_ringbuf[pulutof_ringbuf_wr].timestamps[i]-pulutof_ringbuf[pulutof_ringbuf_wr].timestamps[i-1])/10.0); + } + printf("\n"); + + // todo: ringbuf_wr++ return pulutof_ringbuf[pulutof_ringbuf_wr].status; } -static int pulutof_poll_thread() +int running = 1; + +void request_tof_quit() +{ + running = 0; +} + +void* pulutof_poll_thread() { - while(1) + init_spi(); + while(running) { int next = pulutof_ringbuf_wr+1; if(next >= PULUTOF_RINGBUF_LEN) next = 0; if(next == pulutof_ringbuf_rd) @@ -211,6 +235,7 @@ static int pulutof_poll_thread() if(avail < 250) { + printf("Sleeping %d ms\n", avail); usleep(1000*avail); continue; } @@ -221,8 +246,8 @@ static int pulutof_poll_thread() usleep(1000); } -} - - + deinit_spi(); + return NULL; +} diff --git a/pulutof.h b/pulutof.h index 667e527..9a6847e 100644 --- a/pulutof.h +++ b/pulutof.h @@ -58,6 +58,8 @@ typedef struct __attribute__((packed)) } pulutof_frame_t; +void request_tof_quit(); +void* pulutof_poll_thread(); #endif diff --git a/rn1host.c b/rn1host.c index feded8a..789470a 100644 --- a/rn1host.c +++ b/rn1host.c @@ -46,15 +46,11 @@ #include "tcp_parser.h" #include "routing.h" #include "utlist.h" -#ifdef DEPTHSENSE -#include "tof3d.h" -#endif + +#include "pulutof.h" #include "mcu_micronavi_docu.c" -#ifdef DEPTHSENSE -tof3d_scan_t* get_tof3d(void); -#endif int max_speedlim = 45; int cur_speedlim = 45; @@ -782,7 +778,7 @@ float cal_y_offset = 0.0; float cal_x_sin_mult = 1.125; float cal_y_sin_mult = 1.125; -#ifdef DEPTHSENSE +#ifdef PULUTOF1 void request_tof_quit(void); #endif @@ -827,7 +823,12 @@ void* main_thread() while(1) { // Calculate fd_set size (biggest fd+1) - int fds_size = uart; + int fds_size = +#ifdef SIMULATE_SERIAL + 0; +#else + uart; +#endif if(tcp_listener_sock > fds_size) fds_size = tcp_listener_sock; if(tcp_client_sock > fds_size) fds_size = tcp_client_sock; if(STDIN_FILENO > fds_size) fds_size = STDIN_FILENO; @@ -836,7 +837,9 @@ void* main_thread() fd_set fds; FD_ZERO(&fds); +#ifndef SIMULATE_SERIAL FD_SET(uart, &fds); +#endif FD_SET(STDIN_FILENO, &fds); FD_SET(tcp_listener_sock, &fds); if(tcp_client_sock >= 0) @@ -965,10 +968,12 @@ void* main_thread() if(cmd == 'Z') {turn_and_go_rel_rel(0, -2000, 25, 1);} } +#ifndef SIMULATE_SERIAL if(FD_ISSET(uart, &fds)) { handle_uart(); } +#endif if(tcp_client_sock >= 0 && FD_ISSET(tcp_client_sock, &fds)) { @@ -1380,7 +1385,7 @@ void* main_thread() route_fsm(); autofsm(); -#ifdef DEPTHSENSE +#if 0 //def PULUTOF1 { static double prev_incr = 0.0; double stamp; @@ -1627,7 +1632,7 @@ void* main_thread() { printf("Got DISTORTED significant lidar scan, running mapping early on previous images\n"); int32_t da, dx, dy; -#ifdef DEPTHSENSE +#ifdef PULUTOF1 prevent_3dtoffing(); #endif map_lidars(&world, n_lidars_to_map, lidars_to_map, &da, &dx, &dy); @@ -1647,7 +1652,7 @@ void* main_thread() { if(good_time_for_lidar_mapping) good_time_for_lidar_mapping = 0; int32_t da, dx, dy; -#ifdef DEPTHSENSE +#ifdef PULUTOF1 prevent_3dtoffing(); #endif map_lidars(&world, n_lidars_to_map, lidars_to_map, &da, &dx, &dy); @@ -1732,7 +1737,7 @@ void* main_thread() } } -#ifdef DEPTHSENSE +#ifdef PULUTOF1 request_tof_quit(); #endif @@ -1740,7 +1745,7 @@ void* main_thread() } -#ifdef DEPTHSENSE +#ifdef PULUTOF1 void* start_tof(void*); #endif @@ -1763,8 +1768,8 @@ int main(int argc, char** argv) } } -#ifdef DEPTHSENSE - if( (ret = pthread_create(&thread_tof, NULL, start_tof, (void*)&calib_tof)) ) +#ifdef PULUTOF1 + if( (ret = pthread_create(&thread_tof, NULL, pulutof_poll_thread, NULL)) ) { printf("ERROR: tof3d thread creation, ret = %d\n", ret); return -1; @@ -1773,7 +1778,7 @@ int main(int argc, char** argv) if(!calib_tof) pthread_join(thread_main, NULL); -#ifdef DEPTHSENSE +#ifdef PULUTOF1 pthread_join(thread_tof, NULL); #endif diff --git a/uart.c b/uart.c index c84024b..535f16f 100644 --- a/uart.c +++ b/uart.c @@ -75,6 +75,10 @@ int uart; int init_uart() { + +#ifdef SIMULATE_SERIAL + return 0; +#else uart = open(SERIAL_DEV, O_RDWR | O_NOCTTY | O_NONBLOCK); if(uart < 0) @@ -86,11 +90,14 @@ int init_uart() set_uart_attribs(uart, B115200); return 0; - +#endif } int send_uart(uint8_t* buf, int len) { +#ifdef SIMULATE_SERIAL + return 0; +#else int timeout = 100; while(len > 0) { @@ -118,6 +125,7 @@ int send_uart(uint8_t* buf, int len) } return 0; +#endif } #define UART_RX_BUF_SIZE 65536