aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-03-09 00:41:46 -0700
committerJustin Berger <j.david.berger@gmail.com>2018-03-09 00:41:46 -0700
commitfe88ef57cff4971664b252cb9def6a5ddcc4276d (patch)
tree9a93a78bae2499a8d9e124027d45127071977561
parent3ec96c9a81f0f770fac75a1bdfc16ea96df07d9b (diff)
downloadlibsurvive-fe88ef57cff4971664b252cb9def6a5ddcc4276d.tar.gz
libsurvive-fe88ef57cff4971664b252cb9def6a5ddcc4276d.tar.bz2
Spaces -> Tabs (code formatting change)
-rw-r--r--data_recorder.c250
-rw-r--r--src/survive_default_devices.c309
-rw-r--r--src/survive_default_devices.h18
-rw-r--r--[-rwxr-xr-x]src/survive_playback.c375
4 files changed, 483 insertions, 469 deletions
diff --git a/data_recorder.c b/data_recorder.c
index 63746c3..6123d57 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -1,204 +1,204 @@
-//Data recorder mod with GUI showing light positions.
+// Data recorder mod with GUI showing light positions.
#ifdef __linux__
#include <unistd.h>
#endif
+#include <CNFGFunctions.h>
+#include <os_generic.h>
+#include <stdarg.h>
+#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
-#include <stdint.h>
-#include <survive.h>
#include <string.h>
-#include <os_generic.h>
-#include <CNFGFunctions.h>
-#include <time.h>
+#include <survive.h>
#include <sys/time.h>
-#include <stdarg.h>
+#include <time.h>
#include "redist/os_generic.h"
-struct SurviveContext * ctx;
+struct SurviveContext *ctx;
-FILE* output_file = 0;
+FILE *output_file = 0;
-void HandleKey( int keycode, int bDown )
-{
- if( !bDown ) return;
+void HandleKey(int keycode, int bDown) {
+ if (!bDown)
+ return;
- if( keycode == 'O' || keycode == 'o' )
- {
- survive_send_magic(ctx,1,0,0);
+ if (keycode == 'O' || keycode == 'o') {
+ survive_send_magic(ctx, 1, 0, 0);
}
- if( keycode == 'F' || keycode == 'f' )
- {
- survive_send_magic(ctx,0,0,0);
+ if (keycode == 'F' || keycode == 'f') {
+ survive_send_magic(ctx, 0, 0, 0);
}
}
-void HandleButton( int x, int y, int button, int bDown )
-{
-}
+void HandleButton(int x, int y, int button, int bDown) {}
-void HandleMotion( int x, int y, int mask )
-{
-}
+void HandleMotion(int x, int y, int mask) {}
-void HandleDestroy()
-{
-}
+void HandleDestroy() {}
-int bufferpts[32*2*3];
-char buffermts[32*128*3];
-int buffertimeto[32*3];
+int bufferpts[32 * 2 * 3];
+char buffermts[32 * 128 * 3];
+int buffertimeto[32 * 3];
double timestamp_in_us() {
- static double start_time_us = 0;
- if(start_time_us == 0)
- start_time_us = OGGetAbsoluteTime();
- return OGGetAbsoluteTime() - start_time_us;
+ static double start_time_us = 0;
+ if (start_time_us == 0)
+ start_time_us = OGGetAbsoluteTime();
+ return OGGetAbsoluteTime() - start_time_us;
}
-int write_to_output(const char *format, ...)
-{
- va_list args;
- va_start(args, format);
- fprintf(output_file, "%.17g ", timestamp_in_us());
- vfprintf(output_file, format, args);
+int write_to_output(const char *format, ...) {
+ va_list args;
+ va_start(args, format);
+ fprintf(output_file, "%.17g ", timestamp_in_us());
+ vfprintf(output_file, format, args);
- va_end(args);
+ va_end(args);
}
-void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
-{
- survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
+void my_light_process(struct SurviveObject *so, int sensor_id, int acode,
+ int timeinsweep, uint32_t timecode, uint32_t length,
+ uint32_t lh) {
+ survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode,
+ length, lh);
- if( acode == -1 ) {
- write_to_output( "A %s %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh );
- return;
+ if (acode == -1) {
+ write_to_output("A %s %d %d %d %u %u %u\n", so->codename, sensor_id,
+ acode, timeinsweep, timecode, length, lh);
+ return;
}
int jumpoffset = sensor_id;
- if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
- else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64;
+ if (strcmp(so->codename, "WM0") == 0)
+ jumpoffset += 32;
+ else if (strcmp(so->codename, "WM1") == 0)
+ jumpoffset += 64;
+
+ const char *LH_ID = 0;
+ const char *LH_Axis = 0;
- const char* LH_ID = 0;
- const char* LH_Axis = 0;
-
- switch(acode) {
+ switch (acode) {
case 0:
case 2:
- bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
- LH_ID = "L"; LH_Axis = "X"; break;
+ bufferpts[jumpoffset * 2 + 0] = (timeinsweep - 100000) / 500;
+ LH_ID = "L";
+ LH_Axis = "X";
+ break;
case 1:
case 3:
- bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
- LH_ID = "L"; LH_Axis = "Y"; break;
+ bufferpts[jumpoffset * 2 + 1] = (timeinsweep - 100000) / 500;
+ LH_ID = "L";
+ LH_Axis = "Y";
+ break;
case 4:
case 6:
- bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
- LH_ID = "R"; LH_Axis = "X"; break;
+ bufferpts[jumpoffset * 2 + 0] = (timeinsweep - 100000) / 500;
+ LH_ID = "R";
+ LH_Axis = "X";
+ break;
case 5:
case 7:
- bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
- LH_ID = "R"; LH_Axis = "Y"; break;
- }
+ bufferpts[jumpoffset * 2 + 1] = (timeinsweep - 100000) / 500;
+ LH_ID = "R";
+ LH_Axis = "Y";
+ break;
+ }
- write_to_output( "%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis, so->codename, timecode, sensor_id, acode, timeinsweep, length, lh );
+ write_to_output("%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis,
+ so->codename, timecode, sensor_id, acode, timeinsweep,
+ length, lh);
buffertimeto[jumpoffset] = 0;
-
}
-void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id )
-{
- survive_default_imu_process( so, mask, accelgyro, timecode, id );
- write_to_output( "I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n", so->codename, mask, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
+void my_imu_process(struct SurviveObject *so, int mask, FLT *accelgyro,
+ uint32_t timecode, int id) {
+ survive_default_imu_process(so, mask, accelgyro, timecode, id);
+ write_to_output("I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n",
+ so->codename, mask, timecode, accelgyro[0], accelgyro[1],
+ accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id);
}
-
-
-
-void * GuiThread( void * v )
-{
+void *GuiThread(void *v) {
CNFGBGColor = 0x000000;
CNFGDialogColor = 0x444444;
- CNFGSetup( "Survive GUI Debug", 640, 480 );
+ CNFGSetup("Survive GUI Debug", 640, 480);
short screenx, screeny;
- while(1)
- {
+ while (1) {
CNFGHandleInput();
CNFGClearFrame();
- CNFGColor( 0xFFFFFF );
- CNFGGetDimensions( &screenx, &screeny );
+ CNFGColor(0xFFFFFF);
+ CNFGGetDimensions(&screenx, &screeny);
int i;
- for( i = 0; i < 32*3; i++ )
- {
- if( buffertimeto[i] < 50 )
- {
+ for (i = 0; i < 32 * 3; i++) {
+ if (buffertimeto[i] < 50) {
uint32_t color = i * 3231349;
uint8_t r = color & 0xff;
- uint8_t g = (color>>8) & 0xff;
- uint8_t b = (color>>16) & 0xff;
- r = (r * (5-buffertimeto[i])) / 5 ;
- g = (g * (5-buffertimeto[i])) / 5 ;
- b = (b * (5-buffertimeto[i])) / 5 ;
- CNFGColor( (b<<16) | (g<<8) | r );
- CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 );
- CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1];
- CNFGDrawText( buffermts, 2 );
+ uint8_t g = (color >> 8) & 0xff;
+ uint8_t b = (color >> 16) & 0xff;
+ r = (r * (5 - buffertimeto[i])) / 5;
+ g = (g * (5 - buffertimeto[i])) / 5;
+ b = (b * (5 - buffertimeto[i])) / 5;
+ CNFGColor((b << 16) | (g << 8) | r);
+ CNFGTackRectangle(bufferpts[i * 2 + 0], bufferpts[i * 2 + 1],
+ bufferpts[i * 2 + 0] + 5,
+ bufferpts[i * 2 + 1] + 5);
+ CNFGPenX = bufferpts[i * 2 + 0];
+ CNFGPenY = bufferpts[i * 2 + 1];
+ CNFGDrawText(buffermts, 2);
buffertimeto[i]++;
}
}
CNFGSwapBuffers();
- OGUSleep( 10000 );
+ OGUSleep(10000);
}
}
-int SurviveThreadLoaded=0;
+int SurviveThreadLoaded = 0;
-void *SurviveThread(void *junk)
-{
- ctx = survive_init( 0 );
+void *SurviveThread(void *junk) {
+ ctx = survive_init(0);
- survive_install_light_fn( ctx, my_light_process );
- survive_install_imu_fn( ctx, my_imu_process );
+ survive_install_light_fn(ctx, my_light_process);
+ survive_install_imu_fn(ctx, my_imu_process);
- if( !ctx )
- {
- fprintf( stderr, "Fatal. Could not start\n" );
+ if (!ctx) {
+ fprintf(stderr, "Fatal. Could not start\n");
exit(1);
}
SurviveThreadLoaded = 1;
- while(survive_poll(ctx) == 0)
- {
+ while (survive_poll(ctx) == 0) {
}
- return 0;
+ return 0;
}
-int main(int argc, char** argv)
-{
- if(argc > 1) {
- output_file = fopen(argv[1], "w");
- if(output_file == 0) {
- fprintf(stderr, "Could not open %s for writing", argv[1]);
- return -1;
- }
- } else {
- output_file = stdout;
- }
-
- // Create the libsurvive thread
- OGCreateThread(SurviveThread, 0);
-
- // Wait for the survive thread to load
- while (!SurviveThreadLoaded) { OGUSleep(100); }
-
- // Run the Gui in the main thread
- GuiThread(0);
-}
+int main(int argc, char **argv) {
+ if (argc > 1) {
+ output_file = fopen(argv[1], "w");
+ if (output_file == 0) {
+ fprintf(stderr, "Could not open %s for writing", argv[1]);
+ return -1;
+ }
+ } else {
+ output_file = stdout;
+ }
+
+ // Create the libsurvive thread
+ OGCreateThread(SurviveThread, 0);
+ // Wait for the survive thread to load
+ while (!SurviveThreadLoaded) {
+ OGUSleep(100);
+ }
+
+ // Run the Gui in the main thread
+ GuiThread(0);
+}
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index 1882002..6615f1e 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -1,93 +1,92 @@
+#include "survive_default_devices.h"
#include <jsmn.h>
+#include <stdio.h>
#include <stdlib.h>
#include <string.h>
-#include "survive_default_devices.h"
-#include <stdio.h>
#include "json_helpers.h"
-static SurviveObject* survive_create_device(SurviveContext * ctx,
- const char* driver_name,
- void* driver,
- const char* device_name,
- haptic_func fn) {
- SurviveObject * device = calloc( 1, sizeof( SurviveObject ) );
-
- device->ctx = ctx;
- device->driver = driver;
- memcpy( device->codename, device_name, strlen(device_name) );
- memcpy( device->drivername, driver_name, strlen(driver_name) );
-
- device->timebase_hz = 48000000;
- device->pulsedist_max_ticks = 500000;
- device->pulselength_min_sync = 2200;
- device->pulse_in_clear_time = 35000;
- device->pulse_max_for_sweep = 1800;
- device->pulse_synctime_offset = 20000;
- device->pulse_synctime_slack = 5000;
- device->timecenter_ticks = device->timebase_hz / 240;
-
- device->haptic = fn;
-
- return device;
+static SurviveObject *
+survive_create_device(SurviveContext *ctx, const char *driver_name,
+ void *driver, const char *device_name, haptic_func fn) {
+ SurviveObject *device = calloc(1, sizeof(SurviveObject));
+
+ device->ctx = ctx;
+ device->driver = driver;
+ memcpy(device->codename, device_name, strlen(device_name));
+ memcpy(device->drivername, driver_name, strlen(driver_name));
+
+ device->timebase_hz = 48000000;
+ device->pulsedist_max_ticks = 500000;
+ device->pulselength_min_sync = 2200;
+ device->pulse_in_clear_time = 35000;
+ device->pulse_max_for_sweep = 1800;
+ device->pulse_synctime_offset = 20000;
+ device->pulse_synctime_slack = 5000;
+ device->timecenter_ticks = device->timebase_hz / 240;
+
+ device->haptic = fn;
+
+ return device;
}
-SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver) {
- return survive_create_device(ctx, driver_name, driver, "HMD", 0);
+SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name,
+ void *driver) {
+ return survive_create_device(ctx, driver_name, driver, "HMD", 0);
}
-SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) {
- return survive_create_device(ctx, driver_name, driver, "WM0", fn);
+SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name,
+ void *driver, haptic_func fn) {
+ return survive_create_device(ctx, driver_name, driver, "WM0", fn);
}
-SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func fn) {
- return survive_create_device(ctx, driver_name, driver, "WM1", fn);
+SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name,
+ void *driver, haptic_func fn) {
+ return survive_create_device(ctx, driver_name, driver, "WM1", fn);
}
-SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver) {
- return survive_create_device(ctx, driver_name, driver, "TR0", 0);
+SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name,
+ void *driver) {
+ return survive_create_device(ctx, driver_name, driver, "TR0", 0);
}
-SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver) {
- return survive_create_device(ctx, driver_name, driver, "WW0", 0);
+SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name,
+ void *driver) {
+ return survive_create_device(ctx, driver_name, driver, "WW0", 0);
}
-
static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
- if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
- strncmp(json + tok->start, s, tok->end - tok->start) == 0) {
+ if (tok->type == JSMN_STRING && (int)strlen(s) == tok->end - tok->start &&
+ strncmp(json + tok->start, s, tok->end - tok->start) == 0) {
return 0;
}
return -1;
}
-static int ParsePoints( SurviveContext * ctx, 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;
- jsmntok_t * tk;
+ int pts = t[i + 1].size;
+ jsmntok_t *tk;
so->nr_locations = 0;
- *floats_out = malloc( sizeof( **floats_out ) * 32 * 3 );
+ *floats_out = malloc(sizeof(**floats_out) * 32 * 3);
- for( k = 0; k < pts; k++ )
- {
- tk = &t[i+2+k*4];
+ for (k = 0; k < pts; k++) {
+ tk = &t[i + 2 + k * 4];
int m;
- for( m = 0; m < 3; m++ )
- {
+ for (m = 0; m < 3; m++) {
char ctt[128];
tk++;
int elemlen = tk->end - tk->start;
- if( tk->type != 4 || elemlen > sizeof( ctt )-1 )
- {
- SV_ERROR( "Parse error in JSON\n" );
+ if (tk->type != 4 || elemlen > sizeof(ctt) - 1) {
+ SV_ERROR("Parse error in JSON\n");
return 1;
}
- memcpy( ctt, ct0conf + tk->start, elemlen );
+ memcpy(ctt, ct0conf + tk->start, elemlen);
ctt[elemlen] = 0;
- FLT f = atof( ctt );
- int id = so->nr_locations*3+m;
+ FLT f = atof(ctt);
+ int id = so->nr_locations * 3 + m;
(*floats_out)[id] = f;
}
so->nr_locations++;
@@ -95,106 +94,106 @@ static int ParsePoints( SurviveContext * ctx, SurviveObject * so, char * ct0conf
return 0;
}
-int survive_load_htc_config_format(char* ct0conf, int len, SurviveObject * so) {
- if (len == 0)
- return -1;
-
- SurviveContext* ctx = so->ctx;
- //From JSMN example.
- jsmn_parser p;
- jsmntok_t t[4096];
- jsmn_init(&p);
- int i;
- int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0]));
- if (r < 0) {
- SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
- return -1;
- }
- if (r < 1 || t[0].type != JSMN_OBJECT) {
- SV_INFO("Object expected in HMD configuration\n");
- return -2;
- }
-
- for (i = 1; i < r; i++) {
- jsmntok_t * tk = &t[i];
-
- char ctxo[100];
- int ilen = tk->end - tk->start;
- if( ilen > 99 ) ilen = 99;
- memcpy(ctxo, ct0conf + tk->start, ilen);
- ctxo[ilen] = 0;
-
- // printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
- // printf( "%.*s\n", ilen, ct0conf + tk->start );
-
- if (jsoneq(ct0conf, tk, "modelPoints") == 0) {
- if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i ) )
- {
- break;
+int survive_load_htc_config_format(char *ct0conf, int len, SurviveObject *so) {
+ if (len == 0)
+ return -1;
+
+ SurviveContext *ctx = so->ctx;
+ // From JSMN example.
+ jsmn_parser p;
+ jsmntok_t t[4096];
+ jsmn_init(&p);
+ int i;
+ int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t) / sizeof(t[0]));
+ if (r < 0) {
+ SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
+ return -1;
}
- }
- if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
- if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i ) )
- {
- break;
+ if (r < 1 || t[0].type != JSMN_OBJECT) {
+ SV_INFO("Object expected in HMD configuration\n");
+ return -2;
}
- }
-
-
- if (jsoneq(ct0conf, tk, "acc_bias") == 0) {
- int32_t count = (tk+1)->size;
- FLT* values = NULL;
- if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
- so->acc_bias = values;
- so->acc_bias[0] *= .125; //XXX Wat? Observed by CNL. Biasing by more than this seems to hose things.
- so->acc_bias[1] *= .125;
- so->acc_bias[2] *= .125;
- }
- }
- if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
- int32_t count = (tk+1)->size;
- FLT* values = NULL;
- if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
- so->acc_scale = values;
- }
- }
-
- if (jsoneq(ct0conf, tk, "gyro_bias") == 0) {
- int32_t count = (tk+1)->size;
- FLT* values = NULL;
- if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
- so->gyro_bias = values;
- }
- }
- if (jsoneq(ct0conf, tk, "gyro_scale") == 0) {
- int32_t count = (tk+1)->size;
- FLT* values = NULL;
- if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
- so->gyro_scale = values;
- }
- }
- }
-
-
- char fname[64];
-
- 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;
+
+ for (i = 1; i < r; i++) {
+ jsmntok_t *tk = &t[i];
+
+ char ctxo[100];
+ int ilen = tk->end - tk->start;
+ if (ilen > 99)
+ ilen = 99;
+ memcpy(ctxo, ct0conf + tk->start, ilen);
+ ctxo[ilen] = 0;
+
+ // printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start,
+ //tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
+ // printf( "%.*s\n", ilen, ct0conf + tk->start );
+
+ if (jsoneq(ct0conf, tk, "modelPoints") == 0) {
+ if (ParsePoints(ctx, so, ct0conf, &so->sensor_locations, t, i)) {
+ break;
+ }
+ }
+ if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
+ if (ParsePoints(ctx, so, ct0conf, &so->sensor_normals, t, i)) {
+ break;
+ }
+ }
+
+ if (jsoneq(ct0conf, tk, "acc_bias") == 0) {
+ int32_t count = (tk + 1)->size;
+ FLT *values = NULL;
+ if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+ so->acc_bias = values;
+ so->acc_bias[0] *= .125; // XXX Wat? Observed by CNL. Biasing
+ // by more than this seems to hose
+ // things.
+ so->acc_bias[1] *= .125;
+ so->acc_bias[2] *= .125;
+ }
+ }
+ if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
+ int32_t count = (tk + 1)->size;
+ FLT *values = NULL;
+ if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+ so->acc_scale = values;
+ }
+ }
+
+ if (jsoneq(ct0conf, tk, "gyro_bias") == 0) {
+ int32_t count = (tk + 1)->size;
+ FLT *values = NULL;
+ if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+ so->gyro_bias = values;
+ }
+ }
+ if (jsoneq(ct0conf, tk, "gyro_scale") == 0) {
+ int32_t count = (tk + 1)->size;
+ FLT *values = NULL;
+ if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+ so->gyro_scale = values;
+ }
+ }
+ }
+
+ char fname[64];
+
+ 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;
}
diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h
index fce199d..1fcca72 100644
--- a/src/survive_default_devices.h
+++ b/src/survive_default_devices.h
@@ -3,11 +3,17 @@
#include <survive.h>
-SurviveObject* survive_create_hmd(SurviveContext * ctx, const char* driver_name, void* driver);
-SurviveObject* survive_create_wm0(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb);
-SurviveObject* survive_create_wm1(SurviveContext * ctx, const char* driver_name, void* driver, haptic_func cb);
-SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name, void* driver);
-SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver);
+SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name,
+ void *driver);
+SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name,
+ void *driver, haptic_func cb);
+SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name,
+ void *driver, haptic_func cb);
+SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name,
+ void *driver);
+SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name,
+ void *driver);
-int survive_load_htc_config_format(char* ct0conf, int length, SurviveObject * so);
+int survive_load_htc_config_format(char *ct0conf, int length,
+ SurviveObject *so);
#endif
diff --git a/src/survive_playback.c b/src/survive_playback.c
index 7c49f5b..4b25b4c 100755..100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -1,8 +1,9 @@
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL
+// or LGPL licenses.
-#include <survive.h>
#include <stdio.h>
#include <stdlib.h>
+#include <survive.h>
#include <string.h>
#include <sys/time.h>
@@ -13,221 +14,229 @@
#include "redist/os_generic.h"
struct SurvivePlaybackData {
- SurviveContext * ctx;
- const char* playback_dir;
- FILE* playback_file;
- int lineno;
+ SurviveContext *ctx;
+ const char *playback_dir;
+ FILE *playback_file;
+ int lineno;
- FLT time_factor;
- double next_time_us;
+ FLT time_factor;
+ double next_time_us;
};
typedef struct SurvivePlaybackData SurvivePlaybackData;
-
double timestamp_in_us() {
- static double start_time_us = 0;
- if(start_time_us == 0.)
- start_time_us = OGGetAbsoluteTime();
- return OGGetAbsoluteTime() - start_time_us;
+ static double start_time_us = 0;
+ if (start_time_us == 0.)
+ start_time_us = OGGetAbsoluteTime();
+ return OGGetAbsoluteTime() - start_time_us;
}
-static int parse_and_run_imu(const char* line, SurvivePlaybackData* driver) {
- char dev[10];
- int timecode = 0;
- FLT accelgyro[6];
- int mask;
- int id;
-
- int rr = sscanf(line,"I %s %d %d " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format "%d", dev,
- &mask,
- &timecode,
- &accelgyro[0], &accelgyro[1], &accelgyro[2],
- &accelgyro[3], &accelgyro[4], &accelgyro[5], &id );
-
- if( rr != 10 )
- {
- fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line );
- return -1;
- }
-
- SurviveObject * so = survive_get_so_by_name( driver->ctx, dev);
- if(!so) {
- fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno);
- return -1;
- }
-
- driver->ctx->imuproc( so, mask, accelgyro, timecode, id);
- return 0;
+static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) {
+ char dev[10];
+ int timecode = 0;
+ FLT accelgyro[6];
+ int mask;
+ int id;
+
+ int rr =
+ sscanf(line, "I %s %d %d " FLT_format " " FLT_format " " FLT_format
+ " " FLT_format " " FLT_format " " FLT_format "%d",
+ dev, &mask, &timecode, &accelgyro[0], &accelgyro[1],
+ &accelgyro[2], &accelgyro[3], &accelgyro[4], &accelgyro[5], &id);
+
+ if (rr != 10) {
+ fprintf(stderr, "Warning: On line %d, only %d values read: '%s'\n",
+ driver->lineno, rr, line);
+ return -1;
+ }
+
+ SurviveObject *so = survive_get_so_by_name(driver->ctx, dev);
+ if (!so) {
+ fprintf(stderr, "Could not find device named %s from lineno %d\n", dev,
+ driver->lineno);
+ return -1;
+ }
+
+ driver->ctx->imuproc(so, mask, accelgyro, timecode, id);
+ return 0;
}
+static int parse_and_run_lightcode(const char *line,
+ SurvivePlaybackData *driver) {
+ char lhn[10];
+ char axn[10];
+ char dev[10];
+ uint32_t timecode = 0;
+ int sensor = 0;
+ int acode = 0;
+ int timeinsweep = 0;
+ uint32_t pulselength = 0;
+ uint32_t lh = 0;
+
+ int rr =
+ sscanf(line, "%8s %8s %8s %u %d %d %d %u %u\n", lhn, axn, dev,
+ &timecode, &sensor, &acode, &timeinsweep, &pulselength, &lh);
+
+ if (rr != 9) {
+ fprintf(stderr, "Warning: On line %d, only %d values read: '%s'\n",
+ driver->lineno, rr, line);
+ return -1;
+ }
-static int parse_and_run_lightcode(const char* line, SurvivePlaybackData* driver) {
- char lhn[10];
- char axn[10];
- char dev[10];
- uint32_t timecode = 0;
- int sensor = 0;
- int acode = 0;
- int timeinsweep = 0;
- uint32_t pulselength = 0;
- uint32_t lh = 0;
-
- int rr = sscanf(line,"%8s %8s %8s %u %d %d %d %u %u\n",
- lhn, axn, dev,
- &timecode, &sensor, &acode,
- &timeinsweep, &pulselength, &lh );
-
- if( rr != 9 )
- {
- fprintf( stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line );
- return -1;
- }
-
- SurviveObject * so = survive_get_so_by_name( driver->ctx, dev);
- if(!so) {
- fprintf(stderr, "Could not find device named %s from lineno %d\n", dev, driver->lineno);
- return -1;
+ SurviveObject *so = survive_get_so_by_name(driver->ctx, dev);
+ if (!so) {
+ fprintf(stderr, "Could not find device named %s from lineno %d\n", dev,
+ driver->lineno);
+ return -1;
}
-
- driver->ctx->lightproc( so, sensor, acode, timeinsweep, timecode, pulselength, lh);
+
+ driver->ctx->lightproc(so, sensor, acode, timeinsweep, timecode,
+ pulselength, lh);
return 0;
}
-static int playback_poll( struct SurviveContext * ctx, void * _driver ) {
- SurvivePlaybackData* driver = _driver;
- FILE* f = driver->playback_file;
-
- if(f && !feof(f) && !ferror(f) )
- {
- int i;
- driver->lineno++;
- char * line;
-
- if(driver->next_time_us == 0) {
- char * buffer;
- size_t n = 0;
- ssize_t r = getdelim( &line, &n, ' ', f );
- if( r <= 0 ) return 0;
-
- if(sscanf(line, "%lf", &driver->next_time_us) != 1) {
- free(line);
- return 0;
+static int playback_poll(struct SurviveContext *ctx, void *_driver) {
+ SurvivePlaybackData *driver = _driver;
+ FILE *f = driver->playback_file;
+
+ if (f && !feof(f) && !ferror(f)) {
+ int i;
+ driver->lineno++;
+ char *line;
+
+ if (driver->next_time_us == 0) {
+ char *buffer;
+ size_t n = 0;
+ ssize_t r = getdelim(&line, &n, ' ', f);
+ if (r <= 0)
+ return 0;
+
+ if (sscanf(line, "%lf", &driver->next_time_us) != 1) {
+ free(line);
+ return 0;
+ }
+ free(line);
+ line = 0;
+ }
+
+ if (driver->next_time_us * driver->time_factor > timestamp_in_us())
+ return 0;
+ driver->next_time_us = 0;
+
+ char *buffer;
+ size_t n = 0;
+ ssize_t r = getline(&line, &n, f);
+ if (r <= 0)
+ return 0;
+
+ if ((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') ||
+ line[1] != ' ')
+ return 0;
+
+ switch (line[0]) {
+ case 'L':
+ case 'R':
+ parse_and_run_lightcode(line, driver);
+ break;
+ case 'I':
+ parse_and_run_imu(line, driver);
+ break;
+ }
+
+ free(line);
+ } else {
+ if (f) {
+ fclose(driver->playback_file);
+ }
+ driver->playback_file = 0;
+ return -1;
}
- free(line);
- line = 0;
- }
- if(driver->next_time_us * driver->time_factor > timestamp_in_us())
return 0;
- driver->next_time_us = 0;
-
- char * buffer;
- size_t n = 0;
- ssize_t r = getline( &line, &n, f );
- if( r <= 0 ) return 0;
-
- if((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') || line[1] != ' ' )
- return 0;
-
- switch(line[0]) {
- case 'L':
- case 'R':
- parse_and_run_lightcode(line, driver);
- break;
- case 'I':
- parse_and_run_imu(line, driver);
- break;
- }
-
- free( line );
- } else {
- if(f) {
- fclose(driver->playback_file);
- }
- driver->playback_file = 0;
- return -1;
- }
-
- return 0;
}
-static int playback_close( struct SurviveContext * ctx, void * _driver ) {
- SurvivePlaybackData* driver = _driver;
- if(driver->playback_file)
- fclose(driver->playback_file);
- driver->playback_file = 0;
- return 0;
+static int playback_close(struct SurviveContext *ctx, void *_driver) {
+ SurvivePlaybackData *driver = _driver;
+ if (driver->playback_file)
+ fclose(driver->playback_file);
+ driver->playback_file = 0;
+ return 0;
}
+static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) {
+ SurviveContext *ctx = sv->ctx;
+ char *ct0conf = 0;
-static int LoadConfig( SurvivePlaybackData * sv, SurviveObject * so)
-{
- SurviveContext * ctx = sv->ctx;
- char * ct0conf = 0;
-
char fname[100];
- sprintf( fname, "%s/%s_config.json", sv->playback_dir, so->codename );
- FILE * f = fopen( fname, "r" );
+ sprintf(fname, "%s/%s_config.json", sv->playback_dir, so->codename);
+ FILE *f = fopen(fname, "r");
+
+ if (f == 0 || feof(f) || ferror(f))
+ return 1;
- if(f == 0 || feof(f) || ferror(f) )
- return 1;
-
fseek(f, 0, SEEK_END);
int len = ftell(f);
- fseek(f, 0, SEEK_SET); //same as rewind(f);
+ fseek(f, 0, SEEK_SET); // same as rewind(f);
- ct0conf = malloc(len+1);
- int read = fread( ct0conf, len, 1, f);
- fclose( f );
+ ct0conf = malloc(len + 1);
+ int read = fread(ct0conf, len, 1, f);
+ fclose(f);
ct0conf[len] = 0;
- printf( "Loading config: %d\n", len );
+ printf("Loading config: %d\n", len);
return survive_load_htc_config_format(ct0conf, len, so);
}
+int DriverRegPlayback(SurviveContext *ctx) {
+ const char *playback_dir =
+ config_read_str(ctx->global_config_values, "PlaybackDir", "");
+ if (strlen(playback_dir) == 0) {
+ return 0;
+ }
-int DriverRegPlayback( SurviveContext * ctx )
-{
- const char* playback_dir = config_read_str(ctx->global_config_values,
- "PlaybackDir", "");
-
- if(strlen(playback_dir) == 0) {
- return 0;
- }
-
- SurvivePlaybackData * sp = calloc( 1, sizeof( SurvivePlaybackData ) );
- sp->ctx = ctx;
- sp->playback_dir = playback_dir;
- sp->time_factor = config_read_float(ctx->global_config_values, "PlaybackFactor", 1.);
-
- printf("%s\n", playback_dir);
+ SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData));
+ sp->ctx = ctx;
+ sp->playback_dir = playback_dir;
+ sp->time_factor =
+ config_read_float(ctx->global_config_values, "PlaybackFactor", 1.);
+
+ printf("%s\n", playback_dir);
+
+ char playback_file[100];
+ sprintf(playback_file, "%s/events", playback_dir);
+ sp->playback_file = fopen(playback_file, "r");
+ if (sp->playback_file == 0) {
+ fprintf(stderr, "Could not open playback events file %s",
+ playback_file);
+ return -1;
+ }
+ SurviveObject *hmd = survive_create_hmd(ctx, "Playback", sp);
+ SurviveObject *wm0 = survive_create_wm0(ctx, "Playback", sp, 0);
+ SurviveObject *wm1 = survive_create_wm1(ctx, "Playback", sp, 0);
+ SurviveObject *tr0 = survive_create_tr0(ctx, "Playback", sp);
+ SurviveObject *ww0 = survive_create_ww0(ctx, "Playback", sp);
+
+ if (!LoadConfig(sp, hmd)) {
+ survive_add_object(ctx, hmd);
+ }
+ if (!LoadConfig(sp, wm0)) {
+ survive_add_object(ctx, wm0);
+ }
+ if (!LoadConfig(sp, wm1)) {
+ survive_add_object(ctx, wm1);
+ }
+ if (!LoadConfig(sp, tr0)) {
+ survive_add_object(ctx, tr0);
+ }
+ if (!LoadConfig(sp, ww0)) {
+ survive_add_object(ctx, ww0);
+ }
- char playback_file[100];
- sprintf( playback_file, "%s/events", playback_dir );
- sp->playback_file = fopen( playback_file, "r");
- if(sp->playback_file == 0) {
- fprintf(stderr, "Could not open playback events file %s", playback_file);
- return -1;
-}
- SurviveObject * hmd = survive_create_hmd(ctx, "Playback", sp);
- SurviveObject * wm0 = survive_create_wm0(ctx, "Playback", sp, 0);
- SurviveObject * wm1 = survive_create_wm1(ctx, "Playback", sp, 0);
- SurviveObject * tr0 = survive_create_tr0(ctx, "Playback", sp);
- SurviveObject * ww0 = survive_create_ww0(ctx, "Playback", sp);
-
- if( !LoadConfig( sp, hmd )) { survive_add_object( ctx, hmd ); }
- if( !LoadConfig( sp, wm0 )) { survive_add_object( ctx, wm0 ); }
- if( !LoadConfig( sp, wm1 )) { survive_add_object( ctx, wm1 ); }
- if( !LoadConfig( sp, tr0 )) { survive_add_object( ctx, tr0 ); }
- if( !LoadConfig( sp, ww0 )) { survive_add_object( ctx, ww0 ); }
-
-
- survive_add_driver(ctx, sp, playback_poll, playback_close, 0);
- return 0;
- fail_gracefully:
- return -1;
+ survive_add_driver(ctx, sp, playback_poll, playback_close, 0);
+ return 0;
+fail_gracefully:
+ return -1;
}
-REGISTER_LINKTIME( DriverRegPlayback );
-
+REGISTER_LINKTIME(DriverRegPlayback);