From aa04701c89f912d455f8d2cf7a09c367d3ddd4e6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Jun 2013 19:15:02 +0200
Subject: [PATCH] Added global position to logging

---
 src/modules/sdlog2/sdlog2.c          | 10 +++++++++-
 src/modules/sdlog2/sdlog2_messages.h | 12 ++++++++++++
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 90460cc623..982f843be6 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -685,6 +685,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_AIRS_s log_AIRS;
 			struct log_ARSP_s log_ARSP;
 			struct log_FLOW_s log_FLOW;
+			struct log_GPOS_s log_GPOS;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1056,7 +1057,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- GLOBAL POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
-				// TODO not implemented yet
+				log_msg.msg_type = LOG_GPOS_MSG;
+				log_msg.body.log_GPOS.lat = buf.global_pos.lat;
+				log_msg.body.log_GPOS.lon = buf.global_pos.lon;
+				log_msg.body.log_GPOS.alt = buf.global_pos.alt;
+				log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
+				log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
+				log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
+				LOGBUFFER_WRITE_AND_COUNT(GPOS);
 			}
 
 			/* --- VICON POSITION --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index c100e921b9..1b2237d650 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -198,6 +198,17 @@ struct log_FLOW_s {
 	uint8_t	quality;
 	uint8_t sensor_id;
 };
+
+/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
+#define LOG_GPOS_MSG 16
+struct log_GPOS_s {
+	int32_t lat;
+	int32_t lon;
+	float alt;
+	float vel_n;
+	float vel_e;
+	float vel_d;
+};
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -218,6 +229,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
+	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);