use gyro timestamp directly in fusion
we used to estimate the gyro rate and deduce the period from that
but it turns out this is causing problems.
Bug: 5192288
Change-Id: I8ca826d0e11e488587bcaa1720de99e92b82f191
diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp
index 4837b97..a0a17da 100644
--- a/services/sensorservice/SensorFusion.cpp
+++ b/services/sensorservice/SensorFusion.cpp
@@ -53,8 +53,8 @@
// 200 Hz for gyro events is a good compromise between precision
// and power/cpu usage.
- mGyroRate = 200;
- mTargetDelayNs = 1000000000LL/mGyroRate;
+ mEstimatedGyroRate = 200;
+ mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;
mFusion.init();
}
}
@@ -63,14 +63,15 @@
if (event.type == mGyro.getType()) {
if (mGyroTime != 0) {
const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;
+ mFusion.handleGyro(vec3_t(event.data), dT);
+ // here we estimate the gyro rate (useful for debugging)
const float freq = 1 / dT;
if (freq >= 100 && freq<1000) { // filter values obviously wrong
const float alpha = 1 / (1 + dT); // 1s time-constant
- mGyroRate = freq + (mGyroRate - freq)*alpha;
+ mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;
}
}
mGyroTime = event.timestamp;
- mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate);
} else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
const vec3_t mag(event.data);
mFusion.handleMag(mag);
@@ -142,7 +143,7 @@
"b=< %g, %g, %g >\n",
mEnabled ? "enabled" : "disabled",
mClients.size(),
- mGyroRate,
+ mEstimatedGyroRate,
fusion.getAttitude().x,
fusion.getAttitude().y,
fusion.getAttitude().z,