From 4e0967889cf31ab669f43fa4150440eb0056b4df Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 17 Jan 2024 13:44:16 -0500
Subject: [PATCH] ekf2: add verbose print status (moved out of DEBUG_BUILD)

---
 boards/px4/sitl/default.px4board |  1 +
 src/modules/ekf2/EKF2.cpp        | 26 ++++++++++++++++++++------
 src/modules/ekf2/EKF2.hpp        |  2 +-
 src/modules/ekf2/Kconfig         |  9 +++++++++
 4 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board
index 008dc97a12..1e254b6a9d 100644
--- a/boards/px4/sitl/default.px4board
+++ b/boards/px4/sitl/default.px4board
@@ -14,6 +14,7 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
 CONFIG_MODULES_EKF2=y
+CONFIG_EKF2_VERBOSE_STATUS=y
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index b13d5104fd..af957c24f2 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -381,7 +381,7 @@ bool EKF2::multi_init(int imu, int mag)
 }
 #endif // CONFIG_EKF2_MULTI_INSTANCE
 
-int EKF2::print_status()
+int EKF2::print_status(bool verbose)
 {
 	PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, attitude: %d, local position: %d, global position: %d\n",
 		     _instance, (double)_ekf.get_dt_ekf_avg(), _ekf.attitude_valid(),
@@ -390,9 +390,11 @@ int EKF2::print_status()
 	perf_print_counter(_ekf_update_perf);
 	perf_print_counter(_msg_missed_imu_perf);
 
-#if defined(DEBUG_BUILD)
-	_ekf.print_status();
-#endif // DEBUG_BUILD
+	if (verbose) {
+#if defined(CONFIG_EKF2_VERBOSE_STATUS)
+		_ekf.print_status();
+#endif // CONFIG_EKF2_VERBOSE_STATUS
+	}
 
 	return 0;
 }
@@ -2859,7 +2861,11 @@ timestamps from the sensor topics.
 	PRINT_MODULE_USAGE_NAME("ekf2", "estimator");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	PRINT_MODULE_USAGE_COMMAND("stop");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "print status info");
+#if defined(CONFIG_EKF2_VERBOSE_STATUS)
+	PRINT_MODULE_USAGE_ARG("-v", "verbose (print all states and full covariance matrix)", true);
+#endif // CONFIG_EKF2_VERBOSE_STATUS
 #if defined(CONFIG_EKF2_MULTI_INSTANCE)
 	PRINT_MODULE_USAGE_COMMAND_DESCR("select_instance", "Request switch to new estimator instance");
 	PRINT_MODULE_USAGE_ARG("<instance>", "Specify desired estimator instance", false);
@@ -2919,10 +2925,18 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 			}
 #endif // CONFIG_EKF2_MULTI_INSTANCE
 
+			bool verbose_status = false;
+
+#if defined(CONFIG_EKF2_VERBOSE_STATUS)
+			if (argc > 2 && (strcmp(argv[2], "-v") == 0)) {
+				verbose_status = true;
+			}
+#endif // CONFIG_EKF2_VERBOSE_STATUS
+
 			for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
 				if (_objects[i].load()) {
 					PX4_INFO_RAW("\n");
-					_objects[i].load()->print_status();
+					_objects[i].load()->print_status(verbose_status);
 				}
 			}
 
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index 05b546c977..2107af227b 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -139,7 +139,7 @@ public:
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
 
-	int print_status();
+	int print_status(bool verbose = false);
 
 	bool should_exit() const { return _task_should_exit.load(); }
 
diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig
index e4e94c31d1..a189934415 100644
--- a/src/modules/ekf2/Kconfig
+++ b/src/modules/ekf2/Kconfig
@@ -4,6 +4,15 @@ menuconfig MODULES_EKF2
 	---help---
 		Enable support for ekf2
 
+menuconfig EKF2_VERBOSE_STATUS
+depends on MODULES_EKF2
+	bool "verbose status output"
+	default n
+	depends on !BOARD_CONSTRAINED_MEMORY
+	depends on !BOARD_CONSTRAINED_FLASH
+	---help---
+		ekf2 status verbose output.
+
 menuconfig EKF2_MULTI_INSTANCE
 depends on MODULES_EKF2
         bool "multi-EKF support"