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

stupid fix

This commit is contained in:
Lorenz Meier 2014-05-11 19:29:43 +02:00
parent 5581802f0f
commit eeba000b87
2 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf_att_pos_estimator/estimator.cpp
View File

@ -2004,15 +2004,15 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max)
float ret; float ret;
if (val > max) { if (val > max) {
ret = max; ret = max;
ekf_debug("> max: %8.4f, val: %8.4f", min); ekf_debug("> max: %8.4f, val: %8.4f", max, val);
} else if (val < min) { } else if (val < min) {
ret = min; ret = min;
ekf_debug("< min: %8.4f, val: %8.4f", max); ekf_debug("< min: %8.4f, val: %8.4f", min, val);
} else { } else {
ret = val; ret = val;
} }
if (!isfinite) { if (!isfinite(val)) {
ekf_debug("constrain: non-finite!"); ekf_debug("constrain: non-finite!");
} }
@ -2119,7 +2119,9 @@ void AttPosEKF::ConstrainStates()
} }
// Constrain delta velocity bias // Constrain delta velocity bias
ekf_debug("pre delta vel");
states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU);
ekf_debug("post delta vel");
// Wind velocity limits - assume 120 m/s max velocity // Wind velocity limits - assume 120 m/s max velocity
for (unsigned i = 14; i <= 15; i++) { for (unsigned i = 14; i <= 15; i++) {

2
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

@ -1379,7 +1379,7 @@ FixedwingEstimator::print_status()
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec); printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);