Enable / disable logging while running, enabled black box logging (ringbuffer needed), enabled GPS KML logging (does not yet write outputs)

1 files changed, 182 insertions, 192 deletions

diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index d79343cd9..175392afb 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -73,6 +73,8 @@
 
 #include <systemlib/systemlib.h>
 
+#include <mavlink/mavlink_log.h>
+
 #include "sdlog_ringbuffer.h"
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -83,6 +85,7 @@ 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;
+int mavlink_fd = -1;
 struct sdlog_logbuffer lb;
 
 /* mutex / condition to synchronize threads */
@@ -118,6 +121,8 @@ static int file_exist(const char *filename);
 
 static int file_copy(const char *file_old, const char *file_new);
 
+static void handle_command(struct vehicle_command_s *cmd);
+
 /**
  * Print the current status.
  */
@@ -134,7 +139,7 @@ usage(const char *reason)
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
 
-	errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n");
+	errx(1, "usage: sdlog {start|stop|status} [-s <number of skipped lines>]\n\n");
 }
 
 // XXX turn this into a C++ class
@@ -145,6 +150,9 @@ unsigned sysvector_bytes = 0;
 unsigned blackbox_file_bytes = 0;
 uint64_t starttime = 0;
 
+/* logging on or off, default to true */
+bool logging_enabled = true;
+
 /**
  * The sd log deamon app only briefly exists to start
  * the background job. The stack size assigned in the
@@ -318,25 +326,44 @@ sysvector_write_start(struct sdlog_logbuffer *logbuf)
 
 int sdlog_thread_main(int argc, char *argv[])
 {
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	if (mavlink_fd < 0) {
+		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
+	}
 
-	/* log every 2nd value (skip one) */
+	/* log every n'th value (skip one per default) */
 	int skip_value = 1;
 
-	if (argc > 1) {
-		if (!strcmp(argv[1], "-s") && argc > 2) {
-			int s = atoi(argv[2]);
+	/* work around some stupidity in task_create's argv handling */
+	argc -= 2;
+	argv += 2;
+	int ch;
 
-			if (s > 0 && s < 250) {
-				skip_value = s;
+	while ((ch = getopt(argc, argv, "sr")) != EOF) {
+		switch (ch) {
+		case 's':
+			{
+			/* log only every n'th (gyro clocked) value */
+			unsigned s = strtoul(optarg, NULL, 10);
+
+			if (s < 1 || s > 250) {
+				errx(1, "Wrong skip value of %d, out of range (1..250)\n", s);
 			} else {
-				warnx("Ignoring skip value of %d, out of range (1..250)\n", s);
+				skip_value = s;
 			}
-		}
-	}
+			}
+			break;
 
-	warnx("starting\n");
+		case 'r':
+			/* log only on request, disable logging per default */
+			logging_enabled = false;
+			break;
 
-	warnx("skipping %d sensor packets between logged packets.\n", skip_value);
+		default:
+			usage("unrecognized flag");
+		}
+	}
 
 	if (file_exist(mountpoint) != OK) {
 		errx(1, "logging mount point %s not present, exiting.", mountpoint);
@@ -347,31 +374,15 @@ int sdlog_thread_main(int argc, char *argv[])
 	if (create_logfolder(folder_path))
 		errx(1, "unable to create logging folder, exiting.");
 
-	/* create sensorfile */
-	int sensorfile = -1;
-	int actuator_outputs_file = -1;
-	int actuator_controls_file = -1;
 	FILE *gpsfile;
 	FILE *blackbox_file;
-	// FILE *vehiclefile;
 
-	char path_buf[64] = ""; // string to hold the path to the sensorfile
+	/* string to hold the path to the sensorfile */
+	char path_buf[64] = "";
 
+	/* only print logging path, important to find log file later */
 	warnx("logging to directory %s\n", folder_path);
 
-	/* 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);
-	}
-
-	// /* set up file path: e.g. /mnt/sdcard/session0001/actuator_outputs0.bin */
-	// sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_outputs0");
-	// if (0 == (actuator_outputs_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, "sysvector");
 
@@ -379,13 +390,6 @@ int sdlog_thread_main(int argc, char *argv[])
 		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");
 
@@ -402,6 +406,7 @@ int sdlog_thread_main(int argc, char *argv[])
 		errx(1, "opening %s failed.\n", path_buf);
 	}
 
+	// XXX for fsync() calls
 	int blackbox_file_no = fileno(blackbox_file);
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
@@ -455,12 +460,18 @@ int sdlog_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- GPS POSITION --- */
+	/* subscribe to ORB for global position */
+	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	fds[fdsc_count].fd = subs.gps_pos_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* --- SENSORS RAW VALUE --- */
 	/* subscribe to ORB for sensors raw */
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	fds[fdsc_count].fd = subs.sensor_sub;
-	/* rate-limit raw data updates to 200Hz */
-	orb_set_interval(subs.sensor_sub, 5);
+	/* do not rate limit, instead use skip counter (aliasing on rate limit) */
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -513,13 +524,6 @@ int sdlog_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
-	/* --- GPS POSITION --- */
-	/* subscribe to ORB for global position */
-	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	fds[fdsc_count].fd = subs.gps_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
 	/* --- VICON POSITION --- */
 	/* subscribe to ORB for vicon position */
 	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
@@ -577,18 +581,13 @@ int sdlog_thread_main(int argc, char *argv[])
 
 	starttime = hrt_absolute_time();
 
-	// XXX clock the log for now with the gyro output rate / 2
-	struct pollfd gyro_fd;
-	gyro_fd.fd = subs.sensor_sub;
-	gyro_fd.events = POLLIN;
-
 	/* track skipping */
 	int skip_count = 0;
 
 	while (!thread_should_exit) {
 
 		// XXX only use gyro for now
-		int poll_ret = poll(&gyro_fd, 1, 1000);
+		int poll_ret = poll(fds, 2, 1000);
 
 		// int poll_ret = poll(fds, fdsc_count, timeout);
 
@@ -599,145 +598,107 @@ int sdlog_thread_main(int argc, char *argv[])
 			/* XXX this is seriously bad - should be an emergency */
 		} else {
 
-			/* always copy sensors raw data into local buffer, since poll flags won't clear else */
-			orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
-			/* copy actuator data into local buffer */
-			orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
-			orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
-			orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
-			orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
-			orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
-			orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
-			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);
-
-			if (skip_count < skip_value) {
-				skip_count++;
-				/* do not log data */
-				continue;
-			} else {
-				/* log data, reset */
-				skip_count = 0;
+			int ifds;
+
+			/* --- VEHICLE COMMAND VALUE --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy command into local buffer */
+				orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
+
+				/* always log to blackbox, even when logging disabled */
+				blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d,
+					buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
+					(double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
+
+				handle_command(&buf.cmd);
+			}
+
+			/* --- VEHICLE GPS VALUE --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy gps position into local buffer */
+				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
+
+				/* if logging disabled, continue */
+				if (logging_enabled) {
+
+				/* write KML line */
+				}
 			}
 
-		// 	int ifds = 0;
-
-		// 	if (poll_count % 5000 == 0) {
-		// 		fsync(sensorfile);
-		// 		fsync(actuator_outputs_file);
-		// 		fsync(actuator_controls_file);
-		// 		fsync(blackbox_file_no);
-		// 	}
-
-
-
-		// 	/* --- VEHICLE COMMAND VALUE --- */
-		// 	if (fds[ifds++].revents & POLLIN) {
-		// 		/* copy command into local buffer */
-		// 		orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
-		// 		blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d,
-		// 			buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
-		// 			(double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
-		// 	}
-
-		// 	/* --- SENSORS RAW VALUE --- */
-		// 	if (fds[ifds++].revents & POLLIN) {
-
-		// 		/* copy sensors raw data into local buffer */
-		// 		orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
-		// 		/* write out */
-		// 		sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw));
-		// 	}
-
-		// 	/* --- ATTITUDE VALUE --- */
-		// 	if (fds[ifds++].revents & POLLIN) {
-
-		// 		/* 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 --- */
-		// 	if (fds[ifds++].revents & POLLIN) {
-		// 		/* copy actuator data into local buffer */
-		// 		orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
-		// 		/* write out */
-		// 		// actuator_outputs_bytes += write(actuator_outputs_file, (const char*)&buf.act_outputs, sizeof(buf.act_outputs));
-		// 	}
-
-		// 	/* --- ACTUATOR CONTROL --- */
-		// 	if (fds[ifds++].revents & POLLIN) {
-		// 		orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls);
-		// 		/* write out */
-		// 		actuator_controls_bytes += write(actuator_controls_file, (const char*)&buf.act_controls, sizeof(buf.act_controls));
-		// 	}
-
-
-			/* copy sensors raw data into local buffer */
-			orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
-			/* copy actuator data into local buffer */
-			orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
-			orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
-			orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
-			orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
-			orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
-			orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
-			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);
-			orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure);
-			orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
-
-			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]},
-				.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
-				.baro = buf.raw.baro_pres_mbar,
-				.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]
-				},
-				.vbat = buf.batt.voltage_v,
-				.bat_current = buf.batt.current_a,
-				.bat_discharged = buf.batt.discharged_mah,
-				.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},
-				.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
-				.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
-				.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
-				.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},
-				.diff_pressure = buf.diff_pressure.differential_pressure_mbar,
-				.ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s,
-				.true_airspeed = buf.diff_pressure.true_airspeed_m_s
-			};
-
-			/* put into buffer for later IO */
-			pthread_mutex_lock(&sysvector_mutex);
-			sdlog_logbuffer_write(&lb, &sysvect);
-			/* signal the other thread new data, but not yet unlock */
-			if (lb.count > lb.size / 3) {
-				/* only request write if several packets can be written at once */
-				pthread_cond_signal(&sysvector_cond);
+			/* --- SENSORS RAW VALUE --- */
+			if (fds[ifds++].revents & POLLIN) {
+
+				// /* copy sensors raw data into local buffer */
+				// orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
+				// /* write out */
+				// sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw));
+
+				/* always copy sensors raw data into local buffer, since poll flags won't clear else */
+				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
+				orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
+				orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
+				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
+				orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
+				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
+				orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
+				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);
+				orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure);
+				orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
+
+				/* if skipping is on or logging is disabled, ignore */
+				if (skip_count < skip_value || !logging_enabled) {
+					skip_count++;
+					/* do not log data */
+					continue;
+				} else {
+					/* log data, reset */
+					skip_count = 0;
+				}
+
+				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]},
+					.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
+					.baro = buf.raw.baro_pres_mbar,
+					.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]
+					},
+					.vbat = buf.batt.voltage_v,
+					.bat_current = buf.batt.current_a,
+					.bat_discharged = buf.batt.discharged_mah,
+					.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},
+					.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
+					.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
+					.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
+					.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},
+					.diff_pressure = buf.diff_pressure.differential_pressure_mbar,
+					.ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s,
+					.true_airspeed = buf.diff_pressure.true_airspeed_m_s
+				};
+
+				/* put into buffer for later IO */
+				pthread_mutex_lock(&sysvector_mutex);
+				sdlog_logbuffer_write(&lb, &sysvect);
+				/* signal the other thread new data, but not yet unlock */
+				if ((unsigned)lb.count > lb.size / 3) {
+					/* only request write if several packets can be written at once */
+					pthread_cond_signal(&sysvector_cond);
+				}
+				/* unlock, now the writer thread may run */
+				pthread_mutex_unlock(&sysvector_mutex);
 			}
-			/* unlock, now the writer thread may run */
-			pthread_mutex_unlock(&sysvector_mutex);
+
 		}
 
 	}
@@ -752,9 +713,8 @@ int sdlog_thread_main(int argc, char *argv[])
 
 	warnx("exiting.\n");
 
-	close(sensorfile);
-	close(actuator_outputs_file);
-	close(actuator_controls_file);
+	/* finish KML file */
+	// XXX
 	fclose(gpsfile);
 	fclose(blackbox_file);
 
@@ -821,4 +781,34 @@ int file_copy(const char *file_old, const char *file_new)
 	return ret;
 }
 
+void handle_command(struct vehicle_command_s *cmd)
+{
+	/* result of the command */
+	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+
+	/* request to set different system mode */
+	switch (cmd->command) {
+
+	case VEHICLE_CMD_PREFLIGHT_STORAGE:
+
+		if (((int)(cmd->param3)) == 1)	{
 
+			/* enable logging */
+			mavlink_log_info(mavlink_fd, "[log] file:");
+			mavlink_log_info(mavlink_fd, "logdir");
+			logging_enabled = true;
+		}
+		if (((int)(cmd->param3)) == 0)	{
+
+			/* disable logging */
+			mavlink_log_info(mavlink_fd, "[log] stopped.");
+			logging_enabled = false;
+		}
+		break;
+
+	default:
+		/* silently ignore */
+		break;
+	}
+
+}