diff options
-rw-r--r-- | tools/ootx_decode/Makefile | 4 | ||||
-rw-r--r-- | tools/ootx_decode/ootx_decode.c | 92 | ||||
-rw-r--r-- | tools/ootx_decode/ootx_decoder.c | 207 | ||||
-rw-r--r-- | tools/ootx_decode/ootx_decoder.h | 33 |
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); |