From c04c9fb7dcf80123edec53a2e6709bb128548a73 Mon Sep 17 00:00:00 2001 From: Josh Allen 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