aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--data_recorder.c126
-rw-r--r--include/libsurvive/survive.h4
-rw-r--r--src/survive_playback.c160
-rw-r--r--src/survive_process.c11
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);
}