From 3a029e58c75fd0305a4b9e6724588751c0ae7bfa Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 20 Jan 2017 08:10:30 +1100
Subject: [PATCH] logger: Add mode for thermal calibration logging

Logs the IMU and baro data at 10Hz
---
 src/modules/logger/logger.cpp | 26 ++++++++++++++++++++++++--
 src/modules/logger/logger.h   |  6 ++++++
 src/modules/logger/params.c   |  3 ++-
 3 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index dd3c453008..31b1b6f32f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -383,9 +383,11 @@ Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_inte
 	_log_until_shutdown(log_until_shutdown),
 	_log_name_timestamp(log_name_timestamp),
 	_writer(backend, buffer_size, queue_size),
-	_log_interval(log_interval)
+	_log_interval(log_interval),
+	_sdlog_mode(0)
 {
 	_log_utc_offset = param_find("SDLOG_UTC_OFFSET");
+	_sdlog_mode_handle = param_find("SDLOG_MODE");
 }
 
 Logger::~Logger()
@@ -565,6 +567,15 @@ void Logger::add_default_topics()
 	add_topic("vehicle_land_detected");
 }
 
+void Logger::add_calibration_topics()
+{
+	// Note: try to avoid setting the interval where possible, as it increases RAM usage
+
+	add_topic("sensor_gyro", 100);
+	add_topic("sensor_accel", 100);
+	add_topic("sensor_baro", 100);
+}
+
 int Logger::add_topics_from_file(const char *fname)
 {
 	FILE		*fp;
@@ -671,7 +682,18 @@ void Logger::run()
 		PX4_INFO("logging %d topics from logger_topics.txt", ntopics);
 
 	} else {
-		add_default_topics();
+
+		/* get the logging mode */
+		if (_sdlog_mode_handle != PARAM_INVALID) {
+			param_get(_sdlog_mode_handle, &_sdlog_mode);
+		}
+
+		if (_sdlog_mode == 3) {
+			add_calibration_topics();
+
+		} else {
+			add_default_topics();
+		}
 	}
 
 	int vehicle_command_sub = -1;
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index d63b78f354..066a1dafc6 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -216,6 +216,7 @@ private:
 	int add_topics_from_file(const char *fname);
 
 	void add_default_topics();
+	void add_calibration_topics();
 
 	void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result);
 
@@ -255,6 +256,11 @@ private:
 	orb_advert_t					_mavlink_log_pub = nullptr;
 	uint16_t					_next_topic_id = 0; ///< id of next subscribed ulog topic
 	char						*_replay_file_name = nullptr;
+
+	// control
+	param_t _sdlog_mode_handle;
+	int32_t _sdlog_mode;
+
 };
 
 } //namespace logger
diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c
index ef7f0f6234..7001fd0abd 100644
--- a/src/modules/logger/params.c
+++ b/src/modules/logger/params.c
@@ -60,9 +60,10 @@ PARAM_DEFINE_INT32(SDLOG_UTC_OFFSET, 0);
  * @value 0 when armed until disarm (default)
  * @value 1 from boot until disarm
  * @value 2 from boot until shutdown
+ * @value 3 from boot until shutdown - IMU and Baro data only (used for thermal calibration)
  *
  * @min 0
- * @max 2
+ * @max 3
  * @reboot_required true
  * @group SD Logging
  */