forked from Archive/PX4-Autopilot
Slowed HIL status updates. Also prevented posCor. when gps not init.
This commit is contained in:
parent
9cf3d51aec
commit
68b92cd4fc
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue