From 2a08bb2546f0a6b93f313719a4b81480b660843b Mon Sep 17 00:00:00 2001 From: ultramn Date: Sat, 11 Feb 2017 18:02:02 -0800 Subject: commit issue --- tools/plot_lighthouse/main.c | 69 ++++++++++++++++++++++++++------------------ 1 file changed, 41 insertions(+), 28 deletions(-) diff --git a/tools/plot_lighthouse/main.c b/tools/plot_lighthouse/main.c index c2fd97a..8088828 100755 --- a/tools/plot_lighthouse/main.c +++ b/tools/plot_lighthouse/main.c @@ -73,7 +73,8 @@ float rotz=0.0f; void init() { int i,j; - +//void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ); + // Load the lighthouses LoadLighthousePos("L.txt", &houseL.x,&houseL.y,&houseL.z,&houseL.qi,&houseL.qj,&houseL.qk,&houseL.qreal); LoadLighthousePos("R.txt", &houseR.x,&houseR.y,&houseR.z,&houseR.qi,&houseR.qj,&houseR.qk,&houseR.qreal); @@ -86,14 +87,14 @@ void init() // Spawn the thread to read the hmt angles memset(read_hmdAngleViewed, 0, NUM_HMD*NUM_SWEEP*sizeof(int)); read_mutex = OGCreateMutex(); - for (i=0; i<32; i++) { + for (i=0; i= read_frameno-6 && - read_hmdAngleViewed[1][i] >= read_frameno-6 && - read_hmdAngleViewed[2][i] >= read_frameno-6 && - read_hmdAngleViewed[3][i] >= read_frameno-6 &&*/ - hmdAngles[0][i]!=-9999.0 && hmdAngles[1][i]!=-9999.0 && hmdAngles[2][i]!=-9999.0 && hmdAngles[3][i]!=-9999.0) + for (i=0; i= read_frameno-6 && + read_hmdAngleViewed[SWEEP_LY][i] >= read_frameno-6 && + hmdAngles[SWEEP_LX][i]!=-9999.0 && hmdAngles[SWEEP_LY][i]!=-9999.0) { // Get the hmd bearings - double Ldx,Ldy,Ldz,Rdx,Rdy,Rdz; + double Ldx,Ldy,Ldz; HmdDir(hmdAngles[SWEEP_LX][i], hmdAngles[SWEEP_LY][i], &Ldx, &Ldy, &Ldz); - HmdDir(hmdAngles[SWEEP_RX][i], hmdAngles[SWEEP_RY][i], &Rdx, &Rdy, &Rdz); - - // Rotate by the lighthouse coordinate systems - Quaternion L0,L,R0,R,Lq,Rq; - QuaternionSet(L0,Ldx,Ldy,Ldz,0.0); - QuaternionSet(R0,Rdx,Rdy,Rdz,0.0); - QuaternionSet(Lq,houseL.qi,houseL.qj,houseL.qk,houseL.qreal); - QuaternionSet(Rq,houseR.qi,houseR.qj,houseR.qk,houseR.qreal); - QuaternionRot(L, Lq, L0); // L is world space bearing from Left lighthouse to hmd - QuaternionRot(R, Rq, R0); // R is world space bearing from Right lighthouse to hmd - + + // Rotate the bearings by the lighthouse orientation + FLT L0[3],L[3],Lq[4]; + L0[0]=Ldx; L0[1]=Ldy; L0[2]=Ldz; + Lq[0]=houseL.qreal; Lq[1]=houseL.qi; Lq[2]=houseL.qj; Lq[3]=houseL.qk; + quatrotatevector(L, Lq, L0); + // Plot the lines - const double dist=10.0; glVertex3f(houseL.x, houseL.z, houseL.y); - glVertex3f(houseL.x+dist*L.i, houseL.z+dist*L.k, houseL.y+dist*L.j); + glVertex3f(houseL.x+dist*L[0], houseL.z+dist*L[2], houseL.y+dist*L[1]); + } + + // If the left lighthouse sees it + if (read_hmdAngleViewed[SWEEP_RX][i] >= read_frameno-6 && + read_hmdAngleViewed[SWEEP_RY][i] >= read_frameno-6 && + hmdAngles[SWEEP_RX][i]!=-9999.0 && hmdAngles[SWEEP_RY][i]!=-9999.0) + { + // Get the hmd bearings + double Rdx,Rdy,Rdz; + HmdDir(hmdAngles[SWEEP_RX][i], hmdAngles[SWEEP_RY][i], &Rdx, &Rdy, &Rdz); + + // Rotate the bearings by the lighthouse orientation + FLT R0[3],R[3],Rq[4]; + R0[0]=Rdx; R0[1]=Rdy; R0[2]=Rdz; + Rq[0]=houseR.qreal; Rq[1]=houseR.qi; Rq[2]=houseR.qj; Rq[3]=houseR.qk; + quatrotatevector(R, Rq, R0); + + // Plot the lines glVertex3f(houseR.x, houseR.z, houseR.y); - glVertex3f(houseR.x+dist*R.i, houseR.z+dist*R.k, houseR.y+dist*R.j); + glVertex3f(houseR.x+dist*R[0], houseR.z+dist*R[2], houseR.y+dist*R[1]); } } glEnd(); -- cgit v1.2.3 From b8158f6419c633b994e7bb936baf6ebfec13d42e Mon Sep 17 00:00:00 2001 From: ultramn Date: Sun, 12 Feb 2017 15:01:30 -0800 Subject: this wasn't committed for some reason --- tools/plot_lighthouse/glutil.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tools/plot_lighthouse/glutil.h b/tools/plot_lighthouse/glutil.h index 1014506..936701e 100644 --- a/tools/plot_lighthouse/glutil.h +++ b/tools/plot_lighthouse/glutil.h @@ -10,7 +10,8 @@ #define glutil_h #include -#include "quaternion.h" +//#include "quaternion.h" +#include "linmath.h" #include #ifdef __APPLE__ -- cgit v1.2.3 From b7ce4ea28975b053f3973da551716c12b75940db Mon Sep 17 00:00:00 2001 From: ultramn Date: Sun, 12 Feb 2017 15:01:53 -0800 Subject: this wasn't committed for some reason --- tools/plot_lighthouse/glutil.c | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/tools/plot_lighthouse/glutil.c b/tools/plot_lighthouse/glutil.c index dd022a0..99c15f7 100644 --- a/tools/plot_lighthouse/glutil.c +++ b/tools/plot_lighthouse/glutil.c @@ -50,24 +50,23 @@ void DrawCoordinateSystem( float x, float y, float z, float qx, float qy, float qz, float qr) { - Quaternion i0,j0,k0; - Quaternion i, j, k; - Quaternion q; - - // Calculate the i, j, and k vectors - QuaternionSet(i0, 1, 0, 0, 0); - QuaternionSet(j0, 0, 1, 0, 0); - QuaternionSet(k0, 0, 0, 1, 0); - QuaternionSet(q, qx, qy, qz, qr); - QuaternionRot(i, q, i0); - QuaternionRot(j, q, j0); - QuaternionRot(k, q, k0); + FLT i0[3],j0[3],k0[3]; + FLT i[3],j[3],k[3]; + FLT q[4]; + + i0[0]=1.0; i0[1]=0.0; i0[2]=0.0; + j0[0]=0.0; j0[1]=1.0; j0[2]=0.0; + k0[0]=0.0; k0[1]=0.0; k0[2]=1.0; + q [0]=qr; q [1]=qx; q [2]=qy; q [3]=qz; - // Draw the coordinate system i red, j green, k blue + quatrotatevector(i, q, i0); + quatrotatevector(j, q, j0); + quatrotatevector(k, q, k0); + glBegin(GL_LINES); - glColor3f(1, 0, 0); glVertex3f(x,z,y); glVertex3f(x+i.i,z+i.k,y+i.j); - glColor3f(0, 1, 0); glVertex3f(x,z,y); glVertex3f(x+j.i,z+j.k,y+j.j); - glColor3f(0, 0, 1); glVertex3f(x,z,y); glVertex3f(x+k.i,z+k.k,y+k.j); + glColor3f(1, 0, 0); glVertex3f(x,z,y); glVertex3f(x+i[0],z+i[2],y+i[1]); + glColor3f(0, 1, 0); glVertex3f(x,z,y); glVertex3f(x+j[0],z+j[2],y+j[1]); + glColor3f(0, 0, 1); glVertex3f(x,z,y); glVertex3f(x+k[0],z+k[2],y+k[1]); glEnd(); } -- cgit v1.2.3 From 488c5e9bdf21909564e50ef59aff544917a9758c Mon Sep 17 00:00:00 2001 From: ultramn Date: Sun, 12 Feb 2017 15:02:19 -0800 Subject: I don't think this one changed, but commit anyway --- tools/plot_lighthouse/fileutil.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tools/plot_lighthouse/fileutil.h b/tools/plot_lighthouse/fileutil.h index 714fbbd..e5da244 100644 --- a/tools/plot_lighthouse/fileutil.h +++ b/tools/plot_lighthouse/fileutil.h @@ -2,7 +2,7 @@ #define _fileutil_h_ #include -#include "../../redist/os_generic.h" +#include "os_generic.h" void LoadLighthousePos( const char *path, @@ -10,7 +10,8 @@ void LoadLighthousePos( float *qi, float *qj, float *qk, float *qreal); -#define NUM_HMD 32 +// first 32 are hmd, next 24 wm0 next 24 wm1 +#define NUM_HMD 80 #define NUM_SWEEP 4 #define SWEEP_LX 0 #define SWEEP_LY 1 -- cgit v1.2.3 From ee1ec597c5dcb851be8b72e1458bb70fc6cb96a9 Mon Sep 17 00:00:00 2001 From: ultramn Date: Sun, 12 Feb 2017 15:02:40 -0800 Subject: I don't think this changed, but commit anyway --- tools/plot_lighthouse/fileutil.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tools/plot_lighthouse/fileutil.c b/tools/plot_lighthouse/fileutil.c index f892798..b2c8f64 100644 --- a/tools/plot_lighthouse/fileutil.c +++ b/tools/plot_lighthouse/fileutil.c @@ -110,7 +110,10 @@ void *ThreadReadHmtAngles(void *junk) if ( xy[0] =='Y' ) { sweepId++; } double angle = (PI / 400000.0) * ( (double)timeInSweep-200000.0 ); - if ( strcmp(hmd,"HMD")!=0 ) { continue; } + if ( strcmp(hmd,"HMD")==0 ) { id += 0; } + else if ( strcmp(hmd,"WM0")==0 ) { id += 32; } + else if ( strcmp(hmd,"WM1")==0 ) { id += 56; } + else { continue; } if ( id<0 || id >NUM_HMD) { continue; } -- cgit v1.2.3 From 1ffadeda2fac023741c9a828714bb4ac29fcd815 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 12 Feb 2017 23:24:17 -0500 Subject: Working on closing #15 - still needs work. --- include/survive.h | 4 +++- src/survive_data.c | 70 +++++++++++++++++++++++++++++++++++++++++------------- 2 files changed, 56 insertions(+), 18 deletions(-) diff --git a/include/survive.h b/include/survive.h index d29b487..7d461c7 100644 --- a/include/survive.h +++ b/include/survive.h @@ -35,7 +35,9 @@ struct SurviveObject #ifdef USE_OLD_DISAMBIGUATOR uint32_t last_master_time; uint32_t last_slave_time; - int32_t last_photo_length; + int16_t last_master_length; + int16_t last_slave_length; + int8_t is_on_slave; #else uint32_t last_master_time; struct disambiguator * d; diff --git a/src/survive_data.c b/src/survive_data.c index d380d4a..808c891 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -62,38 +62,71 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * if( le->length > 2200 ) //Pulse longer indicates a sync pulse. { int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; - if( deltat > 2000 || deltat < -2000 ) //New pulse. (may be inverted) + // if( so->codename[0] == 'W' ) + + if( deltat > 2000 ) { - //See if this is a unique pulse, or another one in the same set we need to look at. - if( le->timestamp - so->last_master_time > 1500 + so->last_photo_length ) + //YUGH!! This is really ugly, should refactor. + + if( so->is_on_slave ) { - // check if it is a slave pulse - if (le->timestamp - so->last_master_time < 70000) { - so->last_slave_time = le->timestamp; - return; + //Last was a slave. If new pulse, switch back to master mode. + if( le->timestamp - so->last_slave_time > 1500 + so->last_slave_length ) + { + so->is_on_slave = 0; + so->last_master_time = le->timestamp; + so->last_master_length = le->length; + ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat ); + } + } + else + { + //See if this is a unique pulse, or another one in the same set we need to look at. + if( le->timestamp - so->last_master_time > 1500 + so->last_master_length ) + { + // check if it is a slave pulse + if (le->timestamp - so->last_master_time < 70000) { + so->last_slave_time = le->timestamp; + so->last_slave_length = le->length; + so->is_on_slave = 1; + return; + } + + so->is_on_slave = 0; + so->last_master_time = le->timestamp; + so->last_master_length = le->length; + ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat ); + deltat = 0; } - -// printf("%10u %10u %6u %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time); - so->last_master_time = le->timestamp; - so->last_photo_length = le->length; - ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat ); - deltat = 0; } } //Find longest pulse-length from device in our window and use that one. - if( le->length > so->last_photo_length ) + if( so->is_on_slave ) { // printf("%10u %10u %6d %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time); - so->last_master_time = le->timestamp; - so->last_photo_length = le->length; + + if( le->length > so->last_slave_length ) + { + so->last_slave_time = le->timestamp; + so->last_slave_length = le->length; + } + } + else + { + if( le->length > so->last_master_length ) + { + so->last_master_time = le->timestamp; + so->last_master_length = le->length; + } } } + //See if this is a valid actual pulse. else if( le->length < 1800 && le->length > 40 && ( le->timestamp - so->last_master_time < 380000 ) ) { int32_t dl = so->last_master_time; - int32_t tpco = so->last_photo_length; + int32_t tpco = so->last_master_length; //Adding length //Long pulse-code from IR flood. @@ -103,8 +136,11 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * acode >>= 1; acode -= 6; + if (acode > 3) { + return; dl = so->last_slave_time; + tpco = so->last_slave_length; } //printf( "%s / %d %d ++ %d %d\n", so->codename, dl, tpco, offset_from, acode ); -- cgit v1.2.3 From ff7167fb8afff6633db422290864e5da302d9afb Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 13 Feb 2017 01:16:05 -0500 Subject: Update disambiguator. I think I got it right this time, finally. Also, seems to output OOTX data pretty good. --- Makefile | 4 +- include/survive.h | 15 ++-- src/disambiguator.c | 197 ---------------------------------------------------- src/disambiguator.h | 68 ------------------ src/survive.c | 13 +--- src/survive_data.c | 181 ++++++++++++++++++++++++++--------------------- 6 files changed, 109 insertions(+), 369 deletions(-) delete mode 100644 src/disambiguator.c delete mode 100644 src/disambiguator.h diff --git a/Makefile b/Makefile index 8279efa..dbd4869 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ all : lib data_recorder test -CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto -DUSE_OLD_DISAMBIGUATOR +CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -flto test : test.c lib/libsurvive.so redist/os_generic.o @@ -12,7 +12,7 @@ data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/Dra lib: mkdir lib -lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/disambiguator.c redist/jsmn.o $(DEBUGSTUFF) +lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF) gcc -o $@ $^ $(LDFLAGS) -shared clean : diff --git a/include/survive.h b/include/survive.h index 7d461c7..d2f5bfe 100644 --- a/include/survive.h +++ b/include/survive.h @@ -32,16 +32,11 @@ struct SurviveObject //Flood info, for calculating which laser is currently sweeping. int8_t oldcode; - #ifdef USE_OLD_DISAMBIGUATOR - uint32_t last_master_time; - uint32_t last_slave_time; - int16_t last_master_length; - int16_t last_slave_length; - int8_t is_on_slave; - #else - uint32_t last_master_time; - struct disambiguator * d; - #endif + uint32_t last_time[2]; //0 = master, 1 = slave. Hardcoded, because it cannot simply be expanded. + uint32_t last_length[2]; + int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. Possibly more lighthouses??? + int8_t did_handle_ootx; + uint32_t recent_sync_time; uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device. diff --git a/src/disambiguator.c b/src/disambiguator.c deleted file mode 100644 index f1d310a..0000000 --- a/src/disambiguator.c +++ /dev/null @@ -1,197 +0,0 @@ -// (C) 2016 Julian Picht, MIT/x11 License. -// -// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. - -// -// The theory behind this disambiguator is, that if we just track all pulses and if one could be a sync pulse, we look back in time, -// if we saw as sync pulse X samples ago than it is probably a sync pulse. -// -// X can be 20000 or 400000, depending if it came from the master or the slave. -// - -#include "disambiguator.h" -#include -#include -#include -#include - -typedef uint8_t pulse_data; - -/** - * Translate pulse length to pulse SKIP, DATA, AXIS - * @param length Length of the pulse in (1/48000000) seconds - * @return pulse data - */ -pulse_data get_pulse_data(uint32_t length) { - uint16_t temp = length - 2880; - -#if BETTER_SAFE_THAN_FAST - if (temp < 0 || length > 6525) { - return -1; - } -#endif - - if ((temp % 500) < 150) { - return temp / 500; - } - - return -1; -} - -const uint32_t pulse_types[] = { - 0, 1, 0, 1, - 2, 3, 2, 3, -}; - -#define PULSE_BIT_AXIS 0x1 -#define PULSE_BIT_DATA 0x2 -#define PULSE_BIT_SKIP 0x4 - -#define PULSE_DATA(D) ((D >> 1)&0x1) -#define PULSE_AXIS(D) (D&0x01) -#define PULSE_SKIP(D) ((D >> 2)&0x1) - -void disambiguator_init( struct disambiguator * d ) { - memset(&(d->times), 0x0, sizeof(d->times)); - memset(&(d->scores), 0x0, sizeof(d->scores)); - - d->state = D_STATE_UNLOCKED; - d->last = 0; - d->max_confidence = 0; -} - -inline void disambiguator_discard( struct disambiguator * d ); - -/** - * Drop all data that is outdated - * @param d - * @param age Maximum age of data we care to keep - */ -void disambiguator_discard( struct disambiguator * d ) -{ - long age; - if (d->state == D_STATE_LOCKED) { - age = d->last - 400000; - } else { - age = 1000000; - } - int confidence = 0; - for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { - if (d->times[i] != 0 && d->times[i] < age) { - d->times[i] = 0; - d->scores[i] = 0; - } else { - if (d->scores[i] > confidence) { - confidence = d->scores[i]; - } - } - } - d->max_confidence = confidence; -} - -/** - * Find the index that has the best likelyhood too match up with the timestamp given - * @param t1 Rising edge time, where we expect to find the last sync pulse, if this is a master pulse - * @param t2 Rising edge time, where we expect to find the last sync pulse, if this is a slave pulse - * @param max_diff Maximum difference we are prepared to accept - * @return index inside d->times, if we found something, -1 otherwise - */ -inline int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, int max_diff ); - -int disambiguator_find_nearest( struct disambiguator * d, uint32_t t1, uint32_t t2, int max_diff ) -{ - int diff = max_diff; // max allowed diff for a match - int idx = -1; - for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) { - if (d->times[i] == 0) continue; - - int a_1 = abs(d->times[i] - t1); - int a_2 = abs(d->times[i] - t2); - -// printf("T %d %d %d\n", time, i, a); - if (a_1 < diff) { - idx = i; - diff = a_1; - } else if (a_2 < diff) { - idx = i; - diff = a_2; - } - } - -// if (idx != -1) { -// printf("R %d %d %d\n", idx, d->scores[idx], diff); -// } - - return idx; -} - -pulse_type disambiguator_step_return_helper( struct disambiguator * d, bool sweep_possible ) { - if (d->state == D_STATE_LOCKED && sweep_possible) { - return P_SWEEP; - } - return P_UNKNOWN; -} - -pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length) -{ - uint32_t diff = time - d->last; - bool sweep_possible = (diff > 70000 && diff < 350000); - - // all smaller pulses are most probably sweeps - // TODO: check we are inside the time window of actual sweeps - if (length < 2750) { - return disambiguator_step_return_helper(d, sweep_possible); - } - - // we expected to see a sync pulse earlier ... - if (time - d->last > 401000) { - d->state = D_STATE_UNLOCKED; - } - - // discard all data, that is so old, we don't care about it anymore - disambiguator_discard(d); - - // find the best match for our timestamp and presumed offset - int idx = disambiguator_find_nearest(d, time - 400000, time - 20000, 1000); - - // We did not find a matching pulse, so try find a place to record the current - // one's time of arrival. - if (idx == -1) { - for (int i = 0; i < DIS_NUM_VALUES; ++i) { - if (d->times[i] == 0) { - d->times[i] = time; - break; - } - } - - return d->state == D_STATE_LOCKED && sweep_possible ? P_SWEEP : P_UNKNOWN; - } else { - d->scores[idx]++; - - // we need to be reasonably sure, that we have the right pulses - if (d->scores[idx] >= DIS_NUM_PULSES_BEFORE_LOCK) { - d->state = D_STATE_LOCKED; - } - - // if the offset is about 20000 ticks, then this is a slave pulse - if (diff < 21000) { - if (d->state == D_STATE_LOCKED) { - return P_SLAVE; - } - - return P_UNKNOWN; - } - - d->times[idx] = time; - d->last = time; - - // TODO: why do we need to check the confidence level here? - if (d->state == D_STATE_LOCKED && d->scores[idx] >= d->max_confidence) { - return P_MASTER; - } - - return P_UNKNOWN; - } - - return disambiguator_step_return_helper(d, sweep_possible); -} diff --git a/src/disambiguator.h b/src/disambiguator.h deleted file mode 100644 index 8258a18..0000000 --- a/src/disambiguator.h +++ /dev/null @@ -1,68 +0,0 @@ -// (C) 2016 Julian Picht, MIT/x11 License. -// -//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. -#ifndef DISAMBIGUATOR_H -#define DISAMBIGUATOR_H - -// Determines the number of samples stored in the disambiguator struct. -// Has to be higher than the maximum number of pulses expected between sync pulses. -#define DIS_NUM_VALUES 48 -#define DIS_NUM_PULSES_BEFORE_LOCK 30 -#include - -/** - * internal disambiguator state - */ -typedef enum { - D_STATE_INVALID = 0, - D_STATE_LOCKED = 1, - D_STATE_UNLOCKED = -1, -} dis_state; - -/** - * classification result - */ -typedef enum { - P_UNKNOWN = 0, - P_MASTER = 1, - P_SWEEP = 2, - P_SLAVE = 3, -} pulse_type; - -/** - * internal state of the disambiguator - */ -struct disambiguator { - // the timestamps of the recorded pulses - uint32_t times[DIS_NUM_VALUES]; - // countes how many sync pulses we have seen, that match the time offset at the same offset - uint16_t scores[DIS_NUM_VALUES]; - // current state - dis_state state; - // last sync pulse time - uint32_t last; - // the absolute maximum counter value - int max_confidence; - // the last code type seen - char code; -}; - - -/** - * Initialize a new disambiguator. calloc or memset with 0x00 will work just as well. - * - * @param d Pointer to the struct - */ -void disambiguator_init( struct disambiguator * d); - -/** - * Feed in one pulse to have if classified. - * - * @param d Pointer to disambiguator state - * @param time Rising edge of the pulse - * @param length Length of the pulse - * @return Classification result - */ -pulse_type disambiguator_step( struct disambiguator * d, uint32_t time, int length); - -#endif /* DISAMBIGUATOR_H */ \ No newline at end of file diff --git a/src/survive.c b/src/survive.c index af4d804..ed18da2 100644 --- a/src/survive.c +++ b/src/survive.c @@ -8,7 +8,7 @@ #include #include #include -#include "disambiguator.h" + static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start && @@ -147,21 +147,10 @@ struct SurviveContext * survive_init() ctx->headset.ctx = ctx; memcpy( ctx->headset.codename, "HMD", 4 ); -#ifndef USE_OLD_DISAMBIGUATOR - ctx->headset.d = calloc( 1, sizeof( struct disambiguator ) ); -#endif - ctx->watchman[0].ctx = ctx; memcpy( ctx->watchman[0].codename, "WM0", 4 ); -#ifndef USE_OLD_DISAMBIGUATOR - ctx->watchman[0].d = calloc( 1, sizeof( struct disambiguator ) ); -#endif - ctx->watchman[1].ctx = ctx; memcpy( ctx->watchman[1].codename, "WM1", 4 ); -#ifndef USE_OLD_DISAMBIGUATOR - ctx->watchman[1].d = calloc( 1, sizeof( struct disambiguator ) ); -#endif //USB must happen last. if( r = survive_usb_init( ctx ) ) diff --git a/src/survive_data.c b/src/survive_data.c index 808c891..afda235 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -11,7 +11,6 @@ //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. #include "survive_internal.h" -#include "disambiguator.h" #include #include @@ -32,125 +31,148 @@ struct LightcapElement //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le ) { - struct SurviveContext * ct = so->ctx; - int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; + struct SurviveContext * ctx = so->ctx; + //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; // printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl ); so->tsl = le->timestamp; if( le->length < 20 ) return; -#ifndef USE_OLD_DISAMBIGUATOR - int32_t offset = le->timestamp - so->d->last; - switch( disambiguator_step( so->d, le->timestamp, le->length ) ) { - default: - case P_SLAVE: - // this is only interesting for the OOTX data - break; - case P_UNKNOWN: - // not currently locked - break; - case P_MASTER: - ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset ); - so->d->code = ((le->length+125)/250) - 12; - break; - case P_SWEEP: - if (so->d->code & 1) return; - ct->lightproc( so, le->sensor_id, so->d->code >> 1, offset, le->timestamp, le->length ); - break; + + //The sync pulse finder is taking Charles's old disambiguator code and mixing it with a more linear + //version of Julian Picht's disambiguator, available in 488c5e9. Removed afterwards into this + //unified driver. + + + int ssn = so->sync_set_number; + if( ssn < 0 ) ssn = 0; + int last_sync_time = so->last_time [ssn]; + int last_sync_length = so->last_length[ssn]; + int32_t delta = le->timestamp - last_sync_time; //Handle time wrapping (be sure to be int32) + + if( delta < -500000 || delta > 500000 ) + { + //Reset pulse, etc. + so->sync_set_number = -1; + delta = 500000; } -#else + + if( le->length > 2200 ) //Pulse longer indicates a sync pulse. { - int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; - // if( so->codename[0] == 'W' ) + int is_new_pulse = delta > 1500 + last_sync_length; + + so->did_handle_ootx = 0; - if( deltat > 2000 ) + if( is_new_pulse ) { - //YUGH!! This is really ugly, should refactor. + int is_master_sync_pulse = delta > 40000; - if( so->is_on_slave ) + if( is_master_sync_pulse ) { - //Last was a slave. If new pulse, switch back to master mode. - if( le->timestamp - so->last_slave_time > 1500 + so->last_slave_length ) - { - so->is_on_slave = 0; - so->last_master_time = le->timestamp; - so->last_master_length = le->length; - ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat ); - } + ssn = so->sync_set_number = 0; + so->last_time[ssn] = le->timestamp; + so->last_length[ssn] = le->length; + } + else if( so->sync_set_number == -1 ) + { + //Do nothing. } else { - //See if this is a unique pulse, or another one in the same set we need to look at. - if( le->timestamp - so->last_master_time > 1500 + so->last_master_length ) + ssn = ++so->sync_set_number; + if( so->sync_set_number > 1 ) { - // check if it is a slave pulse - if (le->timestamp - so->last_master_time < 70000) { - so->last_slave_time = le->timestamp; - so->last_slave_length = le->length; - so->is_on_slave = 1; - return; - } - - so->is_on_slave = 0; - so->last_master_time = le->timestamp; - so->last_master_length = le->length; - ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat ); - deltat = 0; + SV_INFO( "Warning. Received an extra, unassociated sync pulse." ); + ssn = so->sync_set_number = -1; + } + else + { + so->last_time[ssn] = le->timestamp; + so->last_length[ssn] = le->length; } - } - } - - //Find longest pulse-length from device in our window and use that one. - if( so->is_on_slave ) - { -// printf("%10u %10u %6d %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time); - - if( le->length > so->last_slave_length ) - { - so->last_slave_time = le->timestamp; - so->last_slave_length = le->length; } } else { - if( le->length > so->last_master_length ) + //Find the longest pulse. + if( le->length > last_sync_length ) { - so->last_master_time = le->timestamp; - so->last_master_length = le->length; + if( so->last_time[ssn] > le->timestamp ) + { + so->last_time[ssn] = le->timestamp; + so->last_length[ssn] = le->length; + } } } } - //See if this is a valid actual pulse. - else if( le->length < 1800 && le->length > 40 && ( le->timestamp - so->last_master_time < 380000 ) ) + else if( le->length < 1800 && le->length > 40 && delta > 30000 && ssn >= 0 ) { - int32_t dl = so->last_master_time; - int32_t tpco = so->last_master_length; + int32_t dl = so->last_time[0]; + int32_t tpco = so->last_length[0]; //Adding length //Long pulse-code from IR flood. //Make sure it fits nicely into a divisible-by-500 time. - int32_t acode = (tpco+125+50)/250; //+10, seems ike that's - if( acode & 1 ) return; + int32_t acode_array[2] = + { + (so->last_length[0]+125+50)/250, + (so->last_length[1]+125+50)/250, + }; - acode >>= 1; - acode -= 6; + //XXX: TODO: Capture error count here. + if( acode_array[0] & 1 ) return; + if( acode_array[1] & 1 ) return; - if (acode > 3) { - return; - dl = so->last_slave_time; - tpco = so->last_slave_length; + acode_array[0] = (acode_array[0]>>1) - 6; + acode_array[1] = (acode_array[1]>>1) - 6; + + int acode = acode_array[0]; + + if( !so->did_handle_ootx ) + { + int32_t delta1 = so->last_time[0] - so->recent_sync_time; + int32_t delta2 = so->last_time[1] - so->last_time[0]; + + ctx->lightproc( so, -1, acode_array[0], delta1, so->last_time[0], so->last_length[0] ); + ctx->lightproc( so, -2, acode_array[1], delta2, so->last_time[1], so->last_length[1] ); + so->recent_sync_time = so->last_time[1]; + + //Throw out everything if our sync pulses look like they're bad. + if( delta1 < 375000 || delta1 > 385000 ) + { + //XXX: TODO: Count faults. + so->sync_set_number = -1; + return; + } + + if( delta2 < 15000 || delta2 > 25000 ) + { + //XXX: TODO: Count faults. + so->sync_set_number = -1; + return; + } + + so->did_handle_ootx = 1; } - //printf( "%s / %d %d ++ %d %d\n", so->codename, dl, tpco, offset_from, acode ); + + if (acode > 3) { + if( ssn == 0 ) + { + SV_INFO( "Warning: got a slave marker but only got a master sync." ); + } + dl = so->last_time[1]; + tpco = so->last_length[1]; + } int32_t offset_from = le->timestamp - dl + le->length/2; //Make sure pulse is in valid window if( offset_from < 380000 && offset_from > 70000 ) { - ct->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); } } else @@ -158,7 +180,6 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * //printf( "FAIL %d %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - le->timestamp ); //Runt pulse, or no sync pulses available. } -#endif } -- cgit v1.2.3 From ee433e4a076f77fa28e4d55d6703d6ed7c599251 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 13 Feb 2017 01:44:53 -0500 Subject: Closes #15 Closes #4 These are now rock solid. Prepare the way for the OOTX Data. Gonna hav eto get @axlecrusher on it. --- src/survive_data.c | 2 ++ tools/plot_lighthouse/Makefile | 4 ++-- tools/plot_lighthouse/main.c | 5 +++++ tools/process_rawcap/process_to_points.c | 3 ++- 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index afda235..b79f96e 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -135,8 +135,10 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * int32_t delta1 = so->last_time[0] - so->recent_sync_time; int32_t delta2 = so->last_time[1] - so->last_time[0]; + //XXX Axlecrusher -> Add your code here!!! ctx->lightproc( so, -1, acode_array[0], delta1, so->last_time[0], so->last_length[0] ); ctx->lightproc( so, -2, acode_array[1], delta2, so->last_time[1], so->last_length[1] ); + so->recent_sync_time = so->last_time[1]; //Throw out everything if our sync pulses look like they're bad. diff --git a/tools/plot_lighthouse/Makefile b/tools/plot_lighthouse/Makefile index d156bdb..db382ea 100644 --- a/tools/plot_lighthouse/Makefile +++ b/tools/plot_lighthouse/Makefile @@ -1,7 +1,7 @@ UNAME := $(shell uname) ifeq ($(UNAME), Linux) -CFLAGS:= -lGL -lGLU -lglut +CFLAGS:= -lGL -lGLU -lglut -I../../redist -DLINUX -lm -lpthread endif # Darwin is Mac OSX !! @@ -10,6 +10,6 @@ CFLAGS:= -w -framework OpenGL -framework GLUT endif all: - gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c $(CFLAGS) + gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c ../../redist/linmath.c $(CFLAGS) clean: rm -f plot_lighthouse diff --git a/tools/plot_lighthouse/main.c b/tools/plot_lighthouse/main.c index 8088828..4a64c28 100644 --- a/tools/plot_lighthouse/main.c +++ b/tools/plot_lighthouse/main.c @@ -13,6 +13,11 @@ #include "glutil.h" #include "fileutil.h" +#ifdef LINUX +#include +#endif + + // Required to set up a window #define WIDTH 800 #define HEIGHT 600 diff --git a/tools/process_rawcap/process_to_points.c b/tools/process_rawcap/process_to_points.c index a1e835d..3ccfcc1 100644 --- a/tools/process_rawcap/process_to_points.c +++ b/tools/process_rawcap/process_to_points.c @@ -192,7 +192,8 @@ int main( int argc, char ** argv ) stddevtim += Sdiff * Sdiff; stddevlen += Ldiff * Ldiff; - Sdiff/=4; + //Cast a wider net for the histogram. + //Sdiff/=4; int llm = Sdiff + (HISTOGRAMSIZE/2.0); if( llm < 0 ) llm = 0; -- cgit v1.2.3