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

Hotfix for attitude estimator EKF init

This commit is contained in:
Lorenz Meier 2013-08-04 19:09:10 +02:00
parent 234ad24081
commit 9bcabc5ba9
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -224,8 +224,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to raw data */ /* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */ /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
orb_set_interval(sub_raw, 4); orb_set_interval(sub_raw, 3);
/* subscribe to param changes */ /* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update)); 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 */ /* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0; int loopcounter = 0;
int printcounter = 0; int printcounter = 0;
@ -384,7 +383,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
static bool const_initialized = false; static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */ /* 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; dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params); parameters_update(&ekf_param_handles, &ekf_params);
@ -424,7 +423,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
continue; 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; last_data = raw.timestamp;