ekf2: add verbose print status (moved out of DEBUG_BUILD)

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

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)
+	if (verbose) {
-	_ekf.print_status();
+#if defined(CONFIG_EKF2_VERBOSE_STATUS)
-#endif // DEBUG_BUILD
+		_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);
 				}
 			}
 

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(); }
 

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"