Internalize data recorder details

2 files changed, 164 insertions, 7 deletions

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);
 }