aboutsummaryrefslogtreecommitdiff
path: root/data_recorder.c
diff options
context:
space:
mode:
Diffstat (limited to 'data_recorder.c')
-rw-r--r--data_recorder.c32
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");