fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 04:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+6 -2
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;
apps/mavlink/mavlink_receiver.c
+6 -6
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;
} }