From 0fcb873636ec2b8c7abcb9587f582d3f79db86a5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 9 Jun 2020 11:36:07 +0200
Subject: [PATCH] logger: add raw FIFO high-rate IMU logging profiles

Minimum requirement to use: set IMU_GYRO_RATEMAX to 400.

Logging rate of a single topic: ~85 KB/s. If multiple should be logged,
a really good SD card has to be used.
---
 src/modules/logger/logged_topics.cpp | 18 ++++++++++++++++++
 src/modules/logger/logged_topics.h   |  7 ++++++-
 src/modules/logger/logger.cpp        | 13 +++++++++++++
 src/modules/logger/params.c          |  6 +++++-
 4 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index 05ec409726..1cb908b2ba 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -201,6 +201,16 @@ void LoggedTopics::add_vision_and_avoidance_topics()
 	add_topic("vehicle_visual_odometry", 30);
 }
 
+void LoggedTopics::add_raw_imu_gyro_fifo()
+{
+	add_topic("sensor_gyro_fifo");
+}
+
+void LoggedTopics::add_raw_imu_accel_fifo()
+{
+	add_topic("sensor_accel_fifo");
+}
+
 void LoggedTopics::add_system_identification_topics()
 {
 	// for system id need to log imu and controls at full rate
@@ -401,4 +411,12 @@ void LoggedTopics::initialize_configured_topics(SDLogProfileMask profile)
 	if (profile & SDLogProfileMask::VISION_AND_AVOIDANCE) {
 		add_vision_and_avoidance_topics();
 	}
+
+	if (profile & SDLogProfileMask::RAW_IMU_GYRO_FIFO) {
+		add_raw_imu_gyro_fifo();
+	}
+
+	if (profile & SDLogProfileMask::RAW_IMU_ACCEL_FIFO) {
+		add_raw_imu_accel_fifo();
+	}
 }
diff --git a/src/modules/logger/logged_topics.h b/src/modules/logger/logged_topics.h
index be794041d6..648d6c046c 100644
--- a/src/modules/logger/logged_topics.h
+++ b/src/modules/logger/logged_topics.h
@@ -51,7 +51,9 @@ enum class SDLogProfileMask : int32_t {
 	HIGH_RATE =             1 << 4,
 	DEBUG_TOPICS =          1 << 5,
 	SENSOR_COMPARISON =     1 << 6,
-	VISION_AND_AVOIDANCE =  1 << 7
+	VISION_AND_AVOIDANCE =  1 << 7,
+	RAW_IMU_GYRO_FIFO =     1 << 8,
+	RAW_IMU_ACCEL_FIFO =    1 << 9
 };
 
 enum class MissionLogType : int32_t {
@@ -139,6 +141,9 @@ private:
 	void add_debug_topics();
 	void add_sensor_comparison_topics();
 	void add_vision_and_avoidance_topics();
+	void add_raw_imu_gyro_fifo();
+	void add_raw_imu_accel_fifo();
+
 	/**
 	 * add a logged topic (called by add_topic() above).
 	 * @return true on success
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index d278eee37f..f93360e8b3 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -490,6 +490,19 @@ bool Logger::initialize_topics()
 		return false;
 	}
 
+	if ((sdlog_profile & SDLogProfileMask::RAW_IMU_ACCEL_FIFO) || (sdlog_profile & SDLogProfileMask::RAW_IMU_GYRO_FIFO)) {
+		// if we are logging high-rate FIFO, reduce the logging interval & increase process priority to avoid missing samples
+		PX4_INFO("Logging FIFO data: increasing task prio and logging rate");
+		_log_interval = 800;
+		sched_param param{};
+		param.sched_priority = SCHED_PRIORITY_ATTITUDE_CONTROL;
+		int ret = pthread_setschedparam(pthread_self(), SCHED_DEFAULT, &param);
+
+		if (ret != 0) {
+			PX4_ERR("pthread_setschedparam failed (%i)", ret);
+		}
+	}
+
 	delete[](_subscriptions);
 	_subscriptions = nullptr;
 
diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c
index 1ac7f248fd..c91ac22a95 100644
--- a/src/modules/logger/params.c
+++ b/src/modules/logger/params.c
@@ -123,9 +123,11 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0);
  * 5 : Debugging topics (debug_*.msg topics, for custom code)
  * 6 : Topics for sensor comparison (low rate raw IMU, Baro and Magnetomer data)
  * 7 : Topics for computer vision and collision avoidance
+ * 8 : Raw FIFO high-rate IMU (Gyro)
+ * 9 : Raw FIFO high-rate IMU (Accel)
  *
  * @min 0
- * @max 255
+ * @max 1023
  * @bit 0 Default set (general log analysis)
  * @bit 1 Estimator replay (EKF2)
  * @bit 2 Thermal calibration
@@ -134,6 +136,8 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0);
  * @bit 5 Debug
  * @bit 6 Sensor comparison
  * @bit 7 Computer Vision and Avoidance
+ * @bit 8 Raw FIFO high-rate IMU (Gyro)
+ * @bit 9 Raw FIFO high-rate IMU (Accel)
  * @reboot_required true
  * @group SD Logging
  */