From 917a7f7754e47567bf9102b6c26b6649ae6b28dd Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Fri, 10 Feb 2017 06:33:38 -0500
Subject: add initial ootx decoder

---
 tools/ootx_decode/HMD_Datagen.c  |  99 ++++++++++++++++++++++++++++++++++
 tools/ootx_decode/Makefile       |   7 +++
 tools/ootx_decode/crc32.c        | 103 +++++++++++++++++++++++++++++++++++
 tools/ootx_decode/ootx_decode.c  |  50 +++++++++++++++++
 tools/ootx_decode/ootx_decoder.c | 112 +++++++++++++++++++++++++++++++++++++++
 tools/ootx_decode/ootx_decoder.h |  21 ++++++++
 6 files changed, 392 insertions(+)
 create mode 100644 tools/ootx_decode/HMD_Datagen.c
 create mode 100644 tools/ootx_decode/Makefile
 create mode 100644 tools/ootx_decode/crc32.c
 create mode 100644 tools/ootx_decode/ootx_decode.c
 create mode 100644 tools/ootx_decode/ootx_decoder.c
 create mode 100644 tools/ootx_decode/ootx_decoder.h

diff --git a/tools/ootx_decode/HMD_Datagen.c b/tools/ootx_decode/HMD_Datagen.c
new file mode 100644
index 0000000..d71d291
--- /dev/null
+++ b/tools/ootx_decode/HMD_Datagen.c
@@ -0,0 +1,99 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+/* generate data to test ootx decoding */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdint.h>
+#include <time.h>
+#include <stdlib.h>
+
+uint32_t time_stamp = -525198892;
+
+char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
+
+void print_bit(uint8_t data);
+void print_preamble();
+void print_uint16(uint16_t d);
+void print_uint32(uint32_t d);
+void print_payload(char* data, uint16_t length);
+uint32_t crc32(uint32_t crc, uint8_t *buf, size_t size);
+
+int main(int argc, char* argv[])
+{
+	char* str = "Hello World!";
+//	printf("%s\n", str);
+
+	srand(time(NULL));
+
+	print_preamble();
+
+	uint16_t payload_lenth = strlen(str);
+	uint32_t crc = crc32(0xffffffff,(uint8_t*)str,payload_lenth);
+
+	print_uint16(payload_lenth);
+	print_payload(str,payload_lenth);
+	print_uint32(crc);
+
+	return 0;
+}
+
+void print_preamble() {
+	int i;
+	for (i=0;i<17;++i) print_bit(0);
+	print_bit(1);
+}
+
+void print_uint16(uint16_t d) {
+	int i;
+	for (i=0;i<16;++i) {
+		print_bit(d & 0x0001);
+		d>>=1;
+	}
+	print_bit(1);
+}
+
+void print_uint32(uint32_t d) {
+	int i = 0;
+	for (;i<16;++i) {
+		print_bit(d & 0x01);
+		d>>=1;
+	}
+	print_bit(1);
+
+	for (;i<32;++i) {
+		print_bit(d & 0x01);
+		d>>=1;
+	}
+	print_bit(1);
+}
+
+void print_payload(char* data, uint16_t length) {
+	int i;
+	for(i=0;i<length;i+=2) {
+		uint16_t d = *((uint16_t*)(data+i));
+//		printf("%d\n", d);
+		print_uint16(d);
+	}
+}
+
+void print_bit(uint8_t data) {
+	uint32_t length = 3000 + (rand()%2)*500 + data*1000 + (rand()%2)*2000;
+	length -= rand()%500;
+	printf(fmt_str, time_stamp, length);
+
+	time_stamp++;
+
+	/*
+	//to decode
+	// 3000 + x*500 + dbit*1000 + y*2000
+	length -= 3000;
+	if (length>=2000) { length-=2000; y = 0x01; }
+	if (length>=1000)  { length-=1000; dbit = 0x01; }
+	if (length>=500)  { x = 0x01; }
+	*/
+
+	//fire off a callback when a full OOTX packet is received
+}
\ No newline at end of file
diff --git a/tools/ootx_decode/Makefile b/tools/ootx_decode/Makefile
new file mode 100644
index 0000000..f72cbb5
--- /dev/null
+++ b/tools/ootx_decode/Makefile
@@ -0,0 +1,7 @@
+all: ootx_decode hmd_datagen
+
+hmd_datagen: HMD_Datagen.c crc32.c
+	gcc -Wall HMD_Datagen.c crc32.c -o hmd_datagen
+
+ootx_decode: ootx_decode.c ootx_decoder.c ootx_decoder.h
+	gcc -Wall ootx_decode.c ootx_decoder.c -o ootx_decode
\ No newline at end of file
diff --git a/tools/ootx_decode/crc32.c b/tools/ootx_decode/crc32.c
new file mode 100644
index 0000000..a719cbf
--- /dev/null
+++ b/tools/ootx_decode/crc32.c
@@ -0,0 +1,103 @@
+/*-
+ *  COPYRIGHT (C) 1986 Gary S. Brown.  You may use this program, or
+ *  code or tables extracted from it, as desired without restriction.
+ *
+ *  First, the polynomial itself and its table of feedback terms.  The
+ *  polynomial is
+ *  X^32+X^26+X^23+X^22+X^16+X^12+X^11+X^10+X^8+X^7+X^5+X^4+X^2+X^1+X^0
+ *
+ *  Note that we take it "backwards" and put the highest-order term in
+ *  the lowest-order bit.  The X^32 term is "implied"; the LSB is the
+ *  X^31 term, etc.  The X^0 term (usually shown as "+1") results in
+ *  the MSB being 1
+ *
+ *  Note that the usual hardware shift register implementation, which
+ *  is what we're using (we're merely optimizing it by doing eight-bit
+ *  chunks at a time) shifts bits into the lowest-order term.  In our
+ *  implementation, that means shifting towards the right.  Why do we
+ *  do it this way?  Because the calculated CRC must be transmitted in
+ *  order from highest-order term to lowest-order term.  UARTs transmit
+ *  characters in order from LSB to MSB.  By storing the CRC this way
+ *  we hand it to the UART in the order low-byte to high-byte; the UART
+ *  sends each low-bit to hight-bit; and the result is transmission bit
+ *  by bit from highest- to lowest-order term without requiring any bit
+ *  shuffling on our part.  Reception works similarly
+ *
+ *  The feedback terms table consists of 256, 32-bit entries.  Notes
+ *
+ *      The table can be generated at runtime if desired; code to do so
+ *      is shown later.  It might not be obvious, but the feedback
+ *      terms simply represent the results of eight shift/xor opera
+ *      tions for all combinations of data and CRC register values
+ *
+ *      The values must be right-shifted by eight bits by the "updcrc
+ *      logic; the shift must be unsigned (bring in zeroes).  On some
+ *      hardware you could probably optimize the shift in assembler by
+ *      using byte-swap instructions
+ *      polynomial $edb88320
+ *
+ *
+ * CRC32 code derived from work by Gary S. Brown.
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+
+static uint32_t crc32_tab[] = {
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
+	0xe963a535, 0x9e6495a3,	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+	0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
+	0xf3b97148, 0x84be41de,	0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,	0x14015c4f, 0x63066cd9,
+	0xfa0f3d63, 0x8d080df5,	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+	0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,	0x35b5a8fa, 0x42b2986c,
+	0xdbbbc9d6, 0xacbcf940,	0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
+	0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+	0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,	0x76dc4190, 0x01db7106,
+	0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
+	0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+	0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
+	0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
+	0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+	0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
+	0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
+	0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+	0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
+	0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
+	0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+	0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
+	0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
+	0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+	0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
+	0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
+	0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+	0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
+	0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
+	0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+	0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
+	0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
+	0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+	0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+uint32_t crc32(uint32_t crc, const void *buf, size_t size)
+{
+	const uint8_t *p;
+
+	p = buf;
+	crc = crc ^ ~0U;
+
+	while (size--)
+		crc = crc32_tab[(crc ^ *p++) & 0xFF] ^ (crc >> 8);
+
+	return crc ^ ~0U;
+}
diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
new file mode 100644
index 0000000..803b39e
--- /dev/null
+++ b/tools/ootx_decode/ootx_decode.c
@@ -0,0 +1,50 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+/* ootx data decoder test*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <assert.h>
+
+#include "ootx_decoder.h"
+
+//char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
+//extern std::istream cin;
+
+void my_test(ootx_packet* packet) {
+	packet->data[packet->length] = 0;
+	printf("%d %s 0x%X\n", packet->length, packet->data, packet->crc32);
+}
+
+int main(int argc, char* argv[])
+{
+	ootx_init_buffer();
+	ootx_packet_clbk = my_test;
+
+	char* line = NULL;
+	size_t line_len = 0;
+	char trash[100] = "";
+	uint32_t ticks = 0x00;
+
+	while (getline(&line,&line_len,stdin)>0) {
+//		printf("%s\n", line);
+		sscanf(line,"%s %s %s %s %s %s %s %d",
+			trash,
+			trash,
+			trash,
+			trash,
+			trash,
+			trash,
+			trash,
+			&ticks);
+//		printf("%d\n", ticks);
+
+		ootx_process_bit(ticks);
+	}
+
+	return 0;
+}
\ No newline at end of file
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
new file mode 100644
index 0000000..70a88b5
--- /dev/null
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -0,0 +1,112 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+/* ootx data decoder */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <assert.h>
+#include "ootx_decoder.h"
+
+//char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
+
+#define MAX_BUFF_SIZE 1024
+uint8_t* buffer = NULL;
+uint16_t buf_offset = 0;
+uint8_t bits_written = 0;
+uint16_t* payload_size = NULL;
+
+void (*ootx_packet_clbk)(ootx_packet* packet) = NULL;
+
+void ootx_init_buffer() {
+	buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
+	payload_size = (uint16_t*)buffer;
+	*payload_size = 0;
+}
+
+uint8_t ootx_decode_bit(uint32_t length) {
+	length = ((length/500)*500)+500;
+
+	length-=3000;
+	if (length>=2000) { length-=2000; }
+	if (length>=1000)  { return 0x01; }
+
+	return 0x00;
+}
+
+uint8_t ootx_detect_preamble(uint8_t dbit) {
+	static uint32_t preamble = 0x00;
+	preamble <<= 1;
+	preamble |= dbit;
+	if ((preamble & 0x0001ffff) == 0x01) return 1;
+	return 0;
+}
+
+void ootx_reset_buffer() {
+	buf_offset = 0;
+	buffer[buf_offset] = 0;
+	bits_written = 0;
+	*payload_size = 0;
+}
+
+void ootx_inc_buffer_offset() {
+	++buf_offset;
+//	if (buf_offset>=MAX_BUFF_SIZE) buf_offset = 0;
+	assert(buf_offset<MAX_BUFF_SIZE);
+	buffer[buf_offset] = 0;
+}
+
+void ootx_write_to_buffer(uint8_t dbit) {
+	uint8_t *current_byte = buffer+buf_offset;
+//	printf("%d\n", dbit);
+	*current_byte >>= 1;
+	*current_byte |= (0x80 * dbit);
+	++bits_written;
+	if (bits_written>7) {
+		bits_written=0;
+//		printf("%d\n", *current_byte);
+		ootx_inc_buffer_offset();
+	}
+}
+
+void ootx_process_bit(uint32_t length) {
+	static uint8_t bits_processed = 0;
+
+	uint8_t dbit = ootx_decode_bit(length);
+	++bits_processed;
+
+//	printf("z %d %d\n", bits_processed,dbit);
+//	printf("d %d\n", bits_processed,dbit);
+
+	if ( ootx_detect_preamble(dbit) ) {
+		/*	data stream can start over at any time so we must
+			always look for preamble bits */
+//		printf("Preamble found\n");
+		ootx_reset_buffer();
+		bits_processed = 0;
+	}
+	else if(bits_processed>16) {
+		//every 17th bit needs to be dropped
+//		printf("drop %d\n", dbit);
+		bits_processed = 0;
+	}
+	else
+	{
+		ootx_write_to_buffer(dbit);
+
+		if (buf_offset >= (*payload_size+6)) {
+			/*	once we have a complete ootx packet, send it out in the callback */
+			ootx_packet op;
+
+			op.length = *(uint16_t*)buffer;
+			op.data = buffer+2;
+			op.crc32 = *(uint32_t*)(buffer+2+op.length);
+			if (ootx_packet_clbk) ootx_packet_clbk(&op);
+
+			ootx_reset_buffer();
+		}
+	}
+}
+
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
new file mode 100644
index 0000000..b714b53
--- /dev/null
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -0,0 +1,21 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+#ifndef OOTX_DECODER_H
+#define OOTX_DECODER_H
+
+#include <stdint.h>
+
+typedef struct {
+	uint16_t length;
+	uint8_t* data;
+	uint32_t crc32;
+} ootx_packet;
+
+void ootx_init_buffer();
+void ootx_process_bit(uint32_t length);
+
+extern void (*ootx_packet_clbk)(ootx_packet* packet);
+
+#endif
\ No newline at end of file
-- 
cgit v1.2.3


From 9ac3cddf749366f81540aac549ac7583602b7170 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Fri, 10 Feb 2017 06:54:58 -0500
Subject: crc check decoded data

---
 tools/ootx_decode/HMD_Datagen.c  |  4 +++-
 tools/ootx_decode/Makefile       |  6 +++---
 tools/ootx_decode/crc32.h        | 13 +++++++++++++
 tools/ootx_decode/ootx_decoder.c | 10 +++++++++-
 tools/ootx_decode/ootx_decoder.h |  1 +
 5 files changed, 29 insertions(+), 5 deletions(-)
 create mode 100644 tools/ootx_decode/crc32.h

diff --git a/tools/ootx_decode/HMD_Datagen.c b/tools/ootx_decode/HMD_Datagen.c
index d71d291..f2e8b19 100644
--- a/tools/ootx_decode/HMD_Datagen.c
+++ b/tools/ootx_decode/HMD_Datagen.c
@@ -10,6 +10,8 @@
 #include <time.h>
 #include <stdlib.h>
 
+#include "crc32.h"
+
 uint32_t time_stamp = -525198892;
 
 char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
@@ -19,7 +21,7 @@ void print_preamble();
 void print_uint16(uint16_t d);
 void print_uint32(uint32_t d);
 void print_payload(char* data, uint16_t length);
-uint32_t crc32(uint32_t crc, uint8_t *buf, size_t size);
+
 
 int main(int argc, char* argv[])
 {
diff --git a/tools/ootx_decode/Makefile b/tools/ootx_decode/Makefile
index f72cbb5..2c0d01a 100644
--- a/tools/ootx_decode/Makefile
+++ b/tools/ootx_decode/Makefile
@@ -1,7 +1,7 @@
 all: ootx_decode hmd_datagen
 
-hmd_datagen: HMD_Datagen.c crc32.c
+hmd_datagen: HMD_Datagen.c crc32.c crc32.h
 	gcc -Wall HMD_Datagen.c crc32.c -o hmd_datagen
 
-ootx_decode: ootx_decode.c ootx_decoder.c ootx_decoder.h
-	gcc -Wall ootx_decode.c ootx_decoder.c -o ootx_decode
\ No newline at end of file
+ootx_decode: ootx_decode.c ootx_decoder.c ootx_decoder.h crc32.c crc32.h
+	gcc -Wall ootx_decode.c ootx_decoder.c crc32.c -o ootx_decode
\ No newline at end of file
diff --git a/tools/ootx_decode/crc32.h b/tools/ootx_decode/crc32.h
new file mode 100644
index 0000000..c771d4a
--- /dev/null
+++ b/tools/ootx_decode/crc32.h
@@ -0,0 +1,13 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+#ifndef CRC32_H
+#define CRC32_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+uint32_t crc32(uint32_t crc, uint8_t *buf, size_t size);
+
+#endif
\ No newline at end of file
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 70a88b5..2bd818f 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -9,6 +9,7 @@
 
 #include <assert.h>
 #include "ootx_decoder.h"
+#include "crc32.h"
 
 //char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
 
@@ -103,7 +104,14 @@ void ootx_process_bit(uint32_t length) {
 			op.length = *(uint16_t*)buffer;
 			op.data = buffer+2;
 			op.crc32 = *(uint32_t*)(buffer+2+op.length);
-			if (ootx_packet_clbk) ootx_packet_clbk(&op);
+
+			uint32_t crc = crc32(0xffffffff,op.data,op.length);
+
+			if (crc != op.crc32) {
+				printf("CRC mismatch\n");
+			}
+
+			if ((crc == op.crc32) && ootx_packet_clbk) ootx_packet_clbk(&op);
 
 			ootx_reset_buffer();
 		}
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index b714b53..3a14f74 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -5,6 +5,7 @@
 #ifndef OOTX_DECODER_H
 #define OOTX_DECODER_H
 
+#include <stddef.h>
 #include <stdint.h>
 
 typedef struct {
-- 
cgit v1.2.3


From a4b9d6de0673f16c82b295ffc4778f6361e81fe8 Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Fri, 10 Feb 2017 18:12:32 -0500
Subject: use decoding contexts to separate simultaneous lighthouse ootx
 decoding.

---
 tools/ootx_decode/ootx_decode.c  |  6 ++-
 tools/ootx_decode/ootx_decoder.c | 94 ++++++++++++++++++++++------------------
 tools/ootx_decode/ootx_decoder.h | 17 +++++++-
 3 files changed, 71 insertions(+), 46 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 803b39e..030b31b 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -22,7 +22,9 @@ void my_test(ootx_packet* packet) {
 
 int main(int argc, char* argv[])
 {
-	ootx_init_buffer();
+	ootx_decoder_context ctx;
+	ootx_init_decoder_context(&ctx);
+//	ootx_init_buffer();
 	ootx_packet_clbk = my_test;
 
 	char* line = NULL;
@@ -43,7 +45,7 @@ int main(int argc, char* argv[])
 			&ticks);
 //		printf("%d\n", ticks);
 
-		ootx_process_bit(ticks);
+		ootx_process_bit(&ctx, ticks);
 	}
 
 	return 0;
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 2bd818f..f5ab7e5 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -14,96 +14,106 @@
 //char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
 
 #define MAX_BUFF_SIZE 1024
-uint8_t* buffer = NULL;
-uint16_t buf_offset = 0;
-uint8_t bits_written = 0;
-uint16_t* payload_size = NULL;
 
 void (*ootx_packet_clbk)(ootx_packet* packet) = NULL;
 
+void ootx_init_decoder_context(ootx_decoder_context *ctx) {
+	ctx->buf_offset = 0;
+	ctx->bits_written = 0;
+
+	ctx->preamble = 0x00;
+	ctx->bits_processed = 0;
+	ctx->found_preamble = 0;
+
+	ctx->buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
+	ctx->payload_size = (uint16_t*)ctx->buffer;
+	*(ctx->payload_size) = 0;
+}
+/*
 void ootx_init_buffer() {
 	buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
 	payload_size = (uint16_t*)buffer;
 	*payload_size = 0;
 }
-
+*/
 uint8_t ootx_decode_bit(uint32_t length) {
 	length = ((length/500)*500)+500;
 
 	length-=3000;
 	if (length>=2000) { length-=2000; }
-	if (length>=1000)  { return 0x01; }
+	if (length>=1000)  { return 0xFF; }
 
 	return 0x00;
 }
 
-uint8_t ootx_detect_preamble(uint8_t dbit) {
-	static uint32_t preamble = 0x00;
-	preamble <<= 1;
-	preamble |= dbit;
-	if ((preamble & 0x0001ffff) == 0x01) return 1;
+uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
+	ctx->preamble <<= 1;
+	ctx->preamble |= (0x01 & dbit);
+	if ((ctx->preamble & 0x0001ffff) == 0x01) return 1;
 	return 0;
 }
 
-void ootx_reset_buffer() {
-	buf_offset = 0;
-	buffer[buf_offset] = 0;
-	bits_written = 0;
-	*payload_size = 0;
+void ootx_reset_buffer(ootx_decoder_context *ctx) {
+	ctx->buf_offset = 0;
+	ctx->buffer[ctx->buf_offset] = 0;
+	ctx->bits_written = 0;
+	*(ctx->payload_size) = 0;
 }
 
-void ootx_inc_buffer_offset() {
-	++buf_offset;
+void ootx_inc_buffer_offset(ootx_decoder_context *ctx) {
+	++(ctx->buf_offset);
 //	if (buf_offset>=MAX_BUFF_SIZE) buf_offset = 0;
-	assert(buf_offset<MAX_BUFF_SIZE);
-	buffer[buf_offset] = 0;
+	if(ctx->buf_offset>=MAX_BUFF_SIZE) {
+		ctx->found_preamble = 0;
+	}
+	assert(ctx->buf_offset<MAX_BUFF_SIZE);
+	ctx->buffer[ctx->buf_offset] = 0;
 }
 
-void ootx_write_to_buffer(uint8_t dbit) {
-	uint8_t *current_byte = buffer+buf_offset;
+void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
+	uint8_t *current_byte = ctx->buffer + ctx->buf_offset;
 //	printf("%d\n", dbit);
 	*current_byte >>= 1;
-	*current_byte |= (0x80 * dbit);
-	++bits_written;
-	if (bits_written>7) {
-		bits_written=0;
+	*current_byte |= (0x80 & dbit);
+	++(ctx->bits_written);
+	if (ctx->bits_written>7) {
+		ctx->bits_written=0;
 //		printf("%d\n", *current_byte);
-		ootx_inc_buffer_offset();
+		ootx_inc_buffer_offset(ctx);
 	}
 }
 
-void ootx_process_bit(uint32_t length) {
-	static uint8_t bits_processed = 0;
-
+void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
 	uint8_t dbit = ootx_decode_bit(length);
-	++bits_processed;
+	++(ctx->bits_processed);
 
 //	printf("z %d %d\n", bits_processed,dbit);
 //	printf("d %d\n", bits_processed,dbit);
 
-	if ( ootx_detect_preamble(dbit) ) {
+	if ( ootx_detect_preamble(ctx, dbit) ) {
 		/*	data stream can start over at any time so we must
 			always look for preamble bits */
 //		printf("Preamble found\n");
-		ootx_reset_buffer();
-		bits_processed = 0;
+		ootx_reset_buffer(ctx);
+		ctx->bits_processed = 0;
+		ctx->found_preamble = 1;
 	}
-	else if(bits_processed>16) {
+	else if(ctx->bits_processed>16) {
 		//every 17th bit needs to be dropped
 //		printf("drop %d\n", dbit);
-		bits_processed = 0;
+		ctx->bits_processed = 0;
 	}
 	else
 	{
-		ootx_write_to_buffer(dbit);
+		ootx_write_to_buffer(ctx, dbit);
 
-		if (buf_offset >= (*payload_size+6)) {
+		if (ctx->buf_offset >= (*(ctx->payload_size)+6)) {
 			/*	once we have a complete ootx packet, send it out in the callback */
 			ootx_packet op;
 
-			op.length = *(uint16_t*)buffer;
-			op.data = buffer+2;
-			op.crc32 = *(uint32_t*)(buffer+2+op.length);
+			op.length = *(ctx->payload_size);
+			op.data = ctx->buffer+2;
+			op.crc32 = *(uint32_t*)(ctx->buffer+2+op.length);
 
 			uint32_t crc = crc32(0xffffffff,op.data,op.length);
 
@@ -113,7 +123,7 @@ void ootx_process_bit(uint32_t length) {
 
 			if ((crc == op.crc32) && ootx_packet_clbk) ootx_packet_clbk(&op);
 
-			ootx_reset_buffer();
+			ootx_reset_buffer(ctx);
 		}
 	}
 }
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 3a14f74..2c0ee63 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -14,8 +14,21 @@ typedef struct {
 	uint32_t crc32;
 } ootx_packet;
 
-void ootx_init_buffer();
-void ootx_process_bit(uint32_t length);
+typedef struct {
+	uint8_t* buffer;
+	uint16_t buf_offset;
+	uint8_t bits_written;
+	uint16_t* payload_size;
+
+	uint32_t preamble;
+	uint8_t bits_processed;
+	uint8_t found_preamble;
+} ootx_decoder_context;
+
+
+//void ootx_init_buffer();
+void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
+void ootx_init_decoder_context(ootx_decoder_context *ctx);
 
 extern void (*ootx_packet_clbk)(ootx_packet* packet);
 
-- 
cgit v1.2.3


From 9317538667e6f54661949fee1c1f300ba89d185c Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Fri, 10 Feb 2017 20:09:33 -0500
Subject: don't write to buffer until preamble is found

---
 tools/ootx_decode/ootx_decoder.c | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index f5ab7e5..5a14113 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -62,11 +62,15 @@ void ootx_reset_buffer(ootx_decoder_context *ctx) {
 
 void ootx_inc_buffer_offset(ootx_decoder_context *ctx) {
 	++(ctx->buf_offset);
-//	if (buf_offset>=MAX_BUFF_SIZE) buf_offset = 0;
+
+//	assert(ctx->buf_offset<MAX_BUFF_SIZE);
+
+	/* the buffer is going to overflow, wrap the buffer and don't write more data until the preamble is found again */
 	if(ctx->buf_offset>=MAX_BUFF_SIZE) {
+		ctx->buf_offset = 0;
 		ctx->found_preamble = 0;
 	}
-	assert(ctx->buf_offset<MAX_BUFF_SIZE);
+
 	ctx->buffer[ctx->buf_offset] = 0;
 }
 
@@ -99,12 +103,17 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
 		ctx->found_preamble = 1;
 	}
 	else if(ctx->bits_processed>16) {
-		//every 17th bit needs to be dropped
+		//every 17th bit needs to be dropped (sync bit)
 //		printf("drop %d\n", dbit);
 		ctx->bits_processed = 0;
 	}
-	else
+	else if (ctx->found_preamble > 0)
 	{
+		/*	only write to buffer if the preamble is found.
+			if the buffer overflows, found_preamble will be cleared
+			and writing will stop. data would be corrupted, so there is no point in continuing
+		*/
+
 		ootx_write_to_buffer(ctx, dbit);
 
 		if (ctx->buf_offset >= (*(ctx->payload_size)+6)) {
-- 
cgit v1.2.3


From acf367aab41e13b047df6670ffbb27cea8afe2d9 Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Fri, 10 Feb 2017 21:09:14 -0500
Subject: add code to try to deal with lighthouse data. need to decode which
 lighthouse is sending the sync pulse. I could not get the disambiguator to
 work.

---
 tools/ootx_decode/ootx_decode.c  | 66 +++++++++++++++++++++++++++++++++++++---
 tools/ootx_decode/ootx_decoder.c | 29 +++++++++++++-----
 tools/ootx_decode/ootx_decoder.h |  2 ++
 3 files changed, 85 insertions(+), 12 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 030b31b..735b5fc 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -20,10 +20,15 @@ void my_test(ootx_packet* packet) {
 	printf("%d %s 0x%X\n", packet->length, packet->data, packet->crc32);
 }
 
-int main(int argc, char* argv[])
-{
-	ootx_decoder_context ctx;
-	ootx_init_decoder_context(&ctx);
+void my_test2(ootx_packet* packet) {
+	printf("completed ootx packet\n");
+//	packet->data[packet->length] = 0;
+//	printf("%d %s 0x%X\n", packet->length, packet->data, packet->crc32);
+}
+
+ootx_decoder_context ctx[2];
+
+void hello_world_test() {
 //	ootx_init_buffer();
 	ootx_packet_clbk = my_test;
 
@@ -45,8 +50,59 @@ int main(int argc, char* argv[])
 			&ticks);
 //		printf("%d\n", ticks);
 
-		ootx_process_bit(&ctx, ticks);
+		ootx_process_bit(ctx, ticks);
 	}
+}
+
+void raw_test() {
+	ootx_packet_clbk = my_test2;
+
+	char* line = NULL;
+	size_t line_len = 0;
+	char trash[100] = "";
+	int32_t atime = 0x00;
+	uint32_t ticks = 0x00;
+	uint32_t delta = 0x00;
+
+	int8_t current_lighthouse = 0;
+
+	while (getline(&line,&line_len,stdin)>0) {
+//		printf("%s\n", line);
+
+		//HMD 20 0 5881 765645903 -5
+		sscanf(line,"%s %s %s %d %d %d",
+			trash,
+			trash,
+			trash,
+			&ticks,
+			&atime,
+			&delta);
+//		printf("%d\n", ticks);
+
+		int8_t lh = ootx_decode_lighthouse_number(current_lighthouse, ticks, delta);
+
+		if (lh > -1) {
+			//change to newly found lighthouse
+			current_lighthouse = lh;
+//			printf("%d %d %d\n", ticks, delta, current_lighthouse);
+			ootx_process_bit(ctx+current_lighthouse, ticks);
+			printf("%d %d %d\n", current_lighthouse, *(ctx->payload_size), ctx->found_preamble);
+		}
+/*
+		if (current_lighthouse >= 0) {
+			ootx_process_bit(ctx+current_lighthouse, ticks);
+		}
+
+*/
+	}	
+}
+
+int main(int argc, char* argv[])
+{
+	ootx_init_decoder_context(ctx);
+	ootx_init_decoder_context(ctx+1);
+
+	raw_test();
 
 	return 0;
 }
\ No newline at end of file
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 5a14113..b386132 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -21,7 +21,7 @@ void ootx_init_decoder_context(ootx_decoder_context *ctx) {
 	ctx->buf_offset = 0;
 	ctx->bits_written = 0;
 
-	ctx->preamble = 0x00;
+	ctx->preamble = 0XFFFFFFFF;
 	ctx->bits_processed = 0;
 	ctx->found_preamble = 0;
 
@@ -36,12 +36,27 @@ void ootx_init_buffer() {
 	*payload_size = 0;
 }
 */
-uint8_t ootx_decode_bit(uint32_t length) {
-	length = ((length/500)*500)+500;
 
-	length-=3000;
-	if (length>=2000) { length-=2000; }
-	if (length>=1000)  { return 0xFF; }
+/*
+	how to decode pulses
+	ticks>2000 && delta>100000== master lighthouse
+	ticks>2000 && delta>10000 == slave lighthouse
+*/
+
+int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta) {
+	if (ticks<2000) return -1; //sweep
+	if ((ticks > 2000) & (delta>100000)) return 0; //master
+	if ((ticks > 2000) & (delta>10000)) return last_num+1; //a slave
+	return -1;
+}
+
+
+uint8_t ootx_decode_bit(uint32_t ticks) {
+	ticks = ((ticks/500)*500)+500;
+
+	ticks-=3000;
+	if (ticks>=2000) { ticks-=2000; }
+	if (ticks>=1000)  { return 0xFF; }
 
 	return 0x00;
 }
@@ -97,7 +112,7 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
 	if ( ootx_detect_preamble(ctx, dbit) ) {
 		/*	data stream can start over at any time so we must
 			always look for preamble bits */
-//		printf("Preamble found\n");
+		printf("Preamble found\n");
 		ootx_reset_buffer(ctx);
 		ctx->bits_processed = 0;
 		ctx->found_preamble = 1;
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 2c0ee63..696fecb 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -29,6 +29,8 @@ typedef struct {
 //void ootx_init_buffer();
 void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
 void ootx_init_decoder_context(ootx_decoder_context *ctx);
+int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta);
+
 
 extern void (*ootx_packet_clbk)(ootx_packet* packet);
 
-- 
cgit v1.2.3


From 7eb1feba7c7a64910cb17f741f2349254a34fddd Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sat, 11 Feb 2017 10:08:16 -0500
Subject: ootx payload length for lighthouse 1 seems to be decoding correctly
 at 0x21. the payload length seems to be oscillating between 0x21 and a much
 larger number. the larger number 0xc8ec I have no idea why. 0xc8ec does not
 last long before a new preamble is found

---
 tools/ootx_decode/ootx_decode.c  | 29 ++++++++++++++--
 tools/ootx_decode/ootx_decoder.c | 74 ++++++++++++++++++++++++++++++++++------
 tools/ootx_decode/ootx_decoder.h |  4 +++
 3 files changed, 94 insertions(+), 13 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 735b5fc..26e0028 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -65,6 +65,7 @@ void raw_test() {
 	uint32_t delta = 0x00;
 
 	int8_t current_lighthouse = 0;
+	ootx_decoder_context *c_ctx = ctx;
 
 	while (getline(&line,&line_len,stdin)>0) {
 //		printf("%s\n", line);
@@ -80,14 +81,37 @@ void raw_test() {
 //		printf("%d\n", ticks);
 
 		int8_t lh = ootx_decode_lighthouse_number(current_lighthouse, ticks, delta);
+//		printf("lh:%d %s\n", lh, line);
+//		if (lh>0) continue;
 
 		if (lh > -1) {
+			//pump last bit
+			ootx_pump_greatest_bit(c_ctx);
+
+			uint16_t s = *(c_ctx->payload_size);
+//			uint16_t ss = (s>>8) | (s<<8);
+			if (c_ctx->found_preamble) printf("LH:%d s:%d 0x%x\t%s", current_lighthouse, s, s, line);
+
 			//change to newly found lighthouse
 			current_lighthouse = lh;
+			c_ctx = ctx+current_lighthouse;
+		}
+
+		if (ticks>2000 && current_lighthouse==0) {
+			ootx_log_bit(c_ctx, ticks);
+		}
+
+		if (lh == -1) {
 //			printf("%d %d %d\n", ticks, delta, current_lighthouse);
-			ootx_process_bit(ctx+current_lighthouse, ticks);
-			printf("%d %d %d\n", current_lighthouse, *(ctx->payload_size), ctx->found_preamble);
+//			ootx_process_bit(ctx+current_lighthouse, ticks);
+
 		}
+//			printf("%d %d %d\n", ticks, delta, current_lighthouse);
+//			ootx_process_bit(ctx+current_lighthouse, ticks);
+
+			//we would expect a length of 40 bytes
+//			printf("%d %d %d\t%s\n", current_lighthouse, *(ctx->payload_size), ctx->found_preamble, line);
+//		}
 /*
 		if (current_lighthouse >= 0) {
 			ootx_process_bit(ctx+current_lighthouse, ticks);
@@ -103,6 +127,7 @@ int main(int argc, char* argv[])
 	ootx_init_decoder_context(ctx+1);
 
 	raw_test();
+//	hello_world_test();
 
 	return 0;
 }
\ No newline at end of file
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index b386132..6ec0857 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -16,6 +16,7 @@
 #define MAX_BUFF_SIZE 1024
 
 void (*ootx_packet_clbk)(ootx_packet* packet) = NULL;
+void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
 
 void ootx_init_decoder_context(ootx_decoder_context *ctx) {
 	ctx->buf_offset = 0;
@@ -45,26 +46,70 @@ void ootx_init_buffer() {
 
 int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta) {
 	if (ticks<2000) return -1; //sweep
-	if ((ticks > 2000) & (delta>100000)) return 0; //master
-	if ((ticks > 2000) & (delta>10000)) return last_num+1; //a slave
+	if (delta>100000) return 0; //master
+	if (delta>10000) return 1; //a slave
 	return -1;
 }
 
+uint8_t decode_internal(uint32_t length) {
+		uint16_t temp = length - 2880;
+//		printf
 
-uint8_t ootx_decode_bit(uint32_t ticks) {
-	ticks = ((ticks/500)*500)+500;
+#if BETTER_SAFE_THAN_FAST
+	if (temp < 0 || length > 6525) {
+		return -1;
+	}
+#endif
+
+	if ((temp % 500) < 150) {
+		return temp / 500;
+	}
 
-	ticks-=3000;
-	if (ticks>=2000) { ticks-=2000; }
-	if (ticks>=1000)  { return 0xFF; }
+	return -1;
+
+}
+
+uint8_t ootx_decode_bit(uint32_t length) {
+	length = ((length/500)*500)+500;
+
+	length-=3000;
+	if (length>=2000) { length-=2000; }
+	if (length>=1000)  { return 0xFF; }
 
 	return 0x00;
 }
+/*
+uint8_t ootx_decode_bit(uint32_t ticks) {
+	int8_t bits = decode_internal(ticks);
+	return bits&0x02;
+}
+*/
+
+void ootx_log_bit(ootx_decoder_context *ctx, uint32_t ticks) {
+	int8_t dbit = ootx_decode_bit(ticks);
+//	printf("%d\n\n", dbit);
+	ctx->bit_count[(dbit&0x01)]++;
+//	printf("%d %d %d\n", dbit, ctx->bit_count[0], ctx->bit_count[1]);
+}
+
+void ootx_pump_greatest_bit(ootx_decoder_context *ctx) {
+	//pump the bit
+	if (ctx->bit_count[0] > ctx->bit_count[1]) {
+//		printf("Pump 0x00\n");
+		ootx_pump_bit( ctx, 0x00 );
+	} else {
+//		printf("Pump 0xFF\n");
+		ootx_pump_bit( ctx, 0xFF );
+	}
+
+	ctx->bit_count[0] = 0;
+	ctx->bit_count[1] = 0;
+}
 
 uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
 	ctx->preamble <<= 1;
 	ctx->preamble |= (0x01 & dbit);
-	if ((ctx->preamble & 0x0001ffff) == 0x01) return 1;
+	if ((ctx->preamble & 0x0001ffff) == 0x00000001) return 1;
 	return 0;
 }
 
@@ -92,8 +137,10 @@ void ootx_inc_buffer_offset(ootx_decoder_context *ctx) {
 void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
 	uint8_t *current_byte = ctx->buffer + ctx->buf_offset;
 //	printf("%d\n", dbit);
-	*current_byte >>= 1;
-	*current_byte |= (0x80 & dbit);
+//	*current_byte >>= 1;
+//	*current_byte |= (0x80 & dbit);
+	*current_byte <<= 1;
+	*current_byte |= (0x01 & dbit);
 	++(ctx->bits_written);
 	if (ctx->bits_written>7) {
 		ctx->bits_written=0;
@@ -103,7 +150,12 @@ void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
 }
 
 void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
-	uint8_t dbit = ootx_decode_bit(length);
+	int8_t dbit = ootx_decode_bit(length);
+	ootx_pump_bit( ctx, dbit );
+}
+
+void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
+//	uint8_t dbit = ootx_decode_bit(length);
 	++(ctx->bits_processed);
 
 //	printf("z %d %d\n", bits_processed,dbit);
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 696fecb..6b181ee 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -23,6 +23,8 @@ typedef struct {
 	uint32_t preamble;
 	uint8_t bits_processed;
 	uint8_t found_preamble;
+
+	uint8_t bit_count[2];
 } ootx_decoder_context;
 
 
@@ -31,6 +33,8 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
 void ootx_init_decoder_context(ootx_decoder_context *ctx);
 int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta);
 
+void ootx_log_bit(ootx_decoder_context *ctx, uint32_t length);
+void ootx_pump_greatest_bit(ootx_decoder_context *ctx);
 
 extern void (*ootx_packet_clbk)(ootx_packet* packet);
 
-- 
cgit v1.2.3


From 998ffc6ec9dc6ca2f0c94501c6d70882dfb13f5b Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sat, 11 Feb 2017 11:27:21 -0500
Subject: print protocol and firmware version

---
 tools/ootx_decode/ootx_decode.c | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 26e0028..aabf1e8 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -89,8 +89,11 @@ void raw_test() {
 			ootx_pump_greatest_bit(c_ctx);
 
 			uint16_t s = *(c_ctx->payload_size);
+			uint16_t fwv = *(c_ctx->buffer+2);
+			uint16_t pv = 0x3f & fwv;
+			fwv>>=6;
 //			uint16_t ss = (s>>8) | (s<<8);
-			if (c_ctx->found_preamble) printf("LH:%d s:%d 0x%x\t%s", current_lighthouse, s, s, line);
+			if (c_ctx->found_preamble) printf("LH:%d s:%d 0x%x fw:%d pv:%d %d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, line);
 
 			//change to newly found lighthouse
 			current_lighthouse = lh;
-- 
cgit v1.2.3


From 5c54917eaed3e1c6777e16baec65e63dffa215ce Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sat, 11 Feb 2017 15:45:10 -0500
Subject: end of frame detection corrected

---
 tools/ootx_decode/ootx_decode.c  | 14 ++++++++++----
 tools/ootx_decode/ootx_decoder.c | 22 ++++++++++++----------
 tools/ootx_decode/ootx_decoder.h |  2 +-
 3 files changed, 23 insertions(+), 15 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index aabf1e8..d3c1c0f 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -86,14 +86,19 @@ void raw_test() {
 
 		if (lh > -1) {
 			//pump last bit
-			ootx_pump_greatest_bit(c_ctx);
+//			printf("LH:%d ", current_lighthouse);
+			uint8_t bit = 0x01;
+
+			if (current_lighthouse==0) bit &= ootx_pump_greatest_bit(c_ctx);
 
 			uint16_t s = *(c_ctx->payload_size);
 			uint16_t fwv = *(c_ctx->buffer+2);
-			uint16_t pv = 0x3f & fwv;
-			fwv>>=6;
+			uint16_t pv = 0x3f & fwv; //protocol version
+			fwv>>=6; //firmware version
 //			uint16_t ss = (s>>8) | (s<<8);
-			if (c_ctx->found_preamble) printf("LH:%d s:%d 0x%x fw:%d pv:%d %d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, line);
+
+			//this will print after any messages from ootx_pump
+			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
 
 			//change to newly found lighthouse
 			current_lighthouse = lh;
@@ -101,6 +106,7 @@ void raw_test() {
 		}
 
 		if (ticks>2000 && current_lighthouse==0) {
+			//only work with master lighthouse for now
 			ootx_log_bit(c_ctx, ticks);
 		}
 
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 6ec0857..8c7f28c 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -92,24 +92,23 @@ void ootx_log_bit(ootx_decoder_context *ctx, uint32_t ticks) {
 //	printf("%d %d %d\n", dbit, ctx->bit_count[0], ctx->bit_count[1]);
 }
 
-void ootx_pump_greatest_bit(ootx_decoder_context *ctx) {
+uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx) {
 	//pump the bit
-	if (ctx->bit_count[0] > ctx->bit_count[1]) {
-//		printf("Pump 0x00\n");
-		ootx_pump_bit( ctx, 0x00 );
-	} else {
-//		printf("Pump 0xFF\n");
-		ootx_pump_bit( ctx, 0xFF );
-	}
+	uint8_t bit = 0x00;
+	if (ctx->bit_count[0] < ctx->bit_count[1]) bit = 0xFF;
+
+	ootx_pump_bit( ctx, bit );
 
 	ctx->bit_count[0] = 0;
 	ctx->bit_count[1] = 0;
+
+	return bit;
 }
 
 uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
 	ctx->preamble <<= 1;
 	ctx->preamble |= (0x01 & dbit);
-	if ((ctx->preamble & 0x0001ffff) == 0x00000001) return 1;
+	if ((ctx->preamble & 0x0003ffff) == 0x00000001) return 1;
 	return 0;
 }
 
@@ -183,7 +182,10 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 
 		ootx_write_to_buffer(ctx, dbit);
 
-		if (ctx->buf_offset >= (*(ctx->payload_size)+6)) {
+		uint16_t padded_length = *(ctx->payload_size);
+		padded_length += (padded_length&0x01); //extra null byte if odd
+
+		if (ctx->buf_offset >= (padded_length+6)) {
 			/*	once we have a complete ootx packet, send it out in the callback */
 			ootx_packet op;
 
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 6b181ee..13f14d6 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -34,7 +34,7 @@ void ootx_init_decoder_context(ootx_decoder_context *ctx);
 int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta);
 
 void ootx_log_bit(ootx_decoder_context *ctx, uint32_t length);
-void ootx_pump_greatest_bit(ootx_decoder_context *ctx);
+uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx);
 
 extern void (*ootx_packet_clbk)(ootx_packet* packet);
 
-- 
cgit v1.2.3


From aab9f4f1db0dd752a00f3ae67531f1d58b9e2980 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sat, 11 Feb 2017 16:27:57 -0500
Subject: correctly decodes lighthouse 1 ootx frames

---
 tools/ootx_decode/Makefile       |  4 ++--
 tools/ootx_decode/ootx_decode.c  |  2 +-
 tools/ootx_decode/ootx_decoder.c | 32 ++++++++++++++++++++++++++++----
 3 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/tools/ootx_decode/Makefile b/tools/ootx_decode/Makefile
index 2c0d01a..8be8618 100644
--- a/tools/ootx_decode/Makefile
+++ b/tools/ootx_decode/Makefile
@@ -3,5 +3,5 @@ all: ootx_decode hmd_datagen
 hmd_datagen: HMD_Datagen.c crc32.c crc32.h
 	gcc -Wall HMD_Datagen.c crc32.c -o hmd_datagen
 
-ootx_decode: ootx_decode.c ootx_decoder.c ootx_decoder.h crc32.c crc32.h
-	gcc -Wall ootx_decode.c ootx_decoder.c crc32.c -o ootx_decode
\ No newline at end of file
+ootx_decode: ootx_decode.c ootx_decoder.c ootx_decoder.h
+	gcc -Wall ootx_decode.c ootx_decoder.c -lz -o ootx_decode
\ No newline at end of file
diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index d3c1c0f..19d6574 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -98,7 +98,7 @@ void raw_test() {
 //			uint16_t ss = (s>>8) | (s<<8);
 
 			//this will print after any messages from ootx_pump
-			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
+//			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
 
 			//change to newly found lighthouse
 			current_lighthouse = lh;
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 8c7f28c..72ce87c 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -6,10 +6,10 @@
 
 #include <stdio.h>
 #include <stdlib.h>
-
+#include <zlib.h>
 #include <assert.h>
 #include "ootx_decoder.h"
-#include "crc32.h"
+//#include "crc32.h"
 
 //char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
 
@@ -153,6 +153,19 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
 	ootx_pump_bit( ctx, dbit );
 }
 
+void print_crc32(uint32_t crc) {
+//	uint8_t* p = (uint32_t*)&crc;
+//	uint8_t i = 0;
+
+	printf("%X\n", crc);
+}
+
+void write_to_file(uint8_t *d, uint16_t length){
+	FILE *fp = fopen("binary.data","w");
+	fwrite(d, length, 1, fp);
+	fclose(fp);
+}
+
 void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 //	uint8_t dbit = ootx_decode_bit(length);
 	++(ctx->bits_processed);
@@ -191,12 +204,23 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 
 			op.length = *(ctx->payload_size);
 			op.data = ctx->buffer+2;
-			op.crc32 = *(uint32_t*)(ctx->buffer+2+op.length);
+			op.crc32 = *(uint32_t*)(op.data+padded_length);
+
+			uint32_t crc = crc32( 0L, Z_NULL, 0 );
+			crc = crc32( crc, op.data,op.length);
+//			uint32_t crc = crc32(0xffffffff,op.data,op.length);
 
-			uint32_t crc = crc32(0xffffffff,op.data,op.length);
 
 			if (crc != op.crc32) {
 				printf("CRC mismatch\n");
+/*
+				printf("r:");
+				print_crc32(op.crc32);
+
+				printf("c:");
+				print_crc32(crc);
+//				write_to_file(op.data,op.length);
+*/
 			}
 
 			if ((crc == op.crc32) && ootx_packet_clbk) ootx_packet_clbk(&op);
-- 
cgit v1.2.3


From c04c9fb7dcf80123edec53a2e6709bb128548a73 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sat, 11 Feb 2017 20:56:50 -0500
Subject: add lighthouse_info_v6

---
 tools/ootx_decode/ootx_decode.c  |  13 +++--
 tools/ootx_decode/ootx_decoder.c | 108 +++++++++++++++++++++++++++++++++++++++
 tools/ootx_decode/ootx_decoder.h |  27 ++++++++++
 3 files changed, 143 insertions(+), 5 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 19d6574..3a6b6f0 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -22,6 +22,10 @@ void my_test(ootx_packet* packet) {
 
 void my_test2(ootx_packet* packet) {
 	printf("completed ootx packet\n");
+
+	lighthouse_info_v6 lhi;
+	init_lighthouse_info_v6(&lhi,packet->data);
+	print_lighthouse_info_v6(&lhi);
 //	packet->data[packet->length] = 0;
 //	printf("%d %s 0x%X\n", packet->length, packet->data, packet->crc32);
 }
@@ -91,11 +95,10 @@ void raw_test() {
 
 			if (current_lighthouse==0) bit &= ootx_pump_greatest_bit(c_ctx);
 
-			uint16_t s = *(c_ctx->payload_size);
-			uint16_t fwv = *(c_ctx->buffer+2);
-			uint16_t pv = 0x3f & fwv; //protocol version
-			fwv>>=6; //firmware version
-//			uint16_t ss = (s>>8) | (s<<8);
+//			uint16_t s = *(c_ctx->payload_size);
+//			uint16_t fwv = *(c_ctx->buffer+2);
+//			uint16_t pv = 0x3f & fwv; //protocol version
+//			fwv>>=6; //firmware version
 
 			//this will print after any messages from ootx_pump
 //			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 72ce87c..6a06744 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -230,3 +230,111 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 	}
 }
 
+uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) {
+	uint8_t* x = data + *idx;
+	*idx += bytes;
+	return x;
+}
+
+float _to_float(uint8_t* data) {
+	uint16_t h = *(uint16_t*)(data);
+	uint16_t h_exp, h_sig;
+	uint32_t f_sgn, f_exp, f_sig;
+
+	h_exp = (h&0x7c00u);
+	f_sgn = ((uint32_t)h&0x8000u) << 16;
+	if (h_exp == 0x0000u) { /* 0 or subnormal */
+		h_sig = (h&0x03ffu);
+		/* Signed zero */
+		if (h_sig == 0) {
+			return f_sgn;
+		}
+		/* Subnormal */
+		h_sig <<= 1;
+		while ((h_sig&0x0400u) == 0) {
+			h_sig <<= 1;
+			h_exp++;
+		}
+		f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23;
+		f_sig = ((uint32_t)(h_sig&0x03ffu)) << 13;
+			return f_sgn + f_exp + f_sig;
+	}
+	else if (h_exp == 0x7c00u) { /* inf or NaN */
+		/* All-ones exponent and a copy of the significand */
+		return f_sgn + 0x7f800000u + (((uint32_t)(h&0x03ffu)) << 13);
+	}
+	else { /* normalized */
+		/* Just need to adjust the exponent and shift */
+		return f_sgn + (((uint32_t)(h&0x7fffu) + 0x1c000u) << 13);
+	}
+}
+
+void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) {
+	uint16_t idx = 0;
+	/*
+	uint16_t fw_version;//Firmware version (bit 15..6), protocol version (bit 5..0)
+	uint32_t id; //Unique identifier of the base station
+	float fcal_0_phase; //"phase" for rotor 0
+	float fcal_1_phase; //"phase" for rotor 1
+	float fcal_0_tilt; //"tilt" for rotor 0
+	float fcal_1_tilt; //"tilt" for rotor 1
+	uint8_t sys_unlock_count; //Lowest 8 bits of the rotor desynchronization counter
+	uint8_t hw_version; //Hardware version
+	float fcal_0_curve; //"curve" for rotor 0
+	float fcal_1_curve; //"curve" for rotor 1
+	int8_t accel_dir_x; //"orientation vector"
+	int8_t accel_dir_y; //"orientation vector"
+	int8_t accel_dir_z; //"orientation vector"
+	float fcal_0_gibphase; //"gibbous phase" for rotor 0 (normalized angle)
+	float fcal_1_gibphase; //"gibbous phase" for rotor 1 (normalized angle)
+	float fcal_0_gibmag; //"gibbous magnitude" for rotor 0
+	float fcal_1_gibmag; //"gibbous magnitude" for rotor 1
+	uint8_t mode_current; //Currently selected mode (default: 0=A, 1=B, 2=C)
+	uint8_t sys_faults; //"fault detect flags" (should be 0)
+	*/
+
+	lhi->fw_version = *(uint16_t*)get_ptr(data,2,&idx);
+	lhi->id = *(uint32_t*)get_ptr(data,4,&idx);
+	lhi->fcal_0_phase = _to_float( get_ptr(data,2,&idx) );
+	lhi->fcal_1_phase = _to_float( get_ptr(data,2,&idx) );
+	lhi->fcal_0_tilt = _to_float( get_ptr(data,2,&idx) );
+	lhi->fcal_1_tilt = _to_float( get_ptr(data,2,&idx) );
+	lhi->sys_unlock_count = *get_ptr(data,1,&idx);
+	lhi->hw_version = *get_ptr(data,1,&idx);
+	lhi->fcal_0_curve = _to_float( get_ptr(data,2,&idx) );
+	lhi->fcal_1_curve = _to_float( get_ptr(data,2,&idx) );
+	lhi->accel_dir_x = *(int8_t*)get_ptr(data,1,&idx);
+	lhi->accel_dir_y = *(int8_t*)get_ptr(data,1,&idx);
+	lhi->accel_dir_z = *(int8_t*)get_ptr(data,1,&idx);
+	lhi->fcal_0_gibphase = _to_float( get_ptr(data,2,&idx) );
+	lhi->fcal_1_gibphase = _to_float( get_ptr(data,2,&idx) );
+	lhi->fcal_0_gibmag = _to_float( get_ptr(data,2,&idx) );
+	lhi->fcal_1_gibmag = _to_float( get_ptr(data,2,&idx) );
+	lhi->mode_current = *get_ptr(data,1,&idx);
+	lhi->sys_faults = *get_ptr(data,1,&idx);
+
+}
+
+void print_lighthouse_info_v6(lighthouse_info_v6* lhi) {
+
+	printf("\t%X\n\t%X\n\t%f\n\t%f\n\t%f\n\t%f\n\t%d\n\t%d\n\t%f\n\t%f\n\t%d\n\t%d\n\t%d\n\t%f\n\t%f\n\t%f\n\t%f\n\t%d\n\t%d\n",
+		lhi->fw_version,
+		lhi->id,
+		lhi->fcal_0_phase,
+		lhi->fcal_1_phase,
+		lhi->fcal_0_tilt,
+		lhi->fcal_1_tilt,
+		lhi->sys_unlock_count,
+		lhi->hw_version,
+		lhi->fcal_0_curve,
+		lhi->fcal_1_curve,
+		lhi->accel_dir_x,
+		lhi->accel_dir_y,
+		lhi->accel_dir_z,
+		lhi->fcal_0_gibphase,
+		lhi->fcal_1_gibphase,
+		lhi->fcal_0_gibmag,
+		lhi->fcal_1_gibmag,
+		lhi->mode_current,
+		lhi->sys_faults);
+}
\ No newline at end of file
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 13f14d6..c7cf55c 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -28,6 +28,33 @@ typedef struct {
 } ootx_decoder_context;
 
 
+typedef float float16;
+
+typedef struct {
+	uint16_t fw_version;//Firmware version (bit 15..6), protocol version (bit 5..0)
+	uint32_t id; //Unique identifier of the base station
+	float16 fcal_0_phase; //"phase" for rotor 0
+	float16 fcal_1_phase; //"phase" for rotor 1
+	float16 fcal_0_tilt; //"tilt" for rotor 0
+	float16 fcal_1_tilt; //"tilt" for rotor 1
+	uint8_t sys_unlock_count; //Lowest 8 bits of the rotor desynchronization counter
+	uint8_t hw_version; //Hardware version
+	float16 fcal_0_curve; //"curve" for rotor 0
+	float16 fcal_1_curve; //"curve" for rotor 1
+	int8_t accel_dir_x; //"orientation vector"
+	int8_t accel_dir_y; //"orientation vector"
+	int8_t accel_dir_z; //"orientation vector"
+	float16 fcal_0_gibphase; //"gibbous phase" for rotor 0 (normalized angle)
+	float16 fcal_1_gibphase; //"gibbous phase" for rotor 1 (normalized angle)
+	float16 fcal_0_gibmag; //"gibbous magnitude" for rotor 0
+	float16 fcal_1_gibmag; //"gibbous magnitude" for rotor 1
+	uint8_t mode_current; //Currently selected mode (default: 0=A, 1=B, 2=C)
+	uint8_t sys_faults; //"fault detect flags" (should be 0)
+} lighthouse_info_v6;
+
+void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data);
+void print_lighthouse_info_v6(lighthouse_info_v6* lhi);
+
 //void ootx_init_buffer();
 void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
 void ootx_init_decoder_context(ootx_decoder_context *ctx);
-- 
cgit v1.2.3


From cc79e01f62be87f7dcc6de3be7ddb981d6899dff Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 08:34:01 -0500
Subject: ootx_log_bit to ootx_accumulate_bit

---
 tools/ootx_decode/ootx_decoder.c | 12 +++++++++---
 tools/ootx_decode/ootx_decoder.h |  2 +-
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 6a06744..c244b80 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -84,19 +84,24 @@ uint8_t ootx_decode_bit(uint32_t ticks) {
 	return bits&0x02;
 }
 */
-
-void ootx_log_bit(ootx_decoder_context *ctx, uint32_t ticks) {
-	int8_t dbit = ootx_decode_bit(ticks);
+/*
+void ootx_accumulate_bit(ootx_decoder_context *ctx, uint32_t ticks) {
+	uint8_t dbit = ootx_decode_bit(ticks);
 //	printf("%d\n\n", dbit);
 	ctx->bit_count[(dbit&0x01)]++;
 //	printf("%d %d %d\n", dbit, ctx->bit_count[0], ctx->bit_count[1]);
 }
+*/
+void ootx_accumulate_bit(ootx_decoder_context *ctx, uint8_t bit) {
+	ctx->bit_count[bit&0x01]++;
+}
 
 uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx) {
 	//pump the bit
 	uint8_t bit = 0x00;
 	if (ctx->bit_count[0] < ctx->bit_count[1]) bit = 0xFF;
 
+//	printf("pump %d\n", bit);
 	ootx_pump_bit( ctx, bit );
 
 	ctx->bit_count[0] = 0;
@@ -150,6 +155,7 @@ void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
 
 void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
 	int8_t dbit = ootx_decode_bit(length);
+
 	ootx_pump_bit( ctx, dbit );
 }
 
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index c7cf55c..7e6af82 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -60,7 +60,7 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
 void ootx_init_decoder_context(ootx_decoder_context *ctx);
 int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta);
 
-void ootx_log_bit(ootx_decoder_context *ctx, uint32_t length);
+void ootx_accumulate_bit(ootx_decoder_context *ctx, uint8_t bit);
 uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx);
 
 extern void (*ootx_packet_clbk)(ootx_packet* packet);
-- 
cgit v1.2.3


From 600df0b1cb3fac30bd7a7c3df3b50f5e003032bf Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 08:36:00 -0500
Subject: use ootx_accumulate_bit

---
 tools/ootx_decode/ootx_decode.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 3a6b6f0..6365a1d 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -110,7 +110,7 @@ void raw_test() {
 
 		if (ticks>2000 && current_lighthouse==0) {
 			//only work with master lighthouse for now
-			ootx_log_bit(c_ctx, ticks);
+			ootx_accumulate_bit(c_ctx, ootx_decode_bit(ticks) );
 		}
 
 		if (lh == -1) {
-- 
cgit v1.2.3


From da93d5821e6da8462ff42f2c8e6e07a3cbe10740 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 09:58:32 -0500
Subject: proper decoding of bith lighthouse ootx data

---
 tools/ootx_decode/ootx_decode.c  | 89 +++++++++++++++++++++++++++++-----------
 tools/ootx_decode/ootx_decoder.c | 19 ++++++---
 tools/ootx_decode/ootx_decoder.h |  2 +
 3 files changed, 81 insertions(+), 29 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 6365a1d..380d7b2 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -91,45 +91,85 @@ void raw_test() {
 		if (lh > -1) {
 			//pump last bit
 //			printf("LH:%d ", current_lighthouse);
-			uint8_t bit = 0x01;
+			uint8_t bit = 0x01; //bit for debugging purposes
 
-			if (current_lighthouse==0) bit &= ootx_pump_greatest_bit(c_ctx);
+//			if (current_lighthouse==1) bit &= ootx_pump_greatest_bit(c_ctx);
+			bit &= ootx_pump_greatest_bit(c_ctx);
 
-//			uint16_t s = *(c_ctx->payload_size);
-//			uint16_t fwv = *(c_ctx->buffer+2);
-//			uint16_t pv = 0x3f & fwv; //protocol version
-//			fwv>>=6; //firmware version
+/*
+			uint16_t s = *(c_ctx->payload_size);
+			uint16_t fwv = *(c_ctx->buffer+2);
+			uint16_t pv = 0x3f & fwv; //protocol version
+			fwv>>=6; //firmware version
 
 			//this will print after any messages from ootx_pump
-//			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
-
+			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
+*/
 			//change to newly found lighthouse
 			current_lighthouse = lh;
 			c_ctx = ctx+current_lighthouse;
 		}
 
-		if (ticks>2000 && current_lighthouse==0) {
-			//only work with master lighthouse for now
-			ootx_accumulate_bit(c_ctx, ootx_decode_bit(ticks) );
+//		if (ticks>2000 && current_lighthouse==1) {
+		if (ticks>2000) {
+				ootx_accumulate_bit(c_ctx, ootx_decode_bit(ticks) );
 		}
+	}	
+}
 
-		if (lh == -1) {
-//			printf("%d %d %d\n", ticks, delta, current_lighthouse);
-//			ootx_process_bit(ctx+current_lighthouse, ticks);
+void acode_test() {
+	ootx_packet_clbk = my_test2;
 
-		}
-//			printf("%d %d %d\n", ticks, delta, current_lighthouse);
-//			ootx_process_bit(ctx+current_lighthouse, ticks);
+	char* line = NULL;
+	size_t line_len = 0;
+	char trash[100] = "";
+	int32_t atime = 0x00;
+//	uint32_t ticks = 0x00;
+//	uint32_t delta = 0x00;
+	uint8_t acode = 0x00;
+	char lighthouse_code = '\0';
 
-			//we would expect a length of 40 bytes
-//			printf("%d %d %d\t%s\n", current_lighthouse, *(ctx->payload_size), ctx->found_preamble, line);
-//		}
-/*
-		if (current_lighthouse >= 0) {
-			ootx_process_bit(ctx+current_lighthouse, ticks);
+	int8_t current_lighthouse = 0;
+	ootx_decoder_context *c_ctx = ctx;
+
+	while (getline(&line,&line_len,stdin)>0) {
+		//L X HMD -1842671365 18 0 175393 222
+		sscanf(line,"%c %s %s %d %s %hhu %s %s",
+			&lighthouse_code,
+			trash,
+			trash,
+			&atime,
+			trash,
+			&acode,
+			trash,
+			trash);
+
+		int8_t lh = lighthouse_code=='R'?0:1;
+//		printf("LH:%d bit:%d %s\n", lh, (acode & 0x02) >> 1,line);
+
+		if (lh != current_lighthouse) {
+			//pump last bit
+			uint8_t bit = 0x01;
+
+			if (current_lighthouse==0) bit &= ootx_pump_greatest_bit(c_ctx);
+//			ootx_pump_greatest_bit(c_ctx);
+
+			uint16_t s = *(c_ctx->payload_size);
+			uint16_t fwv = *(c_ctx->buffer+2);
+			uint16_t pv = 0x3f & fwv; //protocol version
+			fwv>>=6; //firmware version
+
+			//this will print after any messages from ootx_pump
+			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
+
+			//change to newly found lighthouse
+			current_lighthouse = lh;
+			c_ctx = ctx+current_lighthouse;
 		}
 
-*/
+//		if (current_lighthouse==0) {
+			ootx_accumulate_bit(c_ctx, (acode & 0x02) >> 1);
+//		}
 	}	
 }
 
@@ -139,6 +179,7 @@ int main(int argc, char* argv[])
 	ootx_init_decoder_context(ctx+1);
 
 	raw_test();
+//	acode_test();
 //	hello_world_test();
 
 	return 0;
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index c244b80..6c5031c 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -45,12 +45,21 @@ void ootx_init_buffer() {
 */
 
 int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta) {
-	if (ticks<2000) return -1; //sweep
-	if (delta>100000) return 0; //master
-	if (delta>10000) return 1; //a slave
+	if (delta<18000) return -1; //sweep
+//	if (ticks<2000) return -1; //sweep
+//	printf ("%d\n", delta);
+
+
+	if (ticks>2000 && delta>100000) return 0; //master
+	if (delta>100000) return -1; //some kind of sweep related to the master
+
+	/*	slaves are tricky. The first few sensor readings can be confused because their tick count could be too low because of the previous master pulse?
+		so we have to ignore ticks completly
+	*/
+	if (delta>18000) return 1; //a slave, should be at least 20000 but there are some data issues
 	return -1;
 }
-
+/*
 uint8_t decode_internal(uint32_t length) {
 		uint16_t temp = length - 2880;
 //		printf
@@ -68,7 +77,7 @@ uint8_t decode_internal(uint32_t length) {
 	return -1;
 
 }
-
+*/
 uint8_t ootx_decode_bit(uint32_t length) {
 	length = ((length/500)*500)+500;
 
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 7e6af82..2a1b1d2 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -63,6 +63,8 @@ int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t d
 void ootx_accumulate_bit(ootx_decoder_context *ctx, uint8_t bit);
 uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx);
 
+uint8_t ootx_decode_bit(uint32_t length);
+
 extern void (*ootx_packet_clbk)(ootx_packet* packet);
 
 #endif
\ No newline at end of file
-- 
cgit v1.2.3


From 12fd01346d92d24f9e6ecd89c57e25cdaa455e12 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 10:05:58 -0500
Subject: remove float16 to float32 conversion

---
 tools/ootx_decode/ootx_decoder.c | 32 ++------------------------------
 1 file changed, 2 insertions(+), 30 deletions(-)

diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 6c5031c..fb87e41 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -252,36 +252,8 @@ uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) {
 }
 
 float _to_float(uint8_t* data) {
-	uint16_t h = *(uint16_t*)(data);
-	uint16_t h_exp, h_sig;
-	uint32_t f_sgn, f_exp, f_sig;
-
-	h_exp = (h&0x7c00u);
-	f_sgn = ((uint32_t)h&0x8000u) << 16;
-	if (h_exp == 0x0000u) { /* 0 or subnormal */
-		h_sig = (h&0x03ffu);
-		/* Signed zero */
-		if (h_sig == 0) {
-			return f_sgn;
-		}
-		/* Subnormal */
-		h_sig <<= 1;
-		while ((h_sig&0x0400u) == 0) {
-			h_sig <<= 1;
-			h_exp++;
-		}
-		f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23;
-		f_sig = ((uint32_t)(h_sig&0x03ffu)) << 13;
-			return f_sgn + f_exp + f_sig;
-	}
-	else if (h_exp == 0x7c00u) { /* inf or NaN */
-		/* All-ones exponent and a copy of the significand */
-		return f_sgn + 0x7f800000u + (((uint32_t)(h&0x03ffu)) << 13);
-	}
-	else { /* normalized */
-		/* Just need to adjust the exponent and shift */
-		return f_sgn + (((uint32_t)(h&0x7fffu) + 0x1c000u) << 13);
-	}
+	uint16_t x = *(uint16_t*)data;
+	return x;
 }
 
 void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) {
-- 
cgit v1.2.3


From b79a1766aaf207943772b2565f674853e2be8314 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 10:47:17 -0500
Subject: fix copyright

---
 tools/ootx_decode/HMD_Datagen.c  | 2 +-
 tools/ootx_decode/ootx_decode.c  | 2 +-
 tools/ootx_decode/ootx_decoder.c | 2 +-
 tools/ootx_decode/ootx_decoder.h | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/tools/ootx_decode/HMD_Datagen.c b/tools/ootx_decode/HMD_Datagen.c
index f2e8b19..34bf543 100644
--- a/tools/ootx_decode/HMD_Datagen.c
+++ b/tools/ootx_decode/HMD_Datagen.c
@@ -1,4 +1,4 @@
-// (C) 2016 Joshua Allen, MIT/x11 License.
+// (C) 2017 Joshua Allen, MIT/x11 License.
 //
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 380d7b2..0c4a81a 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -1,4 +1,4 @@
-// (C) 2016 Joshua Allen, MIT/x11 License.
+// (C) 2017 Joshua Allen, MIT/x11 License.
 //
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index fb87e41..d7efd76 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -1,4 +1,4 @@
-// (C) 2016 Joshua Allen, MIT/x11 License.
+// (C) 2017 Joshua Allen, MIT/x11 License.
 //
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 2a1b1d2..13afca2 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -1,4 +1,4 @@
-// (C) 2016 Joshua Allen, MIT/x11 License.
+// (C) 2017 Joshua Allen, MIT/x11 License.
 //
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
-- 
cgit v1.2.3


From ac556ccbb2ecd419773b3ad941f7aea78a191381 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 10:57:57 -0500
Subject: clean up code. move some of the debugging code out into the test
 harness

---
 tools/ootx_decode/ootx_decode.c  | 27 ++++++++++++++++++++
 tools/ootx_decode/ootx_decoder.c | 54 ++++++----------------------------------
 tools/ootx_decode/ootx_decoder.h |  1 +
 3 files changed, 35 insertions(+), 47 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 0c4a81a..0abde2d 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -30,11 +30,38 @@ void my_test2(ootx_packet* packet) {
 //	printf("%d %s 0x%X\n", packet->length, packet->data, packet->crc32);
 }
 
+
+void print_crc32(uint32_t crc) {
+//	uint8_t* p = (uint32_t*)&crc;
+//	uint8_t i = 0;
+
+	printf("%X\n", crc);
+}
+
+void write_to_file(uint8_t *d, uint16_t length){
+	FILE *fp = fopen("binary.data","w");
+	fwrite(d, length, 1, fp);
+	fclose(fp);
+}
+
+void bad_crc(ootx_packet* packet) {
+	printf("CRC mismatch\n");
+/*
+	printf("r:");
+	print_crc32(op.crc32);
+
+	printf("c:");
+	print_crc32(crc);
+//	write_to_file(op.data,op.length);
+*/
+}
+
 ootx_decoder_context ctx[2];
 
 void hello_world_test() {
 //	ootx_init_buffer();
 	ootx_packet_clbk = my_test;
+	ootx_bad_crc_clbk = bad_crc;
 
 	char* line = NULL;
 	size_t line_len = 0;
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index d7efd76..36f93fe 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -16,6 +16,8 @@
 #define MAX_BUFF_SIZE 1024
 
 void (*ootx_packet_clbk)(ootx_packet* packet) = NULL;
+void (*ootx_bad_crc_clbk)(ootx_packet* packet) = NULL;
+
 void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
 
 void ootx_init_decoder_context(ootx_decoder_context *ctx) {
@@ -30,13 +32,6 @@ void ootx_init_decoder_context(ootx_decoder_context *ctx) {
 	ctx->payload_size = (uint16_t*)ctx->buffer;
 	*(ctx->payload_size) = 0;
 }
-/*
-void ootx_init_buffer() {
-	buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
-	payload_size = (uint16_t*)buffer;
-	*payload_size = 0;
-}
-*/
 
 /*
 	how to decode pulses
@@ -87,20 +82,7 @@ uint8_t ootx_decode_bit(uint32_t length) {
 
 	return 0x00;
 }
-/*
-uint8_t ootx_decode_bit(uint32_t ticks) {
-	int8_t bits = decode_internal(ticks);
-	return bits&0x02;
-}
-*/
-/*
-void ootx_accumulate_bit(ootx_decoder_context *ctx, uint32_t ticks) {
-	uint8_t dbit = ootx_decode_bit(ticks);
-//	printf("%d\n\n", dbit);
-	ctx->bit_count[(dbit&0x01)]++;
-//	printf("%d %d %d\n", dbit, ctx->bit_count[0], ctx->bit_count[1]);
-}
-*/
+
 void ootx_accumulate_bit(ootx_decoder_context *ctx, uint8_t bit) {
 	ctx->bit_count[bit&0x01]++;
 }
@@ -168,19 +150,6 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
 	ootx_pump_bit( ctx, dbit );
 }
 
-void print_crc32(uint32_t crc) {
-//	uint8_t* p = (uint32_t*)&crc;
-//	uint8_t i = 0;
-
-	printf("%X\n", crc);
-}
-
-void write_to_file(uint8_t *d, uint16_t length){
-	FILE *fp = fopen("binary.data","w");
-	fwrite(d, length, 1, fp);
-	fclose(fp);
-}
-
 void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 //	uint8_t dbit = ootx_decode_bit(length);
 	++(ctx->bits_processed);
@@ -223,22 +192,13 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 
 			uint32_t crc = crc32( 0L, Z_NULL, 0 );
 			crc = crc32( crc, op.data,op.length);
-//			uint32_t crc = crc32(0xffffffff,op.data,op.length);
-
 
 			if (crc != op.crc32) {
-				printf("CRC mismatch\n");
-/*
-				printf("r:");
-				print_crc32(op.crc32);
-
-				printf("c:");
-				print_crc32(crc);
-//				write_to_file(op.data,op.length);
-*/
+				if (ootx_bad_crc_clbk != NULL) ootx_bad_crc_clbk(&op);
+			}
+			else if (ootx_packet_clbk != NULL) {
+				ootx_packet_clbk(&op);
 			}
-
-			if ((crc == op.crc32) && ootx_packet_clbk) ootx_packet_clbk(&op);
 
 			ootx_reset_buffer(ctx);
 		}
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 13afca2..8f6129b 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -66,5 +66,6 @@ uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx);
 uint8_t ootx_decode_bit(uint32_t length);
 
 extern void (*ootx_packet_clbk)(ootx_packet* packet);
+extern void (*ootx_bad_crc_clbk)(ootx_packet* packet);
 
 #endif
\ No newline at end of file
-- 
cgit v1.2.3


From 6ad28759408695a4825738bcc32bc294d1869ca8 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 11:09:38 -0500
Subject: handler for bad crc32

---
 tools/ootx_decode/ootx_decode.c  | 12 ++++++------
 tools/ootx_decode/ootx_decoder.c |  4 ++--
 tools/ootx_decode/ootx_decoder.h |  2 +-
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 0abde2d..9e76d8f 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -44,16 +44,15 @@ void write_to_file(uint8_t *d, uint16_t length){
 	fclose(fp);
 }
 
-void bad_crc(ootx_packet* packet) {
+void bad_crc(ootx_packet* packet, uint32_t crc) {
 	printf("CRC mismatch\n");
-/*
+
 	printf("r:");
-	print_crc32(op.crc32);
+	print_crc32(packet->crc32);
 
 	printf("c:");
 	print_crc32(crc);
-//	write_to_file(op.data,op.length);
-*/
+	write_to_file(packet->data,packet->length);
 }
 
 ootx_decoder_context ctx[2];
@@ -61,7 +60,6 @@ ootx_decoder_context ctx[2];
 void hello_world_test() {
 //	ootx_init_buffer();
 	ootx_packet_clbk = my_test;
-	ootx_bad_crc_clbk = bad_crc;
 
 	char* line = NULL;
 	size_t line_len = 0;
@@ -87,6 +85,7 @@ void hello_world_test() {
 
 void raw_test() {
 	ootx_packet_clbk = my_test2;
+	ootx_bad_crc_clbk = bad_crc;
 
 	char* line = NULL;
 	size_t line_len = 0;
@@ -146,6 +145,7 @@ void raw_test() {
 
 void acode_test() {
 	ootx_packet_clbk = my_test2;
+	ootx_bad_crc_clbk = bad_crc;
 
 	char* line = NULL;
 	size_t line_len = 0;
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 36f93fe..dc5d50a 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -16,7 +16,7 @@
 #define MAX_BUFF_SIZE 1024
 
 void (*ootx_packet_clbk)(ootx_packet* packet) = NULL;
-void (*ootx_bad_crc_clbk)(ootx_packet* packet) = NULL;
+void (*ootx_bad_crc_clbk)(ootx_packet* packet, uint32_t crc) = NULL;
 
 void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
 
@@ -194,7 +194,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 			crc = crc32( crc, op.data,op.length);
 
 			if (crc != op.crc32) {
-				if (ootx_bad_crc_clbk != NULL) ootx_bad_crc_clbk(&op);
+				if (ootx_bad_crc_clbk != NULL) ootx_bad_crc_clbk(&op,crc);
 			}
 			else if (ootx_packet_clbk != NULL) {
 				ootx_packet_clbk(&op);
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 8f6129b..38c273b 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -66,6 +66,6 @@ uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx);
 uint8_t ootx_decode_bit(uint32_t length);
 
 extern void (*ootx_packet_clbk)(ootx_packet* packet);
-extern void (*ootx_bad_crc_clbk)(ootx_packet* packet);
+extern void (*ootx_bad_crc_clbk)(ootx_packet* packet, uint32_t crc);
 
 #endif
\ No newline at end of file
-- 
cgit v1.2.3


From c2ca44be9f6408351cfe32e855b1472ec155b9b2 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 14:34:29 -0500
Subject: free decoder context

---
 tools/ootx_decode/ootx_decode.c  | 5 ++++-
 tools/ootx_decode/ootx_decoder.c | 6 ++++++
 tools/ootx_decode/ootx_decoder.h | 1 +
 3 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 9e76d8f..1e93574 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -197,7 +197,7 @@ void acode_test() {
 //		if (current_lighthouse==0) {
 			ootx_accumulate_bit(c_ctx, (acode & 0x02) >> 1);
 //		}
-	}	
+	}
 }
 
 int main(int argc, char* argv[])
@@ -209,5 +209,8 @@ int main(int argc, char* argv[])
 //	acode_test();
 //	hello_world_test();
 
+	ootx_free_decoder_context(ctx);
+	ootx_free_decoder_context(ctx+1);
+
 	return 0;
 }
\ No newline at end of file
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index dc5d50a..f308bfb 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -33,6 +33,12 @@ void ootx_init_decoder_context(ootx_decoder_context *ctx) {
 	*(ctx->payload_size) = 0;
 }
 
+void ootx_free_decoder_context(ootx_decoder_context *ctx) {
+	free(ctx->buffer);
+	ctx->buffer = NULL;
+	ctx->payload_size = NULL;
+}
+
 /*
 	how to decode pulses
 	ticks>2000 && delta>100000== master lighthouse
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 38c273b..4ec1fd1 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -59,6 +59,7 @@ void print_lighthouse_info_v6(lighthouse_info_v6* lhi);
 void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
 void ootx_init_decoder_context(ootx_decoder_context *ctx);
 int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta);
+void ootx_free_decoder_context(ootx_decoder_context *ctx);
 
 void ootx_accumulate_bit(ootx_decoder_context *ctx, uint8_t bit);
 uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx);
-- 
cgit v1.2.3


From c339b84a145c6af24fe1ce40f4fee23cd88bce10 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 15:23:57 -0500
Subject: buffer does not need to be very large.

---
 tools/ootx_decode/ootx_decoder.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index f308bfb..003322f 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -13,7 +13,7 @@
 
 //char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
 
-#define MAX_BUFF_SIZE 1024
+#define MAX_BUFF_SIZE 64
 
 void (*ootx_packet_clbk)(ootx_packet* packet) = NULL;
 void (*ootx_bad_crc_clbk)(ootx_packet* packet, uint32_t crc) = NULL;
-- 
cgit v1.2.3


From 46e907ad3059001a0d9361098c2f017c36a3dd13 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Sun, 12 Feb 2017 21:56:11 -0500
Subject: function half to float conversion

---
 tools/ootx_decode/ootx_decoder.c | 54 +++++++++++++++++++++++++---------------
 1 file changed, 34 insertions(+), 20 deletions(-)

diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 003322f..eee4e7e 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -218,8 +218,22 @@ uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) {
 }
 
 float _to_float(uint8_t* data) {
+	//this will not handle infinity, NaN, or denormalized floats
+
 	uint16_t x = *(uint16_t*)data;
-	return x;
+	float f = 0;
+
+	uint32_t *ftmp = (uint32_t*)&f; //use the allocated floating point memory
+
+	if (((x & 0x7c00) == 0x0000) && ((x & 0x03ff) == 0)) return f; //zero
+
+	//sign
+	*ftmp = x & 0x8000;
+	*ftmp <<= 16;
+
+	*ftmp += ((((uint32_t)(x & 0x7fff)) + 0x1c000) << 13);
+
+	return f;
 }
 
 void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) {
@@ -246,25 +260,25 @@ void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) {
 	uint8_t sys_faults; //"fault detect flags" (should be 0)
 	*/
 
-	lhi->fw_version = *(uint16_t*)get_ptr(data,2,&idx);
-	lhi->id = *(uint32_t*)get_ptr(data,4,&idx);
-	lhi->fcal_0_phase = _to_float( get_ptr(data,2,&idx) );
-	lhi->fcal_1_phase = _to_float( get_ptr(data,2,&idx) );
-	lhi->fcal_0_tilt = _to_float( get_ptr(data,2,&idx) );
-	lhi->fcal_1_tilt = _to_float( get_ptr(data,2,&idx) );
-	lhi->sys_unlock_count = *get_ptr(data,1,&idx);
-	lhi->hw_version = *get_ptr(data,1,&idx);
-	lhi->fcal_0_curve = _to_float( get_ptr(data,2,&idx) );
-	lhi->fcal_1_curve = _to_float( get_ptr(data,2,&idx) );
-	lhi->accel_dir_x = *(int8_t*)get_ptr(data,1,&idx);
-	lhi->accel_dir_y = *(int8_t*)get_ptr(data,1,&idx);
-	lhi->accel_dir_z = *(int8_t*)get_ptr(data,1,&idx);
-	lhi->fcal_0_gibphase = _to_float( get_ptr(data,2,&idx) );
-	lhi->fcal_1_gibphase = _to_float( get_ptr(data,2,&idx) );
-	lhi->fcal_0_gibmag = _to_float( get_ptr(data,2,&idx) );
-	lhi->fcal_1_gibmag = _to_float( get_ptr(data,2,&idx) );
-	lhi->mode_current = *get_ptr(data,1,&idx);
-	lhi->sys_faults = *get_ptr(data,1,&idx);
+	lhi->fw_version = *(uint16_t*)get_ptr(data,sizeof(uint16_t),&idx);
+	lhi->id = *(uint32_t*)get_ptr(data,sizeof(uint32_t),&idx);
+	lhi->fcal_0_phase = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_phase = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_0_tilt = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_tilt = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->sys_unlock_count = *get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->hw_version = *get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->fcal_0_curve = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_curve = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->accel_dir_x = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->accel_dir_y = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->accel_dir_z = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->fcal_0_gibphase = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_gibphase = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_0_gibmag = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_gibmag = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->mode_current = *get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->sys_faults = *get_ptr(data,sizeof(uint8_t),&idx);
 
 }
 
-- 
cgit v1.2.3


From 1ffadeda2fac023741c9a828714bb4ac29fcd815 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Sun, 12 Feb 2017 23:24:17 -0500
Subject: Working on closing #15 - still needs work.

---
 include/survive.h  |  4 +++-
 src/survive_data.c | 70 +++++++++++++++++++++++++++++++++++++++++-------------
 2 files changed, 56 insertions(+), 18 deletions(-)

diff --git a/include/survive.h b/include/survive.h
index d29b487..7d461c7 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -35,7 +35,9 @@ struct SurviveObject
  #ifdef USE_OLD_DISAMBIGUATOR
 	uint32_t last_master_time;
 	uint32_t last_slave_time;
- 	int32_t last_photo_length;
+ 	int16_t last_master_length;
+ 	int16_t last_slave_length;
+	int8_t   is_on_slave;
  #else
 	uint32_t last_master_time;
  	struct disambiguator * d;
diff --git a/src/survive_data.c b/src/survive_data.c
index d380d4a..808c891 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -62,38 +62,71 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 	if( le->length > 2200 ) //Pulse longer indicates a sync pulse.
 	{
 		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
-		if( deltat > 2000 || deltat < -2000 )		//New pulse. (may be inverted)
+		//        if( so->codename[0] == 'W' )
+
+		if( deltat > 2000 )
 		{
-			//See if this is a unique pulse, or another one in the same set we need to look at.
-			if( le->timestamp - so->last_master_time > 1500 + so->last_photo_length )
+			//YUGH!!  This is really ugly, should refactor.
+
+			if( so->is_on_slave )
 			{
-				// check if it is a slave pulse
-				if (le->timestamp - so->last_master_time < 70000) {
-					so->last_slave_time = le->timestamp;
-					return;
+				//Last was a slave.  If new pulse, switch back to master mode.
+				if( le->timestamp - so->last_slave_time > 1500 + so->last_slave_length )
+				{
+					so->is_on_slave = 0;
+					so->last_master_time = le->timestamp;
+					so->last_master_length = le->length;
+					ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
+				}
+			}
+			else
+			{
+				//See if this is a unique pulse, or another one in the same set we need to look at.
+				if( le->timestamp - so->last_master_time > 1500 + so->last_master_length )
+				{
+					// check if it is a slave pulse
+					if (le->timestamp - so->last_master_time < 70000) {
+						so->last_slave_time = le->timestamp;
+						so->last_slave_length = le->length;
+						so->is_on_slave = 1;
+						return;
+					}
+
+					so->is_on_slave = 0;
+					so->last_master_time = le->timestamp;
+					so->last_master_length = le->length;
+					ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
+					deltat = 0;
 				}
-
-//				printf("%10u %10u %6u %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time);
-				so->last_master_time = le->timestamp;
-				so->last_photo_length = le->length;
-				ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
-				deltat = 0;
 			}
 		}
 
 		//Find longest pulse-length from device in our window and use that one.
-		if( le->length > so->last_photo_length )
+		if( so->is_on_slave )
 		{
 //			printf("%10u %10u %6d %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time);
-			so->last_master_time = le->timestamp;
-			so->last_photo_length = le->length;
+
+			if( le->length > so->last_slave_length )
+			{
+				so->last_slave_time = le->timestamp;
+				so->last_slave_length = le->length;
+			}
+		}
+		else
+		{
+			if( le->length > so->last_master_length )
+			{
+				so->last_master_time = le->timestamp;
+				so->last_master_length = le->length;
+			}
 		}
 	}
+
 	//See if this is a valid actual pulse.
 	else if( le->length < 1800 && le->length > 40 && ( le->timestamp - so->last_master_time < 380000 ) )
 	{
 		int32_t dl = so->last_master_time;
-		int32_t tpco = so->last_photo_length;
+		int32_t tpco = so->last_master_length;
 
 		//Adding length 
 		//Long pulse-code from IR flood.
@@ -103,8 +136,11 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 
 		acode >>= 1;
 		acode -= 6;
+
 		if (acode > 3) {
+			return;
 			dl = so->last_slave_time;
+			tpco = so->last_slave_length;
 		}
 
 		//printf( "%s / %d %d ++ %d %d\n", so->codename, dl, tpco, offset_from, acode );
-- 
cgit v1.2.3


From ff7167fb8afff6633db422290864e5da302d9afb Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Mon, 13 Feb 2017 01:16:05 -0500
Subject: Update disambiguator.  I think I got it right this time, finally.
 Also, seems to output OOTX data pretty good.

---
 Makefile            |   4 +-
 include/survive.h   |  15 ++--
 src/disambiguator.c | 197 ----------------------------------------------------
 src/disambiguator.h |  68 ------------------
 src/survive.c       |  13 +---
 src/survive_data.c  | 181 ++++++++++++++++++++++++++---------------------
 6 files changed, 109 insertions(+), 369 deletions(-)
 delete mode 100644 src/disambiguator.c
 delete mode 100644 src/disambiguator.h

diff --git a/Makefile b/Makefile
index 8279efa..dbd4869 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,6 @@
 all : lib data_recorder test
 
-CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto -DUSE_OLD_DISAMBIGUATOR
+CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto
 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -flto
 
 test : test.c lib/libsurvive.so redist/os_generic.o
@@ -12,7 +12,7 @@ data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/Dra
 lib:
 	mkdir lib
 
-lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/disambiguator.c redist/jsmn.o $(DEBUGSTUFF)
+lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF)
 	gcc -o $@ $^ $(LDFLAGS) -shared
 
 clean :
diff --git a/include/survive.h b/include/survive.h
index 7d461c7..d2f5bfe 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -32,16 +32,11 @@ struct SurviveObject
 	//Flood info, for calculating which laser is currently sweeping.
 	int8_t oldcode;
 
- #ifdef USE_OLD_DISAMBIGUATOR
-	uint32_t last_master_time;
-	uint32_t last_slave_time;
- 	int16_t last_master_length;
- 	int16_t last_slave_length;
-	int8_t   is_on_slave;
- #else
-	uint32_t last_master_time;
- 	struct disambiguator * d;
- #endif
+	uint32_t last_time[2];    //0 = master, 1 = slave.  Hardcoded, because it cannot simply be expanded.
+	uint32_t last_length[2];
+	int8_t   sync_set_number; //0 = master, 1 = slave, -1 = fault.  Possibly more lighthouses???
+	int8_t   did_handle_ootx;
+	uint32_t recent_sync_time;
 
 	uint32_t last_lighttime;  //May be a 24- or 32- bit number depending on what device.
 
diff --git a/src/disambiguator.c b/src/disambiguator.c
deleted file mode 100644
index f1d310a..0000000
--- a/src/disambiguator.c
+++ /dev/null
@@ -1,197 +0,0 @@
-// (C) 2016 Julian Picht, MIT/x11 License.
-//
-// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-
-//
-// The theory behind this disambiguator is, that if we just track all pulses and if one could be a sync pulse, we look back in time,
-// if we saw as sync pulse X samples ago than it is probably a sync pulse.
-//
-// X can be 20000 or 400000, depending if it came from the master or the slave.
-//
-
-#include "disambiguator.h"
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <stdbool.h>
-
-typedef uint8_t pulse_data;
-
-/**
- * Translate pulse length to pulse SKIP, DATA, AXIS
- * @param length Length of the pulse in (1/48000000) seconds
- * @return pulse data
- */
-pulse_data get_pulse_data(uint32_t length) {
-	uint16_t temp = length - 2880;
-
-#if BETTER_SAFE_THAN_FAST
-	if (temp < 0 || length > 6525) {
-		return -1;
-	}
-#endif
-
-	if ((temp % 500) < 150) {
-		return temp / 500;
-	}
-
-	return -1;
-}
-
-const uint32_t pulse_types[] = {
-	0, 1, 0, 1,
-	2, 3, 2, 3,
-};
-
-#define PULSE_BIT_AXIS 0x1
-#define PULSE_BIT_DATA 0x2
-#define PULSE_BIT_SKIP 0x4
-
-#define PULSE_DATA(D) ((D >> 1)&0x1)
-#define PULSE_AXIS(D) (D&0x01)
-#define PULSE_SKIP(D) ((D >> 2)&0x1)
-
-void disambiguator_init( struct disambiguator * d ) {
-	memset(&(d->times), 0x0, sizeof(d->times));
-	memset(&(d->scores), 0x0, sizeof(d->scores));
-
-	d->state = D_STATE_UNLOCKED;
-	d->last = 0;
-	d->max_confidence = 0;
-}
-
-inline void disambiguator_discard( struct disambiguator * d );
-
-/**
- * Drop all data that is outdated
- * @param d
- * @param age Maximum age of data we care to keep
- */
-void disambiguator_discard( struct disambiguator * d )
-{
-	long age;
-	if (d->state == D_STATE_LOCKED) {
-		age = d->last - 400000;
-	} else {
-		age = 1000000;
-	}
-	int confidence = 0;
-	for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
-		if (d->times[i] != 0 && d->times[i] < age) {
-			d->times[i] = 0;
-			d->scores[i] = 0;
-		} else {
-			if (d->scores[i] > confidence) {
-				confidence = d->scores[i];
-			}
-		}
-	}
-	d->max_confidence = confidence;
-}
-
-/**
- * Find the index that has the best likelyhood too match up with the timestamp given
- * @param t1 Rising edge time, where we expect to find the last sync pulse, if this is a master pulse
- * @param t2 Rising edge time, where we expect to find the last sync pulse, if this is a slave pulse
- * @param max_diff Maximum difference we are prepared to accept
- * @return index inside d->times, if we found something, -1 otherwise
- */
-inline int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, int max_diff );
-
-int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, int max_diff )
-{
-	int diff = max_diff; // max allowed diff for a match
-	int idx = -1;
-	for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
-		if (d->times[i] == 0) continue;
-
-		int a_1 = abs(d->times[i] - t1);
-		int a_2 = abs(d->times[i] - t2);
-
-//		printf("T            %d %d %d\n", time, i, a);
-		if (a_1 < diff) {
-			idx = i;
-			diff = a_1;
-		} else if (a_2 < diff) {
-			idx = i;
-			diff = a_2;
-		}
-	}
-
-//	if (idx != -1) {
-//		printf("R            %d %d %d\n", idx, d->scores[idx], diff);
-//	}
-
-	return idx;
-}
-
-pulse_type disambiguator_step_return_helper( struct disambiguator * d, bool sweep_possible ) {
-	if (d->state == D_STATE_LOCKED && sweep_possible) {
-		return P_SWEEP;
-	}
-	return P_UNKNOWN;
-}
-
-pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length)
-{
-	uint32_t diff = time - d->last;
-	bool sweep_possible = (diff > 70000 && diff < 350000);
-
-	// all smaller pulses are most probably sweeps
-	// TODO: check we are inside the time window of actual sweeps
-	if (length < 2750) {
-		return disambiguator_step_return_helper(d, sweep_possible);
-	}
-
-	// we expected to see a sync pulse earlier ...
-	if (time - d->last > 401000) {
-		d->state = D_STATE_UNLOCKED;
-	}
-
-	// discard all data, that is so old, we don't care about it anymore
-	disambiguator_discard(d);
-
-	// find the best match for our timestamp and presumed offset
-	int idx = disambiguator_find_nearest(d, time - 400000, time - 20000, 1000);
-
-	// We did not find a matching pulse, so try find a place to record the current
-	// one's time of arrival.
-	if (idx == -1) {
-		for (int i = 0; i < DIS_NUM_VALUES; ++i) {
-			if (d->times[i] == 0) {
-				d->times[i] = time;
-				break;
-			}
-		}
-
-		return d->state == D_STATE_LOCKED && sweep_possible ? P_SWEEP : P_UNKNOWN;
-	} else {
-		d->scores[idx]++;
-
-		// we need to be reasonably sure, that we have the right pulses
-		if (d->scores[idx] >= DIS_NUM_PULSES_BEFORE_LOCK) {
-			d->state = D_STATE_LOCKED;
-		}
-
-		// if the offset is about 20000 ticks, then this is a slave pulse
-		if (diff < 21000) {
-			if (d->state == D_STATE_LOCKED) {
-				return P_SLAVE;
-			}
-
-			return P_UNKNOWN;
-		}
-
-		d->times[idx] = time;
-		d->last = time;
-
-		// TODO: why do we need to check the confidence level here?
-		if (d->state == D_STATE_LOCKED && d->scores[idx] >= d->max_confidence) {
-			return P_MASTER;
-		}
-
-		return P_UNKNOWN;
-	}
-
-	return disambiguator_step_return_helper(d, sweep_possible);
-}
diff --git a/src/disambiguator.h b/src/disambiguator.h
deleted file mode 100644
index 8258a18..0000000
--- a/src/disambiguator.h
+++ /dev/null
@@ -1,68 +0,0 @@
-// (C) 2016 Julian Picht, MIT/x11 License.
-//
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-#ifndef DISAMBIGUATOR_H
-#define DISAMBIGUATOR_H
-
-// Determines the number of samples stored in the disambiguator struct.
-// Has to be higher than the maximum number of pulses expected between sync pulses.
-#define DIS_NUM_VALUES 48
-#define DIS_NUM_PULSES_BEFORE_LOCK 30
-#include <stdint.h>
-
-/**
- * internal disambiguator state
- */
-typedef enum {
-	D_STATE_INVALID = 0,
-	D_STATE_LOCKED = 1,
-	D_STATE_UNLOCKED = -1,
-} dis_state;
-
-/**
- * classification result
- */
-typedef enum {
-	P_UNKNOWN = 0,
-	P_MASTER = 1,
-	P_SWEEP = 2,
-	P_SLAVE = 3,
-} pulse_type;
-
-/**
- * internal state of the disambiguator
- */
-struct disambiguator {
-	// the timestamps of the recorded pulses
-	uint32_t times[DIS_NUM_VALUES];
-	// countes how many sync pulses we have seen, that match the time offset at the same offset
-	uint16_t scores[DIS_NUM_VALUES];
-	// current state
-	dis_state state;
-	// last sync pulse time
-	uint32_t last;
-	// the absolute maximum counter value
-	int max_confidence;
-	// the last code type seen
-	char code;
-};
-
-
-/**
- * Initialize a new disambiguator. calloc or memset with 0x00 will work just as well.
- *
- * @param d Pointer to the struct
- */
-void disambiguator_init( struct disambiguator * d);
-
-/**
- * Feed in one pulse to have if classified.
- *
- * @param d      Pointer to disambiguator state
- * @param time   Rising edge of the pulse
- * @param length Length of the pulse
- * @return       Classification result
- */
-pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length);
-
-#endif /* DISAMBIGUATOR_H */
\ No newline at end of file
diff --git a/src/survive.c b/src/survive.c
index af4d804..ed18da2 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -8,7 +8,7 @@
 #include <jsmn.h>
 #include <string.h>
 #include <zlib.h>
-#include "disambiguator.h"
+
 
 static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
  if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
@@ -147,21 +147,10 @@ struct SurviveContext * survive_init()
 
 	ctx->headset.ctx = ctx;
 	memcpy( ctx->headset.codename, "HMD", 4 );
-#ifndef USE_OLD_DISAMBIGUATOR
-	ctx->headset.d = calloc( 1, sizeof( struct disambiguator ) );
-#endif
-
 	ctx->watchman[0].ctx = ctx;
 	memcpy( ctx->watchman[0].codename, "WM0", 4 );
-#ifndef USE_OLD_DISAMBIGUATOR
-	ctx->watchman[0].d = calloc( 1, sizeof( struct disambiguator ) );
-#endif
-
 	ctx->watchman[1].ctx = ctx;
 	memcpy( ctx->watchman[1].codename, "WM1", 4 );
-#ifndef USE_OLD_DISAMBIGUATOR
-	ctx->watchman[1].d = calloc( 1, sizeof( struct disambiguator ) );
-#endif
 
 	//USB must happen last.
 	if( r = survive_usb_init( ctx ) )
diff --git a/src/survive_data.c b/src/survive_data.c
index 808c891..afda235 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -11,7 +11,6 @@
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
 #include "survive_internal.h"
-#include "disambiguator.h"
 #include <stdint.h>
 #include <string.h>
 
@@ -32,125 +31,148 @@ struct LightcapElement
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
 {
-	struct SurviveContext * ct = so->ctx;
-	int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
+	struct SurviveContext * ctx = so->ctx;
+	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
 //	printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
 
 	so->tsl = le->timestamp;
 	if( le->length < 20 ) return;
-#ifndef USE_OLD_DISAMBIGUATOR
-	int32_t offset = le->timestamp - so->d->last;
-	switch( disambiguator_step( so->d, le->timestamp, le->length ) ) {
-	default:
-	case P_SLAVE:
-		// this is only interesting for the OOTX data
-		break;
-	case P_UNKNOWN:
-		// not currently locked
-		break;
-	case P_MASTER:
-		ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset );
-		so->d->code = ((le->length+125)/250) - 12;
-		break;
-	case P_SWEEP:
-		if (so->d->code & 1) return;
-		ct->lightproc( so, le->sensor_id, so->d->code >> 1, offset, le->timestamp, le->length );
-		break;
+
+	//The sync pulse finder is taking Charles's old disambiguator code and mixing it with a more linear
+	//version of Julian Picht's disambiguator, available in 488c5e9.  Removed afterwards into this
+	//unified driver.
+
+
+	int ssn = so->sync_set_number;
+	if( ssn < 0 ) ssn = 0;
+	int last_sync_time  =  so->last_time  [ssn];
+	int last_sync_length = so->last_length[ssn];
+	int32_t delta = le->timestamp - last_sync_time;  //Handle time wrapping (be sure to be int32)
+
+	if( delta < -500000 || delta > 500000 )
+	{
+		//Reset pulse, etc.
+		so->sync_set_number = -1;
+		delta = 500000;
 	}
-#else
+
+
 	if( le->length > 2200 ) //Pulse longer indicates a sync pulse.
 	{
-		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
-		//        if( so->codename[0] == 'W' )
+		int is_new_pulse = delta > 1500 + last_sync_length;
+
+		so->did_handle_ootx = 0;
 
-		if( deltat > 2000 )
+		if( is_new_pulse )
 		{
-			//YUGH!!  This is really ugly, should refactor.
+			int is_master_sync_pulse = delta > 40000;
 
-			if( so->is_on_slave )
+			if( is_master_sync_pulse )
 			{
-				//Last was a slave.  If new pulse, switch back to master mode.
-				if( le->timestamp - so->last_slave_time > 1500 + so->last_slave_length )
-				{
-					so->is_on_slave = 0;
-					so->last_master_time = le->timestamp;
-					so->last_master_length = le->length;
-					ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
-				}
+				ssn = so->sync_set_number = 0;
+				so->last_time[ssn] = le->timestamp;
+				so->last_length[ssn] = le->length;
+			}
+			else if( so->sync_set_number == -1 )
+			{
+				//Do nothing.
 			}
 			else
 			{
-				//See if this is a unique pulse, or another one in the same set we need to look at.
-				if( le->timestamp - so->last_master_time > 1500 + so->last_master_length )
+				ssn = ++so->sync_set_number;
+				if( so->sync_set_number > 1 )
 				{
-					// check if it is a slave pulse
-					if (le->timestamp - so->last_master_time < 70000) {
-						so->last_slave_time = le->timestamp;
-						so->last_slave_length = le->length;
-						so->is_on_slave = 1;
-						return;
-					}
-
-					so->is_on_slave = 0;
-					so->last_master_time = le->timestamp;
-					so->last_master_length = le->length;
-					ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
-					deltat = 0;
+					SV_INFO( "Warning.  Received an extra, unassociated sync pulse." );
+					ssn = so->sync_set_number = -1;
+				}
+				else
+				{
+					so->last_time[ssn] = le->timestamp;
+					so->last_length[ssn] = le->length;
 				}
-			}
-		}
-
-		//Find longest pulse-length from device in our window and use that one.
-		if( so->is_on_slave )
-		{
-//			printf("%10u %10u %6d %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time);
-
-			if( le->length > so->last_slave_length )
-			{
-				so->last_slave_time = le->timestamp;
-				so->last_slave_length = le->length;
 			}
 		}
 		else
 		{
-			if( le->length > so->last_master_length )
+			//Find the longest pulse.
+			if( le->length > last_sync_length )
 			{
-				so->last_master_time = le->timestamp;
-				so->last_master_length = le->length;
+				if( so->last_time[ssn] > le->timestamp )
+				{
+					so->last_time[ssn] = le->timestamp;
+					so->last_length[ssn] = le->length;
+				}
 			}
 		}
 	}
-
 	//See if this is a valid actual pulse.
-	else if( le->length < 1800 && le->length > 40 && ( le->timestamp - so->last_master_time < 380000 ) )
+	else if( le->length < 1800 && le->length > 40 && delta > 30000 && ssn >= 0 )
 	{
-		int32_t dl = so->last_master_time;
-		int32_t tpco = so->last_master_length;
+		int32_t dl = so->last_time[0];
+		int32_t tpco = so->last_length[0];
 
 		//Adding length 
 		//Long pulse-code from IR flood.
 		//Make sure it fits nicely into a divisible-by-500 time.
-		int32_t acode = (tpco+125+50)/250;  //+10, seems ike that's
-		if( acode & 1 ) return;
+		int32_t acode_array[2] =
+			{
+				(so->last_length[0]+125+50)/250,
+				(so->last_length[1]+125+50)/250,
+			};
 
-		acode >>= 1;
-		acode -= 6;
+		//XXX: TODO: Capture error count here.
+		if( acode_array[0] & 1 ) return;
+		if( acode_array[1] & 1 ) return;
 
-		if (acode > 3) {
-			return;
-			dl = so->last_slave_time;
-			tpco = so->last_slave_length;
+		acode_array[0] = (acode_array[0]>>1) - 6;
+		acode_array[1] = (acode_array[1]>>1) - 6;
+
+		int acode = acode_array[0];
+
+		if( !so->did_handle_ootx )
+		{
+			int32_t delta1 = so->last_time[0] - so->recent_sync_time;
+			int32_t delta2 = so->last_time[1] - so->last_time[0];
+
+			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_time[0], so->last_length[0] );
+			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_time[1], so->last_length[1] );
+			so->recent_sync_time = so->last_time[1];
+
+			//Throw out everything if our sync pulses look like they're bad.
+			if( delta1 < 375000 || delta1 > 385000 )
+			{
+				//XXX: TODO: Count faults.
+				so->sync_set_number = -1;
+				return;
+			}
+
+			if( delta2 < 15000 || delta2 > 25000 )
+			{
+				//XXX: TODO: Count faults.
+				so->sync_set_number = -1;
+				return;
+			}
+
+			so->did_handle_ootx = 1;
 		}
 
-		//printf( "%s / %d %d ++ %d %d\n", so->codename, dl, tpco, offset_from, acode );
+
+		if (acode > 3) {
+			if( ssn == 0 )
+			{
+				SV_INFO( "Warning: got a slave marker but only got a master sync." );
+			}
+			dl = so->last_time[1];
+			tpco = so->last_length[1];
+		}
 
 		int32_t offset_from = le->timestamp - dl + le->length/2;
 
 		//Make sure pulse is in valid window
 		if( offset_from < 380000 && offset_from > 70000 )
 		{
-			ct->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
+			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
 		}
 	}
 	else
@@ -158,7 +180,6 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		//printf( "FAIL %d   %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - le->timestamp );
 		//Runt pulse, or no sync pulses available.
 	}
-#endif
 }
 
 
-- 
cgit v1.2.3


From ee433e4a076f77fa28e4d55d6703d6ed7c599251 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Mon, 13 Feb 2017 01:44:53 -0500
Subject: Closes #15 Closes #4 These are now rock solid.

Prepare the way for the OOTX Data.  Gonna hav eto get @axlecrusher on it.
---
 src/survive_data.c                       | 2 ++
 tools/plot_lighthouse/Makefile           | 4 ++--
 tools/plot_lighthouse/main.c             | 5 +++++
 tools/process_rawcap/process_to_points.c | 3 ++-
 4 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/survive_data.c b/src/survive_data.c
index afda235..b79f96e 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -135,8 +135,10 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 			int32_t delta1 = so->last_time[0] - so->recent_sync_time;
 			int32_t delta2 = so->last_time[1] - so->last_time[0];
 
+			//XXX Axlecrusher -> Add your code here!!!
 			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_time[0], so->last_length[0] );
 			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_time[1], so->last_length[1] );
+
 			so->recent_sync_time = so->last_time[1];
 
 			//Throw out everything if our sync pulses look like they're bad.
diff --git a/tools/plot_lighthouse/Makefile b/tools/plot_lighthouse/Makefile
index d156bdb..db382ea 100644
--- a/tools/plot_lighthouse/Makefile
+++ b/tools/plot_lighthouse/Makefile
@@ -1,7 +1,7 @@
 UNAME := $(shell uname)
 
 ifeq ($(UNAME), Linux)
-CFLAGS:= -lGL -lGLU -lglut
+CFLAGS:= -lGL -lGLU -lglut -I../../redist -DLINUX -lm -lpthread
 endif
 
 # Darwin is Mac OSX !!
@@ -10,6 +10,6 @@ CFLAGS:= -w -framework OpenGL -framework GLUT
 endif
 
 all:
-	gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c $(CFLAGS)
+	gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c ../../redist/linmath.c $(CFLAGS)
 clean:
 	rm -f plot_lighthouse
diff --git a/tools/plot_lighthouse/main.c b/tools/plot_lighthouse/main.c
index 8088828..4a64c28 100644
--- a/tools/plot_lighthouse/main.c
+++ b/tools/plot_lighthouse/main.c
@@ -13,6 +13,11 @@
 #include "glutil.h"
 #include "fileutil.h"
 
+#ifdef LINUX
+#include <GL/freeglut.h>
+#endif
+
+
 // Required to set up a window
 #define WIDTH  800
 #define HEIGHT 600
diff --git a/tools/process_rawcap/process_to_points.c b/tools/process_rawcap/process_to_points.c
index a1e835d..3ccfcc1 100644
--- a/tools/process_rawcap/process_to_points.c
+++ b/tools/process_rawcap/process_to_points.c
@@ -192,7 +192,8 @@ int main( int argc, char ** argv )
 			stddevtim += Sdiff * Sdiff;
 			stddevlen += Ldiff * Ldiff;
 
-			Sdiff/=4;
+			//Cast a wider net for the histogram.
+			//Sdiff/=4;
 
 			int llm = Sdiff + (HISTOGRAMSIZE/2.0);
 			if( llm < 0 ) llm = 0;
-- 
cgit v1.2.3


From 96c3e7838d96fa87c39b28f78b79b925841e6bd4 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Mon, 13 Feb 2017 15:29:48 -0500
Subject: simplify zero condition

---
 tools/ootx_decode/ootx_decoder.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index eee4e7e..98bac67 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -225,7 +225,7 @@ float _to_float(uint8_t* data) {
 
 	uint32_t *ftmp = (uint32_t*)&f; //use the allocated floating point memory
 
-	if (((x & 0x7c00) == 0x0000) && ((x & 0x03ff) == 0)) return f; //zero
+	if ((x & 0x7FFF) == 0) return f; //zero
 
 	//sign
 	*ftmp = x & 0x8000;
-- 
cgit v1.2.3


From 98db590e55513ca10ea363050f8bcb245116784b Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Mon, 13 Feb 2017 15:31:07 -0500
Subject: fix decoding of length value

---
 tools/ootx_decode/ootx_decoder.c | 30 ++++--------------------------
 1 file changed, 4 insertions(+), 26 deletions(-)

diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 98bac67..510fadb 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -60,33 +60,11 @@ int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t d
 	if (delta>18000) return 1; //a slave, should be at least 20000 but there are some data issues
 	return -1;
 }
-/*
-uint8_t decode_internal(uint32_t length) {
-		uint16_t temp = length - 2880;
-//		printf
-
-#if BETTER_SAFE_THAN_FAST
-	if (temp < 0 || length > 6525) {
-		return -1;
-	}
-#endif
 
-	if ((temp % 500) < 150) {
-		return temp / 500;
-	}
-
-	return -1;
-
-}
-*/
 uint8_t ootx_decode_bit(uint32_t length) {
-	length = ((length/500)*500)+500;
-
-	length-=3000;
-	if (length>=2000) { length-=2000; }
-	if (length>=1000)  { return 0xFF; }
-
-	return 0x00;
+	uint8_t t = (length - 2750) / 500; //why 2750?
+//	return ((t & 0x02)>0)?0xFF:0x00;
+	return ((t & 0x02)>>1);
 }
 
 void ootx_accumulate_bit(ootx_decoder_context *ctx, uint8_t bit) {
@@ -151,7 +129,7 @@ void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
 }
 
 void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
-	int8_t dbit = ootx_decode_bit(length);
+	uint8_t dbit = ootx_decode_bit(length);
 
 	ootx_pump_bit( ctx, dbit );
 }
-- 
cgit v1.2.3


From 72559b7f8d29d07d7fbf382dd7b1650511d50304 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Mon, 13 Feb 2017 15:31:46 -0500
Subject: add code to use cnlohr's improved dataset

---
 tools/ootx_decode/ootx_decode.c | 59 ++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 58 insertions(+), 1 deletion(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 1e93574..a97bc8d 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -200,12 +200,69 @@ void acode_test() {
 	}
 }
 
+void cnlohr_code_test() {
+	ootx_packet_clbk = my_test2;
+	ootx_bad_crc_clbk = bad_crc;
+
+	char* line = NULL;
+	size_t line_len = 0;
+	char trash[100] = "";
+//	int32_t atime = 0x00;
+	int8_t lh_id = 0x00;
+	uint32_t ticks = 0x00;
+	int32_t delta = 0x00;
+//	uint8_t acode = 0x00;
+//	char lighthouse_code = '\0';
+
+//	int8_t current_lighthouse = 0;
+	ootx_decoder_context *c_ctx = ctx;
+
+	while (getline(&line,&line_len,stdin)>0) {
+			//R Y HMD -1575410734 -2 7 19714 6485
+			sscanf(line,"%s %s %s %s %hhd %s %d %d",
+			trash,
+			trash,
+			trash,
+			trash,
+			&lh_id,
+			trash, //sensor id?
+			&delta,
+			&ticks);
+
+//		int8_t lh = lighthouse_code=='R'?0:1;
+//		printf("LH:%d %s\n", lh_id, line);
+		int8_t lh = (lh_id*-1)-1;
+		if (lh_id < 0) {
+//			uint8_t bit = 0x01; //bit for debugging purposes
+
+			//change to newly found lighthouse
+			c_ctx = ctx+lh;
+
+//			uint8_t dbit = ootx_decode_bit(ticks);
+//			printf("LH:%d ticks:%d bit:%X %s", lh, ticks, dbit, line);
+
+			ootx_process_bit(c_ctx, ticks);
+
+/*
+			uint16_t s = *(c_ctx->payload_size);
+			uint16_t fwv = *(c_ctx->buffer+2);
+			uint16_t pv = 0x3f & fwv; //protocol version
+			fwv>>=6; //firmware version
+
+			//this will print after any messages from ootx_pump
+//			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
+*/	
+		}
+	}
+}
+
 int main(int argc, char* argv[])
 {
 	ootx_init_decoder_context(ctx);
 	ootx_init_decoder_context(ctx+1);
 
-	raw_test();
+	cnlohr_code_test();
+//	raw_test();
 //	acode_test();
 //	hello_world_test();
 
-- 
cgit v1.2.3


From cbb6d0b53319a7e0124fff6ce7ffb12cfe94fa3f Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Mon, 13 Feb 2017 15:44:39 -0500
Subject: remove unused code

---
 tools/ootx_decode/ootx_decode.c  | 153 ---------------------------------------
 tools/ootx_decode/ootx_decoder.c |  60 +++------------
 tools/ootx_decode/ootx_decoder.h |   6 +-
 3 files changed, 10 insertions(+), 209 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index a97bc8d..1074698 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -12,9 +12,6 @@
 
 #include "ootx_decoder.h"
 
-//char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
-//extern std::istream cin;
-
 void my_test(ootx_packet* packet) {
 	packet->data[packet->length] = 0;
 	printf("%d %s 0x%X\n", packet->length, packet->data, packet->crc32);
@@ -57,149 +54,6 @@ void bad_crc(ootx_packet* packet, uint32_t crc) {
 
 ootx_decoder_context ctx[2];
 
-void hello_world_test() {
-//	ootx_init_buffer();
-	ootx_packet_clbk = my_test;
-
-	char* line = NULL;
-	size_t line_len = 0;
-	char trash[100] = "";
-	uint32_t ticks = 0x00;
-
-	while (getline(&line,&line_len,stdin)>0) {
-//		printf("%s\n", line);
-		sscanf(line,"%s %s %s %s %s %s %s %d",
-			trash,
-			trash,
-			trash,
-			trash,
-			trash,
-			trash,
-			trash,
-			&ticks);
-//		printf("%d\n", ticks);
-
-		ootx_process_bit(ctx, ticks);
-	}
-}
-
-void raw_test() {
-	ootx_packet_clbk = my_test2;
-	ootx_bad_crc_clbk = bad_crc;
-
-	char* line = NULL;
-	size_t line_len = 0;
-	char trash[100] = "";
-	int32_t atime = 0x00;
-	uint32_t ticks = 0x00;
-	uint32_t delta = 0x00;
-
-	int8_t current_lighthouse = 0;
-	ootx_decoder_context *c_ctx = ctx;
-
-	while (getline(&line,&line_len,stdin)>0) {
-//		printf("%s\n", line);
-
-		//HMD 20 0 5881 765645903 -5
-		sscanf(line,"%s %s %s %d %d %d",
-			trash,
-			trash,
-			trash,
-			&ticks,
-			&atime,
-			&delta);
-//		printf("%d\n", ticks);
-
-		int8_t lh = ootx_decode_lighthouse_number(current_lighthouse, ticks, delta);
-//		printf("lh:%d %s\n", lh, line);
-//		if (lh>0) continue;
-
-		if (lh > -1) {
-			//pump last bit
-//			printf("LH:%d ", current_lighthouse);
-			uint8_t bit = 0x01; //bit for debugging purposes
-
-//			if (current_lighthouse==1) bit &= ootx_pump_greatest_bit(c_ctx);
-			bit &= ootx_pump_greatest_bit(c_ctx);
-
-/*
-			uint16_t s = *(c_ctx->payload_size);
-			uint16_t fwv = *(c_ctx->buffer+2);
-			uint16_t pv = 0x3f & fwv; //protocol version
-			fwv>>=6; //firmware version
-
-			//this will print after any messages from ootx_pump
-			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
-*/
-			//change to newly found lighthouse
-			current_lighthouse = lh;
-			c_ctx = ctx+current_lighthouse;
-		}
-
-//		if (ticks>2000 && current_lighthouse==1) {
-		if (ticks>2000) {
-				ootx_accumulate_bit(c_ctx, ootx_decode_bit(ticks) );
-		}
-	}	
-}
-
-void acode_test() {
-	ootx_packet_clbk = my_test2;
-	ootx_bad_crc_clbk = bad_crc;
-
-	char* line = NULL;
-	size_t line_len = 0;
-	char trash[100] = "";
-	int32_t atime = 0x00;
-//	uint32_t ticks = 0x00;
-//	uint32_t delta = 0x00;
-	uint8_t acode = 0x00;
-	char lighthouse_code = '\0';
-
-	int8_t current_lighthouse = 0;
-	ootx_decoder_context *c_ctx = ctx;
-
-	while (getline(&line,&line_len,stdin)>0) {
-		//L X HMD -1842671365 18 0 175393 222
-		sscanf(line,"%c %s %s %d %s %hhu %s %s",
-			&lighthouse_code,
-			trash,
-			trash,
-			&atime,
-			trash,
-			&acode,
-			trash,
-			trash);
-
-		int8_t lh = lighthouse_code=='R'?0:1;
-//		printf("LH:%d bit:%d %s\n", lh, (acode & 0x02) >> 1,line);
-
-		if (lh != current_lighthouse) {
-			//pump last bit
-			uint8_t bit = 0x01;
-
-			if (current_lighthouse==0) bit &= ootx_pump_greatest_bit(c_ctx);
-//			ootx_pump_greatest_bit(c_ctx);
-
-			uint16_t s = *(c_ctx->payload_size);
-			uint16_t fwv = *(c_ctx->buffer+2);
-			uint16_t pv = 0x3f & fwv; //protocol version
-			fwv>>=6; //firmware version
-
-			//this will print after any messages from ootx_pump
-			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
-
-			//change to newly found lighthouse
-			current_lighthouse = lh;
-			c_ctx = ctx+current_lighthouse;
-		}
-
-//		if (current_lighthouse==0) {
-			ootx_accumulate_bit(c_ctx, (acode & 0x02) >> 1);
-//		}
-	}
-}
-
 void cnlohr_code_test() {
 	ootx_packet_clbk = my_test2;
 	ootx_bad_crc_clbk = bad_crc;
@@ -207,14 +61,10 @@ void cnlohr_code_test() {
 	char* line = NULL;
 	size_t line_len = 0;
 	char trash[100] = "";
-//	int32_t atime = 0x00;
 	int8_t lh_id = 0x00;
 	uint32_t ticks = 0x00;
 	int32_t delta = 0x00;
-//	uint8_t acode = 0x00;
-//	char lighthouse_code = '\0';
 
-//	int8_t current_lighthouse = 0;
 	ootx_decoder_context *c_ctx = ctx;
 
 	while (getline(&line,&line_len,stdin)>0) {
@@ -262,9 +112,6 @@ int main(int argc, char* argv[])
 	ootx_init_decoder_context(ctx+1);
 
 	cnlohr_code_test();
-//	raw_test();
-//	acode_test();
-//	hello_world_test();
 
 	ootx_free_decoder_context(ctx);
 	ootx_free_decoder_context(ctx+1);
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 510fadb..5c64898 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -39,55 +39,16 @@ void ootx_free_decoder_context(ootx_decoder_context *ctx) {
 	ctx->payload_size = NULL;
 }
 
-/*
-	how to decode pulses
-	ticks>2000 && delta>100000== master lighthouse
-	ticks>2000 && delta>10000 == slave lighthouse
-*/
-
-int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta) {
-	if (delta<18000) return -1; //sweep
-//	if (ticks<2000) return -1; //sweep
-//	printf ("%d\n", delta);
-
-
-	if (ticks>2000 && delta>100000) return 0; //master
-	if (delta>100000) return -1; //some kind of sweep related to the master
-
-	/*	slaves are tricky. The first few sensor readings can be confused because their tick count could be too low because of the previous master pulse?
-		so we have to ignore ticks completly
-	*/
-	if (delta>18000) return 1; //a slave, should be at least 20000 but there are some data issues
-	return -1;
-}
-
 uint8_t ootx_decode_bit(uint32_t length) {
 	uint8_t t = (length - 2750) / 500; //why 2750?
-//	return ((t & 0x02)>0)?0xFF:0x00;
+//	return ((t & 0x02)>0)?0xFF:0x00; //easier if we need to bitshift right
 	return ((t & 0x02)>>1);
 }
 
-void ootx_accumulate_bit(ootx_decoder_context *ctx, uint8_t bit) {
-	ctx->bit_count[bit&0x01]++;
-}
-
-uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx) {
-	//pump the bit
-	uint8_t bit = 0x00;
-	if (ctx->bit_count[0] < ctx->bit_count[1]) bit = 0xFF;
-
-//	printf("pump %d\n", bit);
-	ootx_pump_bit( ctx, bit );
-
-	ctx->bit_count[0] = 0;
-	ctx->bit_count[1] = 0;
-
-	return bit;
-}
-
 uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
 	ctx->preamble <<= 1;
-	ctx->preamble |= (0x01 & dbit);
+//	ctx->preamble |= (0x01 & dbit);
+	ctx->preamble |= dbit;
 	if ((ctx->preamble & 0x0003ffff) == 0x00000001) return 1;
 	return 0;
 }
@@ -115,11 +76,11 @@ void ootx_inc_buffer_offset(ootx_decoder_context *ctx) {
 
 void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
 	uint8_t *current_byte = ctx->buffer + ctx->buf_offset;
-//	printf("%d\n", dbit);
-//	*current_byte >>= 1;
-//	*current_byte |= (0x80 & dbit);
+
 	*current_byte <<= 1;
-	*current_byte |= (0x01 & dbit);
+//	*current_byte |= (0x01 & dbit);
+	*current_byte |= dbit;
+
 	++(ctx->bits_written);
 	if (ctx->bits_written>7) {
 		ctx->bits_written=0;
@@ -128,19 +89,16 @@ void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
 	}
 }
 
-void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
+uint8_t ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
 	uint8_t dbit = ootx_decode_bit(length);
-
 	ootx_pump_bit( ctx, dbit );
+	return dbit;
 }
 
 void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 //	uint8_t dbit = ootx_decode_bit(length);
 	++(ctx->bits_processed);
 
-//	printf("z %d %d\n", bits_processed,dbit);
-//	printf("d %d\n", bits_processed,dbit);
-
 	if ( ootx_detect_preamble(ctx, dbit) ) {
 		/*	data stream can start over at any time so we must
 			always look for preamble bits */
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
index 4ec1fd1..c707159 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -55,14 +55,10 @@ typedef struct {
 void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data);
 void print_lighthouse_info_v6(lighthouse_info_v6* lhi);
 
-//void ootx_init_buffer();
-void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
 void ootx_init_decoder_context(ootx_decoder_context *ctx);
-int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta);
 void ootx_free_decoder_context(ootx_decoder_context *ctx);
 
-void ootx_accumulate_bit(ootx_decoder_context *ctx, uint8_t bit);
-uint8_t ootx_pump_greatest_bit(ootx_decoder_context *ctx);
+uint8_t ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
 
 uint8_t ootx_decode_bit(uint32_t length);
 
-- 
cgit v1.2.3


From 56a6bbc24962251d223536285aa407c7652c7c46 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Mon, 13 Feb 2017 15:46:03 -0500
Subject: remove crc32 code. we are using zlib crc32 code

---
 tools/ootx_decode/crc32.c | 103 ----------------------------------------------
 tools/ootx_decode/crc32.h |  13 ------
 2 files changed, 116 deletions(-)
 delete mode 100644 tools/ootx_decode/crc32.c
 delete mode 100644 tools/ootx_decode/crc32.h

diff --git a/tools/ootx_decode/crc32.c b/tools/ootx_decode/crc32.c
deleted file mode 100644
index a719cbf..0000000
--- a/tools/ootx_decode/crc32.c
+++ /dev/null
@@ -1,103 +0,0 @@
-/*-
- *  COPYRIGHT (C) 1986 Gary S. Brown.  You may use this program, or
- *  code or tables extracted from it, as desired without restriction.
- *
- *  First, the polynomial itself and its table of feedback terms.  The
- *  polynomial is
- *  X^32+X^26+X^23+X^22+X^16+X^12+X^11+X^10+X^8+X^7+X^5+X^4+X^2+X^1+X^0
- *
- *  Note that we take it "backwards" and put the highest-order term in
- *  the lowest-order bit.  The X^32 term is "implied"; the LSB is the
- *  X^31 term, etc.  The X^0 term (usually shown as "+1") results in
- *  the MSB being 1
- *
- *  Note that the usual hardware shift register implementation, which
- *  is what we're using (we're merely optimizing it by doing eight-bit
- *  chunks at a time) shifts bits into the lowest-order term.  In our
- *  implementation, that means shifting towards the right.  Why do we
- *  do it this way?  Because the calculated CRC must be transmitted in
- *  order from highest-order term to lowest-order term.  UARTs transmit
- *  characters in order from LSB to MSB.  By storing the CRC this way
- *  we hand it to the UART in the order low-byte to high-byte; the UART
- *  sends each low-bit to hight-bit; and the result is transmission bit
- *  by bit from highest- to lowest-order term without requiring any bit
- *  shuffling on our part.  Reception works similarly
- *
- *  The feedback terms table consists of 256, 32-bit entries.  Notes
- *
- *      The table can be generated at runtime if desired; code to do so
- *      is shown later.  It might not be obvious, but the feedback
- *      terms simply represent the results of eight shift/xor opera
- *      tions for all combinations of data and CRC register values
- *
- *      The values must be right-shifted by eight bits by the "updcrc
- *      logic; the shift must be unsigned (bring in zeroes).  On some
- *      hardware you could probably optimize the shift in assembler by
- *      using byte-swap instructions
- *      polynomial $edb88320
- *
- *
- * CRC32 code derived from work by Gary S. Brown.
- */
-
-#include <stddef.h>
-#include <stdint.h>
-
-static uint32_t crc32_tab[] = {
-	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
-	0xe963a535, 0x9e6495a3,	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
-	0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
-	0xf3b97148, 0x84be41de,	0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
-	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,	0x14015c4f, 0x63066cd9,
-	0xfa0f3d63, 0x8d080df5,	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
-	0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,	0x35b5a8fa, 0x42b2986c,
-	0xdbbbc9d6, 0xacbcf940,	0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
-	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
-	0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
-	0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,	0x76dc4190, 0x01db7106,
-	0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
-	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
-	0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
-	0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
-	0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
-	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
-	0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
-	0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
-	0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
-	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
-	0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
-	0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
-	0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
-	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
-	0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
-	0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
-	0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
-	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
-	0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
-	0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
-	0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
-	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
-	0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
-	0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
-	0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
-	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
-	0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
-	0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
-	0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
-	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
-	0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
-	0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
-};
-
-uint32_t crc32(uint32_t crc, const void *buf, size_t size)
-{
-	const uint8_t *p;
-
-	p = buf;
-	crc = crc ^ ~0U;
-
-	while (size--)
-		crc = crc32_tab[(crc ^ *p++) & 0xFF] ^ (crc >> 8);
-
-	return crc ^ ~0U;
-}
diff --git a/tools/ootx_decode/crc32.h b/tools/ootx_decode/crc32.h
deleted file mode 100644
index c771d4a..0000000
--- a/tools/ootx_decode/crc32.h
+++ /dev/null
@@ -1,13 +0,0 @@
-// (C) 2016 Joshua Allen, MIT/x11 License.
-//
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-
-#ifndef CRC32_H
-#define CRC32_H
-
-#include <stddef.h>
-#include <stdint.h>
-
-uint32_t crc32(uint32_t crc, uint8_t *buf, size_t size);
-
-#endif
\ No newline at end of file
-- 
cgit v1.2.3


From c67f528d10f55e34d0d254fb59494778bcb04e90 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Mon, 13 Feb 2017 16:04:01 -0500
Subject: use zlib crc32

---
 tools/ootx_decode/HMD_Datagen.c | 8 +++++---
 tools/ootx_decode/Makefile      | 6 +++---
 2 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/tools/ootx_decode/HMD_Datagen.c b/tools/ootx_decode/HMD_Datagen.c
index 34bf543..15ed62c 100644
--- a/tools/ootx_decode/HMD_Datagen.c
+++ b/tools/ootx_decode/HMD_Datagen.c
@@ -9,8 +9,9 @@
 #include <stdint.h>
 #include <time.h>
 #include <stdlib.h>
+#include <zlib.h>
 
-#include "crc32.h"
+//this program is broken and does not produce useable data.
 
 uint32_t time_stamp = -525198892;
 
@@ -33,7 +34,8 @@ int main(int argc, char* argv[])
 	print_preamble();
 
 	uint16_t payload_lenth = strlen(str);
-	uint32_t crc = crc32(0xffffffff,(uint8_t*)str,payload_lenth);
+	uint32_t crc = crc32( 0L, Z_NULL, 0 );
+	crc = crc32( crc, (uint8_t*)str,payload_lenth);
 
 	print_uint16(payload_lenth);
 	print_payload(str,payload_lenth);
@@ -98,4 +100,4 @@ void print_bit(uint8_t data) {
 	*/
 
 	//fire off a callback when a full OOTX packet is received
-}
\ No newline at end of file
+}
diff --git a/tools/ootx_decode/Makefile b/tools/ootx_decode/Makefile
index 8be8618..9170ac4 100644
--- a/tools/ootx_decode/Makefile
+++ b/tools/ootx_decode/Makefile
@@ -1,7 +1,7 @@
 all: ootx_decode hmd_datagen
 
-hmd_datagen: HMD_Datagen.c crc32.c crc32.h
-	gcc -Wall HMD_Datagen.c crc32.c -o hmd_datagen
+hmd_datagen: HMD_Datagen.c 
+	gcc -Wall HMD_Datagen.c -lz -o hmd_datagen
 
 ootx_decode: ootx_decode.c ootx_decoder.c ootx_decoder.h
-	gcc -Wall ootx_decode.c ootx_decoder.c -lz -o ootx_decode
\ No newline at end of file
+	gcc -Wall ootx_decode.c ootx_decoder.c -lz -o ootx_decode
-- 
cgit v1.2.3


From bd922d085158f38567c630fe23aa9043ff720fc5 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Mon, 13 Feb 2017 16:50:12 -0700
Subject: Using gradient descent to refine estimate

---
 tools/lighthousefind_tori/torus_localizer.c | 267 +++++++++++++++++++++++-----
 tools/lighthousefind_tori/visualization.c   |   4 +-
 2 files changed, 224 insertions(+), 47 deletions(-)

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 22a0ce2..15177f2 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -400,45 +400,18 @@ double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
 	double pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd));
 	return pythAngle;
 }
-Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
-{
-	PointsAndAngle pna[MAX_POINT_PAIRS];
-
-	size_t pnaCount = 0;
-	for (unsigned int i = 0; i < obj->numSensors; i++)
-	{
-		for (unsigned int j = 0; j < i; j++)
-		{
-			if (pnaCount < MAX_POINT_PAIRS)
-			{
-				pna[pnaCount].a = obj->sensor[i].point;
-				pna[pnaCount].b = obj->sensor[j].point;
-
-				pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
-
-				double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
-
-				pnaCount++;
-			}
-		}
-	}
 
+Point GetInitialEstimate(TrackedObject *obj, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
+{
 	Point **pointCloud = malloc(sizeof(void*)* pnaCount);
 
-	FILE *f = NULL;
-	if (doLogOutput)
-	{
-		f = fopen("pointcloud2.pcd", "wb");
-		writePcdHeader(f);
-		writeAxes(f);
-	}
 
 	for (unsigned int i = 0; i < pnaCount; i++)
 	{
 		torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i]));
-		if (doLogOutput)
+		if (logFile)
 		{
-			writePointCloud(f, pointCloud[i], COLORS[i%MAX_COLORS]);
+			writePointCloud(logFile, pointCloud[i], COLORS[i%MAX_COLORS]);
 		}
 
 	}
@@ -451,13 +424,19 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 		pointCloud[i] = NULL;
 	}
 
-	if (doLogOutput)
+	if (logFile)
 	{
-		markPointWithStar(f, bestMatchA, 0xFF0000);
+		markPointWithStar(logFile, bestMatchA, 0xFF0000);
 	}
 #ifdef TORI_DEBUG
 	printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z);
 #endif
+
+	return bestMatchA;
+}
+
+Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile)
+{
 	// Now, let's add an extra patch or torus near the point we just found.
 
 	double toroidalAngle = 0;
@@ -473,15 +452,15 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 			pna[i].a,
 			pna[i].b,
 			pna[i].angle,
-			bestMatchA,
+			initialEstimate,
 			&toroidalAngle,
 			&poloidalAngle);
 
 		partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.1, toroidalAngle + 0.1, poloidalAngle - 0.2, poloidalAngle + 0.2, pna[i].angle, 800, &(pointCloud2[i]));
 
-		if (doLogOutput)
+		if (logFile)
 		{
-			writePointCloud(f, pointCloud2[i], COLORS[i%MAX_COLORS]);
+			writePointCloud(logFile, pointCloud2[i], COLORS[i%MAX_COLORS]);
 		}
 
 	}
@@ -494,9 +473,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 		pointCloud2[i] = NULL;
 	}
 
-	if (doLogOutput)
+	if (logFile)
 	{
-		markPointWithStar(f, bestMatchB, 0x00FF00);
+		markPointWithStar(logFile, bestMatchB, 0x00FF00);
 	}
 #ifdef TORI_DEBUG
 	printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z);
@@ -516,9 +495,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 
 		partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.05, toroidalAngle + 0.05, poloidalAngle - 0.1, poloidalAngle + 0.1, pna[i].angle, 3000, &(pointCloud3[i]));
 
-		if (doLogOutput)
+		if (logFile)
 		{
-			writePointCloud(f, pointCloud3[i], COLORS[i%MAX_COLORS]);
+			writePointCloud(logFile, pointCloud3[i], COLORS[i%MAX_COLORS]);
 		}
 
 	}
@@ -531,24 +510,222 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 		pointCloud3[i] = NULL;
 	}
 
-	if (doLogOutput)
+	if (logFile)
 	{
-		markPointWithStar(f, bestMatchC, 0xFFFFFF);
+		markPointWithStar(logFile, bestMatchC, 0xFFFFFF);
 	}
 #ifdef TORI_DEBUG
 	printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z);
 #endif
 
 
+
+
+	return bestMatchC;
+}
+
+Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, double poloidalAngle)
+{
+	Point result;
+
+	double distanceBetweenPoints = distance(pna->a, pna->b);
+	Point m = midpoint(pna->a, pna->b);
+	Matrix3x3 rot = GetRotationMatrixForTorus(pna->a, pna->b);
+
+	double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
+	double poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+
+	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*cos(toroidalAngle);
+	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*sin(toroidalAngle);
+	result.z = poloidalRadius*sin(poloidalAngle);
+	result = RotateAndTranslatePoint(result, rot, m);
+
+	return result;
+}
+
+FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
+{
+
+	double toroidalAngle = 0;
+	double poloidalAngle = 0;
+
+	estimateToroidalAndPoloidalAngleOfPoint(
+		pna->a,
+		pna->b,
+		pna->angle,
+		pointIn,
+		&toroidalAngle,
+		&poloidalAngle);
+
+	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalAngle, poloidalAngle);
+
+	return distance(pointIn, torusPoint);
+}
+
+//Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile)
+//
+FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount)
+{
+	FLT fitness;
+
+	FLT resultSum=0;
+
+	for (size_t i = 0; i < pnaCount; i++)
+	{
+		fitness = getPointFitnessForPna(pointIn, &(pna[i]));
+		//printf("Distance[%d]: %f\n", i, fitness);
+		resultSum += SQUARED(fitness);
+	}
+
+	return 1/FLT_SQRT(resultSum);
+}
+
+Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision)
+{
+	Point result;
+
+	Point tmpXplus = pointIn;
+	Point tmpXminus = pointIn;
+	tmpXplus.x = pointIn.x + precision;
+	tmpXminus.x = pointIn.x - precision;
+	result.x = getPointFitness(tmpXplus, pna, pnaCount) - getPointFitness(tmpXminus, pna, pnaCount);
+
+	Point tmpYplus = pointIn;
+	Point tmpYminus = pointIn;
+	tmpYplus.y = pointIn.y + precision;
+	tmpYminus.y = pointIn.y - precision;
+	result.y = getPointFitness(tmpYplus, pna, pnaCount) - getPointFitness(tmpYminus, pna, pnaCount);
+
+	Point tmpZplus = pointIn;
+	Point tmpZminus = pointIn;
+	tmpZplus.z = pointIn.z + precision;
+	tmpZminus.z = pointIn.z - precision;
+	result.z = getPointFitness(tmpZplus, pna, pnaCount) - getPointFitness(tmpZminus, pna, pnaCount);
+
+	return result;
+}
+
+Point getNormalizedVector(Point vectorIn, FLT desiredMagnitude)
+{
+	FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z));
+
+	FLT scale = desiredMagnitude / distanceIn;
+
+	Point result = vectorIn;
+
+	result.x *= scale;
+	result.y *= scale;
+	result.z *= scale;
+
+	return result;
+}
+
+Point getAvgPoints(Point a, Point b)
+{
+	Point result;
+	result.x = (a.x + b.x) / 2;
+	result.y = (a.y + b.y) / 2;
+	result.z = (a.z + b.z) / 2;
+	return result;
+}
+
+// 0.95 is good value for descent
+// 0.1 is a good value for starting precision.
+static Point RefineEstimateUsingGradientDescent(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile, FLT descent, FLT startingPrecision)
+{
+	int i = 0;
+	FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount);
+	Point lastPoint = initialEstimate;
+	Point lastGradient = getGradient(lastPoint, pna, pnaCount, .00000001 /*somewhat arbitrary*/);
+
+	for (FLT f = startingPrecision; f > 0.0001; f *= descent)
+	{
+		Point gradient = getGradient(lastPoint, pna, pnaCount, f/1000 /*somewhat arbitrary*/);
+		gradient = getNormalizedVector(gradient, f);
+
+		//printf("Gradient: (%f, %f, %f)\n", gradient.x, gradient.y, gradient.z);
+
+		// gradient = getAvgPoints(gradient, lastGradient); // doesn't seem to help much.  might hurt in some cases.
+
+		Point newPoint;
+		newPoint.x = lastPoint.x + gradient.x;
+		newPoint.y = lastPoint.y + gradient.y;
+		newPoint.z = lastPoint.z + gradient.z;
+
+		FLT newMatchFitness = getPointFitness(newPoint, pna, pnaCount);
+
+		if (newMatchFitness > lastMatchFitness)
+		{
+			lastMatchFitness = newMatchFitness;
+			lastPoint = newPoint;
+			//printf("%f\n", newMatchFitness);
+			lastGradient = gradient;
+
+			if (logFile)
+			{
+				writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF);
+			}
+		}
+		else
+		{
+			//printf("-");
+		}
+
+		i++;
+	}
+
+	//printf("i = %d\n", i);
+
+	return lastPoint;
+}
+
+Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
+{
+	PointsAndAngle pna[MAX_POINT_PAIRS];
+
+	size_t pnaCount = 0;
+	for (unsigned int i = 0; i < obj->numSensors; i++)
+	{
+		for (unsigned int j = 0; j < i; j++)
+		{
+			if (pnaCount < MAX_POINT_PAIRS)
+			{
+				pna[pnaCount].a = obj->sensor[i].point;
+				pna[pnaCount].b = obj->sensor[j].point;
+
+				pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
+
+				double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
+
+				pnaCount++;
+			}
+		}
+	}
+
+	FILE *logFile = NULL;
 	if (doLogOutput)
 	{
-		updateHeader(f);
-		fclose(f);
+		logFile = fopen("pointcloud2.pcd", "wb");
+		writePcdHeader(logFile);
+		writeAxes(logFile);
 	}
 
+	Point initialEstimate = GetInitialEstimate(obj, pna, pnaCount, logFile);
 
+	//Point refinedEstimatePc = RefineEstimateUsingPointCloud(initialEstimate, pna, pnaCount, obj, logFile);
 
-	return bestMatchC;
+	Point refinedEstimageGd = RefineEstimateUsingGradientDescent(initialEstimate, pna, pnaCount, logFile, 0.95, 0.1);
+
+	//FLT fitPc = getPointFitness(refinedEstimatePc, pna, pnaCount);
+	FLT fitGd = getPointFitness(refinedEstimageGd, pna, pnaCount);
+
+	if (logFile)
+	{
+		updateHeader(logFile);
+		fclose(logFile);
+	}
+
+	return refinedEstimageGd;
 }
 
 static Point makeUnitPoint(Point *p)
diff --git a/tools/lighthousefind_tori/visualization.c b/tools/lighthousefind_tori/visualization.c
index 12cbfee..098e0e2 100644
--- a/tools/lighthousefind_tori/visualization.c
+++ b/tools/lighthousefind_tori/visualization.c
@@ -60,10 +60,10 @@ void writePcdHeader(FILE * file)
 	fprintf(file, "SIZE 4 4 4 4\n");
 	fprintf(file, "TYPE F F F U\n");
 	fprintf(file, "COUNT 1 1 1 1\n");
-	fprintf(file, "WIDTH		\n");
+	fprintf(file, "WIDTH        \n");
 	fprintf(file, "HEIGHT 1\n");
 	fprintf(file, "VIEWPOINT 0 0 0 1 0 0 0\n");
-	fprintf(file, "POINTS		\n");
+	fprintf(file, "POINTS        \n");
 	fprintf(file, "DATA ascii\n");
 
 	//fprintf(file, "100000.0, 100000.0, 100000\n");
-- 
cgit v1.2.3


From b1ccf74484f7400ebd6827fa6aa897b735f40d8e Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Mon, 13 Feb 2017 17:52:09 -0700
Subject: Optimize gradient descent for shallow valley

---
 tools/lighthousefind_tori/torus_localizer.c | 81 ++++++++++++++++++++++++++++-
 1 file changed, 79 insertions(+), 2 deletions(-)

diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 15177f2..b276e99 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -640,7 +640,7 @@ static Point RefineEstimateUsingGradientDescent(Point initialEstimate, PointsAnd
 
 	for (FLT f = startingPrecision; f > 0.0001; f *= descent)
 	{
-		Point gradient = getGradient(lastPoint, pna, pnaCount, f/1000 /*somewhat arbitrary*/);
+		Point gradient = getGradient(lastPoint, pna, pnaCount, f / 1000 /*somewhat arbitrary*/);
 		gradient = getNormalizedVector(gradient, f);
 
 		//printf("Gradient: (%f, %f, %f)\n", gradient.x, gradient.y, gradient.z);
@@ -679,6 +679,78 @@ static Point RefineEstimateUsingGradientDescent(Point initialEstimate, PointsAnd
 	return lastPoint;
 }
 
+// This is modifies the basic gradient descent algorithm to better handle the shallow valley case,
+// which appears to be typical of this convergence.  
+static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
+{
+	int i = 0;
+	FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount);
+	Point lastPoint = initialEstimate;
+	//Point lastGradient = getGradient(lastPoint, pna, pnaCount, .00000001 /*somewhat arbitrary*/);
+
+
+	for (FLT g = 0.1; g > 0.00001; g *= 0.99)
+	{
+		i++;
+		Point point1 = lastPoint;
+		// let's get 3 iterations of gradient descent here.
+		Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/);
+		Point gradientN1 = getNormalizedVector(gradient1, g);
+
+		Point point2;
+		point2.x = point1.x + gradientN1.x;
+		point2.y = point1.y + gradientN1.y;
+		point2.z = point1.z + gradientN1.z;
+
+		Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/);
+		Point gradientN2 = getNormalizedVector(gradient2, g);
+
+		Point point3;
+		point3.x = point2.x + gradientN2.x;
+		point3.y = point2.y + gradientN2.y;
+		point3.z = point2.z + gradientN2.z;
+
+		Point specialGradient = { .x = point3.x - point1.x, .y = point3.y - point1.y, .z = point3.y - point1.y };
+
+		specialGradient = getNormalizedVector(specialGradient, g/2);
+
+		Point point4;
+
+		point4.x = point3.x + specialGradient.x;
+		point4.y = point3.y + specialGradient.y;
+		point4.z = point3.z + specialGradient.z;
+
+		//point4.x = (point1.x + point2.x + point3.x) / 3;
+		//point4.y = (point1.y + point2.y + point3.y) / 3;
+		//point4.z = (point1.z + point2.z + point3.z) / 3;
+
+		FLT newMatchFitness = getPointFitness(point4, pna, pnaCount);
+
+		if (newMatchFitness > lastMatchFitness)
+		{
+			if (logFile)
+			{
+				writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF);
+			}
+
+			lastMatchFitness = newMatchFitness;
+			lastPoint = point4;
+			printf("+");
+		}
+		else
+		{
+			printf("-");
+			g *= 0.7;
+
+		}
+
+
+	}
+	printf("\ni=%d\n", i);
+
+	return lastPoint;
+}
+
 Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 {
 	PointsAndAngle pna[MAX_POINT_PAIRS];
@@ -714,7 +786,12 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 
 	//Point refinedEstimatePc = RefineEstimateUsingPointCloud(initialEstimate, pna, pnaCount, obj, logFile);
 
-	Point refinedEstimageGd = RefineEstimateUsingGradientDescent(initialEstimate, pna, pnaCount, logFile, 0.95, 0.1);
+	//Point refinedEstimageGd = RefineEstimateUsingGradientDescent(initialEstimate, pna, pnaCount, logFile, 0.95, 0.1);
+
+	Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile);
+
+	//Point p = { .x = 8 };
+	//Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p, pna, pnaCount, logFile);
 
 	//FLT fitPc = getPointFitness(refinedEstimatePc, pna, pnaCount);
 	FLT fitGd = getPointFitness(refinedEstimageGd, pna, pnaCount);
-- 
cgit v1.2.3


From f0e743ccc98397ee011ea10a6068263b5f3dbae7 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Mon, 13 Feb 2017 21:22:00 -0500
Subject: rename function for half float to float

---
 tools/ootx_decode/ootx_decoder.c | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
index 5c64898..b5a7b54 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -153,7 +153,7 @@ uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) {
 	return x;
 }
 
-float _to_float(uint8_t* data) {
+float _half_to_float(uint8_t* data) {
 	//this will not handle infinity, NaN, or denormalized floats
 
 	uint16_t x = *(uint16_t*)data;
@@ -198,21 +198,21 @@ void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) {
 
 	lhi->fw_version = *(uint16_t*)get_ptr(data,sizeof(uint16_t),&idx);
 	lhi->id = *(uint32_t*)get_ptr(data,sizeof(uint32_t),&idx);
-	lhi->fcal_0_phase = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_phase = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_0_tilt = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_tilt = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_0_phase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_phase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_0_tilt = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_tilt = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
 	lhi->sys_unlock_count = *get_ptr(data,sizeof(uint8_t),&idx);
 	lhi->hw_version = *get_ptr(data,sizeof(uint8_t),&idx);
-	lhi->fcal_0_curve = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_curve = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_0_curve = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_curve = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
 	lhi->accel_dir_x = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
 	lhi->accel_dir_y = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
 	lhi->accel_dir_z = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
-	lhi->fcal_0_gibphase = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_gibphase = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_0_gibmag = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_gibmag = _to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_0_gibphase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_gibphase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_0_gibmag = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_gibmag = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
 	lhi->mode_current = *get_ptr(data,sizeof(uint8_t),&idx);
 	lhi->sys_faults = *get_ptr(data,sizeof(uint8_t),&idx);
 
-- 
cgit v1.2.3


From 35e08aaa24bd01e6ace453f89ddb73a6bb0508b0 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Tue, 14 Feb 2017 01:00:33 -0500
Subject: Still not working, but getting closer to dynamic OOTX decoding.

---
 Makefile               |   7 ++-
 calibrate.c            | 124 +++++++++++++++++++++++++++++++++++++
 data_recorder.c        |   4 ++
 include/survive.h      |  23 +++++--
 redist/crc32.c         | 104 +++++++++++++++++++++++++++++++
 redist/crc32.h         |  17 ++++++
 src/ootx_decoder.c     | 163 +++++++++++++++++++++++++++++++++++++++++++++++++
 src/ootx_decoder.h     |  41 +++++++++++++
 src/survive_cal.c      |  68 +++++++++++++++++++++
 src/survive_cal.h      |  44 +++++++++++++
 src/survive_data.c     |  12 +++-
 src/survive_internal.h |  26 +++++++-
 src/survive_process.c  |  15 +++--
 13 files changed, 631 insertions(+), 17 deletions(-)
 create mode 100644 calibrate.c
 create mode 100644 redist/crc32.c
 create mode 100644 redist/crc32.h
 create mode 100644 src/ootx_decoder.c
 create mode 100644 src/ootx_decoder.h
 create mode 100644 src/survive_cal.c
 create mode 100644 src/survive_cal.h

diff --git a/Makefile b/Makefile
index dbd4869..c0a4caa 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
-all : lib data_recorder test
+all : lib data_recorder test calibrate
 
 CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto
 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -flto
@@ -9,10 +9,13 @@ test : test.c lib/libsurvive.so redist/os_generic.o
 data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o
 	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
+calibrate :  calibrate.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c redist/crc32.c
+	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
+
 lib:
 	mkdir lib
 
-lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF)
+lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/survive_cal.o src/ootx_decoder.o $(DEBUGSTUFF)
 	gcc -o $@ $^ $(LDFLAGS) -shared
 
 clean :
diff --git a/calibrate.c b/calibrate.c
new file mode 100644
index 0000000..4db2ed0
--- /dev/null
+++ b/calibrate.c
@@ -0,0 +1,124 @@
+//Data recorder mod with GUI showing light positions.
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <survive.h>
+#include <string.h>
+#include <os_generic.h>
+#include <DrawFunctions.h>
+
+struct SurviveContext * ctx;
+
+void HandleKey( int keycode, int bDown )
+{
+	if( !bDown ) return;
+
+	if( keycode == 'O' || keycode == 'o' )
+	{
+		survive_usb_send_magic(ctx,1);
+	}
+	if( keycode == 'F' || keycode == 'f' )
+	{
+		survive_usb_send_magic(ctx,0);
+	}
+}
+
+void HandleButton( int x, int y, int button, int bDown )
+{
+}
+
+void HandleMotion( int x, int y, int mask )
+{
+}
+
+int bufferpts[32*2*3];
+char buffermts[32*128*3];
+int buffertimeto[32*3];
+
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+{
+	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+}
+
+void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+{
+	survive_default_imu_process( so, accelgyro, timecode, id );
+
+return;
+	//if( so->codename[0] == 'H' )
+	if( 1 )
+	{
+		printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
+	}
+}
+
+
+
+
+void * GuiThread( void * v )
+{
+	short screenx, screeny;
+	while(1)
+	{
+		CNFGHandleInput();
+		CNFGClearFrame();
+		CNFGColor( 0xFFFFFF );
+		CNFGGetDimensions( &screenx, &screeny );
+
+		int i;
+		for( i = 0; i < 32*3; i++ )
+		{
+			if( buffertimeto[i] < 50 )
+			{
+				uint32_t color = i * 3231349;
+				uint8_t r = color & 0xff;
+				uint8_t g = (color>>8) & 0xff;
+				uint8_t b = (color>>16) & 0xff;
+				r = (r * (5-buffertimeto[i])) / 5 ;
+				g = (g * (5-buffertimeto[i])) / 5 ;
+				b = (b * (5-buffertimeto[i])) / 5 ;
+				CNFGColor( (b<<16) | (g<<8) | r );
+				CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 );
+				CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1];
+				CNFGDrawText( buffermts, 2 );			
+				buffertimeto[i]++;
+			}
+		}
+
+
+		CNFGSwapBuffers();
+		OGUSleep( 10000 );
+	}
+}
+
+
+
+int main()
+{
+	ctx = survive_init( );
+
+	survive_install_light_fn( ctx,  my_light_process );
+	survive_install_imu_fn( ctx,  my_imu_process );
+
+	survive_cal_install( ctx );
+
+	CNFGBGColor = 0x000000;
+	CNFGDialogColor = 0x444444;
+	CNFGSetup( "Survive GUI Debug", 640, 480 );
+	OGCreateThread( GuiThread, 0 );
+	
+
+	if( !ctx )
+	{
+		fprintf( stderr, "Fatal. Could not start\n" );
+		return 1;
+	}
+
+	while(survive_poll(ctx) == 0)
+	{
+		//Do stuff.
+	}
+}
+
diff --git a/data_recorder.c b/data_recorder.c
index 4c5627f..ced82c4 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -39,6 +39,8 @@ int buffertimeto[32*3];
 
 void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
+	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+
 	if( acode == -1 ) return;
 //return;
 	int jumpoffset = sensor_id;
@@ -77,6 +79,8 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
 
 void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
 {
+	survive_default_imu_process( so, accelgyro, timecode, id );
+
 return;
 	//if( so->codename[0] == 'H' )
 	if( 1 )
diff --git a/include/survive.h b/include/survive.h
index d2f5bfe..4a95a2f 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -10,6 +10,12 @@ struct SurviveContext;
 //DANGER: This structure may be redefined.  Note that it is logically split into 64-bit chunks
 //for optimization on 32- and 64-bit systems.
 
+
+//Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools.
+//It will also require a recompile.  TODO: revisit this and correct the comment once fixed.
+#define NUM_LIGHTHOUSES 2  
+
+
 struct SurviveObject
 {
 	struct SurviveContext * ctx;
@@ -32,10 +38,10 @@ struct SurviveObject
 	//Flood info, for calculating which laser is currently sweeping.
 	int8_t oldcode;
 
-	uint32_t last_time[2];    //0 = master, 1 = slave.  Hardcoded, because it cannot simply be expanded.
-	uint32_t last_length[2];
-	int8_t   sync_set_number; //0 = master, 1 = slave, -1 = fault.  Possibly more lighthouses???
-	int8_t   did_handle_ootx;
+	uint32_t last_time[NUM_LIGHTHOUSES];
+	uint32_t last_length[NUM_LIGHTHOUSES];
+	int8_t   sync_set_number; //0 = master, 1 = slave, -1 = fault. 
+	int8_t   did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit.
 	uint32_t recent_sync_time;
 
 	uint32_t last_lighttime;  //May be a 24- or 32- bit number depending on what device.
@@ -64,8 +70,15 @@ struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, cons
 //Utilitiy functions.
 int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen );
 
-//TODO: Need to make this do haptic responses for hands.
+//TODO: Need to make this do haptic responses for hands. 
 int survive_usb_send_magic( struct SurviveContext * ctx, int on );
 
+//Install the calibrator.
+void survive_cal_install( struct SurviveContext * ctx );
+
+//Call these from your callback if overridden.  
+void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
+void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
+
 #endif
 
diff --git a/redist/crc32.c b/redist/crc32.c
new file mode 100644
index 0000000..834e7f2
--- /dev/null
+++ b/redist/crc32.c
@@ -0,0 +1,104 @@
+/*-
+ *  COPYRIGHT (C) 1986 Gary S. Brown.  You may use this program, or
+ *  code or tables extracted from it, as desired without restriction.
+ *
+ *  First, the polynomial itself and its table of feedback terms.  The
+ *  polynomial is
+ *  X^32+X^26+X^23+X^22+X^16+X^12+X^11+X^10+X^8+X^7+X^5+X^4+X^2+X^1+X^0
+ *
+ *  Note that we take it "backwards" and put the highest-order term in
+ *  the lowest-order bit.  The X^32 term is "implied"; the LSB is the
+ *  X^31 term, etc.  The X^0 term (usually shown as "+1") results in
+ *  the MSB being 1
+ *
+ *  Note that the usual hardware shift register implementation, which
+ *  is what we're using (we're merely optimizing it by doing eight-bit
+ *  chunks at a time) shifts bits into the lowest-order term.  In our
+ *  implementation, that means shifting towards the right.  Why do we
+ *  do it this way?  Because the calculated CRC must be transmitted in
+ *  order from highest-order term to lowest-order term.  UARTs transmit
+ *  characters in order from LSB to MSB.  By storing the CRC this way
+ *  we hand it to the UART in the order low-byte to high-byte; the UART
+ *  sends each low-bit to hight-bit; and the result is transmission bit
+ *  by bit from highest- to lowest-order term without requiring any bit
+ *  shuffling on our part.  Reception works similarly
+ *
+ *  The feedback terms table consists of 256, 32-bit entries.  Notes
+ *
+ *      The table can be generated at runtime if desired; code to do so
+ *      is shown later.  It might not be obvious, but the feedback
+ *      terms simply represent the results of eight shift/xor opera
+ *      tions for all combinations of data and CRC register values
+ *
+ *      The values must be right-shifted by eight bits by the "updcrc
+ *      logic; the shift must be unsigned (bring in zeroes).  On some
+ *      hardware you could probably optimize the shift in assembler by
+ *      using byte-swap instructions
+ *      polynomial $edb88320
+ *
+ *
+ * CRC32 code derived from work by Gary S. Brown.
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+
+static uint32_t crc32_tab[] = {
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
+	0xe963a535, 0x9e6495a3,	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+	0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
+	0xf3b97148, 0x84be41de,	0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,	0x14015c4f, 0x63066cd9,
+	0xfa0f3d63, 0x8d080df5,	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+	0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,	0x35b5a8fa, 0x42b2986c,
+	0xdbbbc9d6, 0xacbcf940,	0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
+	0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+	0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,	0x76dc4190, 0x01db7106,
+	0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
+	0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+	0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
+	0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
+	0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+	0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
+	0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
+	0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+	0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
+	0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
+	0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+	0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
+	0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
+	0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+	0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
+	0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
+	0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+	0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
+	0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
+	0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+	0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
+	0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
+	0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+	0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+uint32_t crc32(uint32_t crc, const void *buf, size_t size)
+{
+	const uint8_t *p;
+
+	p = buf;
+	crc = crc ^ ~0U;
+
+	while (size--)
+		crc = crc32_tab[(crc ^ *p++) & 0xFF] ^ (crc >> 8);
+
+	return crc ^ ~0U;
+}
+
diff --git a/redist/crc32.h b/redist/crc32.h
new file mode 100644
index 0000000..5a8d16a
--- /dev/null
+++ b/redist/crc32.h
@@ -0,0 +1,17 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+//crc32.c under liberal license.
+//
+// You should only include this file if you are going to include the calibration subsystem of libsurvive.
+
+#ifndef CRC32_H
+#define CRC32_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+uint32_t crc32(uint32_t crc, uint8_t *buf, size_t size);
+
+#endif
+
diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
new file mode 100644
index 0000000..b8746bc
--- /dev/null
+++ b/src/ootx_decoder.c
@@ -0,0 +1,163 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+/* ootx data decoder */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include "ootx_decoder.h"
+#include <crc32.h>
+
+//char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
+
+
+void (*ootx_packet_clbk)(ootx_decoder_context *ctx, ootx_packet* packet) = NULL;
+
+void ootx_init_decoder_context(ootx_decoder_context *ctx) {
+	ctx->buf_offset = 0;
+	ctx->bits_written = 0;
+
+	ctx->preamble = 0XFFFFFFFF;
+	ctx->bits_processed = 0;
+	ctx->found_preamble = 0;
+
+	memset( ctx->buffer, 0, sizeof( ctx->buffer ) );
+	ctx->payload_size = (uint16_t*)ctx->buffer;
+	*(ctx->payload_size) = 0;
+}
+/*
+void ootx_init_buffer() {
+	buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
+	payload_size = (uint16_t*)buffer;
+	*payload_size = 0;
+}
+*/
+
+/*
+	how to decode pulses
+	ticks>2000 && delta>100000== master lighthouse
+	ticks>2000 && delta>10000 == slave lighthouse
+*/
+
+int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta) {
+	if (ticks<2000) return -1; //sweep
+	if ((ticks > 2000) & (delta>100000)) return 0; //master
+	if ((ticks > 2000) & (delta>10000)) return last_num+1; //a slave
+	return -1;
+}
+
+/*
+uint8_t ootx_decode_bit(uint32_t ticks) {
+	ticks = ((ticks/500)*500)+500;
+
+	ticks-=3000;
+	if (ticks>=2000) { ticks-=2000; }
+	if (ticks>=1000)  { return 0xFF; }
+
+	return 0x00;
+}*/
+
+uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
+	ctx->preamble <<= 1;
+	ctx->preamble |= (0x01 & dbit);
+	if ((ctx->preamble & 0x0001ffff) == 0x01) return 1;
+	return 0;
+}
+
+void ootx_reset_buffer(ootx_decoder_context *ctx) {
+	ctx->buf_offset = 0;
+	ctx->buffer[ctx->buf_offset] = 0;
+	ctx->bits_written = 0;
+	*(ctx->payload_size) = 0;
+}
+
+void ootx_inc_buffer_offset(ootx_decoder_context *ctx) {
+	++(ctx->buf_offset);
+
+//	assert(ctx->buf_offset<MAX_BUFF_SIZE);
+
+	/* the buffer is going to overflow, wrap the buffer and don't write more data until the preamble is found again */
+	if(ctx->buf_offset>=MAX_OOTX_BUFF_SIZE) {
+		ctx->buf_offset = 0;
+		ctx->found_preamble = 0;
+	}
+
+	ctx->buffer[ctx->buf_offset] = 0;
+}
+
+void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
+	uint8_t *current_byte = ctx->buffer + ctx->buf_offset;
+//	printf("%d\n", dbit);
+	*current_byte >>= 1;
+	*current_byte |= (0x80 & dbit);
+	++(ctx->bits_written);
+	if (ctx->bits_written>7) {
+		ctx->bits_written=0;
+//		printf("%d\n", *current_byte);
+		ootx_inc_buffer_offset(ctx);
+	}
+}
+
+void ootx_process_bit(ootx_decoder_context *ctx, uint8_t dbit) {
+	//uint8_t dbit = ootx_decode_bit(length);
+	++(ctx->bits_processed);
+
+//	printf("z %d %d\n", bits_processed,dbit);
+//	printf("d %d\n", bits_processed,dbit);
+
+	if ( ootx_detect_preamble(ctx, dbit) ) {
+		/*	data stream can start over at any time so we must
+			always look for preamble bits */
+		printf("Preamble found\n");
+		ootx_reset_buffer(ctx);
+		ctx->bits_processed = 0;
+		ctx->found_preamble = 1;
+	}
+	else if(ctx->bits_processed>16) {
+		//every 17th bit needs to be dropped (sync bit)
+//		printf("drop %d\n", dbit);
+		if( !dbit )
+		{
+			printf( "Sync bit missing\n" );
+			ootx_reset_buffer(ctx);
+		}
+		ctx->bits_processed = 0;
+	}
+	else if (ctx->found_preamble > 0)
+	{
+		/*	only write to buffer if the preamble is found.
+			if the buffer overflows, found_preamble will be cleared
+			and writing will stop. data would be corrupted, so there is no point in continuing
+		*/
+
+		ootx_write_to_buffer(ctx, dbit);
+printf( "%d / %d -> ", ctx->buf_offset, *ctx->payload_size );
+int k;
+for( k = 0; k < 32; k++ )
+{
+	printf( "%02x ", ctx->buffer[k] );
+}
+printf( "\n" );
+		if (ctx->buf_offset >= (*(ctx->payload_size)+6)) {
+			/*	once we have a complete ootx packet, send it out in the callback */
+			ootx_packet op;
+
+			op.length = *(ctx->payload_size);
+			op.data = ctx->buffer+2;
+			op.crc32 = *(uint32_t*)(ctx->buffer+2+op.length);
+
+			uint32_t crc = crc32(0xffffffff,op.data,op.length);
+
+			if (crc != op.crc32) {
+				printf("CRC mismatch\n");
+			}
+
+			if ((crc == op.crc32) && ootx_packet_clbk) ootx_packet_clbk(ctx, &op);
+
+			ootx_reset_buffer(ctx);
+		}
+	}
+}
diff --git a/src/ootx_decoder.h b/src/ootx_decoder.h
new file mode 100644
index 0000000..5df5ba2
--- /dev/null
+++ b/src/ootx_decoder.h
@@ -0,0 +1,41 @@
+// (C) 2016 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+#ifndef OOTX_DECODER_H
+#define OOTX_DECODER_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+#define MAX_OOTX_BUFF_SIZE 1024
+
+typedef struct {
+	uint16_t length;
+	uint8_t* data;
+	uint32_t crc32;
+} ootx_packet;
+
+typedef struct {
+	uint8_t buffer[MAX_OOTX_BUFF_SIZE];
+	uint16_t buf_offset;
+	uint8_t bits_written;
+	uint16_t* payload_size;
+
+	uint32_t preamble;
+	uint8_t bits_processed;
+	uint8_t found_preamble;
+	void * user;
+	int user1;
+} ootx_decoder_context;
+
+
+//void ootx_init_buffer();
+void ootx_process_bit(ootx_decoder_context *ctx, uint8_t dbit); //dbit MUST be 0x00 or 0xFF
+void ootx_init_decoder_context(ootx_decoder_context *ctx);
+int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta);
+
+
+extern void (*ootx_packet_clbk)(ootx_decoder_context * ctx, ootx_packet* packet);
+
+#endif
diff --git a/src/survive_cal.c b/src/survive_cal.c
new file mode 100644
index 0000000..4223e70
--- /dev/null
+++ b/src/survive_cal.c
@@ -0,0 +1,68 @@
+// (C) 2016, 2017 Joshua Allen, MIT/x11 License.
+// (C) 2016, 2017 <>< C. N. Lohr, Under MIT/x11 License.
+
+// All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
+
+#include "survive_cal.h"
+#include "survive_internal.h"
+
+void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
+{
+	struct SurviveContext * ctx = (struct SurviveContext*)(ct->user);
+	struct SurviveCalData * cd = ctx->calptr;
+	int id = ct->user1;
+
+	printf( "Got OOTX packet %d %p\n", id, cd );
+}
+
+
+void survive_cal_install( struct SurviveContext * ctx )
+{
+	int i;
+	struct SurviveCalData * cd = ctx->calptr = calloc( 1, sizeof( struct SurviveCalData ) );
+
+	for( i = 0; i < NUM_LIGHTHOUSES; i++ )
+	{
+		ootx_init_decoder_context(&cd->ootx_decoders[i]);
+		cd->ootx_decoders[i].user = ctx;
+		cd->ootx_decoders[i].user1 = i;
+	}
+
+	cd->stage = 1;
+
+	ootx_packet_clbk = ootx_packet_clbk_d;
+
+	ctx->calptr = cd;
+}
+
+
+void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+{
+	struct SurviveContext * ctx = so->ctx;
+	struct SurviveCalData * cd = ctx->calptr;
+
+	if( !cd ) return;
+
+	switch( cd->stage )
+	{
+	default:
+	case 0: //Default, inactive.
+		break;
+
+	case 1:
+		//Collecting OOTX data.
+		if( sensor_id < 0 )
+		{
+			int lhid = -sensor_id-1;
+			if( lhid >= NUM_LIGHTHOUSES-1 && so->codename[0] == 'H' )
+			{
+				uint8_t dbit = (acode & 2)?0xff:0x00;
+				printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit );
+				ootx_process_bit( &cd->ootx_decoders[lhid], dbit );
+			}
+		}
+			
+		break;
+	}
+}
+
diff --git a/src/survive_cal.h b/src/survive_cal.h
new file mode 100644
index 0000000..a74bf95
--- /dev/null
+++ b/src/survive_cal.h
@@ -0,0 +1,44 @@
+// (C) 2016, 2017 <>< C. N. Lohr, Under MIT/x11 License.
+
+// All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
+
+#ifndef _SURVIVE_CAL_H
+#define _SURVIVE_CAL_H
+
+//This is a file that is intended for use with capturing vive data during the
+//setup phase.  This and survive_cal.c/.h should not be included on embedded
+//uses of libsurvive.
+
+//This file handles the following:
+//  1: Decoding the OOTX data from the lighthouses.
+//  2: Setting OOTX props in the survive context.
+//  3: Collect a bunch of data with the vive pointed up and the watchment to either side.
+//  4: Running the code to find the lighthouses.
+//  5: Setting the information needed to develop the worldspace model in the SurviveContext.
+
+
+#include <stdint.h>
+#include "ootx_decoder.h"
+#include "survive_internal.h"
+
+void survive_cal_install( struct SurviveContext * ctx );
+int survive_cal_get_status( struct SurviveContext * ctx, char * description, int max_data );
+
+//void survive_cal_teardown( struct SurviveContext * ctx );
+
+//Called from survive_default_light_process
+void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
+
+struct SurviveCalData
+{
+	//Stage:
+	// 0: Idle
+	// 1: Collecting OOTX data.
+	int stage;
+
+	//OOTX Data is sync'd off of 
+	ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
+};
+
+#endif
+
diff --git a/src/survive_data.c b/src/survive_data.c
index b79f96e..c557951 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -81,7 +81,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 			else
 			{
 				ssn = ++so->sync_set_number;
-				if( so->sync_set_number > 1 )
+				if( so->sync_set_number >= NUM_LIGHTHOUSES )
 				{
 					SV_INFO( "Warning.  Received an extra, unassociated sync pulse." );
 					ssn = so->sync_set_number = -1;
@@ -106,12 +106,20 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 			}
 		}
 	}
+
+
+
 	//See if this is a valid actual pulse.
 	else if( le->length < 1800 && le->length > 40 && delta > 30000 && ssn >= 0 )
 	{
 		int32_t dl = so->last_time[0];
 		int32_t tpco = so->last_length[0];
 
+
+#if NUM_LIGHTHOUSES != 2
+		#error You are going to have to fix the code around here to allow for something other than two base stations.
+#endif
+
 		//Adding length 
 		//Long pulse-code from IR flood.
 		//Make sure it fits nicely into a divisible-by-500 time.
@@ -128,6 +136,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		acode_array[0] = (acode_array[0]>>1) - 6;
 		acode_array[1] = (acode_array[1]>>1) - 6;
 
+
 		int acode = acode_array[0];
 
 		if( !so->did_handle_ootx )
@@ -135,7 +144,6 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 			int32_t delta1 = so->last_time[0] - so->recent_sync_time;
 			int32_t delta2 = so->last_time[1] - so->last_time[0];
 
-			//XXX Axlecrusher -> Add your code here!!!
 			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_time[0], so->last_length[0] );
 			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_time[1], so->last_length[1] );
 
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 446f3c0..11c9d89 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -62,6 +62,22 @@ struct SurviveUSBInterface
 
 //This is defined in survive.h
 struct SurviveObject;
+struct SurviveCalData;
+
+struct BaseStationData
+{
+	uint8_t PositionSet:1;
+	float Position[3];
+	float Quaternion[4];
+
+	uint8_t OOTXSet:1;
+	uint32_t BaseStationID;
+	float fcalphase[2];
+	float fcaltilt[2];
+	float fcalcurve[2];
+	float fcalgibpha[2];
+	float fcalgibmag[2];
+};
 
 struct SurviveContext
 {
@@ -75,9 +91,15 @@ struct SurviveContext
 	light_process_func lightproc;
 	imu_process_func imuproc;
 
-	//Data Subsystem
+	//Calibration data:
+	struct BaseStationData bsd[NUM_LIGHTHOUSES];
+
+	struct SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
+
+	//Data Subsystem.  These should be last, as there may be additional surviveobjects.
 	struct SurviveObject headset;
-	struct SurviveObject watchman[2];
+	struct SurviveObject watchman[2]; //Currently only two supported watchmen.
+
 };
 
 
diff --git a/src/survive_process.c b/src/survive_process.c
index d3a8c4a..184532b 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -1,15 +1,19 @@
 //<>< (C) 2016 C. N. Lohr, FULLY Under MIT/x11 License.
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
-#include "survive_internal.h"
+#include "survive_cal.h"
 
-
-int bufferpts[32*2];
-char buffermts[32*128];
-int buffertimeto[32];
+//XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
+//proper angles, then from there perform the rest of the solution. 
 
 void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
+
+	if( so->ctx->calptr )
+	{
+		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
+	}
+
 	//TODO: Writeme!
 }
 
@@ -18,4 +22,3 @@ void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro
 	//TODO: Writeme!
 }
 
-
-- 
cgit v1.2.3


From 119a205619632076c7b258eaa6c28dd7bcd2c294 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Tue, 14 Feb 2017 10:03:18 -0500
Subject: integrate josh's new work.  It works!  #19 is just about solved!

---
 Makefile                         |   4 +-
 src/ootx_decoder.c               | 213 ++++++++++++++++++++++++----------
 src/ootx_decoder.h               |  47 ++++++--
 src/survive_cal.c                |  12 +-
 tools/ootx_decode/ootx_decoder.c | 243 ---------------------------------------
 tools/ootx_decode/ootx_decoder.h |  68 -----------
 6 files changed, 203 insertions(+), 384 deletions(-)
 delete mode 100644 tools/ootx_decode/ootx_decoder.c
 delete mode 100644 tools/ootx_decode/ootx_decoder.h

diff --git a/Makefile b/Makefile
index c0a4caa..797c257 100644
--- a/Makefile
+++ b/Makefile
@@ -3,13 +3,15 @@ all : lib data_recorder test calibrate
 CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto
 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -flto
 
+# unused: redist/crc32.c
+
 test : test.c lib/libsurvive.so redist/os_generic.o
 	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
 data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o
 	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
-calibrate :  calibrate.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c redist/crc32.c
+calibrate :  calibrate.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c
 	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
 lib:
diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
index b8746bc..b1815d7 100644
--- a/src/ootx_decoder.c
+++ b/src/ootx_decoder.c
@@ -1,4 +1,4 @@
-// (C) 2016 Joshua Allen, MIT/x11 License.
+// (C) 2017 Joshua Allen, MIT/x11 License.
 //
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
@@ -6,15 +6,19 @@
 
 #include <stdio.h>
 #include <stdlib.h>
-#include <string.h>
+#include <zlib.h>
 #include <assert.h>
 #include "ootx_decoder.h"
-#include <crc32.h>
+//#include "crc32.h"
 
 //char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
 
+#define MAX_BUFF_SIZE 64
 
-void (*ootx_packet_clbk)(ootx_decoder_context *ctx, ootx_packet* packet) = NULL;
+void (*ootx_packet_clbk)(ootx_decoder_context * ctx, ootx_packet* packet) = NULL;
+void (*ootx_bad_crc_clbk)(ootx_decoder_context * ctx, ootx_packet* packet, uint32_t crc) = NULL;
+
+void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
 
 void ootx_init_decoder_context(ootx_decoder_context *ctx) {
 	ctx->buf_offset = 0;
@@ -24,46 +28,28 @@ void ootx_init_decoder_context(ootx_decoder_context *ctx) {
 	ctx->bits_processed = 0;
 	ctx->found_preamble = 0;
 
-	memset( ctx->buffer, 0, sizeof( ctx->buffer ) );
+	ctx->buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
 	ctx->payload_size = (uint16_t*)ctx->buffer;
 	*(ctx->payload_size) = 0;
 }
-/*
-void ootx_init_buffer() {
-	buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
-	payload_size = (uint16_t*)buffer;
-	*payload_size = 0;
-}
-*/
-
-/*
-	how to decode pulses
-	ticks>2000 && delta>100000== master lighthouse
-	ticks>2000 && delta>10000 == slave lighthouse
-*/
-
-int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta) {
-	if (ticks<2000) return -1; //sweep
-	if ((ticks > 2000) & (delta>100000)) return 0; //master
-	if ((ticks > 2000) & (delta>10000)) return last_num+1; //a slave
-	return -1;
-}
 
-/*
-uint8_t ootx_decode_bit(uint32_t ticks) {
-	ticks = ((ticks/500)*500)+500;
-
-	ticks-=3000;
-	if (ticks>=2000) { ticks-=2000; }
-	if (ticks>=1000)  { return 0xFF; }
+void ootx_free_decoder_context(ootx_decoder_context *ctx) {
+	free(ctx->buffer);
+	ctx->buffer = NULL;
+	ctx->payload_size = NULL;
+}
 
-	return 0x00;
-}*/
+uint8_t ootx_decode_bit(uint32_t length) {
+	uint8_t t = (length - 2750) / 500; //why 2750?
+//	return ((t & 0x02)>0)?0xFF:0x00; //easier if we need to bitshift right
+	return ((t & 0x02)>>1);
+}
 
 uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
 	ctx->preamble <<= 1;
-	ctx->preamble |= (0x01 & dbit);
-	if ((ctx->preamble & 0x0001ffff) == 0x01) return 1;
+//	ctx->preamble |= (0x01 & dbit);
+	ctx->preamble |= dbit;
+	if ((ctx->preamble & 0x0003ffff) == 0x00000001) return 1;
 	return 0;
 }
 
@@ -80,7 +66,7 @@ void ootx_inc_buffer_offset(ootx_decoder_context *ctx) {
 //	assert(ctx->buf_offset<MAX_BUFF_SIZE);
 
 	/* the buffer is going to overflow, wrap the buffer and don't write more data until the preamble is found again */
-	if(ctx->buf_offset>=MAX_OOTX_BUFF_SIZE) {
+	if(ctx->buf_offset>=MAX_BUFF_SIZE) {
 		ctx->buf_offset = 0;
 		ctx->found_preamble = 0;
 	}
@@ -90,9 +76,11 @@ void ootx_inc_buffer_offset(ootx_decoder_context *ctx) {
 
 void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
 	uint8_t *current_byte = ctx->buffer + ctx->buf_offset;
-//	printf("%d\n", dbit);
-	*current_byte >>= 1;
-	*current_byte |= (0x80 & dbit);
+
+	*current_byte <<= 1;
+//	*current_byte |= (0x01 & dbit);
+	*current_byte |= dbit;
+
 	++(ctx->bits_written);
 	if (ctx->bits_written>7) {
 		ctx->bits_written=0;
@@ -101,17 +89,20 @@ void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
 	}
 }
 
-void ootx_process_bit(ootx_decoder_context *ctx, uint8_t dbit) {
-	//uint8_t dbit = ootx_decode_bit(length);
-	++(ctx->bits_processed);
+uint8_t ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
+	uint8_t dbit = ootx_decode_bit(length);
+	ootx_pump_bit( ctx, dbit );
+	return dbit;
+}
 
-//	printf("z %d %d\n", bits_processed,dbit);
-//	printf("d %d\n", bits_processed,dbit);
+void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
+//	uint8_t dbit = ootx_decode_bit(length);
+	++(ctx->bits_processed);
 
 	if ( ootx_detect_preamble(ctx, dbit) ) {
 		/*	data stream can start over at any time so we must
 			always look for preamble bits */
-		printf("Preamble found\n");
+		//printf("Preamble found\n");
 		ootx_reset_buffer(ctx);
 		ctx->bits_processed = 0;
 		ctx->found_preamble = 1;
@@ -121,7 +112,7 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 //		printf("drop %d\n", dbit);
 		if( !dbit )
 		{
-			printf( "Sync bit missing\n" );
+			printf("Bad sync bit\n");
 			ootx_reset_buffer(ctx);
 		}
 		ctx->bits_processed = 0;
@@ -134,30 +125,132 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 		*/
 
 		ootx_write_to_buffer(ctx, dbit);
-printf( "%d / %d -> ", ctx->buf_offset, *ctx->payload_size );
-int k;
-for( k = 0; k < 32; k++ )
-{
-	printf( "%02x ", ctx->buffer[k] );
-}
-printf( "\n" );
-		if (ctx->buf_offset >= (*(ctx->payload_size)+6)) {
+
+		uint16_t padded_length = *(ctx->payload_size);
+		padded_length += (padded_length&0x01); //extra null byte if odd
+
+/*		int k;
+		printf( ":" );
+		for( k = 0; k < 36; k++ )
+		{
+			printf( "%02x ", ctx->buffer[k] );
+		}
+		printf( "\n" );*/
+
+		if (ctx->buf_offset >= (padded_length+6)) {
 			/*	once we have a complete ootx packet, send it out in the callback */
 			ootx_packet op;
 
 			op.length = *(ctx->payload_size);
 			op.data = ctx->buffer+2;
-			op.crc32 = *(uint32_t*)(ctx->buffer+2+op.length);
+			op.crc32 = *(uint32_t*)(op.data+padded_length);
 
-			uint32_t crc = crc32(0xffffffff,op.data,op.length);
+			uint32_t crc = crc32( 0L, Z_NULL, 0 );
+			crc = crc32( crc, op.data,op.length);
 
 			if (crc != op.crc32) {
-				printf("CRC mismatch\n");
+				if (ootx_bad_crc_clbk != NULL) ootx_bad_crc_clbk(ctx, &op,crc);
+			}
+			else if (ootx_packet_clbk != NULL) {
+				ootx_packet_clbk(ctx,&op);
 			}
-
-			if ((crc == op.crc32) && ootx_packet_clbk) ootx_packet_clbk(ctx, &op);
 
 			ootx_reset_buffer(ctx);
 		}
 	}
 }
+
+uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) {
+	uint8_t* x = data + *idx;
+	*idx += bytes;
+	return x;
+}
+
+float _half_to_float(uint8_t* data) {
+	//this will not handle infinity, NaN, or denormalized floats
+
+	uint16_t x = *(uint16_t*)data;
+	float f = 0;
+
+	uint32_t *ftmp = (uint32_t*)&f; //use the allocated floating point memory
+
+	if ((x & 0x7FFF) == 0) return f; //zero
+
+	//sign
+	*ftmp = x & 0x8000;
+	*ftmp <<= 16;
+
+	*ftmp += ((((uint32_t)(x & 0x7fff)) + 0x1c000) << 13);
+
+	return f;
+}
+
+void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) {
+	uint16_t idx = 0;
+	/*
+	uint16_t fw_version;//Firmware version (bit 15..6), protocol version (bit 5..0)
+	uint32_t id; //Unique identifier of the base station
+	float fcal_0_phase; //"phase" for rotor 0
+	float fcal_1_phase; //"phase" for rotor 1
+	float fcal_0_tilt; //"tilt" for rotor 0
+	float fcal_1_tilt; //"tilt" for rotor 1
+	uint8_t sys_unlock_count; //Lowest 8 bits of the rotor desynchronization counter
+	uint8_t hw_version; //Hardware version
+	float fcal_0_curve; //"curve" for rotor 0
+	float fcal_1_curve; //"curve" for rotor 1
+	int8_t accel_dir_x; //"orientation vector"
+	int8_t accel_dir_y; //"orientation vector"
+	int8_t accel_dir_z; //"orientation vector"
+	float fcal_0_gibphase; //"gibbous phase" for rotor 0 (normalized angle)
+	float fcal_1_gibphase; //"gibbous phase" for rotor 1 (normalized angle)
+	float fcal_0_gibmag; //"gibbous magnitude" for rotor 0
+	float fcal_1_gibmag; //"gibbous magnitude" for rotor 1
+	uint8_t mode_current; //Currently selected mode (default: 0=A, 1=B, 2=C)
+	uint8_t sys_faults; //"fault detect flags" (should be 0)
+	*/
+
+	lhi->fw_version = *(uint16_t*)get_ptr(data,sizeof(uint16_t),&idx);
+	lhi->id = *(uint32_t*)get_ptr(data,sizeof(uint32_t),&idx);
+	lhi->fcal_0_phase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_phase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_0_tilt = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_tilt = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->sys_unlock_count = *get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->hw_version = *get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->fcal_0_curve = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_curve = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->accel_dir_x = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->accel_dir_y = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->accel_dir_z = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->fcal_0_gibphase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_gibphase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_0_gibmag = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->fcal_1_gibmag = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
+	lhi->mode_current = *get_ptr(data,sizeof(uint8_t),&idx);
+	lhi->sys_faults = *get_ptr(data,sizeof(uint8_t),&idx);
+
+}
+
+void print_lighthouse_info_v6(lighthouse_info_v6* lhi) {
+
+	printf("\t%X\n\t%X\n\t%f\n\t%f\n\t%f\n\t%f\n\t%d\n\t%d\n\t%f\n\t%f\n\t%d\n\t%d\n\t%d\n\t%f\n\t%f\n\t%f\n\t%f\n\t%d\n\t%d\n",
+		lhi->fw_version,
+		lhi->id,
+		lhi->fcal_0_phase,
+		lhi->fcal_1_phase,
+		lhi->fcal_0_tilt,
+		lhi->fcal_1_tilt,
+		lhi->sys_unlock_count,
+		lhi->hw_version,
+		lhi->fcal_0_curve,
+		lhi->fcal_1_curve,
+		lhi->accel_dir_x,
+		lhi->accel_dir_y,
+		lhi->accel_dir_z,
+		lhi->fcal_0_gibphase,
+		lhi->fcal_1_gibphase,
+		lhi->fcal_0_gibmag,
+		lhi->fcal_1_gibmag,
+		lhi->mode_current,
+		lhi->sys_faults);
+}
diff --git a/src/ootx_decoder.h b/src/ootx_decoder.h
index 5df5ba2..8ddf527 100644
--- a/src/ootx_decoder.h
+++ b/src/ootx_decoder.h
@@ -1,4 +1,4 @@
-// (C) 2016 Joshua Allen, MIT/x11 License.
+// (C) 2017 Joshua Allen, MIT/x11 License.
 //
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
@@ -8,8 +8,6 @@
 #include <stddef.h>
 #include <stdint.h>
 
-#define MAX_OOTX_BUFF_SIZE 1024
-
 typedef struct {
 	uint16_t length;
 	uint8_t* data;
@@ -17,7 +15,7 @@ typedef struct {
 } ootx_packet;
 
 typedef struct {
-	uint8_t buffer[MAX_OOTX_BUFF_SIZE];
+	uint8_t* buffer;
 	uint16_t buf_offset;
 	uint8_t bits_written;
 	uint16_t* payload_size;
@@ -25,17 +23,50 @@ typedef struct {
 	uint32_t preamble;
 	uint8_t bits_processed;
 	uint8_t found_preamble;
+
+	uint8_t bit_count[2];
+
 	void * user;
 	int user1;
 } ootx_decoder_context;
 
 
-//void ootx_init_buffer();
-void ootx_process_bit(ootx_decoder_context *ctx, uint8_t dbit); //dbit MUST be 0x00 or 0xFF
+typedef float float16;
+
+typedef struct {
+	uint16_t fw_version;//Firmware version (bit 15..6), protocol version (bit 5..0)
+	uint32_t id; //Unique identifier of the base station
+	float16 fcal_0_phase; //"phase" for rotor 0
+	float16 fcal_1_phase; //"phase" for rotor 1
+	float16 fcal_0_tilt; //"tilt" for rotor 0
+	float16 fcal_1_tilt; //"tilt" for rotor 1
+	uint8_t sys_unlock_count; //Lowest 8 bits of the rotor desynchronization counter
+	uint8_t hw_version; //Hardware version
+	float16 fcal_0_curve; //"curve" for rotor 0
+	float16 fcal_1_curve; //"curve" for rotor 1
+	int8_t accel_dir_x; //"orientation vector"
+	int8_t accel_dir_y; //"orientation vector"
+	int8_t accel_dir_z; //"orientation vector"
+	float16 fcal_0_gibphase; //"gibbous phase" for rotor 0 (normalized angle)
+	float16 fcal_1_gibphase; //"gibbous phase" for rotor 1 (normalized angle)
+	float16 fcal_0_gibmag; //"gibbous magnitude" for rotor 0
+	float16 fcal_1_gibmag; //"gibbous magnitude" for rotor 1
+	uint8_t mode_current; //Currently selected mode (default: 0=A, 1=B, 2=C)
+	uint8_t sys_faults; //"fault detect flags" (should be 0)
+} lighthouse_info_v6;
+
+void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data);
+void print_lighthouse_info_v6(lighthouse_info_v6* lhi);
+
 void ootx_init_decoder_context(ootx_decoder_context *ctx);
-int8_t ootx_decode_lighthouse_number(uint8_t last_num, uint32_t ticks, int32_t delta);
+void ootx_free_decoder_context(ootx_decoder_context *ctx);
+
+uint8_t ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
+void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
 
+uint8_t ootx_decode_bit(uint32_t length);
 
-extern void (*ootx_packet_clbk)(ootx_decoder_context * ctx, ootx_packet* packet);
+extern void (*ootx_packet_clbk)(ootx_decoder_context *ctx, ootx_packet* packet);
+extern void (*ootx_bad_crc_clbk)(ootx_decoder_context *ctx, ootx_packet* packet, uint32_t crc);
 
 #endif
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 4223e70..adcb7bc 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -13,6 +13,10 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	int id = ct->user1;
 
 	printf( "Got OOTX packet %d %p\n", id, cd );
+
+	lighthouse_info_v6 v6;
+	init_lighthouse_info_v6(&v6, packet->data);
+	print_lighthouse_info_v6(&v6);
 }
 
 
@@ -54,11 +58,11 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 		if( sensor_id < 0 )
 		{
 			int lhid = -sensor_id-1;
-			if( lhid >= NUM_LIGHTHOUSES-1 && so->codename[0] == 'H' )
+			if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' )
 			{
-				uint8_t dbit = (acode & 2)?0xff:0x00;
-				printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit );
-				ootx_process_bit( &cd->ootx_decoders[lhid], dbit );
+				uint8_t dbit = (acode & 2)>>1;
+				//printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit );
+				ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
 			}
 		}
 			
diff --git a/tools/ootx_decode/ootx_decoder.c b/tools/ootx_decode/ootx_decoder.c
deleted file mode 100644
index b5a7b54..0000000
--- a/tools/ootx_decode/ootx_decoder.c
+++ /dev/null
@@ -1,243 +0,0 @@
-// (C) 2017 Joshua Allen, MIT/x11 License.
-//
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-
-/* ootx data decoder */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <zlib.h>
-#include <assert.h>
-#include "ootx_decoder.h"
-//#include "crc32.h"
-
-//char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
-
-#define MAX_BUFF_SIZE 64
-
-void (*ootx_packet_clbk)(ootx_packet* packet) = NULL;
-void (*ootx_bad_crc_clbk)(ootx_packet* packet, uint32_t crc) = NULL;
-
-void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
-
-void ootx_init_decoder_context(ootx_decoder_context *ctx) {
-	ctx->buf_offset = 0;
-	ctx->bits_written = 0;
-
-	ctx->preamble = 0XFFFFFFFF;
-	ctx->bits_processed = 0;
-	ctx->found_preamble = 0;
-
-	ctx->buffer = (uint8_t*)malloc(MAX_BUFF_SIZE);
-	ctx->payload_size = (uint16_t*)ctx->buffer;
-	*(ctx->payload_size) = 0;
-}
-
-void ootx_free_decoder_context(ootx_decoder_context *ctx) {
-	free(ctx->buffer);
-	ctx->buffer = NULL;
-	ctx->payload_size = NULL;
-}
-
-uint8_t ootx_decode_bit(uint32_t length) {
-	uint8_t t = (length - 2750) / 500; //why 2750?
-//	return ((t & 0x02)>0)?0xFF:0x00; //easier if we need to bitshift right
-	return ((t & 0x02)>>1);
-}
-
-uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
-	ctx->preamble <<= 1;
-//	ctx->preamble |= (0x01 & dbit);
-	ctx->preamble |= dbit;
-	if ((ctx->preamble & 0x0003ffff) == 0x00000001) return 1;
-	return 0;
-}
-
-void ootx_reset_buffer(ootx_decoder_context *ctx) {
-	ctx->buf_offset = 0;
-	ctx->buffer[ctx->buf_offset] = 0;
-	ctx->bits_written = 0;
-	*(ctx->payload_size) = 0;
-}
-
-void ootx_inc_buffer_offset(ootx_decoder_context *ctx) {
-	++(ctx->buf_offset);
-
-//	assert(ctx->buf_offset<MAX_BUFF_SIZE);
-
-	/* the buffer is going to overflow, wrap the buffer and don't write more data until the preamble is found again */
-	if(ctx->buf_offset>=MAX_BUFF_SIZE) {
-		ctx->buf_offset = 0;
-		ctx->found_preamble = 0;
-	}
-
-	ctx->buffer[ctx->buf_offset] = 0;
-}
-
-void ootx_write_to_buffer(ootx_decoder_context *ctx, uint8_t dbit) {
-	uint8_t *current_byte = ctx->buffer + ctx->buf_offset;
-
-	*current_byte <<= 1;
-//	*current_byte |= (0x01 & dbit);
-	*current_byte |= dbit;
-
-	++(ctx->bits_written);
-	if (ctx->bits_written>7) {
-		ctx->bits_written=0;
-//		printf("%d\n", *current_byte);
-		ootx_inc_buffer_offset(ctx);
-	}
-}
-
-uint8_t ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
-	uint8_t dbit = ootx_decode_bit(length);
-	ootx_pump_bit( ctx, dbit );
-	return dbit;
-}
-
-void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
-//	uint8_t dbit = ootx_decode_bit(length);
-	++(ctx->bits_processed);
-
-	if ( ootx_detect_preamble(ctx, dbit) ) {
-		/*	data stream can start over at any time so we must
-			always look for preamble bits */
-		printf("Preamble found\n");
-		ootx_reset_buffer(ctx);
-		ctx->bits_processed = 0;
-		ctx->found_preamble = 1;
-	}
-	else if(ctx->bits_processed>16) {
-		//every 17th bit needs to be dropped (sync bit)
-//		printf("drop %d\n", dbit);
-		ctx->bits_processed = 0;
-	}
-	else if (ctx->found_preamble > 0)
-	{
-		/*	only write to buffer if the preamble is found.
-			if the buffer overflows, found_preamble will be cleared
-			and writing will stop. data would be corrupted, so there is no point in continuing
-		*/
-
-		ootx_write_to_buffer(ctx, dbit);
-
-		uint16_t padded_length = *(ctx->payload_size);
-		padded_length += (padded_length&0x01); //extra null byte if odd
-
-		if (ctx->buf_offset >= (padded_length+6)) {
-			/*	once we have a complete ootx packet, send it out in the callback */
-			ootx_packet op;
-
-			op.length = *(ctx->payload_size);
-			op.data = ctx->buffer+2;
-			op.crc32 = *(uint32_t*)(op.data+padded_length);
-
-			uint32_t crc = crc32( 0L, Z_NULL, 0 );
-			crc = crc32( crc, op.data,op.length);
-
-			if (crc != op.crc32) {
-				if (ootx_bad_crc_clbk != NULL) ootx_bad_crc_clbk(&op,crc);
-			}
-			else if (ootx_packet_clbk != NULL) {
-				ootx_packet_clbk(&op);
-			}
-
-			ootx_reset_buffer(ctx);
-		}
-	}
-}
-
-uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) {
-	uint8_t* x = data + *idx;
-	*idx += bytes;
-	return x;
-}
-
-float _half_to_float(uint8_t* data) {
-	//this will not handle infinity, NaN, or denormalized floats
-
-	uint16_t x = *(uint16_t*)data;
-	float f = 0;
-
-	uint32_t *ftmp = (uint32_t*)&f; //use the allocated floating point memory
-
-	if ((x & 0x7FFF) == 0) return f; //zero
-
-	//sign
-	*ftmp = x & 0x8000;
-	*ftmp <<= 16;
-
-	*ftmp += ((((uint32_t)(x & 0x7fff)) + 0x1c000) << 13);
-
-	return f;
-}
-
-void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) {
-	uint16_t idx = 0;
-	/*
-	uint16_t fw_version;//Firmware version (bit 15..6), protocol version (bit 5..0)
-	uint32_t id; //Unique identifier of the base station
-	float fcal_0_phase; //"phase" for rotor 0
-	float fcal_1_phase; //"phase" for rotor 1
-	float fcal_0_tilt; //"tilt" for rotor 0
-	float fcal_1_tilt; //"tilt" for rotor 1
-	uint8_t sys_unlock_count; //Lowest 8 bits of the rotor desynchronization counter
-	uint8_t hw_version; //Hardware version
-	float fcal_0_curve; //"curve" for rotor 0
-	float fcal_1_curve; //"curve" for rotor 1
-	int8_t accel_dir_x; //"orientation vector"
-	int8_t accel_dir_y; //"orientation vector"
-	int8_t accel_dir_z; //"orientation vector"
-	float fcal_0_gibphase; //"gibbous phase" for rotor 0 (normalized angle)
-	float fcal_1_gibphase; //"gibbous phase" for rotor 1 (normalized angle)
-	float fcal_0_gibmag; //"gibbous magnitude" for rotor 0
-	float fcal_1_gibmag; //"gibbous magnitude" for rotor 1
-	uint8_t mode_current; //Currently selected mode (default: 0=A, 1=B, 2=C)
-	uint8_t sys_faults; //"fault detect flags" (should be 0)
-	*/
-
-	lhi->fw_version = *(uint16_t*)get_ptr(data,sizeof(uint16_t),&idx);
-	lhi->id = *(uint32_t*)get_ptr(data,sizeof(uint32_t),&idx);
-	lhi->fcal_0_phase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_phase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_0_tilt = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_tilt = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->sys_unlock_count = *get_ptr(data,sizeof(uint8_t),&idx);
-	lhi->hw_version = *get_ptr(data,sizeof(uint8_t),&idx);
-	lhi->fcal_0_curve = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_curve = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->accel_dir_x = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
-	lhi->accel_dir_y = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
-	lhi->accel_dir_z = *(int8_t*)get_ptr(data,sizeof(uint8_t),&idx);
-	lhi->fcal_0_gibphase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_gibphase = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_0_gibmag = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->fcal_1_gibmag = _half_to_float( get_ptr(data,sizeof(uint16_t),&idx) );
-	lhi->mode_current = *get_ptr(data,sizeof(uint8_t),&idx);
-	lhi->sys_faults = *get_ptr(data,sizeof(uint8_t),&idx);
-
-}
-
-void print_lighthouse_info_v6(lighthouse_info_v6* lhi) {
-
-	printf("\t%X\n\t%X\n\t%f\n\t%f\n\t%f\n\t%f\n\t%d\n\t%d\n\t%f\n\t%f\n\t%d\n\t%d\n\t%d\n\t%f\n\t%f\n\t%f\n\t%f\n\t%d\n\t%d\n",
-		lhi->fw_version,
-		lhi->id,
-		lhi->fcal_0_phase,
-		lhi->fcal_1_phase,
-		lhi->fcal_0_tilt,
-		lhi->fcal_1_tilt,
-		lhi->sys_unlock_count,
-		lhi->hw_version,
-		lhi->fcal_0_curve,
-		lhi->fcal_1_curve,
-		lhi->accel_dir_x,
-		lhi->accel_dir_y,
-		lhi->accel_dir_z,
-		lhi->fcal_0_gibphase,
-		lhi->fcal_1_gibphase,
-		lhi->fcal_0_gibmag,
-		lhi->fcal_1_gibmag,
-		lhi->mode_current,
-		lhi->sys_faults);
-}
\ No newline at end of file
diff --git a/tools/ootx_decode/ootx_decoder.h b/tools/ootx_decode/ootx_decoder.h
deleted file mode 100644
index c707159..0000000
--- a/tools/ootx_decode/ootx_decoder.h
+++ /dev/null
@@ -1,68 +0,0 @@
-// (C) 2017 Joshua Allen, MIT/x11 License.
-//
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-
-#ifndef OOTX_DECODER_H
-#define OOTX_DECODER_H
-
-#include <stddef.h>
-#include <stdint.h>
-
-typedef struct {
-	uint16_t length;
-	uint8_t* data;
-	uint32_t crc32;
-} ootx_packet;
-
-typedef struct {
-	uint8_t* buffer;
-	uint16_t buf_offset;
-	uint8_t bits_written;
-	uint16_t* payload_size;
-
-	uint32_t preamble;
-	uint8_t bits_processed;
-	uint8_t found_preamble;
-
-	uint8_t bit_count[2];
-} ootx_decoder_context;
-
-
-typedef float float16;
-
-typedef struct {
-	uint16_t fw_version;//Firmware version (bit 15..6), protocol version (bit 5..0)
-	uint32_t id; //Unique identifier of the base station
-	float16 fcal_0_phase; //"phase" for rotor 0
-	float16 fcal_1_phase; //"phase" for rotor 1
-	float16 fcal_0_tilt; //"tilt" for rotor 0
-	float16 fcal_1_tilt; //"tilt" for rotor 1
-	uint8_t sys_unlock_count; //Lowest 8 bits of the rotor desynchronization counter
-	uint8_t hw_version; //Hardware version
-	float16 fcal_0_curve; //"curve" for rotor 0
-	float16 fcal_1_curve; //"curve" for rotor 1
-	int8_t accel_dir_x; //"orientation vector"
-	int8_t accel_dir_y; //"orientation vector"
-	int8_t accel_dir_z; //"orientation vector"
-	float16 fcal_0_gibphase; //"gibbous phase" for rotor 0 (normalized angle)
-	float16 fcal_1_gibphase; //"gibbous phase" for rotor 1 (normalized angle)
-	float16 fcal_0_gibmag; //"gibbous magnitude" for rotor 0
-	float16 fcal_1_gibmag; //"gibbous magnitude" for rotor 1
-	uint8_t mode_current; //Currently selected mode (default: 0=A, 1=B, 2=C)
-	uint8_t sys_faults; //"fault detect flags" (should be 0)
-} lighthouse_info_v6;
-
-void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data);
-void print_lighthouse_info_v6(lighthouse_info_v6* lhi);
-
-void ootx_init_decoder_context(ootx_decoder_context *ctx);
-void ootx_free_decoder_context(ootx_decoder_context *ctx);
-
-uint8_t ootx_process_bit(ootx_decoder_context *ctx, uint32_t length);
-
-uint8_t ootx_decode_bit(uint32_t length);
-
-extern void (*ootx_packet_clbk)(ootx_packet* packet);
-extern void (*ootx_bad_crc_clbk)(ootx_packet* packet, uint32_t crc);
-
-#endif
\ No newline at end of file
-- 
cgit v1.2.3


From c4e11bc415aa17845bad53454e89e7e0703b8c73 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Tue, 14 Feb 2017 16:47:08 -0700
Subject: Transitioned to a gradient-descent only solution

Added empirically derived fudge factor to fitness function
Eliminated dangerous spurious local minimum near the origin.
Now taking into account the normals
No longer using any of the point cloud stuff
Huge speed improvements
Huge memory usage improvements
Added code smell.  Will need to deodorize later.
---
 tools/lighthousefind_tori/main.c            |   3 +
 tools/lighthousefind_tori/tori_includes.h   |   8 +-
 tools/lighthousefind_tori/torus_localizer.c | 188 ++++++++++++++++++++++++++--
 3 files changed, 183 insertions(+), 16 deletions(-)

diff --git a/tools/lighthousefind_tori/main.c b/tools/lighthousefind_tori/main.c
index aa51448..ee56b37 100644
--- a/tools/lighthousefind_tori/main.c
+++ b/tools/lighthousefind_tori/main.c
@@ -51,6 +51,9 @@ static void runTheNumbers()
 			to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0];
 			to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1];
 			to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2];
+			to->sensor[sensorCount].normal.x = hmd_norms[i * 3 + 0];
+			to->sensor[sensorCount].normal.y = hmd_norms[i * 3 + 1];
+			to->sensor[sensorCount].normal.z = hmd_norms[i * 3 + 2];
 			to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0] + LINMATHPI / 2;
 			to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1] + LINMATHPI / 2;
 			sensorCount++;
diff --git a/tools/lighthousefind_tori/tori_includes.h b/tools/lighthousefind_tori/tori_includes.h
index 4cfbcdc..a2b9082 100644
--- a/tools/lighthousefind_tori/tori_includes.h
+++ b/tools/lighthousefind_tori/tori_includes.h
@@ -4,13 +4,15 @@
 #include <stddef.h>
 #include <math.h>
 #include <stdint.h>
+#include "linmath.h"
 
+#define PointToFlts(x) ((FLT*)(x))
 
 typedef struct
 {
-	double x;
-	double y;
-	double z;
+	FLT x;
+	FLT y;
+	FLT z;
 } Point;
 
 typedef struct
diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index b276e99..837b745 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -559,7 +559,22 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 
 	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalAngle, poloidalAngle);
 
-	return distance(pointIn, torusPoint);
+	FLT dist = distance(pointIn, torusPoint);
+
+	// This is some voodoo black magic.  This is here to solve the problem that the origin 
+	// (which is near the center of all the tori) erroniously will rank as a good match.
+	// through a lot of empiracle testing on how to compensate for this, the "fudge factor"
+	// below ended up being the best fit.  As simple as it is, I have a strong suspicion
+	// that there's some crazy complex thesis-level math that could be used to derive this
+	// but it works so we'll run with it.
+	// Note that this may be resulting in a skewing of the found location by several millimeters.
+	// it is not clear if this is actually removing existing skew (to get a more accurate value)
+	// or if it is introducing an undesirable skew.
+	double fudge = FLT_SIN((poloidalAngle - M_PI) / 2);
+	//fudge *= fudge;
+	dist = dist / fudge;
+
+	return dist;
 }
 
 //Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile)
@@ -688,8 +703,20 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 	Point lastPoint = initialEstimate;
 	//Point lastGradient = getGradient(lastPoint, pna, pnaCount, .00000001 /*somewhat arbitrary*/);
 
-
-	for (FLT g = 0.1; g > 0.00001; g *= 0.99)
+	// The values below are somewhat magic, and definitely tunable
+	// The initial vlue of g will represent the biggest step that the gradient descent can take at first.
+	//   bigger values may be faster, especially when the initial guess is wildly off.
+	//   The downside to a bigger starting guess is that if we've picked a good guess at the local minima
+	//   if there are other local minima, we may accidentally jump to such a local minima and get stuck there.
+	//   That's fairly unlikely with the lighthouse problem, from expereince.
+	//   The other downside is that if it's too big, we may have to spend a few iterations before it gets down
+	//   to a size that doesn't jump us out of our minima.
+	// The terminal value of g represents how close we want to get to the local minima before we're "done"
+	// The change in value of g for each iteration is intentionally very close to 1.
+	//   in fact, it probably could probably be 1 without any issue.  The main place where g is decremented
+	//   is in the block below when we've made a jump that results in a worse fitness than we're starting at.
+	//   In those cases, we don't take the jump, and instead lower the value of g and try again.
+	for (FLT g = 0.2; g > 0.00001; g *= 0.99)
 	{
 		i++;
 		Point point1 = lastPoint;
@@ -710,9 +737,19 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 		point3.y = point2.y + gradientN2.y;
 		point3.z = point2.z + gradientN2.z;
 
+		// remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley?
+		// Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic
+		// gradient descent is kinda horrible here.  Instead, think about the shape that a zig-zagging 
+		// converging gradient descent makes.  Instead of using the gradient as the best indicator of 
+		// the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically
+		// following *that* vector.  As it turns out, this works *amazingly* well.  
+
 		Point specialGradient = { .x = point3.x - point1.x, .y = point3.y - point1.y, .z = point3.y - point1.y };
 
-		specialGradient = getNormalizedVector(specialGradient, g/2);
+		// The second parameter to this function is very much a tunable parameter.  Different values will result
+		// in a different number of iterations before we get to the minimum.  Numbers between 3-10 seem to work well
+		// It's not clear what would be optimum here.
+		specialGradient = getNormalizedVector(specialGradient, g/4);
 
 		Point point4;
 
@@ -720,10 +757,6 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 		point4.y = point3.y + specialGradient.y;
 		point4.z = point3.z + specialGradient.z;
 
-		//point4.x = (point1.x + point2.x + point3.x) / 3;
-		//point4.y = (point1.y + point2.y + point3.y) / 3;
-		//point4.z = (point1.z + point2.z + point3.z) / 3;
-
 		FLT newMatchFitness = getPointFitness(point4, pna, pnaCount);
 
 		if (newMatchFitness > lastMatchFitness)
@@ -751,10 +784,104 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 	return lastPoint;
 }
 
+// This torus generator creates a point cloud of the given torus, and attempts to keep the 
+// density of the points fairly uniform across the surface of the torus.
+void AnalyzeToroidalImpact(
+	Point p1,
+	Point p2,
+	double lighthouseAngle,
+	double *vals,
+	PointsAndAngle *pna, 
+	size_t pnaCount)
+{
+	double poloidalRadius = 0;
+	double toroidalRadius = 0;
+
+	Point m = midpoint(p1, p2);
+	double distanceBetweenPoints = distance(p1, p2);
+
+	// ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally
+	// thinking the tori converge at the location of the tracked object instead of at the lighthouse.
+	double centralAngleToIgnore = lighthouseAngle * 3;
+
+	Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2);
+
+	toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle));
+
+	poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+
+	unsigned int pointCount = 0;
+
+	size_t currentPoint = 0;
+	
+	for (size_t ps = 0; ps < 180; ps++)
+	{
+
+		//for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40)
+		for (double toroidalStep = 0; toroidalStep < M_PI / 2; toroidalStep += M_PI / 180 * 2)
+		{
+			double poloidalStep = M_PI + M_PI / 180 * 2 * ps;
+			Point tmp;
+
+			tmp.x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep);
+			tmp.y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep);
+			tmp.z = poloidalRadius*sin(poloidalStep);
+			tmp = RotateAndTranslatePoint(tmp, rot, m);
+
+			vals[ps] += getPointFitness(tmp, pna, pnaCount);
+
+		}
+
+		vals[ps] = vals[ps] / 180; // average.
+	}
+
+}
+
+void AnalyzePoloidalImpact(TrackedObject *obj, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
+{
+	Point **pointCloud = malloc(sizeof(void*)* pnaCount);
+
+	double vals[200][180] = { 0 };
+
+
+	for (unsigned int i = 0; i < pnaCount; i++)
+	{
+		//double tmpVals[180] = { 0 };
+
+		AnalyzeToroidalImpact(
+			pna[i].a,
+			pna[i].b,
+			pna[i].angle,
+			vals[i],
+			pna,
+			pnaCount);
+
+
+		//for (int j = 0; j < 180; j++)
+		//{
+		//	vals[j] += tmpVals[j];
+		//}
+
+	}
+	
+	for (int i = 0; i < 180; i++)
+	{
+		printf("%d", i * 2);
+		for (unsigned int j = 0; j < pnaCount; j++)
+		{
+			printf(", %f", vals[j][i]);
+		}
+		printf("\n");
+	}
+}
+
+
 Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 {
 	PointsAndAngle pna[MAX_POINT_PAIRS];
 
+	Point avgNorm = { 0 };
+
 	size_t pnaCount = 0;
 	for (unsigned int i = 0; i < obj->numSensors; i++)
 	{
@@ -772,7 +899,17 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 				pnaCount++;
 			}
 		}
+
+		avgNorm.x += obj->sensor[i].normal.x;
+		avgNorm.y += obj->sensor[i].normal.y;
+		avgNorm.z += obj->sensor[i].normal.z;
 	}
+	avgNorm.x = avgNorm.x / obj->numSensors;
+	avgNorm.y = avgNorm.y / obj->numSensors;
+	avgNorm.z = avgNorm.z / obj->numSensors;
+
+	FLT avgNormF[3] = { avgNorm.x, avgNorm.y, avgNorm.z };
+
 
 	FILE *logFile = NULL;
 	if (doLogOutput)
@@ -782,26 +919,51 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 		writeAxes(logFile);
 	}
 
-	Point initialEstimate = GetInitialEstimate(obj, pna, pnaCount, logFile);
+	//Point initialEstimate = GetInitialEstimate(obj, pna, pnaCount, logFile);
 
 	//Point refinedEstimatePc = RefineEstimateUsingPointCloud(initialEstimate, pna, pnaCount, obj, logFile);
 
 	//Point refinedEstimageGd = RefineEstimateUsingGradientDescent(initialEstimate, pna, pnaCount, logFile, 0.95, 0.1);
 
-	Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile);
+	// Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile);
+
+	// AnalyzePoloidalImpact(obj, pna, pnaCount, logFile);
+
+	// arbitrarily picking a value of 8 meters out to start from.
+	// intentionally picking the direction of the average normal vector of the sensors that see the lighthouse
+	// since this is least likely to pick the incorrect "mirror" point that would send us 
+	// back into the search for the correct point (see "if (a1 > M_PI / 2)" below)
+	Point p1 = getNormalizedVector(avgNorm, 8); 
+
+	Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
 
-	//Point p = { .x = 8 };
-	//Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p, pna, pnaCount, logFile);
+	FLT pf1[3] = { refinedEstimageGd.x, refinedEstimageGd.y, refinedEstimageGd.z };
+
+	FLT a1 = anglebetween3d(pf1, avgNormF);
+
+	if (a1 > M_PI / 2)
+	{
+		Point p2 = { .x = -refinedEstimageGd.x, .y = -refinedEstimageGd.y, .z = -refinedEstimageGd.z };
+		refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile);
+
+		//FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z };
+
+		//FLT a2 = anglebetween3d(pf2, avgNormF);
+
+	}
 
 	//FLT fitPc = getPointFitness(refinedEstimatePc, pna, pnaCount);
 	FLT fitGd = getPointFitness(refinedEstimageGd, pna, pnaCount);
+	//FLT fitGd2 = getPointFitness(refinedEstimageGd2, pna, pnaCount);
+
+	printf("Fitness is %f\n", fitGd);
 
 	if (logFile)
 	{
 		updateHeader(logFile);
 		fclose(logFile);
 	}
-
+	//fgetc(stdin);
 	return refinedEstimageGd;
 }
 
-- 
cgit v1.2.3


From 55a1f849d77d34f08a555e90120959b3ce9880ea Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Tue, 14 Feb 2017 17:08:56 -0700
Subject: Fix bug in lib math angle calculation

---
 redist/linmath.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/redist/linmath.c b/redist/linmath.c
index d5d54e3..157141f 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -75,7 +75,7 @@ FLT anglebetween3d( FLT * a, FLT * b )
 	FLT bn[3];
 	normalize3d( an, a );
 	normalize3d( bn, b );
-	FLT dot = dot3d( a, b );
+	FLT dot = dot3d(an, bn);
 	if( dot < -0.9999999 ) return LINMATHPI;
 	if( dot >  0.9999999 ) return 0;
 	return FLT_ACOS(dot);
-- 
cgit v1.2.3


From 7d28d767ec888a189a338b984062b8097b63fddd Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Tue, 14 Feb 2017 20:53:19 -0500
Subject: use acode

---
 tools/ootx_decode/ootx_decode.c | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 1074698..74d2dfe 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -64,18 +64,19 @@ void cnlohr_code_test() {
 	int8_t lh_id = 0x00;
 	uint32_t ticks = 0x00;
 	int32_t delta = 0x00;
+	uint8_t acode = 0x00;
 
 	ootx_decoder_context *c_ctx = ctx;
 
 	while (getline(&line,&line_len,stdin)>0) {
 			//R Y HMD -1575410734 -2 7 19714 6485
-			sscanf(line,"%s %s %s %s %hhd %s %d %d",
+			sscanf(line,"%s %s %s %s %hhd %hhd %d %d",
 			trash,
 			trash,
 			trash,
 			trash,
 			&lh_id,
-			trash, //sensor id?
+			&acode,
 			&delta,
 			&ticks);
 
@@ -91,8 +92,8 @@ void cnlohr_code_test() {
 //			uint8_t dbit = ootx_decode_bit(ticks);
 //			printf("LH:%d ticks:%d bit:%X %s", lh, ticks, dbit, line);
 
-			ootx_process_bit(c_ctx, ticks);
-
+//			ootx_process_bit(c_ctx, ticks);
+			ootx_pump_bit( c_ctx, (acode&0x02)>>1 );
 /*
 			uint16_t s = *(c_ctx->payload_size);
 			uint16_t fwv = *(c_ctx->buffer+2);
-- 
cgit v1.2.3


From 6dbd0f2062f5f519c9b6762874c2e940b0639cbd Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Tue, 14 Feb 2017 21:04:20 -0500
Subject: fix ootx_decode tool compile

---
 tools/ootx_decode/Makefile      | 4 ++--
 tools/ootx_decode/ootx_decode.c | 8 ++++----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/tools/ootx_decode/Makefile b/tools/ootx_decode/Makefile
index 9170ac4..b3b07b4 100644
--- a/tools/ootx_decode/Makefile
+++ b/tools/ootx_decode/Makefile
@@ -3,5 +3,5 @@ all: ootx_decode hmd_datagen
 hmd_datagen: HMD_Datagen.c 
 	gcc -Wall HMD_Datagen.c -lz -o hmd_datagen
 
-ootx_decode: ootx_decode.c ootx_decoder.c ootx_decoder.h
-	gcc -Wall ootx_decode.c ootx_decoder.c -lz -o ootx_decode
+ootx_decode: ootx_decode.c ../../src/ootx_decoder.c ../../src/ootx_decoder.h
+	gcc -Wall ootx_decode.c ../../src/ootx_decoder.c -lz -o ootx_decode -I ../../src/
diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 74d2dfe..1823aaa 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -12,12 +12,12 @@
 
 #include "ootx_decoder.h"
 
-void my_test(ootx_packet* packet) {
+void my_test(ootx_decoder_context *ctx, ootx_packet* packet) {
 	packet->data[packet->length] = 0;
 	printf("%d %s 0x%X\n", packet->length, packet->data, packet->crc32);
 }
 
-void my_test2(ootx_packet* packet) {
+void my_test2(ootx_decoder_context *ctx, ootx_packet* packet) {
 	printf("completed ootx packet\n");
 
 	lighthouse_info_v6 lhi;
@@ -41,7 +41,7 @@ void write_to_file(uint8_t *d, uint16_t length){
 	fclose(fp);
 }
 
-void bad_crc(ootx_packet* packet, uint32_t crc) {
+void bad_crc(ootx_decoder_context *ctx, ootx_packet* packet, uint32_t crc) {
 	printf("CRC mismatch\n");
 
 	printf("r:");
@@ -118,4 +118,4 @@ int main(int argc, char* argv[])
 	ootx_free_decoder_context(ctx+1);
 
 	return 0;
-}
\ No newline at end of file
+}
-- 
cgit v1.2.3


From f736678ae0df00f7b5850043d9327da44b068c47 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Tue, 14 Feb 2017 21:26:26 -0500
Subject: use 0 and not buffer offset

---
 src/ootx_decoder.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
index b1815d7..894012a 100644
--- a/src/ootx_decoder.c
+++ b/src/ootx_decoder.c
@@ -55,7 +55,7 @@ uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) {
 
 void ootx_reset_buffer(ootx_decoder_context *ctx) {
 	ctx->buf_offset = 0;
-	ctx->buffer[ctx->buf_offset] = 0;
+	ctx->buffer[0] = 0;
 	ctx->bits_written = 0;
 	*(ctx->payload_size) = 0;
 }
-- 
cgit v1.2.3


From a35428f2125279252c26a645f9ee8f0818b3204c Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Tue, 14 Feb 2017 22:03:36 -0500
Subject: handle infinity and NaN, also signed zero

---
 src/ootx_decoder.c | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
index 894012a..3823beb 100644
--- a/src/ootx_decoder.c
+++ b/src/ootx_decoder.c
@@ -167,20 +167,27 @@ uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) {
 }
 
 float _half_to_float(uint8_t* data) {
-	//this will not handle infinity, NaN, or denormalized floats
+	//this will not handle denormalized floats
 
 	uint16_t x = *(uint16_t*)data;
 	float f = 0;
 
 	uint32_t *ftmp = (uint32_t*)&f; //use the allocated floating point memory
 
-	if ((x & 0x7FFF) == 0) return f; //zero
-
 	//sign
 	*ftmp = x & 0x8000;
 	*ftmp <<= 16;
 
-	*ftmp += ((((uint32_t)(x & 0x7fff)) + 0x1c000) << 13);
+	if ((x & 0x7FFF) == 0) return f; //signed zero
+
+	if((x&0x7c00) == 0x7c00) {
+		//for infinity, fraction is 0 (just copy in 0 bits)
+		//for NaN, fraction is anything non zero (copy in bits, no need to shift)
+		*ftmp |= 0x7f800000 | (x & 0x3ff);
+		return f;
+	}
+
+	*ftmp += ((((uint32_t)(x & 0x7fff)) + 0x1c000u) << 13);
 
 	return f;
 }
-- 
cgit v1.2.3


From e94903cbdbefb51244cb0d5d6cd2463f886287ff Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Wed, 15 Feb 2017 20:09:10 -0500
Subject: fix compile error when not using c99 or c11

---
 src/survive_usb.c | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/survive_usb.c b/src/survive_usb.c
index cf12280..aec76db 100644
--- a/src/survive_usb.c
+++ b/src/survive_usb.c
@@ -125,7 +125,8 @@ static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t i
 static inline int hid_get_feature_report_timeout(libusb_device_handle* device, uint16_t interface, unsigned char *buf, size_t len )
 {
 	int ret;
-    for (unsigned i = 0; i < 100; i++)
+	uint8_t i = 0;
+    for (i = 0; i < 100; i++)
 	{
         ret = getupdate_feature_report(device, interface, buf, len);
 		if( ret != -9 && ( ret != -1 || errno != EPIPE ) ) return ret;
@@ -146,6 +147,7 @@ int survive_usb_init( struct SurviveContext * ctx )
 	}
 
 	int i;
+	int16_t j;
 	libusb_device** devs;
 	int ret = libusb_get_device_list(ctx->usbctx, &devs);
 
@@ -201,7 +203,7 @@ int survive_usb_init( struct SurviveContext * ctx )
 		}
 
 		libusb_set_auto_detach_kernel_driver( ctx->udev[i], 1 );
-		for (int j = 0; j < conf->bNumInterfaces; j++ )
+		for (j = 0; j < conf->bNumInterfaces; j++ )
 		{
 #if 0
 		    if (libusb_kernel_driver_active(ctx->udev[i], j) == 1) {
-- 
cgit v1.2.3


From cbbf33778e288856e48f7bcd23b1b1a8c2707d59 Mon Sep 17 00:00:00 2001
From: Josh Allen <axlecrusher@gmail.com>
Date: Thu, 16 Feb 2017 06:23:12 -0500
Subject: full implementation of half float to single float

---
 src/ootx_decoder.c | 54 +++++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 39 insertions(+), 15 deletions(-)

diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
index 3823beb..8ec16f2 100644
--- a/src/ootx_decoder.c
+++ b/src/ootx_decoder.c
@@ -166,30 +166,54 @@ uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) {
 	return x;
 }
 
-float _half_to_float(uint8_t* data) {
-	//this will not handle denormalized floats
+/* simply doing:
+float f = 0;
+uint32_t *ftmp = (uint32_t*)&f; //use the allocated floating point memory
+This can cause problem when strict aliasing (-O2) is used.
+Reads and writes to f and ftmp would be considered independent and could be 
+be reordered by the compiler. A union solves that problem.
+*/
+union iFloat {
+	uint32_t i;
+	float f;
+};
 
+float _half_to_float(uint8_t* data) {
 	uint16_t x = *(uint16_t*)data;
-	float f = 0;
-
-	uint32_t *ftmp = (uint32_t*)&f; //use the allocated floating point memory
+	union iFloat fnum;
+	fnum.f = 0;
 
 	//sign
-	*ftmp = x & 0x8000;
-	*ftmp <<= 16;
-
-	if ((x & 0x7FFF) == 0) return f; //signed zero
+	fnum.i = (x & 0x8000)<<16;
+
+	if ((x & 0x7FFF) == 0) return fnum.f; //signed zero
+
+	if ((x & 0x7c00) == 0) {
+		//denormalized
+		x = (x&0x3ff)<<1; //only mantissa, advance intrinsic bit forward
+		uint8_t e = 0;
+		//shift until intrinsic bit of mantissa overflows into exponent
+		//increment exponent each time
+		while ((x&0x0400) == 0) {
+			x<<=1;
+			e++;
+		}
+		fnum.i |= ((uint32_t)(112-e))<<23; //bias exponent to 127, half floats are biased 15 so only need to go 112 more.
+		fnum.i |= ((uint32_t)(x&0x3ff))<<13; //insert mantissa
+		return fnum.f;
+	}
 
 	if((x&0x7c00) == 0x7c00) {
-		//for infinity, fraction is 0 (just copy in 0 bits)
-		//for NaN, fraction is anything non zero (copy in bits, no need to shift)
-		*ftmp |= 0x7f800000 | (x & 0x3ff);
-		return f;
+		//for infinity, fraction is 0
+		//for NaN, fraction is anything non zero
+		//we could just copy in bits and not shift, but the mantissa of a NaN can have meaning
+		fnum.i |= 0x7f800000 | ((uint32_t)(x & 0x3ff))<<13;
+		return fnum.f;
 	}
 
-	*ftmp += ((((uint32_t)(x & 0x7fff)) + 0x1c000u) << 13);
+	fnum.i |= ((((uint32_t)(x & 0x7fff)) + 0x1c000u) << 13);
 
-	return f;
+	return fnum.f;
 }
 
 void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) {
-- 
cgit v1.2.3


From 2e5d0355da2376f27dcbe0cc6d04b737145ac853 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 16 Feb 2017 15:47:46 -0500
Subject: update with more integration stuff.

---
 calibrate.c           | 30 ++++++++++++++++++++++++++++++
 src/survive_cal.c     | 27 ++++++++++++++++++++++++---
 src/survive_process.c | 20 ++++++++++++++++++--
 3 files changed, 72 insertions(+), 5 deletions(-)

diff --git a/calibrate.c b/calibrate.c
index 4db2ed0..ce50b67 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -40,6 +40,36 @@ int buffertimeto[32*3];
 void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
 	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+
+	if( acode == -1 ) return;
+//return;
+	int jumpoffset = sensor_id;
+	if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
+	else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64;
+
+
+	if( acode == 0 || acode == 2 ) //data = 0
+	{
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+	if( acode == 1 || acode == 3 ) //data = 1
+	{
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+
+
+	if( acode == 4 || acode == 6 ) //data = 0
+	{
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+	if( acode == 5 || acode == 7 ) //data = 1
+	{
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
 }
 
 void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
diff --git a/src/survive_cal.c b/src/survive_cal.c
index adcb7bc..cdc7783 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -12,11 +12,26 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	struct SurviveCalData * cd = ctx->calptr;
 	int id = ct->user1;
 
-	printf( "Got OOTX packet %d %p\n", id, cd );
+	SV_INFO( "Got OOTX packet %d %p\n", id, cd );
 
 	lighthouse_info_v6 v6;
 	init_lighthouse_info_v6(&v6, packet->data);
-	print_lighthouse_info_v6(&v6);
+
+	struct BaseStationData * b = &ctx->bsd[id];
+	//print_lighthouse_info_v6(&v6);
+
+	b->BaseStationID = v6.id;
+	b->fcalphase[0] = v6.fcal_0_phase;
+	b->fcalphase[1] = v6.fcal_1_phase;
+	b->fcaltilt[0] = v6.fcal_0_tilt;
+	b->fcaltilt[1] = v6.fcal_1_tilt;
+	b->fcalcurve[0] = v6.fcal_0_curve;
+	b->fcalcurve[1] = v6.fcal_1_curve;
+	b->fcalgibpha[0] = v6.fcal_0_gibphase;
+	b->fcalgibpha[1] = v6.fcal_1_gibphase;
+	b->fcalgibmag[0] = v6.fcal_0_gibmag;
+	b->fcalgibmag[1] = v6.fcal_1_gibmag;
+	b->OOTXSet = 1;
 }
 
 
@@ -64,8 +79,14 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 				//printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit );
 				ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
 			}
+			int i;
+			for( i = 0; i < NUM_LIGHTHOUSES; i++ )
+				if( ctx->bsd[i].OOTXSet == 0 ) break;
+			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
 		}
-			
+		break;
+
+	case 2:
 		break;
 	}
 }
diff --git a/src/survive_process.c b/src/survive_process.c
index 184532b..cb53588 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -8,13 +8,29 @@
 
 void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
+	struct SurviveContext * ctx = so->ctx;
+	int base_station = acode >> 2;
+	int axis = acode & 1;
 
-	if( so->ctx->calptr )
+	if( ctx->calptr )
 	{
 		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
 	}
 
-	//TODO: Writeme!
+	if( base_station > NUM_LIGHTHOUSES ) return;
+
+	//No loner need sync information past this point.
+	if( sensor_id < 0 ) return;
+
+	float angle = (timeinsweep - 200000) * (1./200000. * 3.14159265359/2.0);
+
+	//Need to now do angle correction.
+#if 1
+	struct BaseStationData * bsd = &ctx->bsd[base_station];
+
+	//TODO!!!
+#endif
+	
 }
 
 void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
-- 
cgit v1.2.3


From 32fbccbd7d90f1e456d1e477eab2128aaf88df93 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 16 Feb 2017 17:16:31 -0500
Subject: Move to having an angle callback.

---
 Makefile               |  2 +-
 calibrate.c            |  6 ++++++
 include/survive.h      | 38 ++++++++++++++++++++++++++------------
 src/survive.c          | 34 +++++++++++++++++++++++++++++-----
 src/survive_cal.c      | 26 +++++++++++++++++++++++---
 src/survive_cal.h      |  1 +
 src/survive_data.c     | 32 +++++++++++++++++++-------------
 src/survive_internal.h |  9 ++++-----
 src/survive_process.c  | 24 +++++++++++++++++++++---
 9 files changed, 130 insertions(+), 42 deletions(-)

diff --git a/Makefile b/Makefile
index 797c257..ed51e30 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,7 @@
 all : lib data_recorder test calibrate
 
 CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto
-LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -flto
+LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto
 
 # unused: redist/crc32.c
 
diff --git a/calibrate.c b/calibrate.c
index ce50b67..6bd3e77 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -85,6 +85,10 @@ return;
 }
 
 
+void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+	survive_default_angle_process( so, sensor_id, acode, timecode, length, angle );
+}
 
 
 void * GuiThread( void * v )
@@ -125,12 +129,14 @@ void * GuiThread( void * v )
 
 
 
+
 int main()
 {
 	ctx = survive_init( );
 
 	survive_install_light_fn( ctx,  my_light_process );
 	survive_install_imu_fn( ctx,  my_imu_process );
+	survive_install_angle_fn( ctx, my_angle_process );
 
 	survive_cal_install( ctx );
 
diff --git a/include/survive.h b/include/survive.h
index 4a95a2f..3e8dc35 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -3,19 +3,19 @@
 
 #include <stdint.h>
 
-#define SV_FLOAT  		double
+#ifndef FLT
+#define FLT double
+#endif
 
 struct SurviveContext;
 
 //DANGER: This structure may be redefined.  Note that it is logically split into 64-bit chunks
 //for optimization on 32- and 64-bit systems.
 
-
 //Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools.
 //It will also require a recompile.  TODO: revisit this and correct the comment once fixed.
 #define NUM_LIGHTHOUSES 2  
 
-
 struct SurviveObject
 {
 	struct SurviveContext * ctx;
@@ -31,17 +31,26 @@ struct SurviveObject
 	int8_t  ison:1;
 	int8_t  additional_flags:6;
 
-	SV_FLOAT * sensor_locations;
-	SV_FLOAT * sensor_normals;
+	FLT * sensor_locations;
+	FLT * sensor_normals;
+
+	//Timing sensitive data (mostly for disambiguation)
+	int32_t timebase_hz;		//48,000,000 for normal vive hardware.  (checked)
+	int32_t timecenter_ticks; 	//200,000 for normal vive hardware.     (checked)
+	int32_t pulsedist_max_ticks; //500,000 for normal vive hardware.   (guessed)
+	int32_t pulselength_min_sync; //2,200 for normal vive hardware.    (guessed)
+	int32_t pulse_in_clear_time; //35,000 for normal vive hardware.    (guessed)
+	int32_t pulse_max_for_sweep; //1,800 for normal vive hardware.     (guessed)
+	int32_t pulse_synctime_offset; //20,000 for normal vive hardware.  (guessed)
+	int32_t pulse_synctime_slack; //5,000 for normal vive hardware.    (guessed)
 	int8_t nr_locations;
 
 	//Flood info, for calculating which laser is currently sweeping.
 	int8_t oldcode;
-
-	uint32_t last_time[NUM_LIGHTHOUSES];
-	uint32_t last_length[NUM_LIGHTHOUSES];
 	int8_t   sync_set_number; //0 = master, 1 = slave, -1 = fault. 
 	int8_t   did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit.
+	uint32_t last_time[NUM_LIGHTHOUSES];
+	uint32_t last_length[NUM_LIGHTHOUSES];
 	uint32_t recent_sync_time;
 
 	uint32_t last_lighttime;  //May be a 24- or 32- bit number depending on what device.
@@ -50,17 +59,19 @@ struct SurviveObject
 	int tsl;
 };
 
-typedef void (*text_feedback_fnptr)( struct SurviveContext * ctx, const char * fault );
+typedef void (*text_feedback_func)( struct SurviveContext * ctx, const char * fault );
 typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
 typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
+typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
 struct SurviveContext * survive_init();
 
 //For any of these, you may pass in 0 for the function pointer to use default behavior.
-void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp );
-void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp );
+void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func fbp );
+void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_func fbp );
 void survive_install_light_fn( struct SurviveContext * ctx,  light_process_func fbp );
 void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp );
+void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func fbp );
 
 void survive_close( struct SurviveContext * ctx );
 int survive_poll();
@@ -77,8 +88,11 @@ int survive_usb_send_magic( struct SurviveContext * ctx, int on );
 void survive_cal_install( struct SurviveContext * ctx );
 
 //Call these from your callback if overridden.  
-void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
+//Accept higher-level data.
+void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
 void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
+void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+
 
 #endif
 
diff --git a/src/survive.c b/src/survive.c
index ed18da2..aab2ae6 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -30,7 +30,7 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 	fprintf( stderr, "Info: %s\n", fault );
 }
 
-static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, SV_FLOAT ** floats_out, jsmntok_t * t, int i )
+static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
 {
 	int k;
 	int pts = t[i+1].size;
@@ -43,7 +43,7 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so,
 	{
 		tk = &t[i+2+k*4];
 
-		float vals[3];
+		FLT vals[3];
 		int m;
 		for( m = 0; m < 3; m++ )
 		{
@@ -60,7 +60,7 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so,
 
 			memcpy( ctt, ct0conf + tk->start, elemlen );
 			ctt[elemlen] = 0;
-			float f = atof( ctt );
+			FLT f = atof( ctt );
 			int id = so->nr_locations*3+m;
 			(*floats_out)[id] = f;
 		}
@@ -144,6 +144,7 @@ struct SurviveContext * survive_init()
 
 	ctx->lightproc = survive_default_light_process;
 	ctx->imuproc = survive_default_imu_process;
+	ctx->angleproc = survive_default_angle_process;
 
 	ctx->headset.ctx = ctx;
 	memcpy( ctx->headset.codename, "HMD", 4 );
@@ -164,6 +165,18 @@ struct SurviveContext * survive_init()
 	if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); }
 	if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); }
 
+	ctx->headset.timebase_hz = ctx->watchman[0].timebase_hz = ctx->watchman[1].timebase_hz = 48000000;
+	ctx->headset.pulsedist_max_ticks = ctx->watchman[0].pulsedist_max_ticks = ctx->watchman[1].pulsedist_max_ticks = 500000;
+	ctx->headset.pulselength_min_sync = ctx->watchman[0].pulselength_min_sync = ctx->watchman[1].pulselength_min_sync = 2200;
+	ctx->headset.pulse_in_clear_time = ctx->watchman[0].pulse_in_clear_time = ctx->watchman[1].pulse_in_clear_time = 35000;
+	ctx->headset.pulse_max_for_sweep = ctx->watchman[0].pulse_max_for_sweep = ctx->watchman[1].pulse_max_for_sweep = 1800;
+
+	ctx->headset.pulse_synctime_offset = ctx->watchman[0].pulse_synctime_offset = ctx->watchman[1].pulse_synctime_offset = 20000;
+	ctx->headset.pulse_synctime_slack = ctx->watchman[0].pulse_synctime_slack = ctx->watchman[1].pulse_synctime_slack = 5000;
+
+	ctx->headset.timecenter_ticks     = ctx->headset.timebase_hz / 240;
+	ctx->watchman[0].timecenter_ticks = ctx->watchman[0].timebase_hz / 240;
+	ctx->watchman[1].timecenter_ticks = ctx->watchman[1].timebase_hz / 240;
 /*
 	int i;
 	int locs = ctx->headset.nr_locations;
@@ -195,7 +208,7 @@ fail_gracefully:
 	return 0;
 }
 
-void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp )
+void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
 		ctx->notefunction = fbp;
@@ -203,7 +216,7 @@ void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_fnptr
 		ctx->notefunction = survivenote;
 }
 
-void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp )
+void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
 		ctx->faultfunction = fbp;
@@ -227,6 +240,17 @@ void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp
 		ctx->imuproc = survive_default_imu_process;
 }
 
+
+void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func fbp )
+{
+	if( fbp )
+		ctx->angleproc = fbp;
+	else
+		ctx->angleproc = survive_default_angle_process;
+}
+
+
+
 void survive_close( struct SurviveContext * ctx )
 {
 	survive_usb_close( ctx );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index cdc7783..6bb8173 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -5,6 +5,7 @@
 
 #include "survive_cal.h"
 #include "survive_internal.h"
+#include <math.h>
 
 void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 {
@@ -23,8 +24,8 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->BaseStationID = v6.id;
 	b->fcalphase[0] = v6.fcal_0_phase;
 	b->fcalphase[1] = v6.fcal_1_phase;
-	b->fcaltilt[0] = v6.fcal_0_tilt;
-	b->fcaltilt[1] = v6.fcal_1_tilt;
+	b->fcaltilt[0] = tan(v6.fcal_0_tilt);
+	b->fcaltilt[1] = tan(v6.fcal_1_tilt);  //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
 	b->fcalcurve[0] = v6.fcal_0_curve;
 	b->fcalcurve[1] = v6.fcal_1_curve;
 	b->fcalgibpha[0] = v6.fcal_0_gibphase;
@@ -76,7 +77,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 			if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' )
 			{
 				uint8_t dbit = (acode & 2)>>1;
-				//printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit );
 				ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
 			}
 			int i;
@@ -85,9 +85,29 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
 		}
 		break;
+	case 2:      //Taking in angle data.
+		break;
+	}
+}
+
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+	struct SurviveContext * ctx = so->ctx;
+	struct SurviveCalData * cd = ctx->calptr;
+
+	if( !cd ) return;
 
+	switch( cd->stage )
+	{
+	default:
+	case 1:	//Collecting OOTX data. (Don't do anything here, yet.)
+	case 0: //Default, inactive.
+		break;
 	case 2:
+		//if( sensor_id == 0 && so->codename[0] == 'H' ) printf( "%d %f %f\n", acode, length, angle );
 		break;
 	}
 }
 
+
+
diff --git a/src/survive_cal.h b/src/survive_cal.h
index a74bf95..6653a84 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -28,6 +28,7 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 
 //Called from survive_default_light_process
 void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
 struct SurviveCalData
 {
diff --git a/src/survive_data.c b/src/survive_data.c
index c557951..ad834cf 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -37,36 +37,34 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 //	printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
 
 	so->tsl = le->timestamp;
-	if( le->length < 20 ) return;
+	if( le->length < 20 ) return;  ///Assuming 20 is an okay value for here.
 
 	//The sync pulse finder is taking Charles's old disambiguator code and mixing it with a more linear
 	//version of Julian Picht's disambiguator, available in 488c5e9.  Removed afterwards into this
 	//unified driver.
-
-
 	int ssn = so->sync_set_number;
 	if( ssn < 0 ) ssn = 0;
 	int last_sync_time  =  so->last_time  [ssn];
 	int last_sync_length = so->last_length[ssn];
 	int32_t delta = le->timestamp - last_sync_time;  //Handle time wrapping (be sure to be int32)
 
-	if( delta < -500000 || delta > 500000 )
+	if( delta < -so->pulsedist_max_ticks || delta > so->pulsedist_max_ticks )
 	{
 		//Reset pulse, etc.
 		so->sync_set_number = -1;
-		delta = 500000;
+		delta = so->pulsedist_max_ticks;
 	}
 
 
-	if( le->length > 2200 ) //Pulse longer indicates a sync pulse.
+	if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse.
 	{
-		int is_new_pulse = delta > 1500 + last_sync_length;
+		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
 
 		so->did_handle_ootx = 0;
 
 		if( is_new_pulse )
 		{
-			int is_master_sync_pulse = delta > 40000;
+			int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; 
 
 			if( is_master_sync_pulse )
 			{
@@ -110,7 +108,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 
 
 	//See if this is a valid actual pulse.
-	else if( le->length < 1800 && le->length > 40 && delta > 30000 && ssn >= 0 )
+	else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 )
 	{
 		int32_t dl = so->last_time[0];
 		int32_t tpco = so->last_length[0];
@@ -123,10 +121,13 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		//Adding length 
 		//Long pulse-code from IR flood.
 		//Make sure it fits nicely into a divisible-by-500 time.
+
+		int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+
 		int32_t acode_array[2] =
 			{
-				(so->last_length[0]+125+50)/250,
-				(so->last_length[1]+125+50)/250,
+				(so->last_length[0]+main_divisor+50)/(main_divisor*2),  //+50 adds a small offset and seems to help always get it right. 
+				(so->last_length[1]+main_divisor+50)/(main_divisor*2),	//Check the +50 in the future to see how well this works on a variety of hardware.
 			};
 
 		//XXX: TODO: Capture error count here.
@@ -150,14 +151,19 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 			so->recent_sync_time = so->last_time[1];
 
 			//Throw out everything if our sync pulses look like they're bad.
-			if( delta1 < 375000 || delta1 > 385000 )
+
+			int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
+			int32_t center_2 = so->pulse_synctime_offset;
+			int32_t slack = so->pulse_synctime_slack;
+
+			if( delta1 < center_1 - slack || delta1 > center_1 + slack )
 			{
 				//XXX: TODO: Count faults.
 				so->sync_set_number = -1;
 				return;
 			}
 
-			if( delta2 < 15000 || delta2 > 25000 )
+			if( delta2 < center_2 - slack || delta2 > center_2 + slack )
 			{
 				//XXX: TODO: Count faults.
 				so->sync_set_number = -1;
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 11c9d89..0d0b8a4 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -86,10 +86,12 @@ struct SurviveContext
 	struct libusb_device_handle * udev[MAX_USB_DEVS];
 	struct SurviveUSBInterface uiface[MAX_INTERFACES];
 
-	text_feedback_fnptr faultfunction;
-	text_feedback_fnptr notefunction;
+	text_feedback_func faultfunction;
+	text_feedback_func notefunction;
 	light_process_func lightproc;
 	imu_process_func imuproc;
+	angle_process_func angleproc;
+
 
 	//Calibration data:
 	struct BaseStationData bsd[NUM_LIGHTHOUSES];
@@ -112,9 +114,6 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 //Accept Data from backend.
 void survive_data_cb( struct SurviveUSBInterface * si );
 
-//Accept higher-level data.
-void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
-void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
 
 #endif
 
diff --git a/src/survive_process.c b/src/survive_process.c
index cb53588..75453da 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -21,18 +21,36 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in
 
 	//No loner need sync information past this point.
 	if( sensor_id < 0 ) return;
-
-	float angle = (timeinsweep - 200000) * (1./200000. * 3.14159265359/2.0);
+	FLT angle = (timeinsweep - so->timecenter_ticks) * (1./so->timecenter_ticks * 3.14159265359/2.0);
 
 	//Need to now do angle correction.
 #if 1
 	struct BaseStationData * bsd = &ctx->bsd[base_station];
 
+	//XXX TODO: This seriously needs to be worked on.  See: https://github.com/cnlohr/libsurvive/issues/18
+	angle += bsd->fcalphase[axis];
+//	angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
+	
 	//TODO!!!
 #endif
-	
+
+	FLT length_sec = length / (FLT)so->timebase_hz;
+	ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle );
 }
 
+
+void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+	struct SurviveContext * ctx = so->ctx;
+	if( ctx->calptr )
+	{
+		survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
+	}
+
+	//TODO: Writeme!
+}	
+
+
 void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
 {
 	//TODO: Writeme!
-- 
cgit v1.2.3


From 9bba1f9e7888f512a587f76179b9dd8f389c7ae8 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 16 Feb 2017 18:09:19 -0500
Subject: Start collecting data.  Getting closer to having a full cal stack.

---
 calibrate.c       |  7 +++++++
 src/survive_cal.c | 47 ++++++++++++++++++++++++++++++++++++++++++++++-
 src/survive_cal.h | 15 +++++++++++++--
 3 files changed, 66 insertions(+), 3 deletions(-)

diff --git a/calibrate.c b/calibrate.c
index 6bd3e77..2db544a 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -121,6 +121,13 @@ void * GuiThread( void * v )
 			}
 		}
 
+		CNFGColor( 0xffffff );
+		char caldesc[256];
+		survive_cal_get_status( ctx, caldesc, sizeof( caldesc ) );
+		CNFGPenX = 3;
+		CNFGPenY = 3;
+		CNFGDrawText( caldesc, 2 );
+
 
 		CNFGSwapBuffers();
 		OGUSleep( 10000 );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 6bb8173..8069154 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -6,6 +6,9 @@
 #include "survive_cal.h"
 #include "survive_internal.h"
 #include <math.h>
+#include <string.h>
+
+static void handle_calibration( struct SurviveCalData *cd );
 
 void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 {
@@ -35,6 +38,22 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->OOTXSet = 1;
 }
 
+int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
+{
+	struct SurviveCalData * cd = ctx->calptr;
+
+	switch( cd->stage )
+	{
+	case 0:
+		return snprintf( description, description_length, "Not calibrating" );
+	case 1:
+		return snprintf( description, description_length, "Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
+	case 2:
+		return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+	default:
+		return snprintf( description, description_length, "Unkown calibration state" );
+	}
+}
 
 void survive_cal_install( struct SurviveContext * ctx )
 {
@@ -104,10 +123,36 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 	case 0: //Default, inactive.
 		break;
 	case 2:
-		//if( sensor_id == 0 && so->codename[0] == 'H' ) printf( "%d %f %f\n", acode, length, angle );
+	{
+		int sensid = sensor_id;
+		if( strcmp( so->codename, "WM0" ) == 0 )
+			sensid += 32;
+		if( strcmp( so->codename, "WM1" ) == 1 )
+			sensid += 64;
+
+		int lighthouse = acode>>2;
+		int axis = acode & 1;
+		int ct = cd->all_counts[sensid][lighthouse][axis]++;
+		cd->all_lengths[sensid][lighthouse][axis][ct] = length;
+		cd->all_angles[sensid][lighthouse][axis][ct] = angle;
+		if( ct > cd->peak_counts )
+		{
+			cd->peak_counts = ct;
+			if( ct >= DRPTS )
+				handle_calibration( cd ); //This will also reset all cals.
+		}
 		break;
 	}
+	}
 }
 
 
+static void handle_calibration( struct SurviveCalData *cd )
+{
+	//Do stuff.
 
+	memset( cd->all_lengths, 0, sizeof( cd->all_lengths ) );
+	memset( cd->all_angles, 0, sizeof( cd->all_angles ) );
+	memset( cd->all_counts, 0, sizeof( cd->all_counts ) );
+	cd->peak_counts = 0;
+}
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 6653a84..b9d4b11 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -22,7 +22,7 @@
 #include "survive_internal.h"
 
 void survive_cal_install( struct SurviveContext * ctx );
-int survive_cal_get_status( struct SurviveContext * ctx, char * description, int max_data );
+int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length );
 
 //void survive_cal_teardown( struct SurviveContext * ctx );
 
@@ -30,6 +30,9 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
 void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
+#define MAX_TO_CAL 96
+#define DRPTS 512
+
 struct SurviveCalData
 {
 	//Stage:
@@ -37,9 +40,17 @@ struct SurviveCalData
 	// 1: Collecting OOTX data.
 	int stage;
 
-	//OOTX Data is sync'd off of 
+	//OOTX Data is sync'd off of the sync pulses coming from the lighthouses.
 	ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
+
+	//For statistics-gathering phase.
+	FLT all_lengths[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
+	FLT all_angles[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
+	int16_t all_counts[MAX_TO_CAL][NUM_LIGHTHOUSES][2];
+	int peak_counts;
 };
 
+
+
 #endif
 
-- 
cgit v1.2.3


From bd89d46cb01f7069166e85f017f169e07acc7094 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 16 Feb 2017 18:28:54 -0500
Subject: closing in on finding common cal points.

---
 calibrate.c       |  2 +-
 src/survive_cal.c | 47 ++++++++++++++++++++++++++++++++++++++++++-----
 src/survive_cal.h | 13 +++++++------
 3 files changed, 50 insertions(+), 12 deletions(-)

diff --git a/calibrate.c b/calibrate.c
index 2db544a..60f4316 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -126,7 +126,7 @@ void * GuiThread( void * v )
 		survive_cal_get_status( ctx, caldesc, sizeof( caldesc ) );
 		CNFGPenX = 3;
 		CNFGPenY = 3;
-		CNFGDrawText( caldesc, 2 );
+		CNFGDrawText( caldesc, 4 );
 
 
 		CNFGSwapBuffers();
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 8069154..7b0a824 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -8,7 +8,11 @@
 #include <math.h>
 #include <string.h>
 
+#define PTS_BEFORE_COMMON 32
+#define NEEDED_COMMON_POINTS 20
+
 static void handle_calibration( struct SurviveCalData *cd );
+static void reset_calibration( struct SurviveCalData * cd );
 
 void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 {
@@ -49,7 +53,14 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 	case 1:
 		return snprintf( description, description_length, "Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
 	case 2:
-		return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+		if( cd->found_common )
+		{
+			return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+		}
+		else
+		{
+			return snprintf( description, description_length, "Searching for common watchman cal %d/%d", cd->peak_counts, PTS_BEFORE_COMMON );
+		}
 	default:
 		return snprintf( description, description_length, "Unkown calibration state" );
 	}
@@ -141,18 +152,44 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			if( ct >= DRPTS )
 				handle_calibration( cd ); //This will also reset all cals.
 		}
+
+		//TODO: Determine if there is a sensor on a watchman visible from both lighthouses.
+		if( sensid >= 32 && !cd->found_common )
+		{
+			int k;
+			int ok = 1;
+			for( k = 0; k < NUM_LIGHTHOUSES; k++ )
+			{
+
+				if( cd->all_counts[sensid][k][0] < NEEDED_COMMON_POINTS || cd->all_counts[sensid][k][1] < NEEDED_COMMON_POINTS )
+				{
+					ok = 0;
+					break;
+				}
+			}
+			if( ok ) cd->found_common = 1;
+		}
+
+		if( cd->peak_counts > PTS_BEFORE_COMMON && !cd->found_common )
+		{
+			reset_calibration( cd );
+		}
+
 		break;
 	}
 	}
 }
 
+static void reset_calibration( struct SurviveCalData * cd )
+{
+	memset( cd->all_counts, 0, sizeof( cd->all_counts ) );
+	cd->peak_counts = 0;
+	cd->found_common = 0;
+}
 
 static void handle_calibration( struct SurviveCalData *cd )
 {
 	//Do stuff.
 
-	memset( cd->all_lengths, 0, sizeof( cd->all_lengths ) );
-	memset( cd->all_angles, 0, sizeof( cd->all_angles ) );
-	memset( cd->all_counts, 0, sizeof( cd->all_counts ) );
-	cd->peak_counts = 0;
+	reset_calibration( cd );
 }
diff --git a/src/survive_cal.h b/src/survive_cal.h
index b9d4b11..42ff1ee 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -35,11 +35,6 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 
 struct SurviveCalData
 {
-	//Stage:
-	// 0: Idle
-	// 1: Collecting OOTX data.
-	int stage;
-
 	//OOTX Data is sync'd off of the sync pulses coming from the lighthouses.
 	ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
 
@@ -47,7 +42,13 @@ struct SurviveCalData
 	FLT all_lengths[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
 	FLT all_angles[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
 	int16_t all_counts[MAX_TO_CAL][NUM_LIGHTHOUSES][2];
-	int peak_counts;
+	int16_t peak_counts;
+	int8_t found_common;
+
+	//Stage:
+	// 0: Idle
+	// 1: Collecting OOTX data.
+	int8_t stage;
 };
 
 
-- 
cgit v1.2.3