From 0d323e4a42bcfa70150c77f45f6f465f84666e31 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 24 Mar 2018 09:50:40 -0600 Subject: Made playback / record process raw light data --- src/poser.c | 6 ++++- src/survive_disambiguator.c | 3 +++ src/survive_playback.c | 55 ++++++++++++++++++++++++++++++++++++++++----- src/survive_playback.h | 2 +- 4 files changed, 58 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/poser.c b/src/poser.c index 3fb4fe8..2cfe28d 100644 --- a/src/poser.c +++ b/src/poser.c @@ -45,7 +45,11 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui // Now find the space with the same origin, but rotated so that gravity is up SurvivePose lighthouse2objUp = {}, object2objUp = {}; - quatfrom2vectors(object2objUp.Rot, so->activations.accel, up); + if (quatmagnitude(so->activations.accel)) { + quatfrom2vectors(object2objUp.Rot, so->activations.accel, up); + } else { + object2objUp.Rot[0] = 1.0; + } // Calculate the pose of the lighthouse in this space ApplyPoseToPose(&lighthouse2objUp, &object2objUp, &lighthouse2obj); diff --git a/src/survive_disambiguator.c b/src/survive_disambiguator.c index cf89068..6c19475 100644 --- a/src/survive_disambiguator.c +++ b/src/survive_disambiguator.c @@ -1,11 +1,14 @@ #include "survive.h" + #include #include +#include "survive_playback.h" //#define LOG_LIGHTDATA void handle_lightcap(SurviveObject *so, LightcapElement *le) { + survive_recording_lightcap(so, le); #ifdef LOG_LIGHTDATA static FILE * flog; static double start = 0; diff --git a/src/survive_playback.c b/src/survive_playback.c index 9261bb5..172614b 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -16,6 +16,7 @@ typedef struct SurviveRecordingData { bool alwaysWriteStdOut; + bool writeRawLight; FILE *output_file; } SurviveRecordingData; @@ -96,6 +97,16 @@ void survive_recording_angle_process(struct SurviveObject *so, int sensor_id, in angle, lh); } +void survive_recording_lightcap(SurviveObject *so, LightcapElement *le) { + SurviveRecordingData *recordingData = so->ctx->recptr; + if (recordingData == 0) + return; + + if (recordingData->writeRawLight) { + write_to_output(recordingData, "%s C %d %u %u\n", so->codename, le->sensor_id, le->timestamp, le->length); + } +} + void survive_recording_light_process(struct SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { SurviveRecordingData *recordingData = so->ctx->recptr; @@ -133,6 +144,7 @@ void survive_recording_light_process(struct SurviveObject *so, int sensor_id, in LH_Axis = "Y"; break; } + write_to_output(recordingData, "%s %s %s %d %d %d %u %u %u\n", so->codename, LH_ID, LH_Axis, sensor_id, acode, timeinsweep, timecode, length, lh); } @@ -154,6 +166,7 @@ struct SurvivePlaybackData { FLT time_factor; double next_time_us; + bool hasRawLight; }; typedef struct SurvivePlaybackData SurvivePlaybackData; @@ -190,8 +203,31 @@ static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) { return 0; } -static int parse_and_run_lightcode(const char *line, - SurvivePlaybackData *driver) { +static int parse_and_run_rawlight(const char *line, SurvivePlaybackData *driver) { + driver->hasRawLight = 1; + + char dev[10]; + char op[10]; + LightcapElement le; + int rr = sscanf(line, "%s %s %hhu %u %hu\n", dev, op, &le.sensor_id, &le.timestamp, &le.length); + + SurviveObject *so = survive_get_so_by_name(driver->ctx, dev); + if (!so) { + static bool display_once = false; + SurviveContext *ctx = driver->ctx; + if (display_once == false) { + SV_ERROR("Could not find device named %s from lineno %d\n", dev, driver->lineno); + } + display_once = true; + + return -1; + } + + handle_lightcap(so, &le); + return 0; +} + +static int parse_and_run_lightcode(const char *line, SurvivePlaybackData *driver) { char lhn[10]; char axn[10]; char dev[10]; @@ -206,8 +242,7 @@ static int parse_and_run_lightcode(const char *line, &length, &lh); if (rr != 9) { - fprintf(stderr, "Warning: On line %d, only %d values read: '%s'\n", - driver->lineno, rr, line); + fprintf(stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line); return -1; } @@ -263,13 +298,19 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) { char dev[10]; char op[10]; - if (sscanf(line, "%8s %8s", dev, op) < 2) + if (sscanf(line, "%8s %8s", dev, op) < 2) { + free(line); return 0; + } - if ((op[0] != 'R' && op[0] != 'L' && op[0] != 'I') || op[1] != 0) + if (op[1] != 0) { return 0; + } switch (op[0]) { + case 'C': + parse_and_run_rawlight(line, driver); + break; case 'L': case 'R': parse_and_run_lightcode(line, driver); @@ -319,6 +360,8 @@ void survive_install_recording(SurviveContext *ctx) { if (record_to_stdout) { SV_INFO("Recording to stdout"); } + + ctx->recptr->writeRawLight = survive_configi(ctx, "record-rawlight", SC_GET, 1); } } diff --git a/src/survive_playback.h b/src/survive_playback.h index 52638a9..c895120 100644 --- a/src/survive_playback.h +++ b/src/survive_playback.h @@ -5,7 +5,7 @@ void survive_recording_config_process(SurviveObject *so, char *ct0conf, int len) void survive_recording_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, SurvivePose *obj); - +void survive_recording_lightcap(SurviveObject *so, LightcapElement *le); void survive_recording_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); void survive_recording_info_process(SurviveContext *ctx, const char *fault); void survive_recording_angle_process(struct SurviveObject *so, int sensor_id, int acode, uint32_t timecode, FLT length, -- cgit v1.2.3