aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile6
-rw-r--r--README.md1
-rw-r--r--calibrate.c12
-rw-r--r--calibrate_client.c29
-rw-r--r--data_recorder.c6
-rw-r--r--dave/AffineSolve.c322
-rw-r--r--include/libsurvive/poser.h56
-rw-r--r--include/libsurvive/survive.h166
-rw-r--r--include/libsurvive/survive_types.h44
-rw-r--r--include/survive.h102
-rw-r--r--redist/json_helpers.c172
-rw-r--r--redist/json_helpers.h20
-rw-r--r--redist/linmath.c7
-rw-r--r--src/survive.c58
-rw-r--r--src/survive_cal.c11
-rw-r--r--src/survive_cal.h16
-rw-r--r--src/survive_cal_lhfind.c16
-rw-r--r--src/survive_config.c393
-rw-r--r--src/survive_config.h48
-rw-r--r--src/survive_data.c13
-rw-r--r--src/survive_driverman.c2
-rw-r--r--src/survive_driverman.h38
-rw-r--r--src/survive_internal.h94
-rw-r--r--src/survive_process.c39
-rw-r--r--src/survive_vive.c120
-rw-r--r--tools/plot_lighthouse/Makefile2
26 files changed, 1331 insertions, 462 deletions
diff --git a/Makefile b/Makefile
index ffe4c77..3c8431b 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,6 @@
all : lib data_recorder test calibrate calibrate_client
-CFLAGS:=-Iinclude -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99
+CFLAGS:=-Iinclude/libsurvive -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99
LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g
@@ -23,11 +23,11 @@ calibrate_client : calibrate_client.c lib/libsurvive.so redist/os_generic.c red
lib:
mkdir lib
-lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_driverman.o src/survive_vive.o $(DEBUGSTUFF) $(CALS)
+lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_driverman.o src/survive_vive.o src/survive_config.o redist/json_helpers.o src/PoserDummy.o $(DEBUGSTUFF) $(CALS)
gcc -o $@ $^ $(LDFLAGS) -shared
clean :
- rm -rf *.o src/*.o *~ src/*~ test data_recorder lib/libsurvive.so
+ rm -rf *.o src/*.o *~ src/*~ test data_recorder lib/libsurvive.so redist/*.o redist/*~
diff --git a/README.md b/README.md
index f90590f..9428f39 100644
--- a/README.md
+++ b/README.md
@@ -14,6 +14,7 @@ Discord: https://discordapp.com/invite/7QbCAGS
| Fourth livestream | https://www.youtube.com/watch?v=fces1O7kWGY | 4:50:33 |
| Fifth livestream | https://www.youtube.com/watch?v=hHt3twW5_fI | 3:13:38 |
| Sixth livestream | https://www.youtube.com/watch?v=JsfkNRFkFM4 | 3:44:49 |
+| Seventh livestream | https://www.youtube.com/watch?v=EKSHvO3QSWY | 1:17:21 |
Notes from second livestream trying to reverse engineer the watchman protocol: https://gist.github.com/cnlohr/581c433f36f4249f8bbc9c2b6450ef0e
diff --git a/calibrate.c b/calibrate.c
index a1e150b..c557251 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -10,6 +10,8 @@
#include "src/survive_cal.h"
#include <DrawFunctions.h>
+#include "src/survive_config.h"
+
struct SurviveContext * ctx;
void HandleKey( int keycode, int bDown )
@@ -74,15 +76,14 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
}
}
-void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id )
{
- survive_default_imu_process( so, accelgyro, timecode, id );
+ survive_default_imu_process( so, mask, accelgyro, timecode, id );
-return;
//if( so->codename[0] == 'H' )
- if( 1 )
+ if( 0 )
{
- printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
+ printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
}
}
@@ -146,6 +147,7 @@ void * GuiThread( void * v )
int main()
{
ctx = survive_init( 0 );
+ config_init();
survive_install_light_fn( ctx, my_light_process );
survive_install_imu_fn( ctx, my_imu_process );
diff --git a/calibrate_client.c b/calibrate_client.c
index 4c59261..b15b9db 100644
--- a/calibrate_client.c
+++ b/calibrate_client.c
@@ -10,6 +10,8 @@
#include "src/survive_cal.h"
#include <DrawFunctions.h>
+#include "src/survive_config.h"
+
struct SurviveContext * ctx;
void HandleKey( int keycode, int bDown )
@@ -73,15 +75,14 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
}
}
-void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id )
{
- survive_default_imu_process( so, accelgyro, timecode, id );
+ survive_default_imu_process( so, mask, accelgyro, timecode, id );
-return;
//if( so->codename[0] == 'H' )
- if( 1 )
+ if( 0 )
{
- printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
+ printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
}
}
@@ -140,6 +141,23 @@ void * GuiThread( void * v )
int main()
{
+
+ config_init();
+ config_read("config.json");
+ config_set_str(&global_config_values, "Hello","World!");
+ const char *s = config_read_str(&global_config_values, "test123","default");
+ printf("%s\n", s);
+
+ FLT *f;
+
+ uint16_t fs = config_read_float_array(lh_config+1, "fcalgibpha", &f, NULL, 0);
+ printf("%d\n", fs);
+ printf("===> %f %f\n", f[0], f[1]);
+
+
+// config_save("config.json");
+
+
ctx = survive_init( 1 );
survive_install_light_fn( ctx, my_light_process );
@@ -156,7 +174,6 @@ int main()
CNFGDialogColor = 0x444444;
CNFGSetup( "Survive GUI Debug", 640, 480 );
OGCreateThread( GuiThread, 0 );
-
if( !ctx )
{
diff --git a/data_recorder.c b/data_recorder.c
index 951f234..5504d42 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -77,15 +77,15 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
}
-void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id )
{
- survive_default_imu_process( so, accelgyro, timecode, id );
+ survive_default_imu_process( so, mask, accelgyro, timecode, id );
//return;
//if( so->codename[0] == 'H' )
if( 1 )
{
- printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
+ printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
}
}
diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
new file mode 100644
index 0000000..36587e6
--- /dev/null
+++ b/dave/AffineSolve.c
@@ -0,0 +1,322 @@
+//
+// main.c
+// AffineSolve
+//
+// Created by user on 3/2/17.
+// Copyright © 2017 user. All rights reserved.
+//
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+
+#define LH_ID 0
+#define NUM_HMD 32
+
+float hmd_pos[NUM_HMD][3];
+void ReadHmdPoints()
+{
+ int i;
+ FILE *fin = fopen("HMD_points.csv","r");
+ if (fin==NULL) {
+ printf("ERROR: could not open HMD_points.csv for reading\n");
+ exit(1);
+ }
+
+ for (i=0; i<NUM_HMD; i++) {
+ fscanf(fin, "%f %f %f", &(hmd_pos[i][0]), &(hmd_pos[i][1]), &(hmd_pos[i][2]));
+ }
+
+ fclose(fin);
+}
+
+#define MAX_POINTS 128
+#define _ABS(a) ( (a)<=0 ? -(a) : (a) )
+#define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f )
+#define RANDF ( (float)rand() / (float)RAND_MAX )
+
+#define STEP_SIZE_ROT 1.0
+#define STEP_SIZE_POS 1.0
+#define FALLOFF 0.99999
+#define NITER 2000000
+#define TOO_SMALL 0.0001
+#define ORTHOG_PENALTY 1.0
+
+#define PRINT_MAT(A,M,N) { \
+ int m,n; \
+ printf(#A "\n"); \
+ for (m=0; m<M; m++) { \
+ for (n=0; n<N; n++) { \
+ printf("%f\t", A[m][n]); \
+ } \
+ printf("\n"); \
+ } \
+}
+
+void AffineSolve(
+ float T[4][4], // OUTPUT: transform
+ float O[MAX_POINTS][4], // INPUT: points, offsets
+ float N[MAX_POINTS][3], // INPUT: plane normals
+ float D[MAX_POINTS], // INPUT: plane offsets
+ int nPoints, int nIter,
+ float stepSizeRot, float stepSizePos, float falloff, int constrain)
+{
+ int i,j,k,iter;
+ //T[3][3] = 1.0f;
+
+ printf("iter x y z error\n");
+
+ float gradDot = 1.0;
+ float prevGradDot = 1.0;
+ float de_dT[3][4]; // the gradient
+ float conj[3][4]; // the conjugate
+ float errorSq=0.0;
+ for (iter=0; iter<nIter; iter++)
+ {
+ //----------------------------------
+ // Calculate the gradient direction
+ //----------------------------------
+ errorSq = 0.0;
+ memset(de_dT, 0, 3*4*sizeof(float));
+ for (i=0; i<nPoints; i++)
+ {
+ // What is the plane deviation error
+ float Ei = -D[i];
+ for (j=0; j<3; j++) {
+ float Tj_oi = 0.0f;
+ for (k=0; k<4; k++) {
+ Tj_oi += T[j][k] * O[i][k];
+ }
+ Ei += N[i][j] * Tj_oi;
+ }
+// printf("E[%d] %f\n", i, Ei);
+
+ // Figure out contribution to the error
+ for (j=0; j<3; j++) {
+ for (k=0; k<4; k++) {
+ de_dT[j][k] += N[i][j] * O[i][k] * Ei;
+ }
+ }
+
+ errorSq += Ei*Ei;
+ }
+
+ printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq));
+
+ // Constrain the gradient (such that dot products are zero)
+ if (constrain)
+ {
+ float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0;
+ for (k=0; k<3; k++) {
+ T0T1 += T[0][k] * T[1][k];
+ T1T2 += T[1][k] * T[2][k];
+ T2T0 += T[2][k] * T[0][k];
+ }
+// printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0);
+ for (k=0; k<3; k++) {
+ de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k];
+ de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k];
+ de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k];
+ de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k];
+ de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k];
+ de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k];
+ }
+ }
+
+ // Calculate the gradient dot product
+ // (used by conjugate gradient method)
+ prevGradDot = gradDot;
+ gradDot = 0.0;
+ for (j=0; j<3; j++) {
+ for (k=0; k<4; k++) {
+ gradDot += de_dT[j][k] * de_dT[j][k];
+ }
+ }
+
+// printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot);
+
+ //----------------------------------
+ // Calculate the conjugate direction
+ //----------------------------------
+// if (iter==0) {
+ // First iteration, just use the gradient
+ for (j=0; j<3; j++) {
+ for (k=0; k<4; k++) {
+ conj[j][k] = -de_dT[j][k];
+ }
+ }
+/* } else {
+ // Calculate "beta" for Fletcher Reeves method
+ float beta = gradDot / prevGradDot;
+//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta);
+
+ // Update the conjugate
+ for (j=0; j<3; j++) {
+ for (k=0; k<4; k++) {
+ conj[j][k] = beta*conj[j][k] - de_dT[j][k];
+ }
+ }
+ }
+*/
+
+// PRINT_MAT(de_dT,4,4);
+// exit(1);
+
+ //----------------------------------
+ // How large is the gradient ?
+ //----------------------------------
+
+ double gradSizeRot = 0.0;
+ double gradSizePos = 0.0;
+ for (j=0; j<3; j++) {
+ for (k=0; k<3; k++) {
+ gradSizeRot += _ABS(conj[j][k]);
+ }
+ gradSizePos += _ABS(conj[j][k]);
+ }
+ if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; } // Quit, we've totally converged
+
+ //----------------------------------
+ // Descend in the gradient direction
+ //----------------------------------
+ if (gradSizeRot > TOO_SMALL) {
+ float scaleRot = stepSizeRot / gradSizeRot;
+ for (j=0; j<3; j++) {
+ for (k=0; k<3; k++) {
+ T[j][k] += scaleRot * conj[j][k];
+ }
+ }
+ stepSizeRot *= falloff;
+ }
+
+ if (gradSizePos > TOO_SMALL) {
+ float scalePos = stepSizePos / gradSizePos;
+ for (j=0; j<3; j++) {
+ T[j][3] += scalePos * conj[j][3];
+ }
+ stepSizePos *= falloff;
+ }
+
+ // Constrain the gradient (such that scaling is one)
+ if (constrain)
+ {
+ // Measure the scales
+ float len[3] = {0.0, 0.0, 0.0};
+ for (j=0; j<3; j++) {
+ double lenSq = 0.0;
+ for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; }
+ len[j] = sqrt(lenSq);
+ }
+
+ // How far off is the scale?
+ float xzLen = 0.5 * (len[0] + len[2]);
+ if (xzLen > TOO_SMALL) {
+ float inv_xzLen = 1.0 / xzLen;
+ for (j=0; j<3; j++) {
+ T[3][j] *= inv_xzLen;
+ }
+ }
+
+ // Rescale the thing
+ for (j=0; j<3; j++)
+ {
+ if (len[j] > TOO_SMALL) {
+ float inv_len = 1.0 / len[j];
+ for (k=0; k<3; k++) { T[j][k] *= inv_len; }
+ }
+ }
+ }
+ }
+
+}
+
+int main()
+{
+ int i,j,k;
+ float Tcalc[4][4];
+ float O[MAX_POINTS][4];
+ float N[MAX_POINTS][3];
+ float D[MAX_POINTS];
+ int nPoints = 0;
+
+ // Read the hmd points
+ ReadHmdPoints();
+
+ //-------------------------
+ // Read the lighthouse data
+ //-------------------------
+ FILE *fin = fopen("ptinfo.csv", "r");
+ if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); }
+ while (!feof(fin))
+ {
+ // Read the angle
+ int sen,lh,axis,count;
+ float angle, avglen, stddevang, stddevlen;
+ float max_outlier_length, max_outlier_angle;
+ int rt = fscanf( fin, "%d %d %d %d %f %f %f %f %f %f\n",
+ &sen, &lh, &axis, &count,
+ &angle, &avglen, &stddevang, &stddevlen,
+ &max_outlier_length, &max_outlier_angle);
+ if (rt != 10) { break; }
+
+ if (lh == LH_ID && sen < NUM_HMD) {
+ // Set the offset
+ O[nPoints][0] = hmd_pos[sen][0];
+ O[nPoints][1] = hmd_pos[sen][1];
+ O[nPoints][2] = hmd_pos[sen][2];
+ O[nPoints][3] = 1.0;
+
+ // Calculate the plane equation
+ if (axis == 1) { // Horizontal
+ N[nPoints][0] = -cos(angle);
+ N[nPoints][1] = -sin(angle);
+ N[nPoints][2] = 0.0;
+ D[nPoints] = 0.0;
+ } else { // Vertical
+ N[nPoints][0] = 0.0;
+ N[nPoints][1] = -sin(angle);
+ N[nPoints][2] = cos(angle);
+ D[nPoints] = 0.0;
+ }
+
+ printf("pt %d O %.3f %.3f %.3f %.3f N %.3f %.3f %.3f D %.3f\n",
+ nPoints,
+ O[nPoints][0], O[nPoints][1], O[nPoints][2], O[nPoints][3],
+ N[nPoints][0], N[nPoints][1], N[nPoints][2],
+ D[nPoints]);
+
+ nPoints++;
+ }
+ }
+ fclose(fin);
+
+ printf("nPoints %d\n", nPoints);
+
+ // Run the calculation for Tcalc
+ int run;
+ //for (run=0; run<100; run++) {
+
+ // Initialize Tcalc to the identity matrix
+ //memcpy(Tcalc, Torig, 4*4*sizeof(float));
+ memset(Tcalc, 0, 4*4*sizeof(float));
+ for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; }
+
+ // Solve it!
+ AffineSolve(
+ Tcalc, // OUTPUT: transform
+ O, // INPUT: points, offsets
+ N, // INPUT: plane normals
+ D, // INPUT: plane offsets
+ nPoints, NITER,
+ STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF,
+ 1);
+ //}
+
+ PRINT_MAT(Tcalc,4,4);
+
+
+ // insert code here...
+ printf("Hello, World!\n");
+ return 0;
+}
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
new file mode 100644
index 0000000..98c926e
--- /dev/null
+++ b/include/libsurvive/poser.h
@@ -0,0 +1,56 @@
+#ifndef _LSPOSER_H
+#define _LSPOSER_H
+
+#include "survive_types.h"
+
+typedef enum PoserType_t
+{
+ POSERDATA_NONE = 0,
+ POSERDATA_IMU,
+ POSERDATA_LIGHT, //Single lighting event.
+ POSERDATA_FULL_SCENE, //Full, statified X, Y sweep data for both lighthouses.
+ POSERDATA_DISASSOCIATE, //If you get this, it doesn't contain data. It just tells you to please disassociate from the current SurviveObject and delete your poserdata.
+} PoserType;
+
+typedef struct
+{
+ PoserType pt;
+ uint8_t data[0];
+} PoserData;
+
+typedef struct
+{
+ PoserType pt;
+ uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present.
+ FLT accel[3];
+ FLT gyro[3];
+ FLT mag[3];
+ uint32_t timecode; //In object-local ticks.
+} PoserDataIMU;
+
+typedef struct
+{
+ PoserType pt;
+ int sensor_id;
+ int acode; //OOTX Code associated with this sweep. base_station = acode >> 2; axis = acode & 1;
+ uint32_t timecode; //In object-local ticks.
+ FLT length; //In seconds
+ FLT angle; //In radians from center of lighthouse.
+} PoserDataLight;
+
+typedef struct
+{
+ PoserType pt;
+
+ //If "lengths[...]" < 0, means not a valid piece of sweep information.
+ FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+ FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space)
+
+ PoserDataIMU lastimu;
+} PoserDataFullScene;
+
+//When you write your posers, use the following definition, and register with REGISTER_LINKTIME.
+typedef int (*PoserCB)( SurviveObject * so, PoserData * pd );
+
+
+#endif
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
new file mode 100644
index 0000000..7fd6046
--- /dev/null
+++ b/include/libsurvive/survive.h
@@ -0,0 +1,166 @@
+#ifndef _SURVIVE_H
+#define _SURVIVE_H
+
+#include <stdint.h>
+#include "survive_types.h"
+#include "poser.h"
+
+//DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks
+//for optimization on 32- and 64-bit systems.
+
+struct SurviveObject
+{
+ SurviveContext * ctx;
+
+ char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1.
+ char drivername[4]; //3 letters for driver. Currently "HTC"
+ int16_t buttonmask;
+ int16_t axis1;
+
+ int16_t axis2;
+ int16_t axis3;
+ int8_t charge;
+ int8_t charging:1;
+ int8_t ison:1;
+ int8_t additional_flags:6;
+
+ //Pose Information, also "poser" field.
+ FLT PoseConfidence; //0..1
+ SurvivePose OutPose;
+ SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Optionally filled out by poser, contains computed position from each lighthouse.
+ void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants.
+ PoserCB PoserFn;
+
+ //Device-specific information about the location of the sensors. This data will be used by the poser.
+ int8_t nr_locations;
+ FLT * sensor_locations;
+ FLT * sensor_normals;
+
+ //Timing sensitive data (mostly for disambiguation)
+ int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked)
+ int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked)
+ int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed)
+ int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed)
+ int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed)
+ int32_t pulse_max_for_sweep; //1,800 for normal vive hardware. (guessed)
+ int32_t pulse_synctime_offset; //20,000 for normal vive hardware. (guessed)
+ int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed)
+
+ //Flood info, for calculating which laser is currently sweeping.
+ int8_t oldcode;
+ int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault.
+ int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit.
+ uint32_t last_time[NUM_LIGHTHOUSES];
+ uint32_t last_length[NUM_LIGHTHOUSES];
+ uint32_t recent_sync_time;
+
+ uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device.
+
+
+ //Debug
+ int tsl;
+};
+
+
+struct BaseStationData
+{
+ uint8_t PositionSet:1;
+
+ SurvivePose Pose;
+
+ uint8_t OOTXSet:1;
+ uint32_t BaseStationID;
+ FLT fcalphase[2];
+ FLT fcaltilt[2];
+ FLT fcalcurve[2];
+ FLT fcalgibpha[2];
+ FLT fcalgibmag[2];
+};
+
+struct SurviveContext
+{
+ text_feedback_func faultfunction;
+ text_feedback_func notefunction;
+ light_process_func lightproc;
+ imu_process_func imuproc;
+ angle_process_func angleproc;
+
+ //Calibration data:
+ BaseStationData bsd[NUM_LIGHTHOUSES];
+
+ SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
+
+ SurviveObject ** objs;
+ int objs_ct;
+
+ void ** drivers;
+ DeviceDriverCb * driverpolls;
+ DeviceDriverCb * drivercloses;
+ DeviceDriverMagicCb * drivermagics;
+ int driver_ct;
+};
+
+SurviveContext * survive_init( int headless );
+
+//For any of these, you may pass in 0 for the function pointer to use default behavior.
+//In general unless you are doing wacky things like recording or playing back data, you won't need to use this.
+void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp );
+void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp );
+void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp );
+void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp );
+void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp );
+
+void survive_close( SurviveContext * ctx );
+int survive_poll();
+
+SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name );
+
+//Utilitiy functions.
+int survive_simple_inflate( SurviveContext * ctx, const char * input, int inlen, char * output, int outlen );
+
+int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen );
+
+//Install the calibrator.
+void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so.
+
+//Call these from your callback if overridden.
+//Accept higher-level data.
+void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
+void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+
+
+////////////////////// Survive Drivers ////////////////////////////
+
+void RegisterDriver( const char * name, void * data );
+
+#define REGISTER_LINKTIME( func ) \
+ void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); }
+
+
+
+///////////////////////// General stuff for writing drivers ///////
+
+//For device drivers to call. This actually attaches them.
+int survive_add_object( SurviveContext * ctx, SurviveObject * obj );
+void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic );
+
+//For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is.
+//When you write drivers, you can use this to send survive lightcap data.
+typedef struct
+{
+ uint8_t sensor_id;
+ uint8_t type; //Mostly unused. Set to 255 to ignore it.
+ uint16_t length;
+ uint32_t timestamp;
+} __attribute__((packed)) LightcapElement;
+
+//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
+void handle_lightcap( SurviveObject * so, LightcapElement * le );
+
+#define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); }
+#define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); }
+#define SV_KILL() exit(0) //XXX This should likely be re-defined.
+
+#endif
+
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
new file mode 100644
index 0000000..1600e11
--- /dev/null
+++ b/include/libsurvive/survive_types.h
@@ -0,0 +1,44 @@
+#ifndef _SURVIVE_TYPES_H
+#define _SURVIVE_TYPES_H
+
+#ifndef FLT
+#ifdef USE_DOUBLE
+#define FLT double
+#else
+#define FLT float
+#endif
+#endif
+
+typedef struct SurvivePose
+{
+ FLT Pos[3];
+ FLT Rot[4];
+} SurvivePose;
+
+//Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools.
+//It will also require a recompile. TODO: revisit this and correct the comment once fixed.
+#define NUM_LIGHTHOUSES 2
+
+#define INTBUFFSIZE 64
+#define SENSORS_PER_OBJECT 32
+
+typedef struct SurviveObject SurviveObject;
+typedef struct SurviveContext SurviveContext;
+typedef struct BaseStationData BaseStationData;
+typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be removed. Check at a later time for its defunctness.
+
+typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault );
+typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id );
+typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+
+
+//Device drivers (prefix your drivers with "DriverReg") i.e.
+// REGISTER_LINKTIME( DriverRegHTCVive );
+typedef int (*DeviceDriver)( SurviveContext * ctx );
+typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver );
+typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen );
+
+
+#endif
+
diff --git a/include/survive.h b/include/survive.h
deleted file mode 100644
index 51910a6..0000000
--- a/include/survive.h
+++ /dev/null
@@ -1,102 +0,0 @@
-#ifndef _SURVIVE_H
-#define _SURVIVE_H
-
-#include <stdint.h>
-
-#ifndef FLT
-#ifdef USE_DOUBLE
-#define FLT double
-#else
-#define FLT float
-#endif
-#endif
-
-struct SurviveContext;
-
-//DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks
-//for optimization on 32- and 64-bit systems.
-
-//Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools.
-//It will also require a recompile. TODO: revisit this and correct the comment once fixed.
-#define NUM_LIGHTHOUSES 2
-
-struct SurviveObject
-{
- struct SurviveContext * ctx;
-
- char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1.
- char drivername[4]; //3 letters for driver. Currently "HTC"
- int16_t buttonmask;
- int16_t axis1;
-
- int16_t axis2;
- int16_t axis3;
- int8_t charge;
- int8_t charging:1;
- int8_t ison:1;
- int8_t additional_flags:6;
-
- FLT * sensor_locations;
- FLT * sensor_normals;
- int8_t nr_locations;
-
- //Timing sensitive data (mostly for disambiguation)
- int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked)
- int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked)
- int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed)
- int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed)
- int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed)
- int32_t pulse_max_for_sweep; //1,800 for normal vive hardware. (guessed)
- int32_t pulse_synctime_offset; //20,000 for normal vive hardware. (guessed)
- int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed)
-
- //Flood info, for calculating which laser is currently sweeping.
- int8_t oldcode;
- int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault.
- int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit.
- uint32_t last_time[NUM_LIGHTHOUSES];
- uint32_t last_length[NUM_LIGHTHOUSES];
- uint32_t recent_sync_time;
-
- uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device.
-
- //Debug
- int tsl;
-};
-
-typedef void (*text_feedback_func)( struct SurviveContext * ctx, const char * fault );
-typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
-typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
-typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
-
-struct SurviveContext * survive_init( int headless );
-
-//For any of these, you may pass in 0 for the function pointer to use default behavior.
-void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp );
-void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp );
-void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp );
-void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp );
-void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp );
-
-void survive_close( struct SurviveContext * ctx );
-int survive_poll();
-
-struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name );
-
-//Utilitiy functions.
-int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen );
-
-int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen );
-
-//Install the calibrator.
-void survive_cal_install( struct SurviveContext * ctx );
-
-//Call these from your callback if overridden.
-//Accept higher-level data.
-void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
-void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
-void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
-
-
-#endif
-
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
new file mode 100644
index 0000000..b7ccb40
--- /dev/null
+++ b/redist/json_helpers.c
@@ -0,0 +1,172 @@
+// (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
+
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "json_helpers.h"
+#include <jsmn.h>
+
+
+void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) {
+ uint8_t i = 0;
+ char * str1 = NULL;
+ char * str2 = NULL;
+ asprintf(&str1,"\"%s\":[", tag);
+
+ for (i=0;i<count;++i) {
+ if ( (i+1) < count) {
+ asprintf(&str2, "%s\"%f\"", str1,v[i]);
+ } else {
+ asprintf(&str2, "%s\"%f\",", str1,v[i]);
+ }
+ free(str1);
+ str1=str2;
+ str2=NULL;
+ }
+ asprintf(&str2, "%s]", str1);
+ fputs(str2,f);
+ free(str1);
+ free(str2);
+}
+
+void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count) {
+ uint8_t i = 0;
+ char * str1 = NULL;
+ char * str2 = NULL;
+ asprintf(&str1,"\"%s\":[", tag);
+
+ for (i=0;i<count;++i) {
+ if (i<(count-1)) {
+ asprintf(&str2, "%s\"%f\",", str1,v[i]);
+ } else {
+ asprintf(&str2, "%s\"%f\"", str1,v[i]);
+ }
+ free(str1);
+ str1=str2;
+ str2=NULL;
+ }
+ asprintf(&str2, "%s]", str1);
+ fputs(str2,f);
+ free(str1);
+ free(str2);
+}
+
+void json_write_uint32(FILE* f, const char* tag, uint32_t v) {
+ fprintf(f, "\"%s\":\"%d\"", tag, v);
+}
+
+void json_write_float(FILE* f, const char* tag, float v) {
+ fprintf(f, "\"%s\":\"%f\"", tag, v);
+}
+
+void json_write_str(FILE* f, const char* tag, const char* v) {
+ fprintf(f, "\"%s\":\"%s\"", tag, v);
+}
+
+
+
+
+void (*json_begin_object)(char* tag) = NULL;
+void (*json_end_object)() = NULL;
+void (*json_tag_value)(char* tag, char** values, uint16_t count) = NULL;
+
+uint32_t JSON_STRING_LEN;
+
+char* load_file_to_mem(const char* path) {
+ FILE * f = fopen( path, "r" );
+ fseek( f, 0, SEEK_END );
+ int len = ftell( f );
+ fseek( f, 0, SEEK_SET );
+ char * JSON_STRING = malloc( len );
+ fread( JSON_STRING, len, 1, f );
+ fclose( f );
+ return JSON_STRING;
+}
+
+static char* substr(const char* str, uint32_t start, uint32_t end, uint32_t npos) {
+ uint32_t l = end-start+1;
+
+ if (end<=start || end>=npos) return NULL;
+
+ char* x = malloc(l);
+ memcpy(x,str+start,l);
+ x[l-1] = '\0';
+ return x;
+}
+
+static uint16_t json_load_array(const char* JSON_STRING, jsmntok_t* tokens, uint16_t size, char* tag) {
+ jsmntok_t* t = tokens;
+ uint16_t i = 0;
+
+ char* values[size];
+
+ for (i=0;i<size;++i) {
+ t = tokens+i;
+ values[i] = substr(JSON_STRING, t->start, t->end, JSON_STRING_LEN);
+ }
+
+ if (json_tag_value != NULL) json_tag_value(tag, values, i);
+
+ for (i=0;i<size;++i) free(values[i]);
+
+ return size;
+}
+
+void json_load_file(const char* path) {
+ uint32_t i = 0;
+ char* JSON_STRING = load_file_to_mem(path);
+ JSON_STRING_LEN = strlen(JSON_STRING);
+
+ jsmn_parser parser;
+ jsmn_init(&parser);
+
+ uint32_t items = jsmn_parse(&parser, JSON_STRING, JSON_STRING_LEN, NULL, 0);
+ jsmntok_t* tokens = malloc(items * sizeof(jsmntok_t));
+
+ jsmn_init(&parser);
+ items = jsmn_parse(&parser, JSON_STRING, JSON_STRING_LEN, tokens, items);
+
+ int16_t children = -1;
+
+ for (i=0; i<items; i+=2)
+ {
+ //increment i on each successful tag + values combination, not individual tokens
+ jsmntok_t* tag_t = tokens+i;
+ jsmntok_t* value_t = tokens+i+1;
+
+ char* tag = substr(JSON_STRING, tag_t->start, tag_t->end, JSON_STRING_LEN);
+ char* value = substr(JSON_STRING, value_t->start, value_t->end, JSON_STRING_LEN);
+
+ printf("%d %d c:%d %d %s \n", tag_t->start, tag_t->end, tag_t->size, tag_t->type, tag);
+
+
+ if (value_t->type == JSMN_ARRAY) {
+ i += json_load_array(JSON_STRING, tokens+i+2,value_t->size, tag); //look at array children
+ } else if (value_t->type == JSMN_OBJECT) {
+ printf("Begin Object\n");
+ if (json_begin_object != NULL) json_begin_object(tag);
+ children = value_t->size +1; //+1 to account for this loop where we are not yed parsing children
+// i += decode_jsmn_object(JSON_STRING, tokens+i+2,value_t->size);
+ }
+ else {
+ if (json_tag_value != NULL) json_tag_value(tag, &value, 1);
+ }
+
+ if (children>=0) children--;
+ if (children == 0) {
+ children = -1;
+ printf("End Object\n");
+ if (json_end_object!=NULL) json_end_object();
+ }
+
+// printf("%d %s \n", value_t->type, tag);
+
+ free(tag);
+ free(value);
+ }
+
+ free(JSON_STRING);
+}
+
diff --git a/redist/json_helpers.h b/redist/json_helpers.h
new file mode 100644
index 0000000..1670058
--- /dev/null
+++ b/redist/json_helpers.h
@@ -0,0 +1,20 @@
+// (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
+
+#ifndef JSON_HELPERS_H
+#define JSON_HELPERS_H
+
+#include <stdint.h>
+
+void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count);
+void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count);
+void json_write_uint32(FILE* f, const char* tag, uint32_t v);
+void json_write_float(FILE* f, const char* tag, float v);
+void json_write_str(FILE* f, const char* tag, const char* v);
+
+void json_load_file(const char* path);
+extern void (*json_begin_object)(char* tag);
+extern void (*json_end_object)();
+extern void (*json_tag_value)(char* tag, char** values, uint16_t count);
+
+
+#endif \ No newline at end of file
diff --git a/redist/linmath.c b/redist/linmath.c
index 157141f..1c5c25b 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -93,11 +93,6 @@ void quatsetnone(FLT * q)
q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0;
}
-void quatsetidentity(FLT * q)
-{
- q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1;
-}
-
void quatcopy(FLT * qout, const FLT * qin)
{
qout[0] = qin[0];
@@ -447,7 +442,7 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest)
// If dot == 1, vectors are the same
if (d >= 1.0f)
{
- quatsetidentity(q);
+ quatsetnone(q);
return;
}
if (d < (1e-6f - 1.0f))
diff --git a/src/survive.c b/src/survive.c
index 9bc1a2c..efa5d82 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -3,7 +3,6 @@
#include <survive.h>
#include "survive_internal.h"
-#include "survive_driverman.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -21,11 +20,11 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
}
-struct SurviveContext * survive_init( int headless )
+SurviveContext * survive_init( int headless )
{
int r = 0;
int i = 0;
- struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) );
+ SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) );
ctx->faultfunction = survivefault;
ctx->notefunction = survivenote;
@@ -36,7 +35,6 @@ struct SurviveContext * survive_init( int headless )
const char * DriverName;
while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) )
-
{
DeviceDriver dd = GetDriver( DriverName );
printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i );
@@ -44,10 +42,34 @@ struct SurviveContext * survive_init( int headless )
printf( "Driver %s reports status %d\n", DriverName, r );
}
+ i = 0;
+ const char * PreferredPoser = "PoserDummy"; //config_read_str( cg, "DefualtPoser", "PoserDummy" ); /XXX Axlecrusher, can you add config stuff for this?
+ PoserCB PreferredPoserCB = 0;
+ const char * FirstPoser = 0;
+ printf( "Available posers:\n" );
+ while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) )
+ {
+ PoserCB p = GetDriver( DriverName );
+ if( !PreferredPoserCB ) PreferredPoserCB = p;
+ int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0;
+ printf( "\t%c%s\n", ThisPoser?'*':' ', DriverName );
+ if( ThisPoser ) PreferredPoserCB = p;
+ }
+ printf( "Totals %d posers. Using selected poser (or first!).\n", i-1 );
+ if( !PreferredPoserCB )
+ {
+ SV_ERROR( "Error. Cannot find any valid poser." );
+ }
+
+ for( i = 0; i < ctx->objs_ct; i++ )
+ {
+ ctx->objs[i]->PoserFn = PreferredPoserCB;
+ }
+
return ctx;
}
-void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func fbp )
+void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp )
{
if( fbp )
ctx->notefunction = fbp;
@@ -55,7 +77,7 @@ void survive_install_info_fn( struct SurviveContext * ctx, text_feedback_func f
ctx->notefunction = survivenote;
}
-void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func fbp )
+void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp )
{
if( fbp )
ctx->faultfunction = fbp;
@@ -63,7 +85,7 @@ void survive_install_error_fn( struct SurviveContext * ctx, text_feedback_func
ctx->faultfunction = survivefault;
}
-void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp )
+void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp )
{
if( fbp )
ctx->lightproc = fbp;
@@ -71,7 +93,7 @@ void survive_install_light_fn( struct SurviveContext * ctx, light_process_func f
ctx->lightproc = survive_default_light_process;
}
-void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp )
+void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp )
{
if( fbp )
ctx->imuproc = fbp;
@@ -80,7 +102,7 @@ void survive_install_imu_fn( struct SurviveContext * ctx, imu_process_func fbp
}
-void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func fbp )
+void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp )
{
if( fbp )
ctx->angleproc = fbp;
@@ -88,15 +110,15 @@ void survive_install_angle_fn( struct SurviveContext * ctx, angle_process_func
ctx->angleproc = survive_default_angle_process;
}
-int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj )
+int survive_add_object( SurviveContext * ctx, SurviveObject * obj )
{
int oldct = ctx->objs_ct;
- ctx->objs = realloc( ctx->objs, sizeof( struct SurviveObject * ) * (oldct+1) );
+ ctx->objs = realloc( ctx->objs, sizeof( SurviveObject * ) * (oldct+1) );
ctx->objs[oldct] = obj;
ctx->objs_ct = oldct+1;
}
-void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic )
+void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic )
{
int oldct = ctx->driver_ct;
ctx->drivers = realloc( ctx->drivers, sizeof( void * ) * (oldct+1) );
@@ -110,7 +132,7 @@ void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriv
ctx->driver_ct = oldct+1;
}
-int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen )
+int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen )
{
int oldct = ctx->driver_ct;
int i;
@@ -120,7 +142,7 @@ int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data
}
}
-void survive_close( struct SurviveContext * ctx )
+void survive_close( SurviveContext * ctx )
{
const char * DriverName;
int r = 0;
@@ -134,6 +156,14 @@ void survive_close( struct SurviveContext * ctx )
int oldct = ctx->driver_ct;
int i;
+
+ for( i = 0; i < ctx->objs_ct; i++ )
+ {
+ PoserData pd;
+ pd.pt = POSERDATA_DISASSOCIATE;
+ if( ctx->objs[i]->PoserFn ) ctx->objs[i]->PoserFn( ctx->objs[i], &pd );
+ }
+
for( i = 0; i < oldct; i++ )
{
ctx->drivercloses[i]( ctx, ctx->drivers[i] );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2ed1986..2acf51c 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -15,6 +15,8 @@
#include <sys/stat.h>
#include <sys/types.h>
+#include "survive_config.h"
+
#define PTS_BEFORE_COMMON 32
#define NEEDED_COMMON_POINTS 10
#define NEEDED_TIMES_OF_COMMON 5
@@ -25,8 +27,8 @@ static void reset_calibration( struct SurviveCalData * cd );
void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
{
- struct SurviveContext * ctx = (struct SurviveContext*)(ct->user);
- struct SurviveCalData * cd = ctx->calptr;
+ SurviveContext * ctx = (SurviveContext*)(ct->user);
+ SurviveCalData * cd = ctx->calptr;
int id = ct->user1;
SV_INFO( "Got OOTX packet %d %p", id, cd );
@@ -34,7 +36,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
lighthouse_info_v6 v6;
init_lighthouse_info_v6(&v6, packet->data);
- struct BaseStationData * b = &ctx->bsd[id];
+ BaseStationData * b = &ctx->bsd[id];
//print_lighthouse_info_v6(&v6);
b->BaseStationID = v6.id;
@@ -49,6 +51,9 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
b->fcalgibmag[0] = v6.fcal_0_gibmag;
b->fcalgibmag[1] = v6.fcal_1_gibmag;
b->OOTXSet = 1;
+
+ config_set_lighthouse(b,id);
+// config_save("config.json");
}
int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
diff --git a/src/survive_cal.h b/src/survive_cal.h
index bb4eb32..dd2a1e2 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -2,6 +2,8 @@
// All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
+//XXX XXX XXX Warning: This subsystem will likely be mostly re-written.
+
#ifndef _SURVIVE_CAL_H
#define _SURVIVE_CAL_H
@@ -21,21 +23,21 @@
#include "ootx_decoder.h"
#include "survive_internal.h"
-void survive_cal_install( struct SurviveContext * ctx );
-int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length );
+void survive_cal_install( SurviveContext * ctx );
+int survive_cal_get_status( SurviveContext * ctx, char * description, int description_length );
//void survive_cal_teardown( struct SurviveContext * ctx );
//Called from survive_default_light_process
-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 );
+void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
#define MAX_SENSORS_TO_CAL 96
#define DRPTS 1024
#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
struct SurviveCalData
{
- struct SurviveContext * ctx;
+ SurviveContext * ctx;
//OOTX Data is sync'd off of the sync pulses coming from the lighthouses.
ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
@@ -57,7 +59,7 @@ struct SurviveCalData
int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution.
- struct SurviveObject * hmd;
+ SurviveObject * hmd;
//Stage:
// 0: Idle
@@ -68,7 +70,7 @@ struct SurviveCalData
//The following function is not included in the core survive_cal and must be compiled from a camfind file.
//It should use data for stage 4 and report if it found the
-int survive_cal_lhfind( struct SurviveCalData * cd );
+int survive_cal_lhfind( SurviveCalData * cd );
#endif
diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c
index e1e5fc9..a1bb2cc 100644
--- a/src/survive_cal_lhfind.c
+++ b/src/survive_cal_lhfind.c
@@ -7,13 +7,13 @@
#define MIN_HITS_FOR_VALID 10
-FLT static RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat );
+FLT static RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat );
//Values used for RunTest()
-int survive_cal_lhfind( struct SurviveCalData * cd )
+int survive_cal_lhfind( SurviveCalData * cd )
{
- struct SurviveContext * ctx = cd->ctx;
+ SurviveContext * ctx = cd->ctx;
int cycle, i;
int lh = 0;
FLT dx, dy, dz;
@@ -129,7 +129,7 @@ int survive_cal_lhfind( struct SurviveCalData * cd )
fullrange *= 0.25;
}
- if( beste > 0.005 )
+ if( beste > 0.01 )
{
//Error too high
SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste );
@@ -137,8 +137,8 @@ int survive_cal_lhfind( struct SurviveCalData * cd )
}
cd->ctx->bsd[lh].PositionSet = 1;
- copy3d( cd->ctx->bsd[lh].Position, LighthousePos );
- quatcopy( cd->ctx->bsd[lh].Quaternion, LighthouseQuat );
+ copy3d( cd->ctx->bsd[lh].Pose.Pos, LighthousePos );
+ quatcopy( cd->ctx->bsd[lh].Pose.Rot, LighthouseQuat );
}
return 0; //Return 0 if success.
@@ -149,14 +149,14 @@ int survive_cal_lhfind( struct SurviveCalData * cd )
-static FLT RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat )
+static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat )
{
int i, p;
FLT UsToTarget[3];
FLT LastUsToTarget[3];
FLT mux = .9;
quatsetnone( LighthouseQuat );
- struct SurviveObject * hmd = cd->hmd;
+ SurviveObject * hmd = cd->hmd;
FLT * hmd_points = hmd->sensor_locations;
FLT * hmd_normals = hmd->sensor_normals;
diff --git a/src/survive_config.c b/src/survive_config.c
index ed3f6cd..352a15b 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -1,174 +1,359 @@
+// (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
#include <string.h>
#include <assert.h>
#include "survive_config.h"
+#include <json_helpers.h>
+
+#include <errno.h>
#define MAX_CONFIG_ENTRIES 100
+#define MAX_LIGHTHOUSES 2
-config_val config_values[MAX_CONFIG_ENTRIES];
-static uint16_t used_entries = 0;
+config_group global_config_values;
+config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs
-static FILE *config_file = NULL;
+//static uint16_t used_entries = 0;
-void config_init() {
- uint16_t i = 0;
- for (i=0;i<MAX_CONFIG_ENTRIES;++i) {
- config_values[i].str = NULL;
- config_values[i].tag = NULL;
- }
+//static FILE *config_file = NULL;
+const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count);
- used_entries = 0;
+void init_config_entry(config_entry* ce) {
+ ce->data = NULL;
+ ce->tag = NULL;
+ ce->type = CONFIG_UNKNOWN;
+ ce->elements = 0;
}
-void write_float(char* tag, FLT x) {
- fprintf(config_file, "\"%s\":\"%f\"\n", tag, x);
+void destroy_config_entry(config_entry* ce) {
+ if (ce->tag!=NULL) free(ce->tag);
+ if (ce->data!=NULL) free(ce->data);
}
-void write_float_a(char* tag, FLT *x, uint8_t count) {
- uint8_t i = 0;
- char idx[4];
+void init_config_group(config_group *cg, uint16_t count) {
+ uint16_t i = 0;
+ cg->config_entries = malloc(count*sizeof(config_entry));
+ cg->used_entries = 0;
+ cg->max_entries = count;
+
for (i=0;i<count;++i) {
- sprintf(idx,"%d",i);
- fprintf(config_file, "\"%s%s\":\"%f\"\n", tag,idx, x);
+ init_config_entry(cg->config_entries+i);
}
- fprintf(config_file, "\"%s\":\"%f\"\n", tag, x);
}
-void write_uint32(char* tag, uint32_t x) {
- fprintf(config_file, "\"%s\":\"%d\"\n", tag, x);
-}
+void resize_config_group(config_group *cg, uint16_t count) {
+ uint16_t i = 0;
+
+ if (count > cg->max_entries) {
+ config_entry* ptr = realloc(cg->config_entries, sizeof(config_entry)*count);
+ assert(ptr!=NULL);
+
+ cg->config_entries = ptr;
-void config_open(const char* path, const char* mode) {
- config_file = fopen(path, mode);
+ for (i=cg->max_entries;i<count;++i) {
+ init_config_entry(cg->config_entries+i);
+ }
+
+ cg->max_entries = count;
+ }
}
-void config_close() {
- fclose(config_file);
+void config_init() {
+ uint16_t i = 0;
+ init_config_group(&global_config_values, MAX_CONFIG_ENTRIES);
+ for(i=0;i<MAX_LIGHTHOUSES;i++) {
+ init_config_group(lh_config+i, 9);
+ }
}
-void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length) {
- uint8_t i = 0;
- for (i=0;i<length; ++i) {
- write_uint32("id", bsd[i].BaseStationID);
- write_float_a("position", bsd[i].Position, 3);
- write_float_a("quaternion", bsd[i].Quaternion, 4);
- write_float_a("quaternion", bsd[i].Quaternion, 4);
- write_float_a("fcalphase", bsd[i].fcalphase, 2);
- write_float_a("fcaltilt", bsd[i].fcaltilt,2);
- write_float_a("fcalcurve", bsd[i].fcalcurve,2);
- write_float_a("fcalgibpha", bsd[i].fcalgibpha,2);
- write_float_a("fcalgibmag", bsd[i].fcalgibmag,2);
- }
+void config_set_lighthouse(BaseStationData* bsd, uint8_t idx) {
+ config_group *cg = lh_config+idx;
+ config_set_uint32(cg,"index", idx);
+ config_set_uint32(cg,"id", bsd->BaseStationID);
+ config_set_float_a(cg,"pose", &bsd->Pose.Pos[0], 7);
+ config_set_float_a(cg,"fcalphase", bsd->fcalphase, 2);
+ config_set_float_a(cg,"fcaltilt", bsd->fcaltilt,2);
+ config_set_float_a(cg,"fcalcurve", bsd->fcalcurve,2);
+ config_set_float_a(cg,"fcalgibpha", bsd->fcalgibpha,2);
+ config_set_float_a(cg,"fcalgibmag", bsd->fcalgibmag,2);
}
-void sstrcpy(char* dest, const char *src) {
+void sstrcpy(char** dest, const char *src) {
uint32_t len = strlen(src)+1;
- if (dest == NULL) {
- dest = (char*)malloc(len);
- } else {
- dest = (char*)realloc(dest, len);
- }
- strcpy(dest,src);
+ assert(dest!=NULL);
+
+ char* ptr = (char*)realloc(*dest, len); //acts like malloc if dest==NULL
+ assert(ptr!=NULL);
+ *dest = ptr;
+
+ strcpy(*dest,src);
}
-const char* config_read_str(const char *tag, const char *value, const char *def_str) {
+config_entry* find_config_entry(config_group *cg, const char *tag) {
uint16_t i = 0;
- for (i=0;i<used_entries;++i) {
- if ( strcmp(config_values[i].tag, tag) == 0 ) {
- return config_values[i].str;
+ for (i=0;i < cg->used_entries;++i) {
+ if ( strcmp(cg->config_entries[i].tag, tag) == 0 ) {
+ return cg->config_entries+i;
}
}
- assert(used_entries<MAX_CONFIG_ENTRIES);
+ return NULL;
+}
+
+const char* config_read_str(config_group *cg, const char *tag, const char *def) {
+ config_entry *cv = find_config_entry(cg, tag);
- i = used_entries++;
- sstrcpy(config_values[i].tag, tag);
- sstrcpy(config_values[i].str, def_str);
+ if (cv != NULL) return cv->data;
- return config_values[i].str;
+ return config_set_str(cg,tag,def);
}
-uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def) {
- uint16_t i = 0;
- for (i=0;i<used_entries;++i) {
- if ( strcmp(config_values[i].tag, tag) == 0 ) {
- return config_values[i].numeric.i;
- }
- }
- assert(used_entries<MAX_CONFIG_ENTRIES);
+uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def) {
+ config_entry *cv = find_config_entry(cg, tag);
- i = used_entries++;
- sstrcpy(config_values[i].tag, tag);
- config_values[i].numeric.i = def;
+ if (cv != NULL) return cv->numeric.i;
- return config_values[i].numeric.i;
+ return config_set_uint32(cg, tag, def);
}
-FLT config_read_float(const char *tag, const FLT value, const FLT def) {
- uint16_t i = 0;
- for (i=0;i<used_entries;++i) {
- if ( strcmp(config_values[i].tag, tag) == 0 ) {
- return config_values[i].numeric.f;
- }
+FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
+ config_entry *cv = find_config_entry(cg, tag);
+
+ if (cv != NULL) return cv->numeric.f;
+
+ return config_set_float(cg, tag, def);
+}
+
+uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count) {
+ config_entry *cv = find_config_entry(cg, tag);
+
+ if (cv != NULL) {
+ *values = (FLT*)cv->data;
+ return cv->elements;
}
- assert(used_entries<MAX_CONFIG_ENTRIES);
- i = used_entries++;
- sstrcpy(config_values[i].tag, tag);
- config_values[i].numeric.f = def;
+ if (def == NULL) return 0;
- return config_values[i].numeric.f;
+ config_set_float_a(cg, tag, def, count);
+ *values = def;
+ return count;
}
-const char* config_set_str(const char *tag, const char* value) {
- uint16_t i = 0;
+config_entry* next_unused_entry(config_group *cg) {
+ config_entry *cv = NULL;
+// assert(cg->used_entries < cg->max_entries);
+
+ if (cg->used_entries >= cg->max_entries) resize_config_group(cg, cg->max_entries + 10);
+
+ cv = cg->config_entries + cg->used_entries;
- assert(used_entries<MAX_CONFIG_ENTRIES);
+ cg->used_entries++;
+ return cv;
+}
+
+const char* config_set_str(config_group *cg, const char *tag, const char* value) {
+ config_entry *cv = find_config_entry(cg, tag);
+ if (cv == NULL) cv = next_unused_entry(cg);
- i = used_entries++;
- sstrcpy(config_values[i].tag, tag);
- sstrcpy(config_values[i].str, value);
+ sstrcpy(&(cv->tag), tag);
+ sstrcpy(&(cv->data), value);
+ cv->type = CONFIG_STRING;
return value;
}
-const uint32_t config_set_uint32(const char *tag, const uint32_t value) {
- uint16_t i = 0;
+const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value) {
+ config_entry *cv = find_config_entry(cg, tag);
+ if (cv == NULL) cv = next_unused_entry(cg);
- assert(used_entries<MAX_CONFIG_ENTRIES);
+ sstrcpy(&(cv->tag), tag);
+ cv->numeric.i = value;
+ cv->type = CONFIG_UINT32;
- i = used_entries++;
- sstrcpy(config_values[i].tag, tag);
- config_values[i].numeric.i = value;
+ return value;
+}
+
+const FLT config_set_float(config_group *cg, const char *tag, const FLT value) {
+ config_entry *cv = find_config_entry(cg, tag);
+ if (cv == NULL) cv = next_unused_entry(cg);
+
+ sstrcpy(&(cv->tag), tag);
+ cv->numeric.f = value;
+ cv->type = CONFIG_FLOAT;
return value;
}
-const FLT config_set_float(const char *tag, const FLT value) {
+const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count) {
+ config_entry *cv = find_config_entry(cg, tag);
+ if (cv == NULL) cv = next_unused_entry(cg);
+
+ sstrcpy(&(cv->tag), tag);
+
+ char* ptr = (char*)realloc(cv->data, sizeof(FLT)*count);
+ assert(ptr!=NULL);
+ cv->data = ptr;
+
+ printf("float array\n");
+
+ memcpy(cv->data,values,sizeof(FLT)*count);
+ cv->type = CONFIG_FLOAT_ARRAY;
+ cv->elements = count;
+
+ return values;
+}
+
+void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) {
+ #ifdef USE_DOUBLE
+ json_write_double_array(f,tag,v,count);
+ #else
+ json_write_float_array(f,tag,v,count);
+ #endif
+}
+
+void write_config_group(FILE* f, config_group *cg, char *tag) {
uint16_t i = 0;
- assert(used_entries<MAX_CONFIG_ENTRIES);
+ if (tag != NULL) {
+ fprintf(f, "\"%s\":{\n", tag);
+ }
- i = used_entries++;
- sstrcpy(config_values[i].tag, tag);
- config_values[i].numeric.f = value;
+ for (i=0;i < cg->used_entries;++i) {
+ if (cg->config_entries[i].type == CONFIG_FLOAT) {
+ json_write_float(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.f);
+ } else if (cg->config_entries[i].type == CONFIG_UINT32) {
+ json_write_uint32(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.i);
+ } else if (cg->config_entries[i].type == CONFIG_STRING) {
+ json_write_str(f, cg->config_entries[i].tag, cg->config_entries[i].data);
+ } else if (cg->config_entries[i].type == CONFIG_FLOAT_ARRAY) {
+ _json_write_float_array(f, cg->config_entries[i].tag, (FLT*)cg->config_entries[i].data, cg->config_entries[i].elements);
+ }
+ if ((i+1) < cg->used_entries) fprintf(f,",");
+ fprintf(f,"\n");
+ };
- return value;
+ if (tag != NULL) {
+ fprintf(f,"}\n");
+ }
}
+
void config_save(const char* path) {
uint16_t i = 0;
FILE* f = fopen(path, "w");
- for (i=0;i<=used_entries;++i) {
- if (config_values[i].type == CONFIG_FLOAT) {
- fprintf(f, "\"%s\":\"%F\"\n", config_values[i].tag, config_values[i].numeric.f);
- } else if (config_values[i].type == CONFIG_UINT32) {
- fprintf(f, "\"%s\":\"%d\"\n", config_values[i].tag, config_values[i].numeric.i);
- } else if (config_values[i].type == CONFIG_STRING) {
- fprintf(f, "\"%s\":\"%s\"\n", config_values[i].tag, config_values[i].str);
- }
- };
+ write_config_group(f,&global_config_values, NULL);
+ write_config_group(f,lh_config, "lighthouse0");
+ write_config_group(f,lh_config+1, "lighthouse1");
fclose(f);
}
+void print_json_value(char* tag, char** values, uint16_t count) {
+ uint16_t i = 0;
+ for (i=0;i<count; ++i) {
+ printf("%s:%s \n", tag, values[i]);
+ }
+}
+
+config_group* cg_stack[10]; //handle 10 nested objects deep
+uint8_t cg_stack_head = 0;
+
+void handle_config_group(char* tag) {
+ cg_stack_head++;
+ if (strcmp("lighthouse0",tag) == 0) {
+ cg_stack[cg_stack_head] = lh_config;
+ } else if (strcmp("lighthouse1",tag) == 0) {
+ cg_stack[cg_stack_head] = lh_config+1;
+ } else {
+ cg_stack[cg_stack_head] = &global_config_values;
+ }
+}
+
+void pop_config_group() {
+ cg_stack_head--;
+}
+
+
+int parse_floats(char* tag, char** values, uint16_t count) {
+ uint16_t i = 0;
+ FLT f[count];
+ char* end = NULL;
+ config_group* cg = cg_stack[cg_stack_head];
+
+ for(i=0;i<count;++i) {
+
+ #ifdef USE_DOUBLE
+ f[i] = strtod(values[i], &end);
+ #else
+ f[i] = strtof(values[i], &end);
+ #endif
+
+// if (values[i] == end) return 0; //not a float
+ if (*end != '\0') return 0; //not an integer
+ }
+
+ if (count>1) {
+ config_set_float_a(cg, tag, f, count);
+ }
+ else {
+ config_set_float(cg, tag, f[0]);
+ }
+
+ return 1;
+}
+
+int parse_uint32(char* tag, char** values, uint16_t count) {
+ uint16_t i = 0;
+ uint32_t l[count];
+ char* end = NULL;
+ config_group* cg = cg_stack[cg_stack_head];
+
+/*
+ //look for non numeric values
+ for(end=values[0];*end!='\0';++end) {
+ if ((*end<48) || (*end>57)) return 0;
+ }
+
+ end=NULL;
+*/
+ for(i=0;i<count;++i) {
+ l[i] = strtol(values[i], &end, 10);
+// if (values[i] == end) return 0; //not an integer
+ if (*end != '\0') return 0; //not an integer
+ }
+
+// if (count>1)
+// config_set_uint32_array(cg, tag, f, count);
+// else
+ config_set_uint32(cg, tag, l[0]);
+
+ return 1;
+}
+
+void handle_tag_value(char* tag, char** values, uint16_t count) {
+ print_json_value(tag,values,count);
+ config_group* cg = cg_stack[cg_stack_head];
+
+ if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules
+
+ if (parse_floats(tag,values,count) > 0) return;
+
+ //should probably also handle string arrays
+ config_set_str(cg,tag,values[0]);
+// else if (count>1) config_set_str
+}
+
+void config_read(const char* path) {
+ json_begin_object = handle_config_group;
+ json_end_object = pop_config_group;
+ json_tag_value = handle_tag_value;
+
+ cg_stack[0] = &global_config_values;
+
+ json_load_file(path);
+
+ json_begin_object = NULL;
+ json_end_object = NULL;
+ json_tag_value = NULL;
+}
+
diff --git a/src/survive_config.h b/src/survive_config.h
index 24762cd..cd16c76 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -10,14 +10,11 @@ typedef enum {
CONFIG_UNKNOWN = 0,
CONFIG_FLOAT = 1,
CONFIG_UINT32 = 2,
- CONFIG_STRING = 3
+ CONFIG_STRING = 3,
+ CONFIG_FLOAT_ARRAY = 4,
} cval_type;
-/*
-typedef union {
- uint32_t i;
- FLT f;
- } Numeric;
-*/
+
+
typedef struct {
char *tag;
cval_type type;
@@ -25,21 +22,34 @@ typedef struct {
uint32_t i;
FLT f;
} numeric;
- char *str;
-} config_val;
+ char *data;
+ uint32_t elements;
+} config_entry;
+
+typedef struct {
+ config_entry *config_entries;
+ uint16_t used_entries;
+ uint16_t max_entries;
+} config_group;
+
+extern config_group global_config_values;
+extern config_group lh_config[2]; //lighthouse configs
-void config_open(const char* path, const char* mode);
-void config_close();
-void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
+void config_init();
+//void config_open(const char* path, const char* mode);
+void config_read(const char* path);
+//void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
+void config_set_lighthouse(BaseStationData* bsd, uint8_t idx);
void config_save(const char* path);
-const FLT config_set_float(const char *tag, const FLT value);
-const uint32_t config_set_uint32(const char *tag, const uint32_t value);
-const char* config_set_str(const char *tag, const char* value);
-FLT config_read_float(const char *tag, const FLT value, const FLT def);
+const FLT config_set_float(config_group *cg, const char *tag, const FLT value);
+const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value);
+const char* config_set_str(config_group *cg, const char *tag, const char* value);
-uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def);
-const char* config_read_str(const char *tag, const char *value, const char *def_str);
+FLT config_read_float(config_group *cg, const char *tag, const FLT def);
+uint16_t config_read_float_array(config_group *cg, const char *tag, FLT** values, const FLT* def, uint16_t count);
+uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def);
+const char* config_read_str(config_group *cg, const char *tag, const char *def);
-#endif \ No newline at end of file
+#endif
diff --git a/src/survive_data.c b/src/survive_data.c
index e55b640..63cc5c2 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -6,12 +6,19 @@
#include <string.h>
//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
-void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
+void handle_lightcap( SurviveObject * so, LightcapElement * le )
{
- struct SurviveContext * ctx = so->ctx;
+ 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 );
+ //if( so->codename[0] != 'H' )
+ //printf( "*** %s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
+
+ if( le->sensor_id > SENSORS_PER_OBJECT )
+ {
+ return;
+ }
+
so->tsl = le->timestamp;
if( le->length < 20 ) return; ///Assuming 20 is an okay value for here.
diff --git a/src/survive_driverman.c b/src/survive_driverman.c
index 8cdfb71..d694e64 100644
--- a/src/survive_driverman.c
+++ b/src/survive_driverman.c
@@ -3,7 +3,7 @@
// See notice in survive_driverman.h
//
-#include "survive_driverman.h"
+#include "survive_internal.h"
#include <string.h>
#include <stdio.h>
diff --git a/src/survive_driverman.h b/src/survive_driverman.h
deleted file mode 100644
index 5e13caf..0000000
--- a/src/survive_driverman.h
+++ /dev/null
@@ -1,38 +0,0 @@
-// (C) 2017 <>< C. N. Lohr, Under MIT/x11 License.
-//
-// This file is intended to be used for self-registering functions. By using
-// this it means that you do not need to have complicated switch statements or
-// #defines for dfferent inclusion of drivers/other code. You can simply
-// register your function and it will be put into a list.
-//
-//
-
-#ifndef SURVIVE_DRIVERMAN_H
-#define SURVIVE_DRIVERMAN_H
-
-//Driver registration
-#define MAX_DRIVERS 32
-
-void RegisterDriver( const char * name, void * data );
-void * GetDriver( const char * name );
-const char * GetDriverNameMatching( const char * prefix, int place );
-void ListDrivers();
-
-#define REGISTER_LINKTIME( func ) \
- void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); }
-
-
-//
-// Specific types of drivers.
-//
-
-struct SurviveContext;
-
-//Device drivers (prefix your drivers with "DriverReg") i.e.
-// REGISTER_LINKTIME( DriverRegHTCVive );
-typedef int (*DeviceDriver)( struct SurviveContext * ctx );
-
-//more driver types here? i.e. posefinders, etc.
-
-#endif
-
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 5962623..392104a 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -1,15 +1,5 @@
-//<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License.
+//<>< (C) 2016-2017 C. N. Lohr, MOSTLY Under MIT/x11 License.
//
-//Based off of https://github.com/collabora/OSVR-Vive-Libre
-// Originally Copyright 2016 Philipp Zabel
-// Originally Copyright 2016 Lubosz Sarnecki <lubosz.sarnecki@collabora.co.uk>
-// Originally Copyright (C) 2013 Fredrik Hultin
-// Originally Copyright (C) 2013 Jakob Bornecrantz
-//
-//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license.
-//If there are portions of the code too similar to the original, I would like to know so they can be re-written.
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-
#ifndef _SURVIVE_INTERNAL_H
#define _SURVIVE_INTERNAL_H
@@ -17,88 +7,16 @@
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
-#include "survive_driverman.h"
#include <zlib.h>
#include <survive.h>
-#define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); }
-#define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); }
-
-//XXX TODO This one needs to be rewritten.
-#define SV_KILL() exit(0)
-
-#define INTBUFFSIZE 64
-#define SENSORS_PER_OBJECT 32
-
-struct SurviveContext;
-struct SurviveUSBInterface;
-
-typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver );
-typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen );
-
-
-//This is defined in survive.h
-struct SurviveObject;
-struct SurviveCalData;
-
-struct BaseStationData
-{
- uint8_t PositionSet:1;
- FLT Position[3];
- FLT Quaternion[4];
-
- uint8_t OOTXSet:1;
- uint32_t BaseStationID;
- FLT fcalphase[2];
- FLT fcaltilt[2];
- FLT fcalcurve[2];
- FLT fcalgibpha[2];
- FLT fcalgibmag[2];
-};
-
-struct SurviveContext
-{
- text_feedback_func faultfunction;
- text_feedback_func notefunction;
- light_process_func lightproc;
- imu_process_func imuproc;
- angle_process_func angleproc;
-
- //Calibration data:
- struct BaseStationData bsd[NUM_LIGHTHOUSES];
-
- struct SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
-
- struct SurviveObject ** objs;
- int objs_ct;
-
- void ** drivers;
- DeviceDriverCb * driverpolls;
- DeviceDriverCb * drivercloses;
- DeviceDriverMagicCb * drivermagics;
- int driver_ct;
-};
-
-int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj );
-
-void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic );
-
-//For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is.
-struct LightcapElement
-{
- uint8_t sensor_id;
- uint8_t type;
- uint16_t length;
- uint32_t timestamp;
-} __attribute__((packed));
-
-//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
-void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le );
-
-//Accept Data from backend.
-void survive_data_cb( struct SurviveUSBInterface * si );
+//Driver registration
+#define MAX_DRIVERS 32
+void * GetDriver( const char * name );
+const char * GetDriverNameMatching( const char * prefix, int place );
+void ListDrivers();
#endif
diff --git a/src/survive_process.c b/src/survive_process.c
index 75453da..2fea99d 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -6,9 +6,9 @@
//XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
//proper angles, then from there perform the rest of the solution.
-void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
+void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
{
- struct SurviveContext * ctx = so->ctx;
+ SurviveContext * ctx = so->ctx;
int base_station = acode >> 2;
int axis = acode & 1;
@@ -25,7 +25,7 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in
//Need to now do angle correction.
#if 1
- struct BaseStationData * bsd = &ctx->bsd[base_station];
+ BaseStationData * bsd = &ctx->bsd[base_station];
//XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18
angle += bsd->fcalphase[axis];
@@ -39,20 +39,41 @@ void survive_default_light_process( struct SurviveObject * so, int sensor_id, in
}
-void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
{
- struct SurviveContext * ctx = so->ctx;
+ SurviveContext * ctx = so->ctx;
if( ctx->calptr )
{
survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
}
-
- //TODO: Writeme!
+ if( so->PoserFn )
+ {
+ PoserDataLight l = {
+ .pt = POSERDATA_LIGHT,
+ .sensor_id = sensor_id,
+ .acode = acode,
+ .timecode = timecode,
+ .length = length,
+ .angle = angle,
+ };
+ so->PoserFn( so, (PoserData *)&l );
+ }
}
-void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
{
- //TODO: Writeme!
+ if( so->PoserFn )
+ {
+ PoserDataIMU imu = {
+ .pt = POSERDATA_IMU,
+ .datamask = mask,
+ .accel = { accelgyromag[0], accelgyromag[1], accelgyromag[2] },
+ .gyro = { accelgyromag[3], accelgyromag[4], accelgyromag[5] },
+ .mag = { accelgyromag[6], accelgyromag[7], accelgyromag[8] },
+ .timecode = timecode,
+ };
+ so->PoserFn( so, (PoserData *)&imu );
+ }
}
diff --git a/src/survive_vive.c b/src/survive_vive.c
index a402153..7da2897 100644
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -10,26 +10,18 @@
//If there are portions of the code too similar to the original, I would like to know so they can be re-written.
//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
-#include "survive_internal.h"
-#include "survive_driverman.h"
+#include <survive.h>
#include <jsmn.h>
-
-#include "survive_internal.h"
#include <libusb-1.0/libusb.h>
#include <stdio.h>
-#include <unistd.h> //sleep if I ever use it.
+#include <stdlib.h>
+#include <unistd.h>
#include <errno.h>
#include <string.h>
+#include <sys/stat.h>
struct SurviveViveData;
-//USB Subsystem
-void survive_usb_close( struct SurviveContext * t );
-int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 );
-int survive_usb_poll( struct SurviveContext * ctx );
-int survive_get_config( char ** config, struct SurviveViveData * ctx, int devno, int interface, int send_extra_magic );
-
-
const short vidpids[] = {
0x0bb4, 0x2c87, 0, //The main HTC HMD device
0x28de, 0x2000, 0, //Valve lighthouse
@@ -59,15 +51,18 @@ const char * devnames[] = {
#define USB_IF_LIGHTCAP 4
#define MAX_INTERFACES 5
-typedef void (*usb_callback)( struct SurviveUSBInterface * ti );
+typedef struct SurviveUSBInterface SurviveUSBInterface;
+typedef struct SurviveViveData SurviveViveData;
+
+typedef void (*usb_callback)( SurviveUSBInterface * ti );
struct SurviveUSBInterface
{
- struct SurviveViveData * sv;
- struct SurviveContext * ctx;
+ SurviveViveData * sv;
+ SurviveContext * ctx;
struct libusb_transfer * transfer;
- struct SurviveObject * assoc_obj;
+ SurviveObject * assoc_obj;
int actual_len;
uint8_t buffer[INTBUFFSIZE];
usb_callback cb;
@@ -77,16 +72,24 @@ struct SurviveUSBInterface
struct SurviveViveData
{
- struct SurviveContext * ctx;
+ SurviveContext * ctx;
//XXX TODO: UN-STATICIFY THIS.
- struct SurviveUSBInterface uiface[MAX_INTERFACES];
+ SurviveUSBInterface uiface[MAX_INTERFACES];
//USB Subsystem
struct libusb_context* usbctx;
struct libusb_device_handle * udev[MAX_USB_DEVS];
};
+void survive_data_cb( SurviveUSBInterface * si );
+
+//USB Subsystem
+void survive_usb_close( SurviveContext * t );
+int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1 );
+int survive_usb_poll( SurviveContext * ctx );
+int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int interface, int send_extra_magic );
+
@@ -114,10 +117,10 @@ static void handle_transfer(struct libusb_transfer* transfer)
-static int AttachInterface( struct SurviveViveData * sv, struct SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname )
+static int AttachInterface( SurviveViveData * sv, SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname )
{
- struct SurviveContext * ctx = sv->ctx;
- struct SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i];
+ SurviveContext * ctx = sv->ctx;
+ SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i];
iface->ctx = ctx;
iface->sv = sv;
iface->which_interface_am_i = which_interface_am_i;
@@ -592,7 +595,13 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset.
{
propset |= 2;
- w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
+ //XXX XXX BIG TODO!!! Actually recal gyro data.
+ FLT agm[9] = { readdata[1], readdata[2], readdata[3],
+ readdata[4], readdata[5], readdata[6],
+ 0,0,0 };
+
+ w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 );
+
int16_t * k = (int16_t *)readdata+1;
//printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] );
readdata += 13; qty -= 13;
@@ -679,7 +688,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
}
- struct LightcapElement les[10];
+ LightcapElement les[10];
int lese = 0; //les's end
//Second, go through all LEDs and extract the lightevent from them.
@@ -710,7 +719,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
//reverse sorted, but that is to minimize operations. To read it
//in sorted order simply read it back backwards.
//Use insertion sort, since we should most of the time, be in order.
- struct LightcapElement * le = &les[lese++];
+ LightcapElement * le = &les[lese++];
le->sensor_id = led;
le->type = 0xfe;
@@ -724,7 +733,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
int swap = lese-2;
while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp )
{
- struct LightcapElement l;
+ LightcapElement l;
memcpy( &l, &les[swap], sizeof( l ) );
memcpy( &les[swap], &les[swap+1], sizeof( l ) );
memcpy( &les[swap+1], &l, sizeof( l ) );
@@ -743,17 +752,17 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
return;
end:
{
- struct SurviveContext * ctx = w->ctx;
+ SurviveContext * ctx = w->ctx;
SV_INFO( "Light decoding fault: %d\n", fault );
}
}
}
-void survive_data_cb( struct SurviveUSBInterface * si )
+void survive_data_cb( SurviveUSBInterface * si )
{
int size = si->actual_len;
- struct SurviveContext * ctx = si->ctx;
+ SurviveContext * ctx = si->ctx;
#if 0
int i;
printf( "%16s: %d: ", si->hname, len );
@@ -766,7 +775,7 @@ void survive_data_cb( struct SurviveUSBInterface * si )
#endif
int iface = si->which_interface_am_i;
- struct SurviveObject * obj = si->assoc_obj;
+ SurviveObject * obj = si->assoc_obj;
uint8_t * readdata = si->buffer;
int id = POP1;
@@ -807,7 +816,13 @@ void survive_data_cb( struct SurviveUSBInterface * si )
if( cd > 0 )
{
obj->oldcode = code;
- ctx->imuproc( obj, acceldata, timecode, code );
+
+ //XXX XXX BIG TODO!!! Actually recal gyro data.
+ FLT agm[9] = { acceldata[0], acceldata[1], acceldata[2],
+ acceldata[3], acceldata[4], acceldata[5],
+ 0,0,0 };
+
+ ctx->imuproc( obj, 3, agm, timecode, code );
}
}
@@ -817,7 +832,7 @@ void survive_data_cb( struct SurviveUSBInterface * si )
case USB_IF_WATCHMAN1:
case USB_IF_WATCHMAN2:
{
- struct SurviveObject * w = obj;
+ SurviveObject * w = obj;
if( id == 35 )
{
handle_watchman( w, readdata);
@@ -843,7 +858,7 @@ void survive_data_cb( struct SurviveUSBInterface * si )
int i;
for( i = 0; i < 7; i++ )
{
- handle_lightcap( obj, (struct LightcapElement*)&readdata[i*8] );
+ handle_lightcap( obj, (LightcapElement*)&readdata[i*8] );
}
break;
}
@@ -874,7 +889,7 @@ static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
}
-static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
+static int ParsePoints( SurviveContext * ctx, SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
{
int k;
int pts = t[i+1].size;
@@ -913,9 +928,9 @@ static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so,
return 0;
}
-static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, int devno, int iface, int extra_magic )
+static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int iface, int extra_magic )
{
- struct SurviveContext * ctx = sv->ctx;
+ SurviveContext * ctx = sv->ctx;
char * ct0conf = 0;
int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic );
@@ -976,25 +991,46 @@ static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, i
//TODO: Cleanup any remaining USB stuff.
return 1;
}
+
+ char fname[20];
+ mkdir( "calinfo", 0755 );
+
+ sprintf( fname, "calinfo/%s_points.csv", so->codename );
+ FILE * f = fopen( fname, "w" );
+ int j;
+ for( j = 0; j < so->nr_locations; j++ )
+ {
+ fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] );
+ }
+ fclose( f );
+
+ sprintf( fname, "calinfo/%s_normals.csv", so->codename );
+ f = fopen( fname, "w" );
+ for( j = 0; j < so->nr_locations; j++ )
+ {
+ fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] );
+ }
+ fclose( f );
+
return 0;
}
-int survive_vive_close( struct SurviveContext * ctx, void * driver )
+int survive_vive_close( SurviveContext * ctx, void * driver )
{
- struct SurviveViveData * sv = driver;
+ SurviveViveData * sv = driver;
survive_vive_usb_close( sv );
}
-int DriverRegHTCVive( struct SurviveContext * ctx )
+int DriverRegHTCVive( SurviveContext * ctx )
{
int i, r;
- struct SurviveObject * hmd = calloc( 1, sizeof( struct SurviveObject ) );
- struct SurviveObject * wm0 = calloc( 1, sizeof( struct SurviveObject ) );
- struct SurviveObject * wm1 = calloc( 1, sizeof( struct SurviveObject ) );
- struct SurviveViveData * sv = calloc( 1, sizeof( struct SurviveViveData ) );
+ SurviveObject * hmd = calloc( 1, sizeof( SurviveObject ) );
+ SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) );
+ SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) );
+ SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) );
sv->ctx = ctx;
diff --git a/tools/plot_lighthouse/Makefile b/tools/plot_lighthouse/Makefile
index db382ea..38eece0 100644
--- a/tools/plot_lighthouse/Makefile
+++ b/tools/plot_lighthouse/Makefile
@@ -6,7 +6,7 @@ endif
# Darwin is Mac OSX !!
ifeq ($(UNAME), Darwin)
-CFLAGS:= -w -framework OpenGL -framework GLUT
+CFLAGS:= -I../../redist -w -framework OpenGL -framework GLUT
endif
all: