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

EKF: Fix output commands, fix stack size for commandline tool so we do not run out of space during debug printing

This commit is contained in:
Lorenz Meier 2015-10-14 14:08:03 +02:00
parent cff6cde47b
commit 21431cf237
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf_att_pos_estimator/CMakeLists.txt
View File

@ -39,6 +39,7 @@ endif()
px4_add_module(
MODULE modules__ekf_att_pos_estimator
MAIN ekf_att_pos_estimator
STACK 3000
COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS
ekf_att_pos_estimator_main.cpp

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

@ -1163,6 +1163,7 @@ void AttitudePositionEstimatorEKF::print_status()
PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
(double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
(double)_ekf->correctedDelAng.z);
PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1],
(double)_ekf->states[2], (double)_ekf->states[3]);
PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5],
@ -1342,21 +1343,21 @@ void AttitudePositionEstimatorEKF::pollData()
static hrt_abstime lastprint = 0;
if (hrt_elapsed_time(&lastprint) > 1000000) {
warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10);
warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
(double)(_sensor_combined.accelerometer_integral_m_s[0]),
(double)(_sensor_combined.accelerometer_integral_m_s[1]),
(double)(_sensor_combined.accelerometer_integral_m_s[2]));
warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
@ -1666,8 +1667,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "status")) {
PX4_INFO("running");
estimator::g_estimator->print_status();
return 0;
@ -1692,6 +1691,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
return ret;
}
PX4_ERR("unrecognized command");
PX4_WARN("unrecognized command");
return 1;
}