From fcf3528bfeba5e4646dbb0626a87c297d940a787 Mon Sep 17 00:00:00 2001
From: Justin Berger <j.david.berger@gmail.com>
Date: Thu, 7 Jun 2018 23:42:05 -0600
Subject: Added debug message to ootx; mag reading to playback

---
 src/ootx_decoder.c     |  9 ++++++++-
 src/ootx_decoder.h     |  1 +
 src/survive_cal.c      |  7 ++++++-
 src/survive_playback.c | 17 +++++++++++------
 4 files changed, 26 insertions(+), 8 deletions(-)

(limited to 'src')

diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
index 0620284..7d45626 100644
--- a/src/ootx_decoder.c
+++ b/src/ootx_decoder.c
@@ -19,11 +19,17 @@
 
 #define MAX_BUFF_SIZE 64
 
+void (*ootx_error_clbk)(ootx_decoder_context *ctx, const char *msg) = NULL;
 void (*ootx_packet_clbk)(ootx_decoder_context * ctx, ootx_packet* packet) = NULL;
 void (*ootx_bad_crc_clbk)(ootx_decoder_context * ctx, ootx_packet* packet, uint32_t crc) = NULL;
 
 void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
 
+void ootx_error(ootx_decoder_context *ctx, const char *msg) {
+	if (ootx_error_clbk)
+		ootx_error_clbk(ctx, msg);
+}
+
 void ootx_init_decoder_context(ootx_decoder_context *ctx) {
 	ctx->buf_offset = 0;
 	ctx->bits_written = 0;
@@ -106,7 +112,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 	if ( ootx_detect_preamble(ctx, dbit) ) {
 		/*	data stream can start over at any time so we must
 			always look for preamble bits */
-		//printf("Preamble found\n");
+		ootx_error(ctx, "Preamble found");
 		ootx_reset_buffer(ctx);
 		ctx->bits_processed = 0;
 		ctx->found_preamble = 1;
@@ -117,6 +123,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 		if( !dbit )
 		{
 			//printf("Bad sync bit\n");
+			ootx_error(ctx, "OOTX Decoder: Bad sync bit");
 			ootx_reset_buffer(ctx);
 		}
 		ctx->bits_processed = 0;
diff --git a/src/ootx_decoder.h b/src/ootx_decoder.h
index 8ddf527..a96cb03 100644
--- a/src/ootx_decoder.h
+++ b/src/ootx_decoder.h
@@ -66,6 +66,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit);
 
 uint8_t ootx_decode_bit(uint32_t length);
 
+extern void (*ootx_error_clbk)(ootx_decoder_context *ctx, const char *msg);
 extern void (*ootx_packet_clbk)(ootx_decoder_context *ctx, ootx_packet* packet);
 extern void (*ootx_bad_crc_clbk)(ootx_decoder_context *ctx, ootx_packet* packet, uint32_t crc);
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index ad62d22..f809809 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -45,6 +45,11 @@ int mkdir(const char *);
 static void handle_calibration( struct SurviveCalData *cd );
 static void reset_calibration( struct SurviveCalData * cd );
 
+void ootx_error_clbk_d(ootx_decoder_context *ct, const char *msg) {
+	SurviveContext *ctx = (SurviveContext *)(ct->user);
+	SV_INFO("%s", msg);
+}
+
 void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 {
 	static uint8_t lighthouses_completed = 0;
@@ -191,7 +196,7 @@ void survive_cal_install( struct SurviveContext * ctx )
 	cd->ConfigPoserFn = GetDriverByConfig(ctx, "Poser", "configposer", "SBA", 0);
 
 	ootx_packet_clbk = ootx_packet_clbk_d;
-
+	ootx_error_clbk = ootx_error_clbk_d;
 	ctx->calptr = cd;
 }
 
diff --git a/src/survive_playback.c b/src/survive_playback.c
index a5c4519..50b2e5c 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -166,8 +166,9 @@ void survive_recording_imu_process(struct SurviveObject *so, int mask, FLT *acce
 	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);
+	write_to_output(recordingData, "%s I %d %u %0.6f %0.6f %0.6f %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], accelgyro[6], accelgyro[7], accelgyro[8], id);
 }
 
 struct SurvivePlaybackData {
@@ -184,16 +185,20 @@ typedef struct SurvivePlaybackData SurvivePlaybackData;
 static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) {
 	char dev[10];
 	int timecode = 0;
-	FLT accelgyro[6];
+	FLT accelgyro[9] = {};
 	int mask;
 	int id;
 
 	int rr = sscanf(line, "%s I %d %d " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format
-						  " " FLT_format "%d",
+						  " " FLT_format " " FLT_format " " FLT_format " " FLT_format "%d",
 					dev, &mask, &timecode, &accelgyro[0], &accelgyro[1], &accelgyro[2], &accelgyro[3], &accelgyro[4],
-					&accelgyro[5], &id);
+					&accelgyro[5], &accelgyro[6], &accelgyro[7], &accelgyro[8], &id);
 
-	if (rr != 10) {
+	if (rr == 10) {
+		// Older formats might not have mag data
+		id = accelgyro[6];
+		accelgyro[6] = 0;
+	} else if (rr != 13) {
 		fprintf(stderr, "Warning:  On line %d, only %d values read: '%s'\n", driver->lineno, rr, line);
 		return -1;
 	}
-- 
cgit v1.2.3