sdlog2: Use more appropriate priorities and locking strategies.

src/modules/sdlog2/logbuffer.c

@@ -51,6 +51,7 @@ int logbuffer_init(struct logbuffer_s *lb, int size)
 	lb->write_ptr = 0;
 	lb->read_ptr = 0;
 	lb->data = NULL;
+	lb->perf_dropped = perf_alloc(PC_COUNT, "sd drop");
 	return PX4_OK;
 }
 
@@ -91,6 +92,7 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
 
 	if (size > available) {
 		// buffer overflow
+		perf_count(lb->perf_dropped);
 		return false;
 	}
 
@@ -151,5 +153,6 @@ void logbuffer_free(struct logbuffer_s *lb)
 		lb->write_ptr = 0;
 		lb->read_ptr = 0;
 		lb->data = NULL;
+		perf_free(lb->perf_dropped);
 	}
 }

src/modules/sdlog2/logbuffer.h

@@ -44,6 +44,7 @@
 #define SDLOG2_RINGBUFFER_H_
 
 #include <stdbool.h>
+#include <systemlib/perf_counter.h>
 
 struct logbuffer_s {
 	// pointers and size are in bytes
@@ -51,6 +52,7 @@ struct logbuffer_s {
 	int read_ptr;
 	int size;
 	char *data;
+	perf_counter_t perf_dropped;
 };
 
 int logbuffer_init(struct logbuffer_s *lb, int size);

src/modules/sdlog2/params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,7 +42,7 @@
  *
  * @unit Hz
  * @min -1
- * @max  100
+ * @max  250
  * @group SD Logging
  */
 PARAM_DEFINE_INT32(SDLOG_RATE, -1);
@@ -104,7 +104,11 @@ PARAM_DEFINE_INT32(SDLOG_UTC_OFFSET, 0);
  * Increasing the parameter in steps of one increases the priority.
  *
  * @min 0
- * @max 3
+ * @max  3
+ * @value 0 Low priority
+ * @value 1 Default priority
+ * @value 2 Medium priority
+ * @value 3 Max priority
  * @group SD Logging
  */
-PARAM_DEFINE_INT32(SDLOG_PRIO_BOOST, 0);
+PARAM_DEFINE_INT32(SDLOG_PRIO_BOOST, 2);

src/modules/sdlog2/sdlog2.c

@@ -128,11 +128,13 @@
 
 #define PX4_EPOCH_SECS 1234567890L
 
-#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
+#define LOGBUFFER_WRITE_AND_COUNT(_msg) pthread_mutex_lock(&logbuffer_mutex); \
+	if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \
 	} else { \
 		log_msgs_skipped++; \
-	}
+	} \
+	pthread_mutex_unlock(&logbuffer_mutex);
 
 #define SDLOG_MIN(X,Y) ((X) < (Y) ? (X) : (Y))
 
@@ -633,6 +635,8 @@ static void *logwriter_thread(void *arg)
 			pthread_cond_wait(&logbuffer_cond, &logbuffer_mutex);
 		}
 
+		pthread_mutex_lock(&logbuffer_mutex);
+
 		/* only get pointer to thread-safe data, do heavy I/O a few lines down */
 		int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part);
 
@@ -721,8 +725,8 @@ void sdlog2_start_log()
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&logwriter_attr, &param);
-	/* low priority, as this is expensive disk I/O */
-	param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
+	/* low priority, as this is expensive disk I/O. */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 5;
 	if (pthread_attr_setschedparam(&logwriter_attr, &param)) {
 		warnx("sdlog2: failed setting sched params");
 	}
@@ -766,6 +770,8 @@ void sdlog2_stop_log()
 
 	logging_enabled = false;
 
+	unsigned long skipped_count = log_msgs_skipped;
+
 	/* wake up write thread one last time */
 	pthread_mutex_lock(&logbuffer_mutex);
 	logwriter_should_exit = true;
@@ -802,7 +808,7 @@ void sdlog2_stop_log()
 	/* free log buffer */
 	logbuffer_free(&lb);
 
-	mavlink_and_console_log_info(mavlink_fd, "[blackbox] recording stopped");
+	mavlink_and_console_log_info(mavlink_fd, "[blackbox] stopped (%lu drops)", skipped_count);
 
 	sdlog2_status();
 }
@@ -1431,8 +1437,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 			continue;
 		}
 
-		pthread_mutex_lock(&logbuffer_mutex);
-
 		/* write time stamp message */
 		log_msg.msg_type = LOG_TIME_MSG;
 		log_msg.body.log_TIME.t = hrt_absolute_time();
@@ -2117,6 +2121,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(CAMT);
 		}
 
+		pthread_mutex_lock(&logbuffer_mutex);
+
 		/* signal the other thread new data, but not yet unlock */
 		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 			/* only request write if several packets can be written at once */