ADHOC/replicant-frameworks_native
3
0
Fork 0
Code Issues 6 Pull Requests Releases Wiki Activity

use gyro timestamp directly in fusion

Browse Source 
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
This commit is contained in:
Mathias Agopian 2013-05-30 14:18:23 -07:00
parent 9e3cb55b8f
commit 2e2a560c4b
2 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
services/sensorservice/SensorFusion.cpp
View File

@ -53,8 +53,8 @@ SensorFusion::SensorFusion()
// 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 @@ void SensorFusion::process(const sensors_event_t& event) {
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 @@ void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) {
"b=< %g, %g, %g >\n",
mEnabled ? "enabled" : "disabled",
mClients.size(),
mGyroRate,
mEstimatedGyroRate,
fusion.getAttitude().x,
fusion.getAttitude().y,
fusion.getAttitude().z,

4
services/sensorservice/SensorFusion.h
View File

@ -44,7 +44,7 @@ class SensorFusion : public Singleton<SensorFusion> {
Sensor mGyro;
Fusion mFusion;
bool mEnabled;
float mGyroRate;
float mEstimatedGyroRate;
nsecs_t mTargetDelayNs;
nsecs_t mGyroTime;
vec4_t mAttitude;
@ -60,7 +60,7 @@ public:
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);