diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-04 19:09:10 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-04 19:22:32 +0200 |
commit | 8b4413f3b1309deff41ff98c457d2e9abe7817d1 (patch) | |
tree | dc418a4dc4388ab9866b797755c9ab1414f7750e /src | |
parent | c2ee4437e0fd362ed8d73203394d34802e9eb48d (diff) | |
download | px4-firmware-8b4413f3b1309deff41ff98c457d2e9abe7817d1.tar.gz px4-firmware-8b4413f3b1309deff41ff98c457d2e9abe7817d1.tar.bz2 px4-firmware-8b4413f3b1309deff41ff98c457d2e9abe7817d1.zip |
Hotfix for attitude estimator EKF init
Diffstat (limited to 'src')
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 10 |
1 files changed, 5 insertions, 5 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 1eff60e88..9e533ccdf 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -224,8 +224,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); + /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ + orb_set_interval(sub_raw, 3); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -236,7 +236,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - int loopcounter = 0; int printcounter = 0; @@ -384,7 +383,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { + if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); @@ -424,7 +423,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp - last_data > 12000) + printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; |