From 569938e6808286f3aa4e8a03ca4e1c8467955f5f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 25 Oct 2012 15:47:14 +0200
Subject: [PATCH] Copying log analysis file directly to the SD card during
 logging

---
 ROMFS/logging/logconv.m | 57 ++++++++++++++++++++++++++++++++++++++++-
 apps/sdlog/sdlog.c      | 54 +++++++++++++++++++++++++++++++++++---
 2 files changed, 107 insertions(+), 4 deletions(-)

diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m
index 5a615228d6..81fcc1d7e6 100644
--- a/ROMFS/logging/logconv.m
+++ b/ROMFS/logging/logconv.m
@@ -1,6 +1,61 @@
 clear all
 clc
 
+%%%%%%%%%%%%%%%%%%%%%%%
+% SYSTEM VECTOR
+%
+% 			All measurements in NED frame
+% 			
+% 			uint64_t timestamp;
+% 			float gyro[3]; in rad/s
+% 			float accel[3]; in m/s^2
+% 			float mag[3]; in Gauss
+% 			float baro; pressure in millibar
+% 			float baro_alt; altitude above MSL in meters
+% 			float baro_temp; in degrees celcius
+% 			float control[4]; roll, pitch, yaw [-1..1], thrust [0..1]
+% 			float actuators[8]; motor 1-8, in motor units (PWM: 1000-2000,
+% 			AR.Drone: 0-512
+% 			float vbat; battery voltage in volt
+% 			float adc[3]; remaining auxiliary ADC ports in volt
+%           float local_position
+%           int32 gps_raw_position
+
+
+if exist('sysvector.bin', 'file')
+    % Read actuators file
+    myFile = java.io.File('sysvector.bin')
+    fileSize = length(myFile)
+
+    fid = fopen('sysvector.bin', 'r');
+    elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
+
+    for i=1:elements
+        % timestamp
+        sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
+        % actuators 1-16
+        % quadrotor: motor 1-4 on the first four positions
+        sysvector(i, 2:32) = fread(fid,  28+3, 'float', 'ieee-le');
+        sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
+    end
+    
+    sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
+    sysvector_minutes = sysvector_interval_seconds / 60
+    
+    % Normalize time
+    sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
+    
+    % Create some basic plots
+    
+    % Remove zero rows from GPS
+    gps = sysvector(:,33:35);
+    gps(~any(gps,2), :) = [];
+    
+    plot3(gps(:,1), gps(:,2), gps(:,3));
+    
+    plot(sysvector(:,1), sysvector(:,2:32));
+end
+
 if exist('actuator_outputs0.bin', 'file')
     % Read actuators file
     myFile = java.io.File('actuator_outputs0.bin')
@@ -9,7 +64,7 @@ if exist('actuator_outputs0.bin', 'file')
     fid = fopen('actuator_outputs0.bin', 'r');
     elements = int64(fileSize./(16*4+8))
 
-    for i=1:(elements-2)
+    for i=1:elements
         % timestamp
         actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
         % actuators 1-16
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index 312768095e..a934d0d33d 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -72,6 +72,7 @@ static int deamon_task;				/**< Handle of deamon task / thread */
 static const int MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log folders */
 
 static const char *mountpoint = "/fs/microsd";
+static const char *mfile_in = "/etc/logging/logconv.m";
 
 /**
  * SD log management function.
@@ -90,10 +91,12 @@ static void usage(const char *reason);
 
 static int file_exist(const char *filename);
 
+static int file_copy(const char* file_old, const char* file_new);
+
 /**
  * Print the current status.
  */
-static void print_sdlog_status();
+static void print_sdlog_status(void);
 
 /**
  * Create folder for current logging session.
@@ -181,7 +184,17 @@ int create_logfolder(char* folder_path) {
 		/* the result is -1 if the folder exists */
 
 		if (mkdir_ret == 0) {
-			/* folder does not exist */
+			/* folder does not exist, success */
+
+			/* now copy the Matlab/Octave file */
+			char mfile_out[100];
+			sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber);
+			int ret = file_copy(mfile_in, mfile_out);
+			if (!ret) {
+				warnx("copied m file to %s", mfile_out);
+			} else {
+				warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out);
+			}
 			break;
 
 		} else if (mkdir_ret == -1) {
@@ -379,7 +392,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	 * set up poll to block for new data,
 	 * wait for a maximum of 1000 ms (1 second)
 	 */
-	const int timeout = 1000;
+	// const int timeout = 1000;
 
 	thread_running = true;
 
@@ -550,3 +563,38 @@ int file_exist(const char *filename)
 	return stat(filename, &buffer);
 }
 
+int file_copy(const char* file_old, const char* file_new)
+{
+	FILE *source, *target;
+	source = fopen(file_old, "r");
+ 
+	if( source == NULL )
+	{
+		warnx("failed opening input file to copy");
+		return 1;
+	}
+
+	target = fopen(file_new, "w");
+ 
+	if( target == NULL )
+	{
+		fclose(source);
+		warnx("failed to open output file to copy");
+	}
+ 
+	char buf[128];
+	int nread;
+	while ((nread = fread(buf, sizeof(buf), 1, source)) > 0) {
+		int ret = fwrite(buf, sizeof(buf), 1, target);
+		if (ret <= 0) {
+			warnx("error writing file");
+			break;
+		}
+	}
+
+	fclose(source);
+	fclose(target);
+
+   return 0;
+}
+