diff options
Diffstat (limited to 'apps/hott_telemetry')
-rw-r--r-- | apps/hott_telemetry/hott_telemetry_main.c | 76 | ||||
-rw-r--r-- | apps/hott_telemetry/messages.c | 8 | ||||
-rw-r--r-- | apps/hott_telemetry/messages.h | 16 |
3 files changed, 58 insertions, 42 deletions
diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c index ec2ca2b41..1eccf50a1 100644 --- a/apps/hott_telemetry/hott_telemetry_main.c +++ b/apps/hott_telemetry/hott_telemetry_main.c @@ -40,7 +40,7 @@ * The HoTT receiver polls each device at a regular interval at which point * a data packet can be returned if necessary. * - * TODO: Add support for at least the vario and GPS sensor data. + * TODO: Add support for at least the vario and GPS sensor data. * */ @@ -107,6 +107,7 @@ static int open_uart(const char *device, struct termios *uart_config_original) /* Back up the original uart configuration to restore it after exit */ char msg[80]; + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state); close(uart); @@ -121,7 +122,7 @@ static int open_uart(const char *device, struct termios *uart_config_original) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", + sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", device, termios_state); close(uart); FATAL_MSG(msg); @@ -136,24 +137,28 @@ static int open_uart(const char *device, struct termios *uart_config_original) /* Get the appropriate GPIO pin and control register */ uint32_t gpio_uart; uint32_t uart_cr3; - switch(device[strlen(device) - 1]) { - case '0': - gpio_uart = GPIO_USART1_TX; - uart_cr3 = STM32_USART1_CR3; - break; - case '1': - gpio_uart = GPIO_USART2_TX; - uart_cr3 = STM32_USART2_CR3; - break; - case '2': - sprintf(msg, "/dev/ttyS3 is not supported.\n", device); - close(uart); - FATAL_MSG(msg); - break; - case '3': - gpio_uart = GPIO_USART6_TX; - uart_cr3 = STM32_USART6_CR3; - break; + + switch (device[strlen(device) - 1]) { + case '0': + gpio_uart = GPIO_USART1_TX; + uart_cr3 = STM32_USART1_CR3; + break; + + case '1': + gpio_uart = GPIO_USART2_TX; + uart_cr3 = STM32_USART2_CR3; + break; + + case '2': + sprintf(msg, "/dev/ttyS3 is not supported.\n", device); + close(uart); + FATAL_MSG(msg); + break; + + case '3': + gpio_uart = GPIO_USART6_TX; + uart_cr3 = STM32_USART6_CR3; + break; } /* Change the TX port to be open-drain */ @@ -164,7 +169,7 @@ static int open_uart(const char *device, struct termios *uart_config_original) cr = getreg32(uart_cr3); cr |= (USART_CR3_HDSEL); putreg32(cr, uart_cr3); - + return uart; } @@ -174,6 +179,7 @@ int read_data(int uart, int *id) struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; char mode; + if (poll(fds, 1, timeout) > 0) { /* Get the mode: binary or text */ read(uart, &mode, 1); @@ -184,10 +190,12 @@ int read_data(int uart, int *id) if (mode != BINARY_MODE_REQUEST_ID) { return ERROR; } + } else { ERROR_MSG("UART timeout on TX/RX port"); return ERROR; } + return OK; } @@ -196,13 +204,16 @@ int send_data(int uart, uint8_t *buffer, int size) usleep(POST_READ_DELAY_IN_USECS); uint16_t checksum = 0; - for(int i = 0; i < size; i++) { + + for (int i = 0; i < size; i++) { if (i == size - 1) { /* Set the checksum: the first uint8_t is taken as the checksum. */ buffer[i] = checksum & 0xff; + } else { checksum += buffer[i]; } + write(uart, &buffer[i], 1); /* Sleep before sending the next byte. */ @@ -213,11 +224,11 @@ int send_data(int uart, uint8_t *buffer, int size) /* TODO: Fix this!! */ uint8_t dummy[size]; read(uart, &dummy, size); - + return OK; } -int hott_telemetry_thread_main(int argc, char *argv[]) +int hott_telemetry_thread_main(int argc, char *argv[]) { INFO_MSG("starting"); @@ -255,15 +266,18 @@ int hott_telemetry_thread_main(int argc, char *argv[]) uint8_t buffer[MESSAGE_BUFFER_SIZE]; int size = 0; int id = 0; + while (!thread_should_exit) { if (read_data(uart, &id) == OK) { - switch(id) { - case ELECTRIC_AIR_MODULE: - build_eam_response(buffer, &size); - break; - default: - continue; // Not a module we support. + switch (id) { + case ELECTRIC_AIR_MODULE: + build_eam_response(buffer, &size); + break; + + default: + continue; // Not a module we support. } + send_data(uart, buffer, size); } } @@ -313,9 +327,11 @@ int hott_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { INFO_MSG("daemon is running"); + } else { INFO_MSG("daemon not started"); } + exit(0); } diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c index 83cc5348c..dce16f371 100644 --- a/apps/hott_telemetry/messages.c +++ b/apps/hott_telemetry/messages.c @@ -36,7 +36,7 @@ * @file messages.c * */ - + #include "messages.h" #include <string.h> @@ -57,10 +57,10 @@ void build_eam_response(uint8_t *buffer, int *size) struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - + struct eam_module_msg msg; *size = sizeof(msg); - memset(&msg, 0, *size); + memset(&msg, 0, *size); msg.start = START_BYTE; msg.eam_sensor_id = ELECTRIC_AIR_MODULE; @@ -72,7 +72,7 @@ void build_eam_response(uint8_t *buffer, int *size) uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - + msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h index fbb9a2d84..44001e04f 100644 --- a/apps/hott_telemetry/messages.h +++ b/apps/hott_telemetry/messages.h @@ -44,17 +44,17 @@ #include <stdlib.h> /* The buffer size used to store messages. This must be at least as big as the number of - * fields in the largest message struct. + * fields in the largest message struct. */ #define MESSAGE_BUFFER_SIZE 50 -/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request. +/* 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 - * the message after the read which takes some milliseconds. + * the message after the read which takes some milliseconds. */ -#define POST_READ_DELAY_IN_USECS 4000 +#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. + * values can be used in practise though. */ #define POST_WRITE_DELAY_IN_USECS 2000 @@ -74,10 +74,10 @@ struct eam_module_msg { uint8_t eam_sensor_id; /**< EAM sensor ID */ uint8_t warning; uint8_t sensor_id; /**< Sensor ID, why different? */ - uint8_t alarm_inverse1; - uint8_t alarm_inverse2; + uint8_t alarm_inverse1; + uint8_t alarm_inverse2; uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ - uint8_t cell2_L; + uint8_t cell2_L; uint8_t cell3_L; uint8_t cell4_L; uint8_t cell5_L; |