diff options
Diffstat (limited to 'tools')
-rw-r--r-- | tools/ootx_decode/HMD_Datagen.c | 103 | ||||
-rw-r--r-- | tools/ootx_decode/Makefile | 7 | ||||
-rw-r--r-- | tools/ootx_decode/ootx_decode.c | 120 | ||||
-rw-r--r-- | tools/ootx_decode/ootx_decoder.c | 243 | ||||
-rw-r--r-- | tools/ootx_decode/ootx_decoder.h | 68 |
5 files changed, 541 insertions, 0 deletions
diff --git a/tools/ootx_decode/HMD_Datagen.c b/tools/ootx_decode/HMD_Datagen.c new file mode 100644 index 0000000..15ed62c --- /dev/null +++ b/tools/ootx_decode/HMD_Datagen.c @@ -0,0 +1,103 @@ +// (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. + +/* generate data to test ootx decoding */ + +#include <stdio.h> +#include <string.h> +#include <stdint.h> +#include <time.h> +#include <stdlib.h> +#include <zlib.h> + +//this program is broken and does not produce useable data. + +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); + + +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( 0L, Z_NULL, 0 ); + crc = crc32( crc, (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 +} diff --git a/tools/ootx_decode/Makefile b/tools/ootx_decode/Makefile new file mode 100644 index 0000000..9170ac4 --- /dev/null +++ b/tools/ootx_decode/Makefile @@ -0,0 +1,7 @@ +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 diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c new file mode 100644 index 0000000..1074698 --- /dev/null +++ b/tools/ootx_decode/ootx_decode.c @@ -0,0 +1,120 @@ +// (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 test*/ + +#include <stdio.h> +#include <stdlib.h> +#include <stdint.h> +#include <string.h> +#include <assert.h> + +#include "ootx_decoder.h" + +void my_test(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) { + 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); +} + + +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, uint32_t crc) { + printf("CRC mismatch\n"); + + printf("r:"); + print_crc32(packet->crc32); + + printf("c:"); + print_crc32(crc); + write_to_file(packet->data,packet->length); +} + +ootx_decoder_context ctx[2]; + +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] = ""; + int8_t lh_id = 0x00; + uint32_t ticks = 0x00; + int32_t delta = 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", + 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); + + cnlohr_code_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 new file mode 100644 index 0000000..b5a7b54 --- /dev/null +++ b/tools/ootx_decode/ootx_decoder.c @@ -0,0 +1,243 @@ +// (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 new file mode 100644 index 0000000..c707159 --- /dev/null +++ b/tools/ootx_decode/ootx_decoder.h @@ -0,0 +1,68 @@ +// (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 |