From 68b92cd4fc2c4df3de15ef19e723edb67a108ea0 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 13:21:13 -0500 Subject: Slowed HIL status updates. Also prevented posCor. when gps not init. --- apps/examples/kalman_demo/KalmanNav.cpp | 8 ++++++-- apps/mavlink/mavlink_receiver.c | 12 ++++++------ 2 files changed, 12 insertions(+), 8 deletions(-) (limited to 'apps') diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 918dfd9df..38cdb6ca0 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 fa63c419f..0b63c30a4 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; } -- cgit v1.2.3