pulutof development

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:

pulutof.c

@@ -31,6 +31,8 @@
 
 */
 
+#define _BSD_SOURCE  // glibc backwards incompatibility workaround to bring usleep back.
+
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -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;
+}
 

pulutof.h

@@ -58,6 +58,8 @@ typedef struct __attribute__((packed))
 
 } pulutof_frame_t;
 
+void request_tof_quit();
+void* pulutof_poll_thread();
 
 
 #endif

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,15 +1737,15 @@ void* main_thread()
 		}
 	}
 
-#ifdef DEPTHSENSE
+#ifdef PULUTOF1
 	request_tof_quit();
 #endif
 
 	return NULL;
 }
 
 
-#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
 

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