Browse Source

Smaller hotfixes for att pos estimator

sbg
Lorenz Meier 11 years ago
parent
commit
3042731d26
  1. 9
      src/modules/att_pos_estimator_ekf/KalmanNav.cpp

9
src/modules/att_pos_estimator_ekf/KalmanNav.cpp

@ -34,7 +34,7 @@
/** /**
* @file KalmanNav.cpp * @file KalmanNav.cpp
* *
* kalman filter navigation code * Kalman filter navigation code
*/ */
#include <poll.h> #include <poll.h>
@ -228,10 +228,7 @@ void KalmanNav::update()
updateSubscriptions(); updateSubscriptions();
// initialize attitude when sensors online // initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate && if (!_attitudeInitialized && sensorsUpdate) {
_sensors.accelerometer_counter > 10 &&
_sensors.gyro_counter > 10 &&
_sensors.magnetometer_counter > 10) {
if (correctAtt() == ret_ok) _attitudeInitCounter++; if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) { if (_attitudeInitCounter > 100) {
@ -643,7 +640,7 @@ int KalmanNav::correctAtt()
if (beta > _faultAtt.get()) { if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta); warnx("fault in attitude: beta = %8.4f", (double)beta);
warnx("y:\n"); y.print(); warnx("y:"); y.print();
} }
// update quaternions from euler // update quaternions from euler

Loading…
Cancel
Save