aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile4
-rw-r--r--include/survive.h13
-rw-r--r--src/disambiguator.c197
-rw-r--r--src/disambiguator.h68
-rw-r--r--src/survive.c13
-rw-r--r--src/survive_data.c167
-rw-r--r--tools/plot_lighthouse/Makefile4
-rw-r--r--tools/plot_lighthouse/fileutil.c5
-rw-r--r--tools/plot_lighthouse/fileutil.h5
-rw-r--r--tools/plot_lighthouse/glutil.c31
-rw-r--r--tools/plot_lighthouse/glutil.h3
-rw-r--r--tools/plot_lighthouse/main.c5
-rw-r--r--tools/process_rawcap/process_to_points.c3
13 files changed, 154 insertions, 364 deletions
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 d29b487..d2f5bfe 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -32,14 +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;
- int32_t last_photo_length;
- #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 <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <stdbool.h>
-
-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 <stdint.h>
-
-/**
- * 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 <jsmn.h>
#include <string.h>
#include <zlib.h>
-#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 d380d4a..b79f96e 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 <stdint.h>
#include <string.h>
@@ -32,89 +31,150 @@ 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( deltat > 2000 || deltat < -2000 ) //New pulse. (may be inverted)
+ int is_new_pulse = delta > 1500 + last_sync_length;
+
+ so->did_handle_ootx = 0;
+
+ if( is_new_pulse )
{
- //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 )
+ int is_master_sync_pulse = delta > 40000;
+
+ if( is_master_sync_pulse )
+ {
+ 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 )
{
- // check if it is a slave pulse
- if (le->timestamp - so->last_master_time < 70000) {
- so->last_slave_time = le->timestamp;
- return;
+ //Do nothing.
+ }
+ else
+ {
+ ssn = ++so->sync_set_number;
+ if( so->sync_set_number > 1 )
+ {
+ 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;
}
-
-// 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 )
+ else
{
-// 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;
+ //Find the longest pulse.
+ if( le->length > last_sync_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_photo_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;
- if (acode > 3) {
- dl = so->last_slave_time;
+ //XXX: TODO: Capture error count here.
+ if( acode_array[0] & 1 ) return;
+ if( acode_array[1] & 1 ) return;
+
+ 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];
+
+ //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.
+ 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
@@ -122,7 +182,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
}
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/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; }
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 <pthread.h>
-#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
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();
}
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 <stdio.h>
-#include "quaternion.h"
+//#include "quaternion.h"
+#include "linmath.h"
#include <stdio.h>
#ifdef __APPLE__
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 <GL/freeglut.h>
+#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;