// 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();
}
}
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);
"b=< %g, %g, %g >\n",
mEnabled ? "enabled" : "disabled",
mClients.size(),
- mGyroRate,
+ mEstimatedGyroRate,
fusion.getAttitude().x,
fusion.getAttitude().y,
fusion.getAttitude().z,
Sensor mGyro;
Fusion mFusion;
bool mEnabled;
- float mGyroRate;
+ float mEstimatedGyroRate;
nsecs_t mTargetDelayNs;
nsecs_t mGyroTime;
vec4_t mAttitude;
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);