diff options
author | CNLohr <charles@cnlohr.com> | 2018-03-16 00:02:03 -0400 |
---|---|---|
committer | GitHub <noreply@github.com> | 2018-03-16 00:02:03 -0400 |
commit | 6e67cc1996fbcba36e11cb281c78d3b00c4de85d (patch) | |
tree | 543390fcbd17dfce8845dff942fc6ecd928270b7 /data_recorder.c | |
parent | 61a01d0ddfd8e99c97e3d235d4f0782fbf0ed032 (diff) | |
parent | 029b2909b37cc58ba35e0f46be1802866ee59730 (diff) | |
download | libsurvive-6e67cc1996fbcba36e11cb281c78d3b00c4de85d.tar.gz libsurvive-6e67cc1996fbcba36e11cb281c78d3b00c4de85d.tar.bz2 |
Merge pull request #113 from cnlohr/epnp
Epnp
Diffstat (limited to 'data_recorder.c')
-rw-r--r-- | data_recorder.c | 21 |
1 files changed, 18 insertions, 3 deletions
diff --git a/data_recorder.c b/data_recorder.c index 8116946..e835c73 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -62,6 +62,16 @@ void write_to_output(const char *format, ...) { va_end(args); } +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], + 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], + pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); +} void my_light_process(struct SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, @@ -69,13 +79,14 @@ void my_light_process(struct SurviveObject *so, int sensor_id, int acode, survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lh); - if (acode == -1) { + 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); return; } int jumpoffset = sensor_id; + if (strcmp(so->codename, "WM0") == 0) jumpoffset += 32; else if (strcmp(so->codename, "WM1") == 0) @@ -170,7 +181,9 @@ void *SurviveThread(void *junk) { 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"); exit(1); @@ -194,7 +207,8 @@ int main(int argc, char **argv) { } else { output_file = stdout; } - + SurviveThread(0); + /* // Create the libsurvive thread OGCreateThread(SurviveThread, 0); @@ -205,4 +219,5 @@ int main(int argc, char **argv) { // Run the Gui in the main thread GuiThread(0); + */ } |