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
TEST=Pass CTS sensor tests with fix ag/207083
Change-Id: I4e2d3e544884047d66e7fdbce2282f1f8234eae9
(cherry picked from commit f66684a6fb2a2991e84a085673629db2a0494fc6)
diff --git a/services/sensorservice/CorrectedGyroSensor.cpp b/services/sensorservice/CorrectedGyroSensor.cpp
index b07d544..7b1f346 100644
--- a/services/sensorservice/CorrectedGyroSensor.cpp
+++ b/services/sensorservice/CorrectedGyroSensor.cpp
@@ -58,12 +58,12 @@
status_t CorrectedGyroSensor::activate(void* ident, bool enabled) {
mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
}
status_t CorrectedGyroSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
mSensorDevice.setDelay(ident, mGyro.getHandle(), ns);
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
}
Sensor CorrectedGyroSensor::getSensor() const {
diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
index 4f63c31..359d289 100644
--- a/services/sensorservice/Fusion.cpp
+++ b/services/sensorservice/Fusion.cpp
@@ -24,28 +24,44 @@
// -----------------------------------------------------------------------
+/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/
+
+/* Note:
+ * If a platform uses software fusion, it is necessary to tune the following
+ * parameters to fit the hardware sensors prior to release.
+ *
+ * The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode.
+ * The GEOMAG_ parameters will be used in FUSION_NOGYRO mode.
+ */
+
/*
- * gyroVAR gives the measured variance of the gyro's output per
+ * GYRO_VAR gives the measured variance of the gyro's output per
* Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,
* which is independent of the sampling frequency.
*
* The variance of gyro's output at a given sampling period can be
* calculated as:
- * variance(T) = gyroVAR / T
+ * variance(T) = GYRO_VAR / T
*
* The variance of the INTEGRATED OUTPUT at a given sampling period can be
* calculated as:
- * variance_integrate_output(T) = gyroVAR * T
- *
+ * variance_integrate_output(T) = GYRO_VAR * T
*/
-static const float gyroVAR = 1e-7; // (rad/s)^2 / Hz
-static const float biasVAR = 1e-8; // (rad/s)^2 / s (guessed)
+static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz
+static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed)
+static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz
+static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed)
/*
* Standard deviations of accelerometer and magnetometer
*/
-static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
-static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5)
+static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)
+static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
+static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
+static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
+
+
+/* ====================== END FUSION SENSOR PARAMETER ========================*/
static const float SYMMETRY_TOLERANCE = 1e-10f;
@@ -54,7 +70,8 @@
* ill-conditioning and div by zeros.
* Threshhold: 10% of g, in m/s^2
*/
-static const float FREE_FALL_THRESHOLD = 0.981f;
+static const float NOMINAL_GRAVITY = 9.81f;
+static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY);
static const float FREE_FALL_THRESHOLD_SQ =
FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD;
@@ -87,6 +104,9 @@
static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;
+static const float W_EPS = 1e-4f;
+static const float SQRT_3 = 1.732f;
+static const float WVEC_EPS = 1e-4f/SQRT_3;
// -----------------------------------------------------------------------
template <typename TYPE, size_t C, size_t R>
@@ -173,7 +193,7 @@
init();
}
-void Fusion::init() {
+void Fusion::init(int mode) {
mInitState = 0;
mGyroRate = 0;
@@ -183,6 +203,19 @@
mCount[2] = 0;
mData = 0;
+ mMode = mode;
+
+ if (mMode != FUSION_NOGYRO) { //normal or game rotation
+ mParam.gyroVar = DEFAULT_GYRO_VAR;
+ mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR;
+ mParam.accStdev = DEFAULT_ACC_STDEV;
+ mParam.magStdev = DEFAULT_MAG_STDEV;
+ } else {
+ mParam.gyroVar = GEOMAG_GYRO_VAR;
+ mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR;
+ mParam.accStdev = GEOMAG_ACC_STDEV;
+ mParam.magStdev = GEOMAG_MAG_STDEV;
+ }
}
void Fusion::initFusion(const vec4_t& q, float dT)
@@ -205,11 +238,11 @@
const float dT3 = dT2*dT;
// variance of integrated output at 1/dT Hz (random drift)
- const float q00 = gyroVAR * dT + 0.33333f * biasVAR * dT3;
+ const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;
// variance of drift rate ramp
- const float q11 = biasVAR * dT;
- const float q10 = 0.5f * biasVAR * dT2;
+ const float q11 = mParam.gyroBiasVar * dT;
+ const float q10 = 0.5f * mParam.gyroBiasVar * dT2;
const float q01 = q10;
GQGt[0][0] = q00; // rad^2
@@ -223,7 +256,9 @@
}
bool Fusion::hasEstimate() const {
- return (mInitState == (MAG|ACC|GYRO));
+ return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) &&
+ ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) &&
+ (mInitState & ACC);
}
bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
@@ -234,6 +269,9 @@
mData[0] += d * (1/length(d));
mCount[0]++;
mInitState |= ACC;
+ if (mMode == FUSION_NOGYRO ) {
+ mGyroRate = dT;
+ }
} else if (what == MAG) {
mData[1] += d * (1/length(d));
mCount[1]++;
@@ -242,25 +280,29 @@
mGyroRate = dT;
mData[2] += d*dT;
mCount[2]++;
- if (mCount[2] == 64) {
- // 64 samples is good enough to estimate the gyro drift and
- // doesn't take too much time.
- mInitState |= GYRO;
- }
+ mInitState |= GYRO;
}
- if (mInitState == (MAG|ACC|GYRO)) {
+ if (hasEstimate()) {
// Average all the values we collected so far
mData[0] *= 1.0f/mCount[0];
- mData[1] *= 1.0f/mCount[1];
+ if (mMode != FUSION_NOMAG) {
+ mData[1] *= 1.0f/mCount[1];
+ }
mData[2] *= 1.0f/mCount[2];
// calculate the MRPs from the data collection, this gives us
// a rough estimate of our initial state
mat33_t R;
- vec3_t up(mData[0]);
- vec3_t east(cross_product(mData[1], up));
- east *= 1/length(east);
+ vec3_t up(mData[0]);
+ vec3_t east;
+
+ if (mMode != FUSION_NOMAG) {
+ east = normalize(cross_product(mData[1], up));
+ } else {
+ east = getOrthogonal(up);
+ }
+
vec3_t north(cross_product(up, east));
R << east << north << up;
const vec4_t q = matrixToQuat(R);
@@ -278,21 +320,43 @@
predict(w, dT);
}
-status_t Fusion::handleAcc(const vec3_t& a) {
+status_t Fusion::handleAcc(const vec3_t& a, float dT) {
+ if (!checkInitComplete(ACC, a, dT))
+ return BAD_VALUE;
+
// ignore acceleration data if we're close to free-fall
- if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) {
+ const float l = length(a);
+ if (l < FREE_FALL_THRESHOLD) {
return BAD_VALUE;
}
- if (!checkInitComplete(ACC, a))
- return BAD_VALUE;
+ const float l_inv = 1.0f/l;
- const float l = 1/length(a);
- update(a*l, Ba, accSTDEV*l);
+ if ( mMode == FUSION_NOGYRO ) {
+ //geo mag
+ vec3_t w_dummy;
+ w_dummy = x1; //bias
+ predict(w_dummy, dT);
+ }
+
+ if ( mMode == FUSION_NOMAG) {
+ vec3_t m;
+ m = getRotationMatrix()*Bm;
+ update(m, Bm, mParam.magStdev);
+ }
+
+ vec3_t unityA = a * l_inv;
+ const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY));
+ const float p = l_inv * mParam.accStdev*expf(d);
+
+ update(unityA, Ba, p);
return NO_ERROR;
}
status_t Fusion::handleMag(const vec3_t& m) {
+ if (!checkInitComplete(MAG, m))
+ return BAD_VALUE;
+
// the geomagnetic-field should be between 30uT and 60uT
// reject if too large to avoid spurious magnetic sources
const float magFieldSq = length_squared(m);
@@ -304,9 +368,6 @@
return BAD_VALUE;
}
- if (!checkInitComplete(MAG, m))
- return BAD_VALUE;
-
// Orthogonalize the magnetic field to the gravity field, mapping it into
// tangent to Earth.
const vec3_t up( getRotationMatrix() * Ba );
@@ -324,10 +385,10 @@
// then pass it in as the update.
vec3_t north( cross_product(up, east) );
- const float l = 1 / length(north);
- north *= l;
+ const float l_inv = 1 / length(north);
+ north *= l_inv;
- update(north, Bm, magSTDEV*l);
+ update(north, Bm, mParam.magStdev*l_inv);
return NO_ERROR;
}
@@ -372,8 +433,11 @@
void Fusion::predict(const vec3_t& w, float dT) {
const vec4_t q = x0;
const vec3_t b = x1;
- const vec3_t we = w - b;
+ vec3_t we = w - b;
+ if (length(we) < WVEC_EPS) {
+ we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS;
+ }
// q(k+1) = O(we)*q(k)
// --------------------
//
@@ -406,7 +470,7 @@
const mat33_t wx2(wx*wx);
const float lwedT = length(we)*dT;
const float hlwedT = 0.5f*lwedT;
- const float ilwe = 1/length(we);
+ const float ilwe = 1.f/length(we);
const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
const float k1 = sinf(lwedT);
const float k2 = cosf(hlwedT);
@@ -422,6 +486,7 @@
Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
x0 = O*q;
+
if (x0.w < 0)
x0 = -x0;
@@ -466,15 +531,37 @@
const vec3_t e(z - Bb);
const vec3_t dq(K[0]*e);
- const vec3_t db(K[1]*e);
q += getF(q)*(0.5f*dq);
x0 = normalize_quat(q);
- x1 += db;
+
+ if (mMode != FUSION_NOMAG) {
+ const vec3_t db(K[1]*e);
+ x1 += db;
+ }
checkState();
}
+vec3_t Fusion::getOrthogonal(const vec3_t &v) {
+ vec3_t w;
+ if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) {
+ w[0]=0.f;
+ w[1] = v[2];
+ w[2] = -v[1];
+ } else if (fabsf(v[1]) <= fabsf(v[2])) {
+ w[0] = v[2];
+ w[1] = 0.f;
+ w[2] = -v[0];
+ }else {
+ w[0] = v[1];
+ w[1] = -v[0];
+ w[2] = 0.f;
+ }
+ return normalize(w);
+}
+
+
// -----------------------------------------------------------------------
}; // namespace android
diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h
index 7062999..602779f 100644
--- a/services/sensorservice/Fusion.h
+++ b/services/sensorservice/Fusion.h
@@ -27,6 +27,13 @@
typedef mat<float, 3, 4> mat34_t;
+enum FUSION_MODE{
+ FUSION_9AXIS, // use accel gyro mag
+ FUSION_NOMAG, // use accel gyro (game rotation, gravity)
+ FUSION_NOGYRO, // use accel mag (geomag rotation)
+ NUM_FUSION_MODE
+};
+
class Fusion {
/*
* the state vector is made of two sub-vector containing respectively:
@@ -55,9 +62,9 @@
public:
Fusion();
- void init();
+ void init(int mode = FUSION_9AXIS);
void handleGyro(const vec3_t& w, float dT);
- status_t handleAcc(const vec3_t& a);
+ status_t handleAcc(const vec3_t& a, float dT);
status_t handleMag(const vec3_t& m);
vec4_t getAttitude() const;
vec3_t getBias() const;
@@ -65,12 +72,21 @@
bool hasEstimate() const;
private:
+ struct Parameter {
+ float gyroVar;
+ float gyroBiasVar;
+ float accStdev;
+ float magStdev;
+ } mParam;
+
mat<mat33_t, 2, 2> Phi;
vec3_t Ba, Bm;
uint32_t mInitState;
float mGyroRate;
vec<vec3_t, 3> mData;
size_t mCount[3];
+ int mMode;
+
enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
bool checkInitComplete(int, const vec3_t& w, float d = 0);
void initFusion(const vec4_t& q0, float dT);
@@ -78,6 +94,7 @@
void predict(const vec3_t& w, float dT);
void update(const vec3_t& z, const vec3_t& Bi, float sigma);
static mat34_t getF(const vec4_t& p);
+ static vec3_t getOrthogonal(const vec3_t &v);
};
}; // namespace android
diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp
index 3cb3745..f8279d2 100644
--- a/services/sensorservice/GravitySensor.cpp
+++ b/services/sensorservice/GravitySensor.cpp
@@ -47,9 +47,9 @@
const static double NS2S = 1.0 / 1000000000.0;
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
vec3_t g;
- if (!mSensorFusion.hasEstimate())
+ if (!mSensorFusion.hasEstimate(FUSION_NOMAG))
return false;
- const mat33_t R(mSensorFusion.getRotationMatrix());
+ const mat33_t R(mSensorFusion.getRotationMatrix(FUSION_NOMAG));
// FIXME: we need to estimate the length of gravity because
// the accelerometer may have a small scaling error. This
// translates to an offset in the linear-acceleration sensor.
@@ -67,11 +67,11 @@
}
status_t GravitySensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_NOMAG, ident, enabled);
}
status_t GravitySensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_NOMAG, ident, ns);
}
Sensor GravitySensor::getSensor() const {
diff --git a/services/sensorservice/OrientationSensor.cpp b/services/sensorservice/OrientationSensor.cpp
index 6d85cca..d55f336 100644
--- a/services/sensorservice/OrientationSensor.cpp
+++ b/services/sensorservice/OrientationSensor.cpp
@@ -66,11 +66,11 @@
}
status_t OrientationSensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
}
status_t OrientationSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
}
Sensor OrientationSensor::getSensor() const {
diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp
index cb305eb..238845b 100644
--- a/services/sensorservice/RotationVectorSensor.cpp
+++ b/services/sensorservice/RotationVectorSensor.cpp
@@ -27,9 +27,10 @@
namespace android {
// ---------------------------------------------------------------------------
-RotationVectorSensor::RotationVectorSensor()
+RotationVectorSensor::RotationVectorSensor(int mode)
: mSensorDevice(SensorDevice::getInstance()),
- mSensorFusion(SensorFusion::getInstance())
+ mSensorFusion(SensorFusion::getInstance()),
+ mMode(mode)
{
}
@@ -37,15 +38,15 @@
const sensors_event_t& event)
{
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
- if (mSensorFusion.hasEstimate()) {
- const vec4_t q(mSensorFusion.getAttitude());
+ if (mSensorFusion.hasEstimate(mMode)) {
+ const vec4_t q(mSensorFusion.getAttitude(mMode));
*outEvent = event;
outEvent->data[0] = q.x;
outEvent->data[1] = q.y;
outEvent->data[2] = q.z;
outEvent->data[3] = q.w;
- outEvent->sensor = '_rov';
- outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
+ outEvent->sensor = getSensorToken();
+ outEvent->type = getSensorType();
return true;
}
}
@@ -53,20 +54,20 @@
}
status_t RotationVectorSensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(mMode, ident, enabled);
}
status_t RotationVectorSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(mMode, ident, ns);
}
Sensor RotationVectorSensor::getSensor() const {
sensor_t hwSensor;
- hwSensor.name = "Rotation Vector Sensor";
+ hwSensor.name = getSensorName();
hwSensor.vendor = "AOSP";
hwSensor.version = 3;
- hwSensor.handle = '_rov';
- hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR;
+ hwSensor.handle = getSensorToken();
+ hwSensor.type = getSensorType();
hwSensor.maxRange = 1;
hwSensor.resolution = 1.0f / (1<<24);
hwSensor.power = mSensorFusion.getPowerUsage();
@@ -75,6 +76,48 @@
return sensor;
}
+int RotationVectorSensor::getSensorType() const {
+ switch(mMode) {
+ case FUSION_9AXIS:
+ return SENSOR_TYPE_ROTATION_VECTOR;
+ case FUSION_NOMAG:
+ return SENSOR_TYPE_GAME_ROTATION_VECTOR;
+ case FUSION_NOGYRO:
+ return SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
+ default:
+ assert(0);
+ return 0;
+ }
+}
+
+const char* RotationVectorSensor::getSensorName() const {
+ switch(mMode) {
+ case FUSION_9AXIS:
+ return "Rotation Vector Sensor";
+ case FUSION_NOMAG:
+ return "Game Rotation Vector Sensor";
+ case FUSION_NOGYRO:
+ return "GeoMag Rotation Vector Sensor";
+ default:
+ assert(0);
+ return NULL;
+ }
+}
+
+int RotationVectorSensor::getSensorToken() const {
+ switch(mMode) {
+ case FUSION_9AXIS:
+ return '_rov';
+ case FUSION_NOMAG:
+ return '_gar';
+ case FUSION_NOGYRO:
+ return '_geo';
+ default:
+ assert(0);
+ return 0;
+ }
+}
+
// ---------------------------------------------------------------------------
GyroDriftSensor::GyroDriftSensor()
@@ -102,11 +145,11 @@
}
status_t GyroDriftSensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
}
status_t GyroDriftSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
}
Sensor GyroDriftSensor::getSensor() const {
diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h
index bb97fe1..1fc316b 100644
--- a/services/sensorservice/RotationVectorSensor.h
+++ b/services/sensorservice/RotationVectorSensor.h
@@ -35,9 +35,14 @@
class RotationVectorSensor : public SensorInterface {
SensorDevice& mSensorDevice;
SensorFusion& mSensorFusion;
+ int mMode;
+
+ int getSensorType() const;
+ const char* getSensorName() const ;
+ int getSensorToken() const ;
public:
- RotationVectorSensor();
+ RotationVectorSensor(int mode = FUSION_9AXIS);
virtual bool process(sensors_event_t* outEvent,
const sensors_event_t& event);
virtual status_t activate(void* ident, bool enabled);
@@ -46,6 +51,16 @@
virtual bool isVirtual() const { return true; }
};
+class GameRotationVectorSensor : public RotationVectorSensor {
+public:
+ GameRotationVectorSensor() : RotationVectorSensor(FUSION_NOMAG) {}
+};
+
+class GeoMagRotationVectorSensor : public RotationVectorSensor {
+public:
+ GeoMagRotationVectorSensor() : RotationVectorSensor(FUSION_NOGYRO) {}
+};
+
class GyroDriftSensor : public SensorInterface {
SensorDevice& mSensorDevice;
SensorFusion& mSensorFusion;
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);
}
// ---------------------------------------------------------------------------
diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h
index 432adbc..ad636d5 100644
--- a/services/sensorservice/SensorFusion.h
+++ b/services/sensorservice/SensorFusion.h
@@ -42,30 +42,52 @@
Sensor mAcc;
Sensor mMag;
Sensor mGyro;
- Fusion mFusion;
- bool mEnabled;
+
+ Fusion mFusions[NUM_FUSION_MODE]; // normal, no_mag, no_gyro
+
+ bool mEnabled[NUM_FUSION_MODE];
+
+ vec4_t &mAttitude;
+ vec4_t mAttitudes[NUM_FUSION_MODE];
+
+ SortedVector<void*> mClients[3];
+
float mEstimatedGyroRate;
nsecs_t mTargetDelayNs;
+
nsecs_t mGyroTime;
- vec4_t mAttitude;
- SortedVector<void*> mClients;
+ nsecs_t mAccTime;
SensorFusion();
public:
void process(const sensors_event_t& event);
- bool isEnabled() const { return mEnabled; }
- bool hasEstimate() const { return mFusion.hasEstimate(); }
- mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); }
- vec4_t getAttitude() const { return mAttitude; }
- vec3_t getGyroBias() const { return mFusion.getBias(); }
+ bool isEnabled() const {
+ return mEnabled[FUSION_9AXIS] ||
+ mEnabled[FUSION_NOMAG] ||
+ mEnabled[FUSION_NOGYRO];
+ }
+
+ bool hasEstimate(int mode = FUSION_9AXIS) const {
+ return mFusions[mode].hasEstimate();
+ }
+
+ mat33_t getRotationMatrix(int mode = FUSION_9AXIS) const {
+ return mFusions[mode].getRotationMatrix();
+ }
+
+ vec4_t getAttitude(int mode = FUSION_9AXIS) const {
+ return mAttitudes[mode];
+ }
+
+ vec3_t getGyroBias() const { return mFusions[FUSION_9AXIS].getBias(); }
float getEstimatedRate() const { return mEstimatedGyroRate; }
- status_t activate(void* ident, bool enabled);
- status_t setDelay(void* ident, int64_t ns);
+ status_t activate(int mode, void* ident, bool enabled);
+ status_t setDelay(int mode, void* ident, int64_t ns);
- float getPowerUsage() const;
+ float getPowerUsage(int mode=FUSION_9AXIS) const;
int32_t getMinDelay() const;
void dump(String8& result);
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index fd72b23..8d1f83f 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -88,11 +88,14 @@
uint32_t virtualSensorsNeeds =
(1<<SENSOR_TYPE_GRAVITY) |
(1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
- (1<<SENSOR_TYPE_ROTATION_VECTOR);
+ (1<<SENSOR_TYPE_ROTATION_VECTOR) |
+ (1<<SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR) |
+ (1<<SENSOR_TYPE_GAME_ROTATION_VECTOR);
mLastEventSeen.setCapacity(count);
for (ssize_t i=0 ; i<count ; i++) {
- registerSensor( new HardwareSensor(list[i]) );
+ bool useThisSensor=true;
+
switch (list[i].type) {
case SENSOR_TYPE_ACCELEROMETER:
hasAccel = true;
@@ -110,9 +113,18 @@
case SENSOR_TYPE_GRAVITY:
case SENSOR_TYPE_LINEAR_ACCELERATION:
case SENSOR_TYPE_ROTATION_VECTOR:
- virtualSensorsNeeds &= ~(1<<list[i].type);
+ case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR:
+ case SENSOR_TYPE_GAME_ROTATION_VECTOR:
+ if (IGNORE_HARDWARE_FUSION) {
+ useThisSensor = false;
+ } else {
+ virtualSensorsNeeds &= ~(1<<list[i].type);
+ }
break;
}
+ if (useThisSensor) {
+ registerSensor( new HardwareSensor(list[i]) );
+ }
}
// it's safe to instantiate the SensorFusion object here
@@ -124,26 +136,15 @@
mUserSensorList = mSensorList;
if (hasGyro && hasAccel && hasMag) {
- Sensor aSensor;
-
// Add Android virtual sensors if they're not already
// available in the HAL
+ Sensor aSensor;
aSensor = registerVirtualSensor( new RotationVectorSensor() );
if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
mUserSensorList.add(aSensor);
}
- aSensor = registerVirtualSensor( new GravitySensor(list, count) );
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
- mUserSensorList.add(aSensor);
- }
-
- aSensor = registerVirtualSensor( new LinearAccelerationSensor(list, count) );
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
- mUserSensorList.add(aSensor);
- }
-
aSensor = registerVirtualSensor( new OrientationSensor() );
if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
// if we are doing our own rotation-vector, also add
@@ -151,11 +152,46 @@
mUserSensorList.replaceAt(aSensor, orientationIndex);
}
+ aSensor = registerVirtualSensor(
+ new LinearAccelerationSensor(list, count) );
+ if (virtualSensorsNeeds &
+ (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
+ mUserSensorList.add(aSensor);
+ }
+
// virtual debugging sensors are not added to mUserSensorList
registerVirtualSensor( new CorrectedGyroSensor(list, count) );
registerVirtualSensor( new GyroDriftSensor() );
}
+ if (hasAccel && hasGyro) {
+ Sensor aSensor;
+
+ aSensor = registerVirtualSensor(
+ new GravitySensor(list, count) );
+ if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
+ mUserSensorList.add(aSensor);
+ }
+
+ aSensor = registerVirtualSensor(
+ new GameRotationVectorSensor() );
+ if (virtualSensorsNeeds &
+ (1<<SENSOR_TYPE_GAME_ROTATION_VECTOR)) {
+ mUserSensorList.add(aSensor);
+ }
+ }
+
+ if (hasAccel && hasMag) {
+ Sensor aSensor;
+
+ aSensor = registerVirtualSensor(
+ new GeoMagRotationVectorSensor() );
+ if (virtualSensorsNeeds &
+ (1<<SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR)) {
+ mUserSensorList.add(aSensor);
+ }
+ }
+
// debugging sensor list
mUserSensorListDebug = mSensorList;
diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h
index 9a573ae..7d81d6e 100644
--- a/services/sensorservice/SensorService.h
+++ b/services/sensorservice/SensorService.h
@@ -46,7 +46,7 @@
#endif
// ---------------------------------------------------------------------------
-
+#define IGNORE_HARDWARE_FUSION false
#define DEBUG_CONNECTIONS false
// Max size is 100 KB which is enough to accept a batch of about 1000 events.
#define MAX_SOCKET_BUFFER_SIZE_BATCHED 100 * 1024
