Fixed mavlink timestamps, fixed SD logger, ready for flight tests

3 files changed, 105 insertions, 37 deletions

diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 004c9a5fe..78267d9eb 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -748,6 +748,13 @@ static void *uorb_receiveloop(void *arg)
 	 */
 	const int timeout = 1000;
 
+	/*
+	 * Last sensor loop time
+	 * some outputs are better timestamped
+	 * with this "global" reference.
+	 */
+	uint64_t last_sensor_timestamp = 0;
+
 	while (!thread_should_exit) {
 
 		int poll_ret = poll(fds, fdsc_count, timeout);
@@ -767,8 +774,10 @@ static void *uorb_receiveloop(void *arg)
 				/* copy sensors raw data into local buffer */
 				orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw);
 
+				last_sensor_timestamp = buf.raw.timestamp;
+
 				/* send raw imu data */
-				mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0],
+				mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_raw[0],
 					buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0],
 					buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0],
 					buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
@@ -804,7 +813,7 @@ static void *uorb_receiveloop(void *arg)
 					baro_counter = buf.raw.baro_counter;
 				}
 
-				mavlink_msg_highres_imu_send(MAVLINK_COMM_0, buf.raw.timestamp,
+				mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
 					buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1],
 					buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0],
 					buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2],
@@ -826,7 +835,7 @@ static void *uorb_receiveloop(void *arg)
 				orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att);
 
 				/* send sensor values */
-				mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
+				mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
 
 				attitude_counter++;
 			}
@@ -933,17 +942,17 @@ static void *uorb_receiveloop(void *arg)
 
 					/* HIL message as per MAVLink spec */
 					mavlink_msg_hil_controls_send(chan,
-										hrt_absolute_time(),
-										buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */
-										buf.att_sp.pitch_body,
-										buf.att_sp.yaw_body,
-										buf.att_sp.thrust,
-										0,
-										0,
-										0,
-										0,
-										mavlink_mode,
-										0);
+						hrt_absolute_time(),
+						buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */
+						buf.att_sp.pitch_body,
+						buf.att_sp.yaw_body,
+						buf.att_sp.thrust,
+						0,
+						0,
+						0,
+						0,
+						mavlink_mode,
+						0);
 				}
 			}
 
@@ -951,7 +960,7 @@ static void *uorb_receiveloop(void *arg)
 			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);
-				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
 					0 /* port 0 */,
 					buf.act_outputs.output[0],
 					buf.act_outputs.output[1],
@@ -979,7 +988,7 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[ifds++].revents & POLLIN) {
 				/* copy actuator data into local buffer */
 				orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs);
-				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
 					(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/,
 					buf.act_outputs.output[0],
 					buf.act_outputs.output[1],
@@ -990,7 +999,7 @@ static void *uorb_receiveloop(void *arg)
 					buf.act_outputs.output[6],
 					buf.act_outputs.output[7]);
 				if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
-					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
 					3 /* port 3 */,
 					buf.act_outputs.output[ 8],
 					buf.act_outputs.output[ 9],
@@ -1007,7 +1016,7 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[ifds++].revents & POLLIN) {
 				/* copy actuator data into local buffer */
 				orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs);
-				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
 					(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */,
 					buf.act_outputs.output[0],
 					buf.act_outputs.output[1],
@@ -1018,7 +1027,7 @@ static void *uorb_receiveloop(void *arg)
 					buf.act_outputs.output[6],
 					buf.act_outputs.output[7]);
 				if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
-					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
 					5 /* port 5 */,
 					buf.act_outputs.output[ 8],
 					buf.act_outputs.output[ 9],
@@ -1035,7 +1044,7 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[ifds++].revents & POLLIN) {
 				/* copy actuator data into local buffer */
 				orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs);
-				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
 					(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */,
 					buf.act_outputs.output[0],
 					buf.act_outputs.output[1],
@@ -1046,7 +1055,7 @@ static void *uorb_receiveloop(void *arg)
 					buf.act_outputs.output[6],
 					buf.act_outputs.output[7]);
 				if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
-					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
 					7 /* port 7 */,
 					buf.act_outputs.output[ 8],
 					buf.act_outputs.output[ 9],
@@ -1075,16 +1084,16 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators);
 				/* send, add spaces so that string buffer is at least 10 chars long */
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0     ", buf.actuators.control[0]);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1     ", buf.actuators.control[1]);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2     ", buf.actuators.control[2]);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3     ", buf.actuators.control[3]);
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl0       ", buf.actuators.control[0]);
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1       ", buf.actuators.control[1]);
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2       ", buf.actuators.control[2]);
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3       ", buf.actuators.control[3]);
 			}
 
 			/* --- DEBUG KEY/VALUE --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, buf.debug.key, buf.debug.value);
+				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.debug.key, buf.debug.value);
 			}
 		}
 	}
diff --git a/apps/sdlog/Makefile b/apps/sdlog/Makefile
index 73490f6e5..61f224e99 100644
--- a/apps/sdlog/Makefile
+++ b/apps/sdlog/Makefile
@@ -36,7 +36,7 @@
 #
 
 APPNAME		 = sdlog
-PRIORITY	 = SCHED_PRIORITY_DEFAULT - 1
+PRIORITY	 = SCHED_PRIORITY_DEFAULT - 30
 STACKSIZE	 = 2048
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index ab5f5c2a8..1ef838850 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -50,6 +50,8 @@
 #include <stdlib.h>
 #include <string.h>
 #include <systemlib/err.h>
+#include <unistd.h>
+#include <arch/board/up_hrt.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
@@ -57,6 +59,7 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_command.h>
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
@@ -117,12 +120,15 @@ int sdlog_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 10, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 30, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+		if (!thread_running) {
+			printf("\tsdlog is not started\n");
+		}
 		thread_should_exit = true;
 		exit(0);
 	}
@@ -190,17 +196,31 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 	/* create sensorfile */
 	int sensorfile;
+	unsigned sensor_combined_bytes = 0;
+	int actuator_outputs_file;
+	unsigned actuator_outputs_bytes = 0;
 	FILE *gpsfile;
+	unsigned blackbox_file_bytes = 0;
+	FILE *blackbox_file;
 	// FILE *vehiclefile;
 
 	char path_buf[64] = ""; // string to hold the path to the sensorfile
 
+	printf("[sdlog] logging to directory %s\n", folder_path);
+
 	/* set up file path: e.g. /mnt/sdcard/session0001/sensors_combined.bin */
 	sprintf(path_buf, "%s/%s.bin", folder_path, "sensors_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);
+	}
+	int actuator_outputs_file_no = actuator_outputs_file;
+
 	/* 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"))) {
@@ -208,6 +228,13 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	}
 	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 --- */
 	/* number of messages */
 	const ssize_t fdsc = 25;
@@ -223,9 +250,11 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		struct vehicle_attitude_setpoint_s att_sp;
 		struct actuator_outputs_s act_outputs;
 		struct actuator_controls_s actuators;
+		struct vehicle_command_s cmd;
 	} buf;
 
 	struct {
+		int cmd_sub;
 		int sensor_sub;
 		int att_sub;
 		int spa_sub;
@@ -233,6 +262,13 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		int actuators_sub;
 	} subs;
 
+	/* --- MANAGEMENT - LOGGING COMMAND --- */
+	/* subscribe to ORB for sensors raw */
+	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	fds[fdsc_count].fd = subs.cmd_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));
@@ -243,7 +279,6 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	/* --- ATTITUDE VALUE --- */
 	/* subscribe to ORB for attitude */
 	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	orb_set_interval(subs.att_sub, 100);
 	fds[fdsc_count].fd = subs.att_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
@@ -252,7 +287,6 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	/* subscribe to ORB for attitude setpoint */
 	/* struct already allocated */
 	subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	orb_set_interval(subs.spa_sub, 2000);	/* 0.5 Hz updates */
 	fds[fdsc_count].fd = subs.spa_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
@@ -287,6 +321,10 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 	thread_running = true;
 
+	int poll_count = 0;
+
+	uint64_t starttime = hrt_absolute_time();
+
 	while (!thread_should_exit) {
 
 		int poll_ret = poll(fds, fdsc_count, timeout);
@@ -300,13 +338,29 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 			int ifds = 0;
 
+			if (poll_count % 2000 == 0) {
+				fsync(sensorfile);
+				fsync(actuator_outputs_file_no);
+				fsync(blackbox_file_no);
+			}
+			poll_count++;
+
+			/* --- 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-%f-%f-%f-%f-%f-%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 */
-				write(sensorfile, (const char*)&buf.raw, sizeof(buf.raw));
+				sensor_combined_bytes += write(sensorfile, (const char*)&buf.raw, sizeof(buf.raw));
 			}
 
 			/* --- ATTITUDE VALUE --- */
@@ -329,7 +383,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
 			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.raw, sizeof(buf.raw));
 			}
 
 			/* --- ACTUATOR CONTROL --- */
@@ -337,17 +392,21 @@ int sdlog_thread_main(int argc, char *argv[]) {
 				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.actuators_sub, &buf.actuators);
 				
 			}
-
-			/* enforce write to disk */
-			fsync(sensorfile);
-			fsync(gpsfile_no);
 		}
 	}
 
-	warn("exiting.");
+	unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes;
+	float mebibytes = bytes / 1024.0f / 1024.0f;
+	float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f;
+
+	printf("[sdlog] wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds));
+
+	printf("[sdlog] exiting.\n");
 
 	close(sensorfile);
+	close(gpsfile);
 	fclose(gpsfile);
+	fclose(blackbox_file);
 
 	thread_running = false;