aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--calibrate.c30
-rw-r--r--src/ootx_decoder.c55
-rw-r--r--src/survive_cal.c27
-rw-r--r--src/survive_process.c20
-rw-r--r--src/survive_usb.c6
-rw-r--r--tools/ootx_decode/Makefile4
-rw-r--r--tools/ootx_decode/ootx_decode.c17
7 files changed, 130 insertions, 29 deletions
diff --git a/calibrate.c b/calibrate.c
index 4db2ed0..ce50b67 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -40,6 +40,36 @@ int buffertimeto[32*3];
void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
{
survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+
+ if( acode == -1 ) return;
+//return;
+ int jumpoffset = sensor_id;
+ if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
+ else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64;
+
+
+ if( acode == 0 || acode == 2 ) //data = 0
+ {
+ bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+ buffertimeto[jumpoffset] = 0;
+ }
+ if( acode == 1 || acode == 3 ) //data = 1
+ {
+ bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+ buffertimeto[jumpoffset] = 0;
+ }
+
+
+ if( acode == 4 || acode == 6 ) //data = 0
+ {
+ bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+ buffertimeto[jumpoffset] = 0;
+ }
+ if( acode == 5 || acode == 7 ) //data = 1
+ {
+ bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+ buffertimeto[jumpoffset] = 0;
+ }
}
void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
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) {
diff --git a/tools/ootx_decode/Makefile b/tools/ootx_decode/Makefile
index 9170ac4..b3b07b4 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
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
+ootx_decode: ootx_decode.c ../../src/ootx_decoder.c ../../src/ootx_decoder.h
+ gcc -Wall ootx_decode.c ../../src/ootx_decoder.c -lz -o ootx_decode -I ../../src/
diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
index 1074698..1823aaa 100644
--- a/tools/ootx_decode/ootx_decode.c
+++ b/tools/ootx_decode/ootx_decode.c
@@ -12,12 +12,12 @@
#include "ootx_decoder.h"
-void my_test(ootx_packet* packet) {
+void my_test(ootx_decoder_context *ctx, 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) {
+void my_test2(ootx_decoder_context *ctx, ootx_packet* packet) {
printf("completed ootx packet\n");
lighthouse_info_v6 lhi;
@@ -41,7 +41,7 @@ void write_to_file(uint8_t *d, uint16_t length){
fclose(fp);
}
-void bad_crc(ootx_packet* packet, uint32_t crc) {
+void bad_crc(ootx_decoder_context *ctx, ootx_packet* packet, uint32_t crc) {
printf("CRC mismatch\n");
printf("r:");
@@ -64,18 +64,19 @@ void cnlohr_code_test() {
int8_t lh_id = 0x00;
uint32_t ticks = 0x00;
int32_t delta = 0x00;
+ uint8_t acode = 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",
+ sscanf(line,"%s %s %s %s %hhd %hhd %d %d",
trash,
trash,
trash,
trash,
&lh_id,
- trash, //sensor id?
+ &acode,
&delta,
&ticks);
@@ -91,8 +92,8 @@ void cnlohr_code_test() {
// 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);
-
+// ootx_process_bit(c_ctx, ticks);
+ ootx_pump_bit( c_ctx, (acode&0x02)>>1 );
/*
uint16_t s = *(c_ctx->payload_size);
uint16_t fwv = *(c_ctx->buffer+2);
@@ -117,4 +118,4 @@ int main(int argc, char* argv[])
ootx_free_decoder_context(ctx+1);
return 0;
-} \ No newline at end of file
+}