Ua venture hottupdate (#5289)

* Change default port to Serial4 and improve status outputs.

* Reduce stack size.

src/drivers/hott/hott_telemetry/hott_telemetry.cpp

@@ -52,12 +52,13 @@
 #include <sys/ioctl.h>
 #include <unistd.h>
 #include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 
 #include "../comms.h"
 #include "../messages.h"
 
-#define DEFAULT_UART "/dev/ttyS0";		/**< USART1 */
+#define DEFAULT_UART "/dev/ttyS6";		/**< Serial4 */
 
 /* Oddly, ERROR is not defined for C++ */
 #ifdef ERROR
@@ -69,7 +70,22 @@ static int thread_should_exit = false;		/**< Deamon exit flag */
 static int thread_running = false;		/**< Deamon status flag */
 static int deamon_task;				/**< Handle of deamon task / thread */
 static const char daemon_name[] = "hott_telemetry";
-static const char commandline_usage[] = "usage: hott_telemetry start|status|stop [-d <device>]";
+static const char commandline_usage[] =
+	"usage: hott_telemetry start|status|stop [-d <device>] [-t <timeout ms>] [-r <read delay us>] [-w <write delay us>]";
+
+static uint8_t read_log[16];
+
+static int timeout_ms = POLL_TIMEOUT_IN_MSECS;
+static int read_delay_us = POST_READ_DELAY_IN_USECS;
+static int write_delay_us = POST_WRITE_DELAY_IN_USECS;
+
+perf_counter_t	reqs_count;
+perf_counter_t	connect_count;
+perf_counter_t	recon_port;
+perf_counter_t	bin_reply;
+perf_counter_t	txt_reply;
+perf_counter_t	bad_reply;
+perf_counter_t	dead_reply;
 
 /**
  * Deamon management function.
@@ -87,8 +103,6 @@ static int send_data(int uart, uint8_t *buffer, size_t size);
 int
 recv_req_id(int uart, uint8_t *id)
 {
-	static const int timeout_ms = 1000;  // TODO make it a define
-
 	uint8_t mode;
 
 	struct pollfd fds;
@@ -99,6 +113,15 @@ recv_req_id(int uart, uint8_t *id)
 		/* Get the mode: binary or text  */
 		read(uart, &mode, sizeof(mode));
 
+		perf_count(reqs_count);
+
+		// Debug log
+		for (int x = 15; x > 0; x--) {
+			read_log[x] = read_log[x - 1];
+		}
+
+		read_log[0] = mode;
+
 		/* if we have a binary mode request */
 		if (mode != BINARY_MODE_REQUEST_ID) {
 			return ERROR;
@@ -118,7 +141,7 @@ recv_req_id(int uart, uint8_t *id)
 int
 send_data(int uart, uint8_t *buffer, size_t size)
 {
-	usleep(POST_READ_DELAY_IN_USECS);
+	usleep(read_delay_us);
 
 	uint16_t checksum = 0;
 
@@ -134,7 +157,7 @@ send_data(int uart, uint8_t *buffer, size_t size)
 		write(uart, &buffer[i], sizeof(buffer[i]));
 
 		/* Sleep before sending the next byte. */
-		usleep(POST_WRITE_DELAY_IN_USECS);
+		usleep(write_delay_us);
 	}
 
 	/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
@@ -150,6 +173,14 @@ hott_telemetry_thread_main(int argc, char *argv[])
 {
 	warnx("starting");
 
+	connect_count = perf_alloc(PC_COUNT,	"reconnects       ");
+	recon_port = perf_alloc(PC_COUNT,		"reopen port      ");
+	reqs_count = perf_alloc(PC_COUNT,		"requests        ");
+	bin_reply = perf_alloc(PC_COUNT,		"bin replies     ");
+	txt_reply = perf_alloc(PC_COUNT,		"text replies    ");
+	bad_reply = perf_alloc(PC_COUNT,		"unknown replies ");
+	dead_reply = perf_alloc(PC_COUNT,		"dead replies    ");
+
 	thread_running = true;
 
 	const char *device = DEFAULT_UART;
@@ -168,7 +199,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
 	}
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	const int uart = open_uart(device);
+	int uart = open_uart(device);
 
 	if (uart < 0) {
 		errx(1, "Failed opening HoTT UART, exiting.");
@@ -180,7 +211,9 @@ hott_telemetry_thread_main(int argc, char *argv[])
 	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
 	size_t size = 0;
 	uint8_t id = 0;
+
 	bool connected = true;
+	int recon = 0;
 
 	while (!thread_should_exit) {
 		// Listen for and serve poll from the receiver.
@@ -193,25 +226,48 @@ hott_telemetry_thread_main(int argc, char *argv[])
 			switch (id) {
 			case EAM_SENSOR_ID:
 				build_eam_response(buffer, &size);
+				perf_count(bin_reply);
 				break;
 
 			case GAM_SENSOR_ID:
 				build_gam_response(buffer, &size);
+				perf_count(bin_reply);
 				break;
 
 			case GPS_SENSOR_ID:
 				build_gps_response(buffer, &size);
+				perf_count(bin_reply);
+				break;
+
+			case BINARY_MODE_REQUEST_ID:
+				perf_count(dead_reply);
 				break;
 
 			default:
+				perf_count(bad_reply);
 				continue;	// Not a module we support.
 			}
 
 			send_data(uart, buffer, size);
 
 		} else {
-			connected = false;
-			warnx("syncing");
+			if (connected) {
+				connected = false;
+
+			} else {
+				recon++;
+			}
+
+			if (recon > 100) {
+				perf_count(recon_port);
+				close(uart);
+				uart = open_uart(device);
+				perf_reset(reqs_count);
+				perf_reset(bin_reply);
+				perf_reset(txt_reply);
+				perf_reset(dead_reply);
+				perf_reset(bad_reply);
+			}
 		}
 	}
 
@@ -221,6 +277,14 @@ hott_telemetry_thread_main(int argc, char *argv[])
 
 	thread_running = false;
 
+	perf_free(connect_count);
+	perf_free(recon_port);
+	perf_free(reqs_count);
+	perf_free(bin_reply);
+	perf_free(txt_reply);
+	perf_free(bad_reply);
+	perf_free(dead_reply);
+
 	return 0;
 }
 
@@ -234,6 +298,36 @@ hott_telemetry_main(int argc, char *argv[])
 		errx(1, "missing command\n%s", commandline_usage);
 	}
 
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--timeout") == 0) { //device set
+			if (argc > i + 1) {
+				timeout_ms = atoi(argv[i + 1]);
+
+			} else {
+				errx(1, "missing parameter to -t\n%s", commandline_usage);
+			}
+		}
+
+		if (strcmp(argv[i], "-r") == 0 || strcmp(argv[i], "--read-delay") == 0) { //device set
+			if (argc > i + 1) {
+				read_delay_us = atoi(argv[i + 1]);
+
+			} else {
+				errx(1, "missing parameter to -r\n%s", commandline_usage);
+			}
+		}
+
+		if (strcmp(argv[i], "-w") == 0 || strcmp(argv[i], "--write-delay") == 0) { //device set
+			if (argc > i + 1) {
+				write_delay_us = atoi(argv[i + 1]);
+
+			} else {
+				errx(1, "missing parameter to -w\n%s", commandline_usage);
+			}
+		}
+	}
+
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
@@ -245,7 +339,7 @@ hott_telemetry_main(int argc, char *argv[])
 		deamon_task = px4_task_spawn_cmd(daemon_name,
 						 SCHED_DEFAULT,
 						 SCHED_PRIORITY_DEFAULT,
-						 2048,
+						 1500,
 						 hott_telemetry_thread_main,
 						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
 		exit(0);
@@ -260,6 +354,22 @@ hott_telemetry_main(int argc, char *argv[])
 		if (thread_running) {
 			warnx("is running");
 
+
+			for (int x = 15; x >= 0; x--) {
+				printf("%2x ", read_log[x]);
+			}
+
+			printf("\npoll timeout     : %i ms\n", timeout_ms);
+			printf("post write delay : %i us\n", write_delay_us);
+			printf("post read delay  : %i us\n", read_delay_us);
+			perf_print_counter(recon_port);
+			perf_print_counter(connect_count);
+			perf_print_counter(reqs_count);
+			perf_print_counter(bin_reply);
+			perf_print_counter(txt_reply);
+			perf_print_counter(bad_reply);
+			perf_print_counter(dead_reply);
+
 		} else {
 			warnx("not started");
 		}

src/drivers/hott/messages.h

@@ -44,18 +44,21 @@
 
 #include <stdlib.h>
 
+#define POLL_TIMEOUT_IN_MSECS		3500
+
 /* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
- * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting
+ * Note that the value specified here is lower than 5000 (5ms) as time is lost constructing
  * the message after the read which takes some milliseconds.
  */
 #define POST_READ_DELAY_IN_USECS	4000
+
 /* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower
  * values can be used in practise though.
  */
-#define POST_WRITE_DELAY_IN_USECS	2000
+#define POST_WRITE_DELAY_IN_USECS	3000
 
 // Protocol constants.
-#define BINARY_MODE_REQUEST_ID	0x80	// Binary mode request.
+#define BINARY_MODE_REQUEST_ID	0x80
 #define START_BYTE		0x7c
 #define STOP_BYTE		0x7d
 #define TEMP_ZERO_CELSIUS	0x14