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

Fix usage of right time stamps

Browse Source
This commit is contained in:
Lorenz Meier
2014-03-17 13:25:22 +01:00
parent 2db7d30400
commit ffd0d10320
2 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+2 -2
View File
@@ -393,7 +393,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1; update_vect[1] = 1;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp; sensor_last_timestamp[1] = raw.accelerometer_timestamp;
} }
hrt_abstime vel_t = 0; hrt_abstime vel_t = 0;
@@ -445,7 +445,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1; update_vect[2] = 1;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp; sensor_last_timestamp[2] = raw.magnetometer_timestamp;
} }
z_k[6] = raw.magnetometer_ga[0]; z_k[6] = raw.magnetometer_ga[0];
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+2 -2
View File
@@ -539,7 +539,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1; update_vect[1] = 1;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp; sensor_last_timestamp[1] = raw.accelerometer_timestamp;
} }
acc[0] = raw.accelerometer_m_s2[0]; acc[0] = raw.accelerometer_m_s2[0];
@@ -550,7 +550,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1; update_vect[2] = 1;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp; sensor_last_timestamp[2] = raw.magnetometer_timestamp;
} }
mag[0] = raw.magnetometer_ga[0]; mag[0] = raw.magnetometer_ga[0];