Introducing an initial implementation of HoTT telemetry support.

4 files changed, 384 insertions, 19 deletions

diff --git a/apps/hott_telemetry/Makefile b/apps/hott_telemetry/Makefile
index 4660f89a3..528306dab 100644
--- a/apps/hott_telemetry/Makefile
+++ b/apps/hott_telemetry/Makefile
@@ -35,6 +35,8 @@
 # Graupner HoTT Telemetry application.
 #
 
+INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
 APPNAME		 = hott_telemetry
 PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 2048
diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c
index 0350c29df..c5932baeb 100644
--- a/apps/hott_telemetry/hott_telemetry_main.c
+++ b/apps/hott_telemetry/hott_telemetry_main.c
@@ -36,16 +36,31 @@
  * @file hott_telemetry_main.c
  *
  * Graupner HoTT Telemetry implementation.
+ *
+ * The HoTT receiver polls each device at a regular interval at which point
+ * a data packet can be returned if necessary.
  */
 
+#include <fcntl.h>
 #include <nuttx/config.h>
-#include <unistd.h>
+#include <poll.h>
+#include <stdlib.h>
 #include <stdio.h>
-
+#include <string.h>
+#include <termios.h>
+#include <unistd.h>
 #include <systemlib/systemlib.h>
 
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
+#include "messages.h"
+
+// The following are equired for UART direct manipulation.
+#include <arch/board/board.h>
+#include "up_arch.h"
+#include "chip.h"
+#include "stm32_internal.h"
+
+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 */
 
 /**
@@ -58,10 +73,109 @@ __EXPORT int hott_telemetry_main(int argc, char *argv[]);
  */
 int hott_telemetry_thread_main(int argc, char *argv[]);
 
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
+static int read_data(int uart);
+static int send_data(int uart, const struct eam_module_msg *msg);
+static void uart_disable_rx(void);
+static void uart_disable_tx(void);
+
+static int open_uart(const char *uart_name, struct termios *uart_config_original)
+{
+	/* baud rate */
+	int speed = B19200;
+	int uart;
+
+	/* open uart */
+	uart = open(uart_name, O_RDWR | O_NOCTTY);
+
+	if (uart < 0) {
+		fprintf(stderr, "[hott_telemetry] ERROR opening port: %s\n", uart_name);
+		return ERROR;
+	}
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+
+	/* Back up the original uart configuration to restore it after exit */
+	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
+		fprintf(stderr, "[hott_telemetry] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+		close(uart);
+		return ERROR;
+	}
+
+	/* Fill the struct for the new configuration */
+	tcgetattr(uart, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		fprintf(stderr, "[hott_telemetry] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		close(uart);
+		return ERROR;
+	}
+
+	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+		fprintf(stderr, "[hott_telemetry] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		close(uart);
+		return ERROR;
+	}
+
+	return uart;
+}
+
+int hott_telemetry_thread_main(int argc, char *argv[]) 
+{
+	printf("[hott_telemetry] starting\n");
+
+	thread_running = true;
+
+	char *device = "/dev/ttyS2";  // UART5
+	char *commandline_usage = "\tusage: hott_telemetry start|status|stop [-d device]\n";
+
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
+				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
+			}
+		}
+	}
+
+	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
+	struct termios uart_config_original;
+	int uart = open_uart(device, &uart_config_original);
+
+	if (uart < 0) {
+		fprintf(stderr, "[hott_telemetry] Failed opening HoTT UART, exiting.\n");
+		thread_running = false;
+		exit(ERROR);
+	}
+
+	messages_init();
+
+	struct eam_module_msg msg;
+	while (!thread_should_exit) {
+		build_eam_response(&msg);
+		if (read_data(uart) == OK) {
+			send_data(uart, &msg);
+		}
+	}
+
+	printf("[hott_telemetry] exiting.\n");
+
+	/* close uarts */
+	close(uart);
+
+	thread_running = false;
+
+	return 0;
+}
 
 static void
 usage(const char *reason)
@@ -96,8 +210,8 @@ int hott_telemetry_main(int argc, char *argv[])
 		thread_should_exit = false;
 		deamon_task = task_spawn("hott_telemetry",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_DEFAULT,
-					 4096,
+					 SCHED_PRIORITY_MAX - 20,
+					 2048,
 					 hott_telemetry_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
@@ -121,20 +235,81 @@ int hott_telemetry_main(int argc, char *argv[])
 	exit(1);
 }
 
-int hott_telemetry_thread_main(int argc, char *argv[]) {
+int read_data(int uart)
+{
+        uart_disable_tx();
 
-	printf("[hott_telemetry] starting\n");
+	const int timeout = 1000;
+	struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
 
-	thread_running = true;
+	if (poll(fds, 1, timeout) > 0) {
+		// get the mode: binary or text
+		char mode;
+		read(uart, &mode, 1);
+		
+		// read the poll ID (device ID being targetted)
+		char id;
+		read(uart, &id, 1);
 
-	while (!thread_should_exit) {
-		printf("The HoTT telemetry deamon is here!\n");
-		sleep(10);
+		//printf("Reading: mode='%x' id='%x'\n", mode, id);
+
+		// if we have a binary mode request for our sensor ID let's run with it
+		if (mode != BINARY_MODE_REQUEST_ID || id != ELECTRIC_AIR_MODULE) {
+			return ERROR;	// not really an error, rather uninteresting.
+		}
+	} else {
+		printf("Timeout\n");
+		return ERROR;
 	}
+	return OK;
+}
 
-	printf("[deamon] exiting.\n");
+int send_data(int uart, const struct eam_module_msg *msg)
+{
+	usleep(POST_READ_DELAY_IN_USECS);
 
-	thread_running = false;
+	uart_disable_rx();
 
-	return 0;
+	uint16_t checksum = 0;
+  	int size = sizeof(*msg);
+  	char buffer[size];
+
+	memcpy(buffer, msg, size);
+
+	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];
+		}
+
+		//printf("%x ", buffer[i]);
+		write(uart, &buffer[i], 1);
+		
+		// Sleep before sending the next uint8_t.
+		usleep(POST_WRITE_DELAY_IN_USECS);
+	}
+
+	return OK;
 }
+
+void uart_disable_rx()
+{
+	uint32_t cr;
+	cr  = getreg32(STM32_UART5_CR1);
+	cr &= ~(USART_CR1_RE);	// turn off RX
+	cr |= (USART_CR1_TE);	// turn on TX
+        putreg32(cr, STM32_UART5_CR1);
+}
+
+void uart_disable_tx()
+{
+	uint32_t cr;
+	cr  = getreg32(STM32_UART5_CR1);
+	cr |= (USART_CR1_RE);	// turn on RX
+	cr &= ~(USART_CR1_TE);	// turn off TX
+        putreg32(cr, STM32_UART5_CR1);
+}
+
+
diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c
new file mode 100644
index 000000000..eee78a01e
--- /dev/null
+++ b/apps/hott_telemetry/messages.c
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file messages.c
+ *
+ */
+ 
+#include "messages.h"
+
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <unistd.h>
+#include <uORB/topics/sensor_combined.h>
+
+static int sensor_sub = -1;
+
+void messages_init(void)
+{
+	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+}
+
+void build_eam_response(struct eam_module_msg *msg)
+{
+	/* get a local copy of the current sensor values */
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+	
+	memset(msg, 0, sizeof(*msg));	
+
+	msg->start = START_BYTE;
+	msg->eam_sensor_id = ELECTRIC_AIR_MODULE;
+	msg->sensor_id = EAM_SENSOR_ID;
+	msg->temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
+	msg->temperature2 = TEMP_ZERO_CELSIUS;
+	msg->main_voltage_L = (uint8_t)(raw.battery_voltage_v * 10);
+
+	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;
+}
\ No newline at end of file
diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h
new file mode 100644
index 000000000..562010a06
--- /dev/null
+++ b/apps/hott_telemetry/messages.h
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file messages.h
+ *
+ * Graupner HoTT Telemetry message generator.
+ *
+ */
+#ifndef MESSAGES_H_
+#define MESSAGES_H
+
+#include <stdlib.h>
+
+// The HoTT receiver demands a minimum 5ms period of silence after delivering its request. 
+#define POST_READ_DELAY_IN_USECS	4500 
+// 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	1500
+
+// Protocol constants.
+#define BINARY_MODE_REQUEST_ID	0x80	// Binary mode request.
+#define START_BYTE		0x7c
+#define STOP_BYTE		0x7d
+#define TEMP_ZERO_CELSIUS	0x14
+
+// Electric Air Module (EAM) constants.
+#define ELECTRIC_AIR_MODULE	0x8e
+#define EAM_SENSOR_ID		0xe0
+
+struct eam_module_msg {
+	uint8_t start;			/**< Start byte   				*/
+	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 cell1_L;		/**< Lipo cell voltages. Not supported. 	*/
+	uint8_t cell2_L;				
+	uint8_t cell3_L;
+	uint8_t cell4_L;
+	uint8_t cell5_L;
+	uint8_t cell6_L;
+	uint8_t cell7_L;
+	uint8_t cell1_H;
+	uint8_t cell2_H;
+	uint8_t cell3_H;
+	uint8_t cell4_H;
+	uint8_t cell5_H;
+	uint8_t cell6_H;
+	uint8_t cell7_H;
+	uint8_t batt1_voltage_L;	/**< Battery 1 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt1_voltage_H;
+	uint8_t batt2_voltage_L;	/**< Battery 2 voltage, lower 8-bits in steps of 0.02V 	*/
+	uint8_t batt2_voltage_H;
+	uint8_t temperature1;		/**< Temperature sensor 1. 20 = 0 degrees 		*/
+	uint8_t temperature2;
+	uint8_t altitude_L;		/**< Attitude (meters) lower 8-bits. 500 = 0 meters 	*/
+	uint8_t altitude_H;
+	uint8_t current_L;		/**< Current (mAh) lower 8-bits in steps of 0.1V 	*/
+	uint8_t current_H;
+	uint8_t main_voltage_L;		/**< Main power voltage lower 8-bits in steps of 0.1V 	*/
+	uint8_t main_voltage_H;
+	uint8_t battery_capacity_L;	/**< Used battery capacity in steps of 10mAh 		*/
+	uint8_t battery_capacity_H;
+	uint8_t climbrate_L;		/**< Climb rate in 0.01m/s. 0m/s = 30000 		*/
+	uint8_t climbrate_H;
+	uint8_t climbrate_3s;		/**< Climb rate in m/3sec. 0m/3sec = 120 		*/
+	uint8_t rpm_L;			/**< RPM Lower 8-bits In steps of 10 U/min 		*/
+	uint8_t rpm_H;
+	uint8_t electric_min;		/**< Flight time in minutes. 				*/
+	uint8_t electric_sec;		/**< Flight time in seconds. 				*/
+	uint8_t speed_L;		/**< Airspeed in km/h in steps of 1 km/h 		*/
+	uint8_t speed_H;
+	uint8_t stop;			/**< Stop byte 						*/
+	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. 			*/
+};
+
+void messages_init(void);
+void build_eam_response(struct eam_module_msg *msg);
+
+#endif /* MESSAGES_H_ */