Merge changes Id5584bc1,I8ca826d0
* changes:
revert parts of dc5b63e40, which made gyro drift estimation unstable
use gyro timestamp directly in fusion
diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
index 93d6127..4f63c31 100644
--- a/services/sensorservice/Fusion.cpp
+++ b/services/sensorservice/Fusion.cpp
@@ -220,22 +220,6 @@
// initial covariance: Var{ x(t0) }
// TODO: initialize P correctly
P = 0;
-
- // it is unclear how to set the initial covariance. It does affect
- // how quickly the fusion converges. Experimentally it would take
- // about 10 seconds at 200 Hz to estimate the gyro-drift with an
- // initial covariance of 0, and about a second with an initial covariance
- // of about 1 deg/s.
- const float covv = 0;
- const float covu = 0.5f * (float(M_PI) / 180);
- mat33_t& Pv = P[0][0];
- Pv[0][0] = covv;
- Pv[1][1] = covv;
- Pv[2][2] = covv;
- mat33_t& Pu = P[1][1];
- Pu[0][0] = covu;
- Pu[1][1] = covu;
- Pu[2][2] = covu;
}
bool Fusion::hasEstimate() const {
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,
diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h
index 4c99bcb..3c2244e 100644
--- a/services/sensorservice/SensorFusion.h
+++ b/services/sensorservice/SensorFusion.h
@@ -44,7 +44,7 @@
Sensor mGyro;
Fusion mFusion;
bool mEnabled;
- float mGyroRate;
+ float mEstimatedGyroRate;
nsecs_t mTargetDelayNs;
nsecs_t mGyroTime;
vec4_t mAttitude;
@@ -60,7 +60,7 @@
mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); }
vec4_t getAttitude() const { return mAttitude; }
vec3_t getGyroBias() const { return mFusion.getBias(); }
- float getEstimatedRate() const { return mGyroRate; }
+ float getEstimatedRate() const { return mEstimatedGyroRate; }
status_t activate(void* ident, bool enabled);
status_t setDelay(void* ident, int64_t ns);