aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--81-vive.rules13
-rw-r--r--calibrate.c1
-rw-r--r--src/survive_cal.c231
-rw-r--r--src/survive_cal.h10
-rw-r--r--src/survive_data.c3
5 files changed, 230 insertions, 28 deletions
diff --git a/81-vive.rules b/81-vive.rules
new file mode 100644
index 0000000..d317e91
--- /dev/null
+++ b/81-vive.rules
@@ -0,0 +1,13 @@
+#libsurvive
+
+SUBSYSTEM=="usb", ATTR{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC HMD
+SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Light input
+SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Watchman
+
+#SUBSYSTEM=="usb", ATTR{idVendor}=="abcd", GROUP="adm", MODE="0666"
+#SUBSYSTEM=="usb", ATTR{idProduct}=="f003", ATTRS{idVendor}=="abcd", MODE="0666", GROUP="colorchord"
+# OpenVR (SteamVR) / OSVR-HTC-Vive
+
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Valve
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Valve
diff --git a/calibrate.c b/calibrate.c
index 60f4316..04ea9a8 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -7,6 +7,7 @@
#include <survive.h>
#include <string.h>
#include <os_generic.h>
+#include "src/survive_cal.h"
#include <DrawFunctions.h>
struct SurviveContext * ctx;
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 7b0a824..d4c3447 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -2,14 +2,23 @@
// (C) 2016, 2017 <>< C. N. Lohr, Under MIT/x11 License.
// All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
+//
+// This file is primarily geared to the calibration phase, to produce the world cal information.
+// Once world cal is produced, it's unlikely you will need this file at all. The plan is
+// to not include it at all on any stripped-down versions of libsurvive.
+//
#include "survive_cal.h"
#include "survive_internal.h"
#include <math.h>
#include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
#define PTS_BEFORE_COMMON 32
-#define NEEDED_COMMON_POINTS 20
+#define NEEDED_COMMON_POINTS 10
+#define NEEDED_TIMES_OF_COMMON 5
+#define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4))
static void handle_calibration( struct SurviveCalData *cd );
static void reset_calibration( struct SurviveCalData * cd );
@@ -49,20 +58,21 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
switch( cd->stage )
{
case 0:
- return snprintf( description, description_length, "Not calibrating" );
+ return snprintf( description, description_length, "0 Not calibrating" );
case 1:
- return snprintf( description, description_length, "Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
+ return snprintf( description, description_length, "1 Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
case 2:
+ case 3:
if( cd->found_common )
{
- return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+ return snprintf( description, description_length, "%d Collecting Sweep Data %d/%d", cd->stage, cd->peak_counts, DRPTS );
}
else
{
- return snprintf( description, description_length, "Searching for common watchman cal %d/%d", cd->peak_counts, PTS_BEFORE_COMMON );
+ return snprintf( description, description_length, "%d Searching for common watchman cal %d/%d (%d/%d)", cd->stage, cd->peak_counts, PTS_BEFORE_COMMON, cd->times_found_common, NEEDED_TIMES_OF_COMMON );
}
default:
- return snprintf( description, description_length, "Unkown calibration state" );
+ return snprintf( description, description_length, "%d Unkown calibration state", cd->stage );
}
}
@@ -79,6 +89,7 @@ void survive_cal_install( struct SurviveContext * ctx )
}
cd->stage = 1;
+ cd->ctx = ctx;
ootx_packet_clbk = ootx_packet_clbk_d;
@@ -127,6 +138,17 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
if( !cd ) return;
+ int sensid = sensor_id;
+ if( strcmp( so->codename, "WM0" ) == 0 )
+ sensid += 32;
+ if( strcmp( so->codename, "WM1" ) == 1 )
+ sensid += 64;
+
+ if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return;
+
+ int lighthouse = acode>>2;
+ int axis = acode & 1;
+
switch( cd->stage )
{
default:
@@ -135,32 +157,21 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
break;
case 2:
{
- int sensid = sensor_id;
- if( strcmp( so->codename, "WM0" ) == 0 )
- sensid += 32;
- if( strcmp( so->codename, "WM1" ) == 1 )
- sensid += 64;
-
- int lighthouse = acode>>2;
- int axis = acode & 1;
int ct = cd->all_counts[sensid][lighthouse][axis]++;
cd->all_lengths[sensid][lighthouse][axis][ct] = length;
cd->all_angles[sensid][lighthouse][axis][ct] = angle;
if( ct > cd->peak_counts )
{
cd->peak_counts = ct;
- if( ct >= DRPTS )
- handle_calibration( cd ); //This will also reset all cals.
}
- //TODO: Determine if there is a sensor on a watchman visible from both lighthouses.
- if( sensid >= 32 && !cd->found_common )
+ //Determine if there is a sensor on a watchman visible from both lighthouses.
+ if( sensid >= 32 )
{
int k;
int ok = 1;
for( k = 0; k < NUM_LIGHTHOUSES; k++ )
{
-
if( cd->all_counts[sensid][k][0] < NEEDED_COMMON_POINTS || cd->all_counts[sensid][k][1] < NEEDED_COMMON_POINTS )
{
ok = 0;
@@ -170,13 +181,45 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
if( ok ) cd->found_common = 1;
}
- if( cd->peak_counts > PTS_BEFORE_COMMON && !cd->found_common )
+ if( cd->peak_counts >= PTS_BEFORE_COMMON )
{
- reset_calibration( cd );
- }
+ SV_INFO( "Stage 2 cal: %d %d %d\n", cd->peak_counts, cd->found_common, cd->times_found_common );
+ if( cd->found_common )
+ {
+ if( cd->times_found_common >= NEEDED_TIMES_OF_COMMON )
+ {
+ reset_calibration( cd );
+ cd->stage = 3;
+ cd->found_common = 1;
+ }
+ else
+ {
+ cd->times_found_common++;
+ reset_calibration( cd );
+ }
+ }
+ else
+ {
+ reset_calibration( cd );
+ cd->times_found_common = 0;
+ }
+ }
break;
}
+ case 3:
+ {
+ int ct = cd->all_counts[sensid][lighthouse][axis]++;
+ cd->all_lengths[sensid][lighthouse][axis][ct] = length;
+ cd->all_angles[sensid][lighthouse][axis][ct] = angle;
+ if( ct > cd->peak_counts )
+ {
+ cd->peak_counts = ct;
+ if( ct >= DRPTS )
+ handle_calibration( cd ); //This must also reset all cals.
+ }
+ break;
+ }
}
}
@@ -185,11 +228,153 @@ static void reset_calibration( struct SurviveCalData * cd )
memset( cd->all_counts, 0, sizeof( cd->all_counts ) );
cd->peak_counts = 0;
cd->found_common = 0;
+ cd->times_found_common = 0;
+ cd->stage = 2;
}
static void handle_calibration( struct SurviveCalData *cd )
{
- //Do stuff.
+ struct SurviveContext * ctx = cd->ctx;
+
+ //Either advance to stage 4 or go resetting will go back to stage 2.
+ //What is stage 4? Are we done then?
+
+ mkdir( "calinfo", 0666 );
+ FILE * hists = fopen( "calinfo/histograms.csv", "w" );
+ FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" );
+ int sen, axis, lh;
+ for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
+ for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+ for( axis = 0; axis < 2; axis++ )
+ {
+ int dpmax = cd->all_counts[sen][lh][axis];
+ if( dpmax < 50 ) continue;
+ int i;
+
+ FLT sumsweepangle = 0;
+ FLT sumlentime = 0;
+
+ //Find initial guess at average
+ for( i = 0; i < dpmax; i++ )
+ {
+ FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+ FLT datalen = cd->all_lengths[sen][lh][axis][i];
+ sumsweepangle += sweepangle;
+ sumlentime += datalen;
+ }
+
+ #define OUTLIER_ANGLE 0.01 //TODO: Tune
+ #define OUTLIER_LENGTH 0.01 //TODO: Tune
+ #define ANGLE_STDEV_TOO_HIGH 0.01 //TODO: Tune
+
+ FLT avgsweep = sumsweepangle / dpmax;
+ FLT avglen = sumlentime / dpmax;
+ int count = 0;
+
+ FLT max_outlier_angle = 0;
+ FLT max_outlier_length = 0;
+
+ //Get rid of outliers
+ for( i = 0; i < dpmax; i++ )
+ {
+ FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+ FLT datalen = cd->all_lengths[sen][lh][axis][i];
+ FLT Sdiff = sweepangle - avgsweep;
+ FLT Ldiff = datalen - avglen;
+ FLT Sdiff2 = Sdiff * Sdiff;
+ FLT Ldiff2 = Ldiff * Ldiff;
+
+ if( Sdiff2 > max_outlier_angle ) max_outlier_angle = Sdiff2;
+ if( Ldiff2 > max_outlier_length ) max_outlier_length = Ldiff2;
+
+ if( Sdiff2 > OUTLIER_ANGLE || Ldiff2 > OUTLIER_LENGTH )
+ {
+ cd->all_lengths[sen][lh][axis][i] = -1;
+ }
+ else
+ {
+ count++;
+ }
+ }
+
+ if( count < DRPTS_NEEDED_FOR_AVG )
+ {
+ //Not enough for this point to be considered.
+ continue;
+ }
+
+ sumsweepangle = 0;
+ sumlentime = 0;
+ //Redo, finding new average:
+ for( i = 0; i < dpmax; i++ )
+ {
+ FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+ FLT datalen = cd->all_lengths[sen][lh][axis][i];
+ if( datalen < 0 ) continue;
+ sumsweepangle += sweepangle;
+ sumlentime += datalen;
+ }
+
+ avgsweep = sumsweepangle / count;
+ avglen = sumlentime / count;
+
+ FLT stddevang = 0;
+ FLT stddevlen = 0;
+
+ #define HISTOGRAMSIZE 31
+ #define HISTOGRAMBINANG 0.001 //TODO: Tune
+
+ int histo[HISTOGRAMSIZE];
+ memset( histo, 0, sizeof( histo ) );
+ count = 0;
+
+ for( i = 0; i < dpmax; i++ )
+ {
+ FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+ FLT datalen = cd->all_lengths[sen][lh][axis][i];
+ if( datalen < 0 ) continue;
+
+ FLT Sdiff = sweepangle - avgsweep;
+ FLT Ldiff = datalen - avglen;
+ FLT Sdiff2 = Sdiff * Sdiff;
+ FLT Ldiff2 = Ldiff * Ldiff;
+
+ stddevang += Sdiff2;
+ stddevlen += Ldiff2;
+
+ int llm = Sdiff / HISTOGRAMBINANG + (HISTOGRAMSIZE/2.0);
+ if( llm < 0 ) llm = 0;
+ if( llm >= HISTOGRAMSIZE ) llm = HISTOGRAMSIZE-1;
+
+ histo[llm]++;
+ }
+
+ stddevang /= count;
+ stddevlen /= count;
+
+ if( stddevang > ANGLE_STDEV_TOO_HIGH )
+ {
+ SV_INFO( "DROPPED: %02d dropped because stddev (%f) was too high.\n", sen, stddevang );
+ continue;
+ }
+
+ fprintf( hists, "%02d, ", sen );
+
+ for( i = 0; i < HISTOGRAMSIZE; i++ )
+ {
+ fprintf( hists, "%d ", histo[i] );
+ }
+ fprintf( hists, "\n" );
+
+ fprintf( ptinfo, "%d %d %f %f %f %f %f %f\n", sen, count, avgsweep, avglen, stddevang, stddevang, max_outlier_length, max_outlier_angle );
+ }
+ fclose( hists );
+ fclose( ptinfo );
+ //XXX TODO More
reset_calibration( cd );
}
+
+
+
+
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 42ff1ee..dd69b57 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -30,20 +30,22 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
-#define MAX_TO_CAL 96
+#define MAX_SENSORS_TO_CAL 96
#define DRPTS 512
struct SurviveCalData
{
+ struct SurviveContext * ctx;
//OOTX Data is sync'd off of the sync pulses coming from the lighthouses.
ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
//For statistics-gathering phase.
- FLT all_lengths[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
- FLT all_angles[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
- int16_t all_counts[MAX_TO_CAL][NUM_LIGHTHOUSES][2];
+ FLT all_lengths[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
+ FLT all_angles[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
+ int16_t all_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2];
int16_t peak_counts;
int8_t found_common;
+ int8_t times_found_common;
//Stage:
// 0: Idle
diff --git a/src/survive_data.c b/src/survive_data.c
index ad834cf..402282d 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -177,7 +177,8 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
if (acode > 3) {
if( ssn == 0 )
{
- SV_INFO( "Warning: got a slave marker but only got a master sync." );
+ //SV_INFO( "Warning: got a slave marker but only got a master sync." );
+ //This happens too frequently. Consider further examination.
}
dl = so->last_time[1];
tpco = so->last_length[1];