Copy estimator status updates to system status logging

1 files changed, 71 insertions, 27 deletions

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 0a5b26f4a..1e2582952 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -135,6 +135,11 @@ public:
 	 */
 	int		trip_nan();
 
+	/**
+	 * Enable logging.
+	 */
+	int		enable_logging(bool enable);
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -202,6 +207,7 @@ private:
 	bool						_gyro_valid;
 	bool						_accel_valid;
 	bool						_mag_valid;
+	bool						_ekf_logging;		///< log EKF state
 
 	int						_mavlink_fd;
 
@@ -356,6 +362,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_gyro_valid(false),
 	_accel_valid(false),
 	_mag_valid(false),
+	_ekf_logging(true),
 	_mavlink_fd(-1),
 	_ekf(nullptr)
 {
@@ -448,6 +455,14 @@ FixedwingEstimator::~FixedwingEstimator()
 }
 
 int
+FixedwingEstimator::enable_logging(bool logging)
+{
+	_ekf_logging = logging;
+
+	return 0;
+}
+
+int
 FixedwingEstimator::parameters_update()
 {
 
@@ -992,20 +1007,57 @@ FixedwingEstimator::task_main()
 				warnx("NUMERIC ERROR IN FILTER");
 			}
 
+			struct estimator_status_report rep;
+			memset(&rep, 0, sizeof(rep));
+
+			struct ekf_status_report ekf_report;
+
 			// If non-zero, we got a filter reset
 			if (check > 0 && check != 3) {
 
-				struct ekf_status_report ekf_report;
-
 				_ekf->GetLastErrorState(&ekf_report);
 
-				struct estimator_status_report rep;
-				memset(&rep, 0, sizeof(rep));
+				/* set sensors to de-initialized state */
+				_gyro_valid = false;
+				_accel_valid = false;
+				_mag_valid = false;
+
+				_baro_init = false;
+				_gps_initialized = false;
+				_initialized = false;
+				last_sensor_timestamp = hrt_absolute_time();
+				last_run = last_sensor_timestamp;
+
+				_ekf->ZeroVariables();
+				_ekf->statesInitialised = false;
+				_ekf->dtIMU = 0.01f;
+
+				// Let the system re-initialize itself
+				continue;
+
+			} else if (_ekf_logging) {
+				_ekf->GetFilterState(&ekf_report);
+			}
+
+			if (_ekf_logging || check) {
 				rep.timestamp = hrt_absolute_time();
 
-				rep.states_nan = ekf_report.statesNaN;
-				rep.covariance_nan = ekf_report.covarianceNaN;
-				rep.kalman_gain_nan = ekf_report.kalmanGainsNaN;
+				rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0);
+				rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1);
+				rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2);
+				rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3);
+				rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4);
+				rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5);
+				rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6);
+				rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7);
+
+				rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0);
+				rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
+				rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
+
+				rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
+				rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
+				rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2);
 
 				// Copy all states or at least all that we can fit
 				unsigned i = 0;
@@ -1024,25 +1076,6 @@ FixedwingEstimator::task_main()
 				} else {
 					_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
 				}
-
-				/* set sensors to de-initialized state */
-				_gyro_valid = false;
-				_accel_valid = false;
-				_mag_valid = false;
-
-				_baro_init = false;
-				_gps_initialized = false;
-				_initialized = false;
-				last_sensor_timestamp = hrt_absolute_time();
-				last_run = last_sensor_timestamp;
-
-				_ekf->ZeroVariables();
-				_ekf->statesInitialised = false;
-				_ekf->dtIMU = 0.01f;
-
-				// Let the system re-initialize itself
-				continue;
-
 			}
 
 
@@ -1503,7 +1536,7 @@ int FixedwingEstimator::trip_nan() {
 int ekf_att_pos_estimator_main(int argc, char *argv[])
 {
 	if (argc < 1)
-		errx(1, "usage: ekf_att_pos_estimator {start|stop|status}");
+		errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}");
 
 	if (!strcmp(argv[1], "start")) {
 
@@ -1557,6 +1590,17 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 		}
 	}
 
+	if (!strcmp(argv[1], "logging")) {
+		if (estimator::g_estimator) {
+			int ret = estimator::g_estimator->enable_logging(true);
+
+			exit(ret);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
 	warnx("unrecognized command");
 	return 1;
 }