From 46e907ad3059001a0d9361098c2f017c36a3dd13 Mon Sep 17 00:00:00 2001 From: Josh Allen 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/ootx_decode') 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