diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/ootx_decoder.c | 55 | ||||
| -rw-r--r-- | src/survive_cal.c | 27 | ||||
| -rw-r--r-- | src/survive_process.c | 20 | ||||
| -rw-r--r-- | src/survive_usb.c | 6 |
4 files changed, 89 insertions, 19 deletions
diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c index b1815d7..8ec16f2 100644 --- a/src/ootx_decoder.c +++ b/src/ootx_decoder.c @@ -55,7 +55,7 @@ uint8_t ootx_detect_preamble(ootx_decoder_context *ctx, uint8_t dbit) { void ootx_reset_buffer(ootx_decoder_context *ctx) { ctx->buf_offset = 0; - ctx->buffer[ctx->buf_offset] = 0; + ctx->buffer[0] = 0; ctx->bits_written = 0; *(ctx->payload_size) = 0; } @@ -166,23 +166,54 @@ uint8_t* get_ptr(uint8_t* data, uint8_t bytes, uint16_t* idx) { return x; } -float _half_to_float(uint8_t* data) { - //this will not handle infinity, NaN, or denormalized floats +/* simply doing: +float f = 0; +uint32_t *ftmp = (uint32_t*)&f; //use the allocated floating point memory +This can cause problem when strict aliasing (-O2) is used. +Reads and writes to f and ftmp would be considered independent and could be +be reordered by the compiler. A union solves that problem. +*/ +union iFloat { + uint32_t i; + float f; +}; +float _half_to_float(uint8_t* data) { 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 + union iFloat fnum; + fnum.f = 0; //sign - *ftmp = x & 0x8000; - *ftmp <<= 16; + fnum.i = (x & 0x8000)<<16; + + if ((x & 0x7FFF) == 0) return fnum.f; //signed zero + + if ((x & 0x7c00) == 0) { + //denormalized + x = (x&0x3ff)<<1; //only mantissa, advance intrinsic bit forward + uint8_t e = 0; + //shift until intrinsic bit of mantissa overflows into exponent + //increment exponent each time + while ((x&0x0400) == 0) { + x<<=1; + e++; + } + fnum.i |= ((uint32_t)(112-e))<<23; //bias exponent to 127, half floats are biased 15 so only need to go 112 more. + fnum.i |= ((uint32_t)(x&0x3ff))<<13; //insert mantissa + return fnum.f; + } + + if((x&0x7c00) == 0x7c00) { + //for infinity, fraction is 0 + //for NaN, fraction is anything non zero + //we could just copy in bits and not shift, but the mantissa of a NaN can have meaning + fnum.i |= 0x7f800000 | ((uint32_t)(x & 0x3ff))<<13; + return fnum.f; + } - *ftmp += ((((uint32_t)(x & 0x7fff)) + 0x1c000) << 13); + fnum.i |= ((((uint32_t)(x & 0x7fff)) + 0x1c000u) << 13); - return f; + return fnum.f; } void init_lighthouse_info_v6(lighthouse_info_v6* lhi, uint8_t* data) { diff --git a/src/survive_cal.c b/src/survive_cal.c index adcb7bc..cdc7783 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -12,11 +12,26 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) struct SurviveCalData * cd = ctx->calptr; int id = ct->user1; - printf( "Got OOTX packet %d %p\n", id, cd ); + SV_INFO( "Got OOTX packet %d %p\n", id, cd ); lighthouse_info_v6 v6; init_lighthouse_info_v6(&v6, packet->data); - print_lighthouse_info_v6(&v6); + + struct BaseStationData * b = &ctx->bsd[id]; + //print_lighthouse_info_v6(&v6); + + b->BaseStationID = v6.id; + b->fcalphase[0] = v6.fcal_0_phase; + b->fcalphase[1] = v6.fcal_1_phase; + b->fcaltilt[0] = v6.fcal_0_tilt; + b->fcaltilt[1] = v6.fcal_1_tilt; + b->fcalcurve[0] = v6.fcal_0_curve; + b->fcalcurve[1] = v6.fcal_1_curve; + b->fcalgibpha[0] = v6.fcal_0_gibphase; + b->fcalgibpha[1] = v6.fcal_1_gibphase; + b->fcalgibmag[0] = v6.fcal_0_gibmag; + b->fcalgibmag[1] = v6.fcal_1_gibmag; + b->OOTXSet = 1; } @@ -64,8 +79,14 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit ); ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); } + int i; + for( i = 0; i < NUM_LIGHTHOUSES; i++ ) + if( ctx->bsd[i].OOTXSet == 0 ) break; + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. } - + break; + + case 2: break; } } diff --git a/src/survive_process.c b/src/survive_process.c index 184532b..cb53588 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -8,13 +8,29 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) { + struct SurviveContext * ctx = so->ctx; + int base_station = acode >> 2; + int axis = acode & 1; - if( so->ctx->calptr ) + if( ctx->calptr ) { survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); } - //TODO: Writeme! + if( base_station > NUM_LIGHTHOUSES ) return; + + //No loner need sync information past this point. + if( sensor_id < 0 ) return; + + float angle = (timeinsweep - 200000) * (1./200000. * 3.14159265359/2.0); + + //Need to now do angle correction. +#if 1 + struct BaseStationData * bsd = &ctx->bsd[base_station]; + + //TODO!!! +#endif + } void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) diff --git a/src/survive_usb.c b/src/survive_usb.c index cf12280..aec76db 100644 --- a/src/survive_usb.c +++ b/src/survive_usb.c @@ -125,7 +125,8 @@ static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t i static inline int hid_get_feature_report_timeout(libusb_device_handle* device, uint16_t interface, unsigned char *buf, size_t len ) { int ret; - for (unsigned i = 0; i < 100; i++) + uint8_t i = 0; + for (i = 0; i < 100; i++) { ret = getupdate_feature_report(device, interface, buf, len); if( ret != -9 && ( ret != -1 || errno != EPIPE ) ) return ret; @@ -146,6 +147,7 @@ int survive_usb_init( struct SurviveContext * ctx ) } int i; + int16_t j; libusb_device** devs; int ret = libusb_get_device_list(ctx->usbctx, &devs); @@ -201,7 +203,7 @@ int survive_usb_init( struct SurviveContext * ctx ) } libusb_set_auto_detach_kernel_driver( ctx->udev[i], 1 ); - for (int j = 0; j < conf->bNumInterfaces; j++ ) + for (j = 0; j < conf->bNumInterfaces; j++ ) { #if 0 if (libusb_kernel_driver_active(ctx->udev[i], j) == 1) { |