From 8949a88f2514cc8f1506ea50061963574f9df3e2 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Mon, 18 Jan 2016 20:01:52 -0700
Subject: [PATCH] fix CRC calc and add battery voltage report

---
 src/drivers/sPort_telemetry/sPort_data.c      | 37 ++++++++++++++-----
 src/drivers/sPort_telemetry/sPort_data.h      |  1 +
 src/drivers/sPort_telemetry/sPort_telemetry.c | 10 ++---
 3 files changed, 33 insertions(+), 15 deletions(-)

diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c
index c3028003cd..7030212fd9 100644
--- a/src/drivers/sPort_telemetry/sPort_data.c
+++ b/src/drivers/sPort_telemetry/sPort_data.c
@@ -109,13 +109,13 @@ void sPort_init()
 /**
  * Sends a 0x10 start byte.
  */
-static void sPort_send_start(int uart)
-{
-	static const uint8_t c = 0x10;
-	write(uart, &c, 1);
-}
+//static void sPort_send_start(int uart)
+//{
+//	static const uint8_t c = 0x10;
+//	write(uart, &c, 1);
+//}
 
-static void update_crc(uint16_t *crc, uint8_t b)
+static void update_crc(uint16_t *crc, unsigned char b)
 {
 	*crc += b;
 	*crc += *crc >> 8;
@@ -155,9 +155,12 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
 		uint8_t byte[4];
 	} buf;
 
-	uint16_t crc = 0;
-	sPort_send_start(uart);
-//	write(uart, 0x10, 1);
+	/* send start byte */
+	static const uint8_t c = 0x10;
+	write(uart, &c, 1);
+
+	/* init crc */
+	uint16_t crc = c;
 
 	buf.word = id;
 
@@ -173,7 +176,21 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
 		sPort_send_byte(uart, buf.byte[i]);      /* LSB first */
 	}
 
-	sPort_send_byte(uart, crc); 
+	sPort_send_byte(uart, 0xFF - crc);
+}
+
+
+void sPort_send_A2(int uart)
+{
+	/* get a local copy of the battery data */
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
+	/* send data for A2 */
+//	uint32_t voltage = (int)(255 * battery.voltage_v / 18.4f);
+	uint32_t voltage = (int)(255 * 5.5f / 18.4f);
+	sPort_send_data(uart, 0xf103, voltage);
 }
 
 #ifdef xxxx
diff --git a/src/drivers/sPort_telemetry/sPort_data.h b/src/drivers/sPort_telemetry/sPort_data.h
index 07088b16ee..24dfefb169 100644
--- a/src/drivers/sPort_telemetry/sPort_data.h
+++ b/src/drivers/sPort_telemetry/sPort_data.h
@@ -47,5 +47,6 @@
 // Public functions
 void sPort_init(void);
 void sPort_send_data(int uart, uint16_t id, uint32_t data);
+void sPort_send_A2(int uart);
 
 #endif /* _SPORT_TELEMETRY_H */
diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c
index cd1500038b..b8a382e94d 100644
--- a/src/drivers/sPort_telemetry/sPort_telemetry.c
+++ b/src/drivers/sPort_telemetry/sPort_telemetry.c
@@ -191,12 +191,12 @@ static int sPort_telemetry_thread_main(int argc, char *argv[])
 		if (newBytes < 1 || sbuf[0] != 0x7E) continue;
 		
 		/* device ID 4 */
-		static uint8_t counter = 0;
-		if (sbuf[1] == 0xe4) {		
-			/* send data for A2 */
-			sPort_send_data(uart, 0xf103, counter++);
+		if (sbuf[1] == 0x1B) {
+			/* send battery voltage */
+			sPort_send_A2(uart);
+			
 			/* read it back */
-			read(uart, &sbuf[0], sizeof(sbuf));
+			read(uart, &sbuf[0], sizeof(sbuf));		
 		}
 	}