From e76037233cccbc36d3ac6747f6931d1392377e7a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Aug 2015 00:16:45 +0200 Subject: [PATCH] Attitude only EKF: Update to new sensor combined topic --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 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 a320ffe518..02940241c7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -383,10 +383,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_timestamp[0] != raw.timestamp) { + if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; + sensor_last_timestamp[0] = raw.gyro_timestamp[0]; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; @@ -394,7 +394,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp;