diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index f5aa0ec58a..dff8d43a77 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -52,12 +52,13 @@ #include #include #include +#include #include #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 ]"; +static const char commandline_usage[] = + "usage: hott_telemetry start|status|stop [-d ] [-t ] [-r ] [-w ]"; + +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"); } diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index ab1bef6c98..e8d0477c61 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -44,18 +44,21 @@ #include +#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