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

(limited to 'tools')

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

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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 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.
---
 tools/plot_lighthouse/Makefile           | 4 ++--
 tools/plot_lighthouse/main.c             | 5 +++++
 tools/process_rawcap/process_to_points.c | 3 ++-
 3 files changed, 9 insertions(+), 3 deletions(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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 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!

---
 tools/ootx_decode/ootx_decoder.c | 243 ---------------------------------------
 tools/ootx_decode/ootx_decoder.h |  68 -----------
 2 files changed, 311 deletions(-)
 delete mode 100644 tools/ootx_decode/ootx_decoder.c
 delete mode 100644 tools/ootx_decode/ootx_decoder.h

(limited to 'tools')

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(-)

(limited to 'tools')

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 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(-)

(limited to 'tools')

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(-)

(limited to 'tools')

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