aboutsummaryrefslogtreecommitdiff
path: root/tools
diff options
context:
space:
mode:
Diffstat (limited to 'tools')
-rw-r--r--tools/ootx_decode/Makefile4
-rw-r--r--tools/ootx_decode/ootx_decode.c92
-rw-r--r--tools/ootx_decode/ootx_decoder.c207
-rw-r--r--tools/ootx_decode/ootx_decoder.h33
4 files changed, 310 insertions, 26 deletions
diff --git a/tools/ootx_decode/Makefile b/tools/ootx_decode/Makefile
index 2c0d01a..8be8618 100644
--- a/tools/ootx_decode/Makefile
+++ b/tools/ootx_decode/Makefile
@@ -3,5 +3,5 @@ all: ootx_decode hmd_datagen
hmd_datagen: HMD_Datagen.c crc32.c crc32.h
gcc -Wall HMD_Datagen.c crc32.c -o hmd_datagen
-ootx_decode: ootx_decode.c ootx_decoder.c ootx_decoder.h crc32.c crc32.h
- gcc -Wall ootx_decode.c ootx_decoder.c crc32.c -o ootx_decode \ No newline at end of file
+ootx_decode: ootx_decode.c ootx_decoder.c ootx_decoder.h
+ gcc -Wall ootx_decode.c ootx_decoder.c -lz -o ootx_decode \ No newline at end of file
diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 735b5fc..380d7b2 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);
}
@@ -65,6 +69,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,20 +85,91 @@ 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
+// 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;
-// 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);
+ c_ctx = ctx+current_lighthouse;
}
-/*
- if (current_lighthouse >= 0) {
- ootx_process_bit(ctx+current_lighthouse, ticks);
+
+// 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;
+
+ 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);
+// }
}
}
@@ -103,6 +179,8 @@ int main(int argc, char* argv[])
ootx_init_decoder_context(ctx+1);
raw_test();
+// acode_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..fb87e41 100644
--- a/tools/ootx_decode/ootx_decoder.c
+++ b/tools/ootx_decode/ootx_decoder.c
@@ -6,16 +6,17 @@
#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";
#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;
@@ -44,27 +45,84 @@ 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<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
+
+#if BETTER_SAFE_THAN_FAST
+ if (temp < 0 || length > 6525) {
+ return -1;
+ }
+#endif
+ if ((temp % 500) < 150) {
+ return temp / 500;
+ }
-uint8_t ootx_decode_bit(uint32_t ticks) {
- ticks = ((ticks/500)*500)+500;
+ return -1;
- ticks-=3000;
- if (ticks>=2000) { ticks-=2000; }
- if (ticks>=1000) { return 0xFF; }
+}
+*/
+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_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;
+ 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) == 0x01) return 1;
+ if ((ctx->preamble & 0x0003ffff) == 0x00000001) return 1;
return 0;
}
@@ -92,8 +150,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 +163,26 @@ 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 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);
// printf("z %d %d\n", bits_processed,dbit);
@@ -131,18 +210,32 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
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;
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);
@@ -152,3 +245,83 @@ void ootx_process_bit(ootx_decoder_context *ctx, uint32_t length) {
}
}
+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 x = *(uint16_t*)data;
+ return x;
+}
+
+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 696fecb..2a1b1d2 100644
--- a/tools/ootx_decode/ootx_decoder.h
+++ b/tools/ootx_decode/ootx_decoder.h
@@ -23,14 +23,47 @@ typedef struct {
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_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_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);