diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-04 09:52:34 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-04 09:52:34 +0200 |
commit | 19154f29d822afdc0a33bf3be55fab63b32f23c5 (patch) | |
tree | cc7a17c4ed983c5bae42b1f118cc8d0be78aed60 /src/modules | |
parent | cf67e810a4e21b97a12862fd55572b5d025156b5 (diff) | |
download | px4-firmware-19154f29d822afdc0a33bf3be55fab63b32f23c5.tar.gz px4-firmware-19154f29d822afdc0a33bf3be55fab63b32f23c5.tar.bz2 px4-firmware-19154f29d822afdc0a33bf3be55fab63b32f23c5.zip |
Copy estimator status updates to system status logging
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 98 |
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; } |