dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Slowed HIL status updates. Also prevented posCor. when gps not init.

This commit is contained in:
James Goppert 2013-01-15 13:21:13 -05:00
parent 9cf3d51aec
commit 68b92cd4fc
2 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
apps/examples/kalman_demo/KalmanNav.cpp
View File

@ -239,9 +239,10 @@ void KalmanNav::update()
} }
// output // output
if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp; _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; _navFrames = 0;
_missFast = 0; _missFast = 0;
_missSlow = 0; _missSlow = 0;
@ -616,6 +617,9 @@ void KalmanNav::correctAtt()
void KalmanNav::correctPos() void KalmanNav::correctPos()
{ {
using namespace math; using namespace math;
if (!_positionInitialized) return;
Vector y(5); Vector y(5);
y(0) = _gps.vel_n - vN; y(0) = _gps.vel_n - vN;
y(1) = _gps.vel_e - vE; y(1) = _gps.vel_e - vE;

12
apps/mavlink/mavlink_receiver.c
View File

@ -367,8 +367,8 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ; hil_frames += 1 ;
// output // output
if ((timestamp - old_timestamp) > 1000000) { if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil imu at %d hz\n", hil_frames); printf("receiving hil imu at %d hz\n", hil_frames/10);
old_timestamp = timestamp; old_timestamp = timestamp;
hil_frames = 0; hil_frames = 0;
} }
@ -412,8 +412,8 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ; hil_frames += 1 ;
// output // output
if ((timestamp - old_timestamp) > 1000000) { if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil gps at %d hz\n", hil_frames); printf("receiving hil gps at %d hz\n", hil_frames/10);
old_timestamp = timestamp; old_timestamp = timestamp;
hil_frames = 0; hil_frames = 0;
} }
@ -454,8 +454,8 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ; hil_frames += 1 ;
// output // output
if ((timestamp - old_timestamp) > 1000000) { if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil pressure at %d hz\n", hil_frames); printf("receiving hil pressure at %d hz\n", hil_frames/10);
old_timestamp = timestamp; old_timestamp = timestamp;
hil_frames = 0; hil_frames = 0;
} }