dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

estimator: Introduce debug level to allow high-res bench debugging - set with ekf_att_pos_estimator debug <level, e.g. 100>

This commit is contained in:
Lorenz Meier 2014-06-24 20:48:18 +02:00
parent 32319722a6
commit 7f41ec52f1
1 changed files with 47 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -120,7 +120,7 @@ public:
/** /**
* Start the sensors task. * Start the sensors task.
* *
* @return OK on success. * @return OK on success.
*/ */
int start(); int start();
@ -136,9 +136,18 @@ public:
/** /**
* Enable logging. * Enable logging.
*
* @param enable Set to true to enable logging, false to disable
*/ */
int enable_logging(bool enable); int enable_logging(bool enable);
/**
* Set debug level.
*
* @param debug Desired debug level - 0 to disable.
*/
int set_debuglevel(unsigned debug) { _debug = debug; }
private: private:
bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_should_exit; /**< if true, sensor task should exit */
@ -210,6 +219,7 @@ private:
bool _accel_valid; bool _accel_valid;
bool _mag_valid; bool _mag_valid;
bool _ekf_logging; ///< log EKF state bool _ekf_logging; ///< log EKF state
unsigned _debug; ///< debug level - default 0
int _mavlink_fd; int _mavlink_fd;
@ -377,6 +387,7 @@ FixedwingEstimator::FixedwingEstimator() :
_accel_valid(false), _accel_valid(false),
_mag_valid(false), _mag_valid(false),
_ekf_logging(true), _ekf_logging(true),
_debug(0),
_mavlink_fd(-1), _mavlink_fd(-1),
_ekf(nullptr), _ekf(nullptr),
_velocity_xy_filtered(0.0f), _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.posTimeout) << 1);
rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); 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 // 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])); unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; 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]; 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) { if (_estimator_status_pub > 0) {
orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
} else { } 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"); warnx("unrecognized command");
return 1; return 1;
} }