From 2e980136715acd4692dd61be4442dc823219764d Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 14:43:58 -0600 Subject: Results out of epnp --- data_recorder.c | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) (limited to 'data_recorder.c') diff --git a/data_recorder.c b/data_recorder.c index 8116946..fbc9bdc 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 testprog_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,8 @@ 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_cal_install(ctx); if (!ctx) { fprintf(stderr, "Fatal. Could not start\n"); exit(1); @@ -194,7 +206,8 @@ int main(int argc, char **argv) { } else { output_file = stdout; } - + SurviveThread(0); + /* // Create the libsurvive thread OGCreateThread(SurviveThread, 0); @@ -205,4 +218,5 @@ int main(int argc, char **argv) { // Run the Gui in the main thread GuiThread(0); + */ } -- cgit v1.2.3