forked from Archive/PX4-Autopilot
stupid fix
This commit is contained in:
parent
5581802f0f
commit
eeba000b87
|
@ -2004,15 +2004,15 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max)
|
|||
float ret;
|
||||
if (val > 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) {
|
||||
ret = min;
|
||||
ekf_debug("< min: %8.4f, val: %8.4f", max);
|
||||
ekf_debug("< min: %8.4f, val: %8.4f", min, val);
|
||||
} else {
|
||||
ret = val;
|
||||
}
|
||||
|
||||
if (!isfinite) {
|
||||
if (!isfinite(val)) {
|
||||
ekf_debug("constrain: non-finite!");
|
||||
}
|
||||
|
||||
|
@ -2119,7 +2119,9 @@ void AttPosEKF::ConstrainStates()
|
|||
}
|
||||
|
||||
// Constrain delta velocity bias
|
||||
ekf_debug("pre delta vel");
|
||||
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
|
||||
for (unsigned i = 14; i <= 15; i++) {
|
||||
|
|
|
@ -1379,7 +1379,7 @@ FixedwingEstimator::print_status()
|
|||
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
|
||||
// 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("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]);
|
||||
|
|
Loading…
Reference in New Issue