Added VICON logging, finally.

2 files changed, 31 insertions, 11 deletions

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index e62b0fafc..83ee93408 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -226,11 +226,11 @@ sdlog2_usage(const char *reason)
 	}
 
 	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
-	     "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
-	     "\t-b\tLog buffer size in KiB, default is 8\n"
-	     "\t-e\tEnable logging by default (if not, can be started by command)\n"
-	     "\t-a\tLog only when armed (can be still overriden by command)\n"
-	     "\t-t\tUse date/time for naming log directories and files\n");
+		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
+		 "\t-b\tLog buffer size in KiB, default is 8\n"
+		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
+		 "\t-a\tLog only when armed (can be still overriden by command)\n"
+		 "\t-t\tUse date/time for naming log directories and files\n");
 }
 
 /**
@@ -257,11 +257,11 @@ int sdlog2_main(int argc, char *argv[])
 
 		main_thread_should_exit = false;
 		deamon_task = task_spawn_cmd("sdlog2",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_DEFAULT - 30,
-					     3000,
-					     sdlog2_thread_main,
-					     (const char **)argv);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_DEFAULT - 30,
+						 3000,
+						 sdlog2_thread_main,
+						 (const char **)argv);
 		exit(0);
 	}
 
@@ -833,6 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_TELE_s log_TELE;
 			struct log_ESTM_s log_ESTM;
 			struct log_PWR_s log_PWR;
+			struct log_VICON_s log_VICON;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1163,7 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- VICON POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
-			// TODO not implemented yet
+            log_msg.msg_type = LOG_VICON_MSG;
+            log_msg.body.log_VICON.x = buf.vicon_pos.x;
+            log_msg.body.log_VICON.y = buf.vicon_pos.y;
+            log_msg.body.log_VICON.z = buf.vicon_pos.z;
+            log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
+            log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
+            log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
+            LOGBUFFER_WRITE_AND_COUNT(VICON);
 		}
 
 		/* --- FLOW --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a1a5856bc..eee5ae590 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -301,6 +301,17 @@ struct log_PWR_s {
 	uint8_t high_power_rail_overcurrent;
 };
 
+/* --- VICON - VICON POSITION --- */
+#define LOG_VICON_MSG 25
+struct log_VICON_s {
+	float x;
+	float y;
+	float z;
+	float roll;
+	float pitch;
+	float yaw;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -351,6 +362,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
 	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
 	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
+	LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */