From ff690dda8072bb8068772cebfe4cd7979d491151 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sun, 17 Jan 2016 20:41:36 -0700
Subject: [PATCH] increase priority of sPort_telemetry to 200

---
 src/drivers/sPort_telemetry/sPort_data.c      | 33 ++++++------
 src/drivers/sPort_telemetry/sPort_telemetry.c | 50 +++++++++----------
 2 files changed, 40 insertions(+), 43 deletions(-)

diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c
index a5f4cdcbf7..c3028003cd 100644
--- a/src/drivers/sPort_telemetry/sPort_data.c
+++ b/src/drivers/sPort_telemetry/sPort_data.c
@@ -112,14 +112,12 @@ void sPort_init()
 static void sPort_send_start(int uart)
 {
 	static const uint8_t c = 0x10;
-	write(uart, &c, sizeof(c));
+	write(uart, &c, 1);
 }
 
-static void update_crc(uint16_t *crc)
+static void update_crc(uint16_t *crc, uint8_t b)
 {
-	*crc += 0x10;
-	*crc += *crc >> 8;
-	*crc &= 0xFF;
+	*crc += b;
 	*crc += *crc >> 8;
 	*crc &= 0xFF;
 }
@@ -152,27 +150,30 @@ static void sPort_send_byte(int uart, uint8_t value)
  */
 void sPort_send_data(int uart, uint16_t id, uint32_t data)
 {
-    union {
-        uint16_t word;
-        uint8_t byte[2];
-    } wbuf;
-    
+	union {
+		uint32_t word;
+		uint8_t byte[4];
+	} buf;
+
 	uint16_t crc = 0;
 	sPort_send_start(uart);
+//	write(uart, 0x10, 1);
 
-	wbuf.word = id;
+	buf.word = id;
 
 	for (int i = 0; i < 2; i++) {
-		update_crc(&crc);
-		sPort_send_byte(uart, wbuf.byte[i]);      /* LSB first */
+		update_crc(&crc, buf.byte[i]);
+		sPort_send_byte(uart, buf.byte[i]);      /* LSB first */
 	}
 
-	uint8_t *bbuf = (uint8_t *)data;
+	buf.word = data;
 
 	for (int i = 0; i < 4; i++) {
-		update_crc(&crc);
-		sPort_send_byte(uart, bbuf[i]);      /* LSB first */
+		update_crc(&crc, buf.byte[i]);
+		sPort_send_byte(uart, buf.byte[i]);      /* LSB first */
 	}
+
+	sPort_send_byte(uart, crc); 
 }
 
 #ifdef xxxx
diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c
index d8e0675385..cd1500038b 100644
--- a/src/drivers/sPort_telemetry/sPort_telemetry.c
+++ b/src/drivers/sPort_telemetry/sPort_telemetry.c
@@ -49,11 +49,13 @@
 #include <stdbool.h>
 #include <string.h>
 #include <sys/types.h>
+#include <poll.h>
 #include <fcntl.h>
 #include <unistd.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <termios.h>
+#include <drivers/drv_hrt.h>
 
 #include "sPort_data.h"
 
@@ -76,23 +78,13 @@ __EXPORT int sPort_telemetry_main(int argc, char *argv[]);
 static int sPort_open_uart(const char *uart_name, struct termios *uart_config_original)
 {
 	/* Open UART */
-	const int uart = open(uart_name, O_RDWR | O_NOCTTY);
+	const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
+//	const int uart = open(uart_name, O_RDWR | O_NOCTTY);
 
 	if (uart < 0) {
 		err(1, "Error opening port: %s", uart_name);
 	}
 
-	/* make sure uart FD is blocking */
-	int flags = fcntl(uart, F_GETFL);
-	if (flags < 0) {
-		err(1, "Error getting FD flags: %s", uart_name);
-	}
-	flags &= ~O_NONBLOCK;
-	flags = fcntl(uart, F_SETFL, flags);
-	if (flags < 0) {
-		err(1, "Error setting FD flags: %s", uart_name);
-	}
-	
 	/* Back up the original UART configuration to restore it after exit */
 	int termios_state;
 
@@ -175,33 +167,37 @@ static int sPort_telemetry_thread_main(int argc, char *argv[])
 		err(1, "could not open %s", device_name);
 	}
 
+	/* poll descriptor */
+	struct pollfd fds[1];
+	fds[0].fd = uart;
+	fds[0].events = POLLIN;
+
 	/* Subscribe to topics */
 	sPort_init();
 
 	thread_running = true;
 
 	/* Main thread loop */
-	char sbuf[2];
-	uint8_t fiftyfive = 0x55;
+	char sbuf[20];
 
 	while (!thread_should_exit) {
 
 		/* wait for poll frame starting with value 0x7E */
-		int newBytes = read(uart, &sbuf[0], 1);
-//		warnx("%x, %x \n", sbuf[0], sbuf[1]);
+		int status = poll(fds, sizeof(fds) / sizeof(fds[0]), -1);
+		if (status < 1) continue;
 		
+		// read 2 bytes
+		int newBytes = read(uart, &sbuf[0], 2);
 		if (newBytes < 1 || sbuf[0] != 0x7E) continue;
 		
-		/* read the ID byte */
-		sbuf[1] = read(uart, &sbuf[0], 1);
-		
-//		sPort_send_data(uart, id, 7);
-		
-		/*** test ***/
-		/* write single byte */
-		write(uart, &fiftyfive, 1);
-		/* read it back */
-		read(uart, &sbuf[0], 1);
+		/* device ID 4 */
+		static uint8_t counter = 0;
+		if (sbuf[1] == 0xe4) {		
+			/* send data for A2 */
+			sPort_send_data(uart, 0xf103, counter++);
+			/* read it back */
+			read(uart, &sbuf[0], sizeof(sbuf));
+		}
 	}
 
 	/* Reset the UART flags to original state */
@@ -233,7 +229,7 @@ int sPort_telemetry_main(int argc, char *argv[])
 		thread_should_exit = false;
 		sPort_task = px4_task_spawn_cmd("sPort_telemetry",
 						SCHED_DEFAULT,
-						SCHED_PRIORITY_DEFAULT,
+						200,
 						2000,
 						sPort_telemetry_thread_main,
 						(char *const *)argv);