From 6f54646482e44051af1d3e2965012888b00e2038 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 14:41:16 -0600 Subject: Internalize data recorder details --- data_recorder.c | 126 ++-------------------------------- include/libsurvive/survive.h | 4 +- src/survive_playback.c | 160 +++++++++++++++++++++++++++++++++++++++++-- src/survive_process.c | 11 +++ 4 files changed, 171 insertions(+), 130 deletions(-) diff --git a/data_recorder.c b/data_recorder.c index 7e5384a..f2d89e7 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -22,134 +22,16 @@ struct SurviveContext *ctx; -bool alwaysWriteStdOut = false; -FILE *output_file = 0; - -double timestamp_in_us() { - static double start_time_us = 0; - if (start_time_us == 0) - start_time_us = OGGetAbsoluteTime(); - return OGGetAbsoluteTime() - start_time_us; -} - -void write_to_output(const char *format, ...) { - double ts = timestamp_in_us(); - - va_list args; - va_start(args, format); - fprintf(output_file, "%0.6f ", ts); - vfprintf(output_file, format, args); - va_end(args); - - if (alwaysWriteStdOut) { - va_list args; - va_start(args, format); - fprintf(stdout, "%0.6f ", ts); - vfprintf(stdout, format, args); - 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 *lh_pose, SurvivePose *obj) { - survive_default_lighthouse_pose_process(ctx, lighthouse, lh_pose, obj); - write_to_output("%d LH_POSE %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, lh_pose->Pos[0], - lh_pose->Pos[1], lh_pose->Pos[2], lh_pose->Rot[0], lh_pose->Rot[1], lh_pose->Rot[2], - lh_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("%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]); -} - -void my_info_process(SurviveContext *ctx, const char *fault) { write_to_output("INFO LOG %s\n", fault); } -void my_angle_process(struct SurviveObject *so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, - uint32_t lh) { - survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); - write_to_output("%s A %d %d %u %0.6f %0.6f %u\n", so->codename, sensor_id, acode, timecode, length, angle, lh); -} - -void my_light_process(struct SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, - uint32_t length, uint32_t lh) { - survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lh); - - if (acode == -1) { - write_to_output("%s S %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh); - return; - } - - const char *LH_ID = 0; - const char *LH_Axis = 0; - - switch (acode) { - case 0: - case 2: - LH_ID = "L"; - LH_Axis = "X"; - break; - case 1: - case 3: - LH_ID = "L"; - LH_Axis = "Y"; - break; - case 4: - case 6: - LH_ID = "R"; - LH_Axis = "X"; - break; - case 5: - case 7: - LH_ID = "R"; - LH_Axis = "Y"; - break; - } - write_to_output("%s %s %s %d %d %d %u %u %u\n", so->codename, LH_ID, LH_Axis, sensor_id, acode, timeinsweep, - timecode, length, lh); -} - -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("%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); -} - int main(int argc, char **argv) { ctx = survive_init(argc, argv); if (ctx == 0) // implies -help or similiar return 0; - const char *outfile = survive_configs(ctx, "o", SC_GET, ""); - if (strlen(outfile) > 0) { - output_file = fopen(outfile, "w"); - if (output_file == 0) { - fprintf(stderr, "Could not open %s for writing\n", argv[1]); - return -1; - } - alwaysWriteStdOut = survive_configi(ctx, "stdout", SC_GET, 0); - } else { - output_file = stdout; - alwaysWriteStdOut = false; - } + const char *outfile = survive_configs(ctx, "dataoutfile", SC_GET, ""); - survive_install_htc_config_fn(ctx, 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_install_angle_fn(ctx, my_angle_process); - survive_install_info_fn(ctx, my_info_process); + if (strlen(outfile) == 0) { + survive_configi(ctx, "datastdout", SC_SETCONFIG | SC_OVERRIDE, 1); + } survive_startup(ctx); if (survive_configi(ctx, "calibrate", SC_GET, 1)) { diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index cb144bd..0b85bf3 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -159,6 +159,8 @@ typedef struct typedef enum { SURVIVE_STOPPED = 0, SURVIVE_RUNNING, SURVIVE_CLOSING, SURVIVE_STATE_MAX } SurviveState; +struct SurviveRecordingData; + struct SurviveContext { text_feedback_func faultfunction; @@ -180,7 +182,7 @@ struct SurviveContext int activeLighthouses; BaseStationData bsd[NUM_LIGHTHOUSES]; SurviveCalData * calptr; //If and only if the calibration subsystem is attached. - + struct SurviveRecordingData *recptr; // Iff recording is attached SurviveObject ** objs; int objs_ct; diff --git a/src/survive_playback.c b/src/survive_playback.c index 54b97f6..45eea85 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -12,6 +12,139 @@ #include "survive_default_devices.h" #include "os_generic.h" +#include "stdarg.h" + +typedef struct SurviveRecordingData { + bool alwaysWriteStdOut; + FILE *output_file; +} SurviveRecordingData; + +static double timestamp_in_us() { + static double start_time_us = 0; + if (start_time_us == 0.) + start_time_us = OGGetAbsoluteTime(); + return OGGetAbsoluteTime() - start_time_us; +} + +static void write_to_output(SurviveRecordingData *recordingData, const char *format, ...) { + double ts = timestamp_in_us(); + + if (recordingData->output_file) { + va_list args; + va_start(args, format); + fprintf(recordingData->output_file, "%0.6f ", ts); + vfprintf(recordingData->output_file, format, args); + va_end(args); + } + + if (recordingData->alwaysWriteStdOut) { + va_list args; + va_start(args, format); + fprintf(stdout, "%0.6f ", ts); + vfprintf(stdout, format, args); + va_end(args); + } +} +void survive_recording_config_process(SurviveObject *so, char *ct0conf, int len) { + SurviveRecordingData *recordingData = so->ctx->recptr; + if (recordingData == 0) + return; + + char *buffer = malloc(len); + memcpy(buffer, ct0conf, len); + for (int i = 0; i < len; i++) + if (buffer[i] == '\n') + buffer[i] = ' '; + + write_to_output(recordingData, "%s CONFIG %.*s\n", so->codename, len, buffer); +} + +void survive_recording_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, + SurvivePose *obj) { + SurviveRecordingData *recordingData = ctx->recptr; + if (recordingData == 0) + return; + + write_to_output(recordingData, "%d LH_POSE %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, + lh_pose->Pos[0], lh_pose->Pos[1], lh_pose->Pos[2], lh_pose->Rot[0], lh_pose->Rot[1], + lh_pose->Rot[2], lh_pose->Rot[3]); +} +void survive_recording_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { + SurviveRecordingData *recordingData = so->ctx->recptr; + if (recordingData == 0) + return; + + write_to_output(recordingData, "%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]); +} + +void survive_recording_info_process(SurviveContext *ctx, const char *fault) { + SurviveRecordingData *recordingData = ctx->recptr; + if (recordingData == 0) + return; + + write_to_output(recordingData, "INFO LOG %s\n", fault); +} + +void survive_recording_angle_process(struct SurviveObject *so, int sensor_id, int acode, uint32_t timecode, FLT length, + FLT angle, uint32_t lh) { + SurviveRecordingData *recordingData = so->ctx->recptr; + if (recordingData == 0) + return; + + write_to_output(recordingData, "%s A %d %d %u %0.6f %0.6f %u\n", so->codename, sensor_id, acode, timecode, length, + angle, lh); +} + +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; + if (recordingData == 0) + return; + + if (acode == -1) { + write_to_output(recordingData, "%s S %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, + timecode, length, lh); + return; + } + + const char *LH_ID = 0; + const char *LH_Axis = 0; + + switch (acode) { + case 0: + case 2: + LH_ID = "L"; + LH_Axis = "X"; + break; + case 1: + case 3: + LH_ID = "L"; + LH_Axis = "Y"; + break; + case 4: + case 6: + LH_ID = "R"; + LH_Axis = "X"; + break; + case 5: + case 7: + LH_ID = "R"; + 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); +} + +void survive_recording_imu_process(struct SurviveObject *so, int mask, FLT *accelgyro, uint32_t timecode, int id) { + SurviveRecordingData *recordingData = so->ctx->recptr; + if (recordingData == 0) + return; + + write_to_output(recordingData, "%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); +} struct SurvivePlaybackData { SurviveContext *ctx; @@ -24,13 +157,6 @@ struct SurvivePlaybackData { }; typedef struct SurvivePlaybackData SurvivePlaybackData; -double timestamp_in_us() { - static double start_time_us = 0; - if (start_time_us == 0.) - start_time_us = OGGetAbsoluteTime(); - return OGGetAbsoluteTime() - start_time_us; -} - static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) { char dev[10]; int timecode = 0; @@ -174,7 +300,27 @@ static int playback_close(struct SurviveContext *ctx, void *_driver) { return 0; } +void install_recording(SurviveContext *ctx) { + const char *dataout_file = survive_configs(ctx, "dataoutfile", SC_SETCONFIG, ""); + int record_to_stdout = survive_configi(ctx, "datastdout", SC_SETCONFIG, 0); + + if (strlen(dataout_file) > 0 || record_to_stdout) { + ctx->recptr = calloc(1, sizeof(struct SurviveRecordingData)); + + ctx->recptr->output_file = fopen(dataout_file, "w"); + if (ctx->recptr->output_file == 0 && !record_to_stdout) { + SV_INFO("Could not open %s for writing\n", dataout_file); + free(ctx->recptr); + ctx->recptr = 0; + return; + } + ctx->recptr->alwaysWriteStdOut = record_to_stdout; + } +} + int DriverRegPlayback(SurviveContext *ctx) { + install_recording(ctx); + const char *playback_file = survive_configs(ctx, "playbackfile", SC_SETCONFIG, ""); if (strlen(playback_file) == 0) { diff --git a/src/survive_process.c b/src/survive_process.c index 1402dab..6136148 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -4,6 +4,7 @@ #include "survive_cal.h" #include "survive_config.h" #include "survive_default_devices.h" +#include "survive_playback.h" //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. @@ -18,6 +19,8 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh); } + survive_recording_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lh); + //We don't use sync times, yet. if (sensor_id <= -1) { if (so->PoserFn) { @@ -79,6 +82,8 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode SurviveSensorActivations_add(&so->activations, &l); + survive_recording_angle_process(so, sensor_id, acode, timecode, length, angle, lh); + if (ctx->calptr) { survive_cal_angle(so, sensor_id, acode, timecode, length, angle, lh); } @@ -130,6 +135,7 @@ void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, Sur //printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); so->OutPose = *pose; so->FromLHPose[lighthouse] = *pose; + survive_recording_raw_pose_process(so, lighthouse, pose); } void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, @@ -143,9 +149,12 @@ void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighth config_set_lighthouse(ctx->lh_config, &ctx->bsd[lighthouse], lighthouse); config_save(ctx, "config.json"); + + survive_recording_lighthouse_process(ctx, lighthouse, lighthouse_pose, object_pose); } int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len) { + survive_recording_config_process(so, ct0conf, len); return survive_load_htc_config_format(so, ct0conf, len); } void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) @@ -167,5 +176,7 @@ void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyrom if (so->PoserFn) { so->PoserFn( so, (PoserData *)&imu ); } + + survive_recording_imu_process(so, mask, accelgyromag, timecode, id); } -- cgit v1.2.3