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;
|
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++) {
|
||||||
|
|
|
@ -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]);
|
||||||
|
|
Loading…
Reference in New Issue