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

Removed bogus sensor counters, replaced them with proper timestamps

Browse Source
This commit is contained in:
Lorenz Meier
2014-01-12 12:57:53 +01:00
parent 0d3a743f75
commit e421260f7c
8 changed files with 63 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+5 -8
View File
@@ -526,9 +526,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) {
if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
@@ -538,11 +537,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
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];
@@ -550,11 +548,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
acc[2] = raw.accelerometer_m_s2[2];
/* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) {
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
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];