Browse Source

Reducing pos/att correction update rates for debugging.

sbg
James Goppert 12 years ago
parent
commit
afb69cd054
  1. 8
      apps/examples/kalman_demo/KalmanNav.cpp
  2. 2
      apps/mavlink/orb_listener.c

8
apps/examples/kalman_demo/KalmanNav.cpp

@ -65,8 +65,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
C_nb(), C_nb(),
q(), q(),
// subscriptions // subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 Hz _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications // publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)), _pos(&getPublications(), ORB_ID(vehicle_global_position)),
@ -208,7 +208,6 @@ void KalmanNav::update()
predictFast(dtFast); predictFast(dtFast);
// count fast frames // count fast frames
_navFrames += 1; _navFrames += 1;
} else _missFast++; } else _missFast++;
// slow prediction step // slow prediction step
@ -216,7 +215,6 @@ void KalmanNav::update()
if (dtSlow > 1.0f / 100) { // 100 Hz if (dtSlow > 1.0f / 100) { // 100 Hz
_slowTimeStamp = _sensors.timestamp; _slowTimeStamp = _sensors.timestamp;
if (dtSlow < 1.0f / 50) predictSlow(dtSlow); if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
else _missSlow ++; else _missSlow ++;
} }
@ -227,7 +225,7 @@ void KalmanNav::update()
} }
// attitude correction step // attitude correction step
if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
_attTimeStamp = _sensors.timestamp; _attTimeStamp = _sensors.timestamp;
correctAtt(); correctAtt();
} }

2
apps/mavlink/orb_listener.c

@ -702,7 +702,7 @@ uorb_receive_start(void)
/* --- GLOBAL POS VALUE --- */ /* --- GLOBAL POS VALUE --- */
mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
orb_set_interval(mavlink_subs.global_pos_sub, 5); /* 200 Hz active updates */ orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */
/* --- LOCAL POS VALUE --- */ /* --- LOCAL POS VALUE --- */
mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));

Loading…
Cancel
Save