aboutsummaryrefslogtreecommitdiff
path: root/apps/hott_telemetry
diff options
context:
space:
mode:
authorSimon Wilks <sjwilks@gmail.com>2012-11-30 08:06:19 +0100
committerSimon Wilks <sjwilks@gmail.com>2012-11-30 08:06:19 +0100
commitc149b26dd44ebdd84ee3fd72cda3caea893191b2 (patch)
treee00fbc7c637de2b0c64d2a1ebe42f12ef03c3463 /apps/hott_telemetry
parent2eccf5ff196d3c46f95cd4a5246e37d89555f5b4 (diff)
downloadpx4-firmware-c149b26dd44ebdd84ee3fd72cda3caea893191b2.tar.gz
px4-firmware-c149b26dd44ebdd84ee3fd72cda3caea893191b2.tar.bz2
px4-firmware-c149b26dd44ebdd84ee3fd72cda3caea893191b2.zip
Code formatting cleanup.
Fixed code style with Tools/fix_code_style.sh
Diffstat (limited to 'apps/hott_telemetry')
-rw-r--r--apps/hott_telemetry/hott_telemetry_main.c76
-rw-r--r--apps/hott_telemetry/messages.c8
-rw-r--r--apps/hott_telemetry/messages.h16
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;