diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 918dfd9df8..38cdb6ca01 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -239,9 +239,10 @@ void KalmanNav::update() } // output - if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz + if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow); + printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", + _navFrames/10, _missFast/10, _missSlow/10); _navFrames = 0; _missFast = 0; _missSlow = 0; @@ -616,6 +617,9 @@ void KalmanNav::correctAtt() void KalmanNav::correctPos() { using namespace math; + + if (!_positionInitialized) return; + Vector y(5); y(0) = _gps.vel_n - vN; y(1) = _gps.vel_e - vE; diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index fa63c419f0..0b63c30a4a 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -367,8 +367,8 @@ handle_message(mavlink_message_t *msg) hil_frames += 1 ; // output - if ((timestamp - old_timestamp) > 1000000) { - printf("receiving hil imu at %d hz\n", hil_frames); + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil imu at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } @@ -412,8 +412,8 @@ handle_message(mavlink_message_t *msg) hil_frames += 1 ; // output - if ((timestamp - old_timestamp) > 1000000) { - printf("receiving hil gps at %d hz\n", hil_frames); + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil gps at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } @@ -454,8 +454,8 @@ handle_message(mavlink_message_t *msg) hil_frames += 1 ; // output - if ((timestamp - old_timestamp) > 1000000) { - printf("receiving hil pressure at %d hz\n", hil_frames); + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil pressure at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; }