Added logging with worker thread for microSD writes, untested, but feature-complete

1 files changed, 113 insertions, 52 deletions

diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index 9b4cd1622..407c47811 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -69,6 +69,8 @@
 
 #include <systemlib/systemlib.h>
 
+#include "sdlog_ringbuffer.h"
+
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
 static int deamon_task;				/**< Handle of deamon task / thread */
@@ -76,6 +78,18 @@ 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";
+int sysvector_file = -1;
+struct sdlog_logbuffer lb;
+
+/**
+ * System state vector log buffer writing
+ */
+static void *sdlog_sysvector_write_thread(void *arg);
+
+/**
+ * Create the thread to write the system vector
+ */
+pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf);
 
 /**
  * SD log management function.
@@ -94,7 +108,7 @@ 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);
+static int file_copy(const char *file_old, const char *file_new);
 
 /**
  * Print the current status.
@@ -104,13 +118,14 @@ static void print_sdlog_status(void);
 /**
  * Create folder for current logging session.
  */
-static int create_logfolder(char* folder_path);
+static int create_logfolder(char *folder_path);
 
 static void
 usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
+
 	errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n");
 }
 
@@ -126,7 +141,7 @@ uint64_t starttime = 0;
  * The sd log deamon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
  * to task_spawn().
  */
@@ -157,6 +172,7 @@ int sdlog_main(int argc, char *argv[])
 		if (!thread_running) {
 			printf("\tsdlog is not started\n");
 		}
+
 		thread_should_exit = true;
 		exit(0);
 	}
@@ -164,9 +180,11 @@ int sdlog_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			print_sdlog_status();
+
 		} else {
 			printf("\tsdlog not started\n");
 		}
+
 		exit(0);
 	}
 
@@ -174,7 +192,8 @@ int sdlog_main(int argc, char *argv[])
 	exit(1);
 }
 
-int create_logfolder(char* folder_path) {
+int create_logfolder(char *folder_path)
+{
 	/* make folder on sdcard */
 	uint16_t foldernumber = 1; // start with folder 0001
 	int mkdir_ret;
@@ -193,11 +212,14 @@ int create_logfolder(char* folder_path) {
 			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) {
@@ -216,11 +238,60 @@ int create_logfolder(char* folder_path) {
 		warn("all %d possible folders exist already", MAX_NO_LOGFOLDER);
 		return -1;
 	}
+
 	return 0;
 }
 
 
-int sdlog_thread_main(int argc, char *argv[]) {
+static void *
+sdlog_sysvector_write_thread(void *arg)
+{
+	struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg;
+
+	int poll_count = 0;
+	struct sdlog_sysvector sysvect;
+	memset(&sysvect, 0, sizeof(sysvect));
+
+	while (!thread_should_exit) {
+		//pthread_mutex..
+		if (sdlog_logbuffer_read(logbuf, &sysvect) == OK) {
+			sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect));
+		}
+
+		if (poll_count % 100 == 0) {
+			fsync(sysvector_file);
+		}
+
+		poll_count++;
+		usleep(3000);
+	}
+
+	fsync(sysvector_file);
+
+	return OK;
+}
+
+pthread_t
+sysvector_write_start(struct sdlog_logbuffer *logbuf)
+{
+	pthread_attr_t receiveloop_attr;
+	pthread_attr_init(&receiveloop_attr);
+
+	struct sched_param param;
+	/* low priority, as this is expensive disk I/O */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
+	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
+
+	pthread_attr_setstacksize(&receiveloop_attr, 2048);
+
+	pthread_t thread;
+	pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf);
+	return thread;
+}
+
+
+int sdlog_thread_main(int argc, char *argv[])
+{
 
 	warnx("starting\n");
 
@@ -229,6 +300,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	}
 
 	char folder_path[64];
+
 	if (create_logfolder(folder_path))
 		errx(1, "unable to create logging folder, exiting.");
 
@@ -236,7 +308,6 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	int sensorfile = -1;
 	int actuator_outputs_file = -1;
 	int actuator_controls_file = -1;
-	int sysvector_file = -1;
 	FILE *gpsfile;
 	FILE *blackbox_file;
 	// FILE *vehiclefile;
@@ -247,6 +318,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 	/* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */
 	sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined");
+
 	if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
 		errx(1, "opening %s failed.\n", path_buf);
 	}
@@ -259,28 +331,34 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 	/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
 	sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector");
+
 	if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
 		errx(1, "opening %s failed.\n", path_buf);
 	}
 
 	/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
 	sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_controls0");
+
 	if (0 == (actuator_controls_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
 		errx(1, "opening %s failed.\n", path_buf);
 	}
 
 	/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
 	sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
+
 	if (NULL == (gpsfile = fopen(path_buf, "w"))) {
 		errx(1, "opening %s failed.\n", path_buf);
 	}
+
 	int gpsfile_no = fileno(gpsfile);
 
 	/* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */
 	sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox");
+
 	if (NULL == (blackbox_file = fopen(path_buf, "w"))) {
 		errx(1, "opening %s failed.\n", path_buf);
 	}
+
 	int blackbox_file_no = fileno(blackbox_file);
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
@@ -426,7 +504,10 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 	thread_running = true;
 
-	int poll_count = 0;
+	/* Initialize log buffer with a size of 5 */
+	sdlog_logbuffer_init(&lb, 5);
+	/* start logbuffer emptying thread */
+	pthread_t sysvector_pthread = sysvector_write_start(&lb);
 
 	starttime = hrt_absolute_time();
 
@@ -450,7 +531,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		// 		fsync(blackbox_file_no);
 		// 	}
 
-			
+
 
 		// 	/* --- VEHICLE COMMAND VALUE --- */
 		// 	if (fds[ifds++].revents & POLLIN) {
@@ -476,14 +557,14 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		// 		/* copy attitude data into local buffer */
 		// 		orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
 
-				
+
 		// 	}
 
 		// 	/* --- VEHICLE ATTITUDE SETPOINT --- */
 		// 	if (fds[ifds++].revents & POLLIN) {
 		// 		/* copy local position data into local buffer */
 		// 		orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
-				
+
 		// 	}
 
 		// 	/* --- ACTUATOR OUTPUTS 0 --- */
@@ -502,12 +583,6 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		// 	}
 		// }
 
-		if (poll_count % 100 == 0) {
-			fsync(sysvector_file);
-		}
-
-		poll_count++;
-
 
 		/* copy sensors raw data into local buffer */
 		orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
@@ -523,27 +598,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
 		orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
 
-		#pragma pack(push, 1)
-		struct {
-			uint64_t timestamp; //[us]
-			float gyro[3]; //[rad/s]
-			float accel[3]; //[m/s^2]
-			float mag[3]; //[gauss]
-			float baro; //pressure [millibar]
-			float baro_alt; //altitude above MSL [meter]
-			float baro_temp; //[degree 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 [volt]
-			float local_position[3]; //tangent plane mapping into x,y,z [m]
-			int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
-			float attitude[3]; //pitch, roll, yaw [rad]
-			float rotMatrix[9]; //unitvectors
-			float vicon[6];
-			float control_effective[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
-			float flow[6]; // flow raw x, y, flow metric x, y, flow ground dist, flow quality
-		} sysvector = {
+		struct sdlog_sysvector sysvect = {
 			.timestamp = buf.raw.timestamp,
 			.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
 			.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
@@ -552,8 +607,10 @@ int sdlog_thread_main(int argc, char *argv[]) {
 			.baro_alt = buf.raw.baro_alt_meter,
 			.baro_temp = buf.raw.baro_temp_celcius,
 			.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
-			.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
-					buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
+			.actuators = {
+				buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
+				buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]
+			},
 			.vbat = 0.0f, /* XXX use battery_status uORB topic */
 			.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
 			.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
@@ -563,18 +620,21 @@ int sdlog_thread_main(int argc, char *argv[]) {
 			.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
 			.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
 			.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}
+			/* XXX add calculated airspeed */
 		};
-		#pragma pack(pop)
 
-		sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
+		/* put into buffer for later IO */
+		sdlog_logbuffer_write(&lb, &sysvect);
+		// XXX notify writing thread through pthread mutex
 
 		usleep(3500);   // roughly 150 Hz
 	}
 
-	fsync(sysvector_file);
-
 	print_sdlog_status();
 
+	/* wait for write thread to return */
+	(void)pthread_join(sysvector_pthread, NULL);
+
 	warnx("exiting.\n");
 
 	close(sensorfile);
@@ -606,43 +666,44 @@ int file_exist(const char *filename)
 	return stat(filename, &buffer);
 }
 
-int file_copy(const char* file_old, const char* file_new)
+int file_copy(const char *file_old, const char *file_new)
 {
 	FILE *source, *target;
 	source = fopen(file_old, "r");
 	int ret = 0;
- 
-	if( source == NULL )
-	{
+
+	if (source == NULL) {
 		warnx("failed opening input file to copy");
 		return 1;
 	}
 
 	target = fopen(file_new, "w");
- 
-	if( target == NULL )
-	{
+
+	if (target == NULL) {
 		fclose(source);
 		warnx("failed to open output file to copy");
 		return 1;
 	}
- 
+
 	char buf[128];
 	int nread;
+
 	while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) {
 		int ret = fwrite(buf, 1, nread, target);
+
 		if (ret <= 0) {
 			warnx("error writing file");
 			ret = 1;
 			break;
 		}
 	}
+
 	fsync(fileno(target));
 
 	fclose(source);
 	fclose(target);
 
-   return ret;
+	return ret;
 }