Add AOSP Geomag and Game Rotation, and Gravity
Providing AOSP software implementation of Geomag Rotation Vector, Game
Rotation Vector and Gravity sensors for platforms that does not have
hardware implementation of these sensors but do have primitive sensors
(accelerometers, gyrometers and magnetometers).
Previously, AOSP Gravity sensor is enabled only when all primitive sensors are
available. This is changed so that AOSP Gravity will be available even
no magnetometer is in the device.
Related bug/feature request:
* b/17508800
* b/22610016
Change-Id: I4e2d3e544884047d66e7fdbce2282f1f8234eae9
diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp
index 6d93009..9863f62 100644
--- a/services/sensorservice/SensorFusion.cpp
+++ b/services/sensorservice/SensorFusion.cpp
@@ -25,11 +25,17 @@
SensorFusion::SensorFusion()
: mSensorDevice(SensorDevice::getInstance()),
- mEnabled(false), mGyroTime(0)
+ mAttitude(mAttitudes[FUSION_9AXIS]),
+ mGyroTime(0), mAccTime(0)
{
sensor_t const* list;
Sensor uncalibratedGyro;
ssize_t count = mSensorDevice.getSensorList(&list);
+
+ mEnabled[FUSION_9AXIS] = false;
+ mEnabled[FUSION_NOMAG] = false;
+ mEnabled[FUSION_NOGYRO] = false;
+
if (count > 0) {
for (size_t i=0 ; i<size_t(count) ; i++) {
if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
@@ -55,81 +61,121 @@
// and power/cpu usage.
mEstimatedGyroRate = 200;
mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;
- mFusion.init();
+
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ mFusions[i].init(i);
+ }
}
}
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);
+ float dT;
+ if ( event.timestamp - mGyroTime> 0 &&
+ event.timestamp - mGyroTime< (int64_t)(5e7) ) { //0.05sec
+
+ dT = (event.timestamp - mGyroTime) / 1000000000.0f;
// 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
mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;
}
+
+ const vec3_t gyro(event.data);
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ if (mEnabled[i]) {
+ // fusion in no gyro mode will ignore
+ mFusions[i].handleGyro(gyro, dT);
+ }
+ }
}
mGyroTime = event.timestamp;
} else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
const vec3_t mag(event.data);
- mFusion.handleMag(mag);
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ if (mEnabled[i]) {
+ mFusions[i].handleMag(mag);// fusion in no mag mode will ignore
+ }
+ }
} else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
- const vec3_t acc(event.data);
- mFusion.handleAcc(acc);
- mAttitude = mFusion.getAttitude();
+ float dT;
+ if ( event.timestamp - mAccTime> 0 &&
+ event.timestamp - mAccTime< (int64_t)(1e8) ) { //0.1sec
+ dT = (event.timestamp - mAccTime) / 1000000000.0f;
+
+ const vec3_t acc(event.data);
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ if (mEnabled[i]) {
+ mFusions[i].handleAcc(acc, dT);
+ mAttitudes[i] = mFusions[i].getAttitude();
+ }
+ }
+ }
+ mAccTime = event.timestamp;
}
}
template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
-status_t SensorFusion::activate(void* ident, bool enabled) {
+status_t SensorFusion::activate(int mode, void* ident, bool enabled) {
ALOGD_IF(DEBUG_CONNECTIONS,
- "SensorFusion::activate(ident=%p, enabled=%d)",
- ident, enabled);
+ "SensorFusion::activate(mode=%d, ident=%p, enabled=%d)",
+ mode, ident, enabled);
- const ssize_t idx = mClients.indexOf(ident);
+ const ssize_t idx = mClients[mode].indexOf(ident);
if (enabled) {
if (idx < 0) {
- mClients.add(ident);
+ mClients[mode].add(ident);
}
} else {
if (idx >= 0) {
- mClients.removeItemsAt(idx);
+ mClients[mode].removeItemsAt(idx);
+ }
+ }
+
+ const bool newState = mClients[mode].size() != 0;
+ if (newState != mEnabled[mode]) {
+ mEnabled[mode] = newState;
+ if (newState) {
+ mFusions[mode].init(mode);
}
}
mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
- mSensorDevice.activate(ident, mMag.getHandle(), enabled);
- mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
+ if (mode != FUSION_NOMAG) {
+ mSensorDevice.activate(ident, mMag.getHandle(), enabled);
+ }
+ if (mode != FUSION_NOGYRO) {
+ mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
+ }
- const bool newState = mClients.size() != 0;
- if (newState != mEnabled) {
- mEnabled = newState;
- if (newState) {
- mFusion.init();
- mGyroTime = 0;
- }
+ return NO_ERROR;
+}
+
+status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) {
+ // Call batch with timeout zero instead of setDelay().
+ if (ns > (int64_t)5e7) {
+ ns = (int64_t)(5e7);
+ }
+ mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
+ if (mode != FUSION_NOMAG) {
+ mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(20), 0);
+ }
+ if (mode != FUSION_NOGYRO) {
+ mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
}
return NO_ERROR;
}
-status_t SensorFusion::setDelay(void* ident, int64_t ns) {
- // Call batch with timeout zero instead of setDelay().
- mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
- mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(20), 0);
- mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
- return NO_ERROR;
-}
-
-float SensorFusion::getPowerUsage() const {
+float SensorFusion::getPowerUsage(int mode) const {
float power = mAcc.getPowerUsage() +
- mMag.getPowerUsage() +
- mGyro.getPowerUsage();
+ ((mode != FUSION_NOMAG) ? mMag.getPowerUsage() : 0) +
+ ((mode != FUSION_NOGYRO) ? mGyro.getPowerUsage() : 0);
return power;
}
@@ -138,21 +184,55 @@
}
void SensorFusion::dump(String8& result) {
- const Fusion& fusion(mFusion);
+ const Fusion& fusion_9axis(mFusions[FUSION_9AXIS]);
result.appendFormat("9-axis fusion %s (%zd clients), gyro-rate=%7.2fHz, "
"q=< %g, %g, %g, %g > (%g), "
"b=< %g, %g, %g >\n",
- mEnabled ? "enabled" : "disabled",
- mClients.size(),
+ mEnabled[FUSION_9AXIS] ? "enabled" : "disabled",
+ mClients[FUSION_9AXIS].size(),
mEstimatedGyroRate,
- fusion.getAttitude().x,
- fusion.getAttitude().y,
- fusion.getAttitude().z,
- fusion.getAttitude().w,
- length(fusion.getAttitude()),
- fusion.getBias().x,
- fusion.getBias().y,
- fusion.getBias().z);
+ fusion_9axis.getAttitude().x,
+ fusion_9axis.getAttitude().y,
+ fusion_9axis.getAttitude().z,
+ fusion_9axis.getAttitude().w,
+ length(fusion_9axis.getAttitude()),
+ fusion_9axis.getBias().x,
+ fusion_9axis.getBias().y,
+ fusion_9axis.getBias().z);
+
+ const Fusion& fusion_nomag(mFusions[FUSION_NOMAG]);
+ result.appendFormat("game fusion(no mag) %s (%zd clients), "
+ "gyro-rate=%7.2fHz, "
+ "q=< %g, %g, %g, %g > (%g), "
+ "b=< %g, %g, %g >\n",
+ mEnabled[FUSION_NOMAG] ? "enabled" : "disabled",
+ mClients[FUSION_NOMAG].size(),
+ mEstimatedGyroRate,
+ fusion_nomag.getAttitude().x,
+ fusion_nomag.getAttitude().y,
+ fusion_nomag.getAttitude().z,
+ fusion_nomag.getAttitude().w,
+ length(fusion_nomag.getAttitude()),
+ fusion_nomag.getBias().x,
+ fusion_nomag.getBias().y,
+ fusion_nomag.getBias().z);
+
+ const Fusion& fusion_nogyro(mFusions[FUSION_NOGYRO]);
+ result.appendFormat("geomag fusion (no gyro) %s (%zd clients), "
+ "gyro-rate=%7.2fHz, "
+ "q=< %g, %g, %g, %g > (%g), "
+ "b=< %g, %g, %g >\n",
+ mEnabled[FUSION_NOGYRO] ? "enabled" : "disabled",
+ mClients[FUSION_NOGYRO].size(),
+ mEstimatedGyroRate,
+ fusion_nogyro.getAttitude().x,
+ fusion_nogyro.getAttitude().y,
+ fusion_nogyro.getAttitude().z,
+ fusion_nogyro.getAttitude().w,
+ length(fusion_nogyro.getAttitude()),
+ fusion_nogyro.getBias().x,
+ fusion_nogyro.getBias().y,
+ fusion_nogyro.getBias().z);
}
// ---------------------------------------------------------------------------