diff options
Diffstat (limited to 'src/drivers/hott_telemetry/hott_telemetry_main.c')
-rw-r--r-- | src/drivers/hott_telemetry/hott_telemetry_main.c | 44 |
1 files changed, 44 insertions, 0 deletions
diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c index 1d2bdd92e..2c954e41e 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott_telemetry/hott_telemetry_main.c @@ -150,6 +150,29 @@ recv_req_id(int uart, uint8_t *id) } int +recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t id) +{ + usleep(100000); + + static const int timeout_ms = 200; + struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; + + int i = 0; + if (poll(fds, 1, timeout_ms) > 0) { + while (true) { + read(uart, &buffer[i], sizeof(buffer[i])); + + if (&buffer[i] == STOP_BYTE) { + *size = ++i; + id = &buffer[1]; + return OK; + } + } + } + return ERROR; +} + +int send_data(int uart, uint8_t *buffer, size_t size) { usleep(POST_READ_DELAY_IN_USECS); @@ -218,6 +241,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) bool connected = true; while (!thread_should_exit) { + // Listen for and serve poll from the receiver. if (recv_req_id(uart, &id) == OK) { if (!connected) { connected = true; @@ -242,6 +266,26 @@ hott_telemetry_thread_main(int argc, char *argv[]) connected = false; warnx("syncing"); } + + // Poll the next HoTT devices. + // TODO(sjwilks): Currently there is only one but if there would be more we would round-robin + // calling one for every loop iteration. + build_esc_request(&buffer, &size); + send_data(uart, buffer, size); + + // Listen for a response. + recv_data(uart, &buffer, &size, &id); + + for (size_t i = 0; i < size; i++) { + warnx("%d", buffer[i]); + } + + // Determine which moduel sent it and process accordingly. + if (id == ESC_SENSOR_ID) { + extract_esc_message(buffer); + } else { + warnx("Unknown sensor ID received: %d", id); + } } warnx("exiting"); |