diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-24 20:48:18 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-24 20:48:18 +0200 |
commit | 7f41ec52f15191f7a0bbc0ec32301c3135184840 (patch) | |
tree | 019ec640259d53e7f122d23e085efdd4b4a838e6 /src/modules | |
parent | 32319722a60db2a43a2ac80a579eee09cb0a5dd0 (diff) | |
download | px4-firmware-7f41ec52f15191f7a0bbc0ec32301c3135184840.tar.gz px4-firmware-7f41ec52f15191f7a0bbc0ec32301c3135184840.tar.bz2 px4-firmware-7f41ec52f15191f7a0bbc0ec32301c3135184840.zip |
estimator: Introduce debug level to allow high-res bench debugging - set with ekf_att_pos_estimator debug <level, e.g. 100>
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 49 |
1 files changed, 47 insertions, 2 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 cb29b913c..5b0e7acf2 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 @@ -120,7 +120,7 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ int start(); @@ -136,9 +136,18 @@ public: /** * Enable logging. + * + * @param enable Set to true to enable logging, false to disable */ int enable_logging(bool enable); + /** + * Set debug level. + * + * @param debug Desired debug level - 0 to disable. + */ + int set_debuglevel(unsigned debug) { _debug = debug; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -210,6 +219,7 @@ private: bool _accel_valid; bool _mag_valid; bool _ekf_logging; ///< log EKF state + unsigned _debug; ///< debug level - default 0 int _mavlink_fd; @@ -377,6 +387,7 @@ FixedwingEstimator::FixedwingEstimator() : _accel_valid(false), _mag_valid(false), _ekf_logging(true), + _debug(0), _mavlink_fd(-1), _ekf(nullptr), _velocity_xy_filtered(0.0f), @@ -655,8 +666,26 @@ FixedwingEstimator::check_filter_state() rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); + if (_debug > 10) { + + if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { + warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", + ((rep.timeout_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.timeout_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.timeout_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.timeout_flags & (1 << 3)) ? "OK" : "ERR")); + } + + if (rep.timeout_flags) { + warnx("timeout: %s%s%s", + ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), + ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), + ((rep.timeout_flags & (1 << 2)) ? "HGT " : "")); + } + } + // Copy all states or at least all that we can fit - unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); + unsigned ekf_n_states = ekf_report.n_states; unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; @@ -664,6 +693,10 @@ FixedwingEstimator::check_filter_state() rep.states[i] = ekf_report.states[i]; } + for (unsigned i = 0; i < rep.n_states; i++) { + rep.states[i] = ekf_report.states[i]; + } + if (_estimator_status_pub > 0) { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); } else { @@ -1683,6 +1716,18 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } } + if (!strcmp(argv[1], "debug")) { + if (estimator::g_estimator) { + int debug = strtoul(argv[2], NULL, 10); + int ret = estimator::g_estimator->set_debuglevel(debug); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + warnx("unrecognized command"); return 1; } |