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/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