Log messages sent via MAVLink

1 files changed, 29 insertions, 0 deletions

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index c182cfdb9..a4ed14914 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -341,11 +341,18 @@ private:
 	/* do not allow top copying this class */
 	MavlinkStreamStatustext(MavlinkStreamStatustext &);
 	MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
+	FILE *fp = nullptr;
 
 protected:
 	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
+	~MavlinkStreamStatustext() {
+		if (fp) {
+			fclose(fp);
+		}
+	}
+
 	void send(const hrt_abstime t)
 	{
 		if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) {
@@ -359,6 +366,28 @@ protected:
 				strncpy(msg.text, logmsg.text, sizeof(msg.text));
 
 				_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
+
+				if (fp) {
+					fputs(msg.text, fp);
+					fputs("\n", fp);
+					fsync(fileno(fp));
+				} else {
+					/* string to hold the path to the log */
+					char log_file_name[32] = "";
+					char log_file_path[64] = "";
+
+					timespec ts;
+					clock_gettime(CLOCK_REALTIME, &ts);
+					/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
+					time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+					struct tm t;
+					gmtime_r(&gps_time_sec, &t);
+
+					// XXX we do not want to interfere here with the SD log app
+					strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
+					snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
+					fp = fopen(log_file_path, "ab");
+				}
 			}
 		}
 	}