diff options
Diffstat (limited to 'data_recorder.c')
-rw-r--r-- | data_recorder.c | 32 |
1 files changed, 21 insertions, 11 deletions
diff --git a/data_recorder.c b/data_recorder.c index e835c73..e406387 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -62,14 +62,26 @@ void write_to_output(const char *format, ...) { va_end(args); } +int my_config_process(SurviveObject *so, char *ct0conf, int len) { + char *buffer = malloc(len); + memcpy(buffer, ct0conf, len); + for (int i = 0; i < len; i++) + if (buffer[i] == '\n') + buffer[i] = ' '; + + write_to_output("%s CONFIG ", so->codename); + fwrite(buffer, 1, len, output_file); + fwrite("\n", 1, 1, output_file); + return survive_default_htc_config_process(so, ct0conf, len); +} void my_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { survive_default_lighthouse_pose_process(ctx, lighthouse, pose); - write_to_output("LH_POSE %d %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, pose->Pos[0], pose->Pos[1], + write_to_output("%d LH_POSE %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); } void my_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { survive_default_raw_pose_process(so, lighthouse, pose); - write_to_output("POSE %s %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", so->codename, pose->Pos[0], pose->Pos[1], + write_to_output("%s POSE %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", so->codename, pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); } @@ -80,8 +92,7 @@ void my_light_process(struct SurviveObject *so, int sensor_id, int acode, length, lh); if (acode == -1 || sensor_id < 0) { - write_to_output("A %s %d %d %d %u %u %u\n", so->codename, sensor_id, - acode, timeinsweep, timecode, length, lh); + write_to_output("%s A %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh); return; } @@ -122,18 +133,16 @@ void my_light_process(struct SurviveObject *so, int sensor_id, int acode, 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", so->codename, LH_ID, LH_Axis, 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 %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %d\n", - so->codename, mask, timecode, accelgyro[0], accelgyro[1], - accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id); + write_to_output("%s I %d %u %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %d\n", so->codename, mask, timecode, accelgyro[0], + accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id); } void *GuiThread(void *v) { @@ -177,12 +186,13 @@ void *GuiThread(void *v) { int SurviveThreadLoaded = 0; void *SurviveThread(void *junk) { - ctx = survive_init(0); + ctx = survive_init_with_config_cb(0, my_config_process); survive_install_light_fn(ctx, my_light_process); survive_install_imu_fn(ctx, my_imu_process); survive_install_lighthouse_pose_fn(ctx, my_lighthouse_process); survive_install_raw_pose_fn(ctx, my_raw_pose_process); + survive_cal_install(ctx); if (!ctx) { fprintf(stderr, "Fatal. Could not start\n"); |