use quaternions instead of MRPs
also use correct time propagation equation
disable the fused sensors when gyro is not present since
they were unusable in practice.
Change-Id: Iad797425784e67dc6c5690e97c71c583418cc5b5
diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk
index 57a3b15..ba3e6e5 100644
--- a/services/sensorservice/Android.mk
+++ b/services/sensorservice/Android.mk
@@ -8,7 +8,6 @@
LinearAccelerationSensor.cpp \
OrientationSensor.cpp \
RotationVectorSensor.cpp \
- SecondOrderLowPassFilter.cpp \
SensorDevice.cpp \
SensorFusion.cpp \
SensorInterface.cpp \
diff --git a/services/sensorservice/CorrectedGyroSensor.cpp b/services/sensorservice/CorrectedGyroSensor.cpp
index 9b75b70..1857443 100644
--- a/services/sensorservice/CorrectedGyroSensor.cpp
+++ b/services/sensorservice/CorrectedGyroSensor.cpp
@@ -45,7 +45,7 @@
const sensors_event_t& event)
{
if (event.type == SENSOR_TYPE_GYROSCOPE) {
- const vec3_t bias(mSensorFusion.getGyroBias() * mSensorFusion.getEstimatedRate());
+ const vec3_t bias(mSensorFusion.getGyroBias());
*outEvent = event;
outEvent->data[0] -= bias.x;
outEvent->data[1] -= bias.y;
diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
index 56ac9f9..b5f97e0 100644
--- a/services/sensorservice/Fusion.cpp
+++ b/services/sensorservice/Fusion.cpp
@@ -24,15 +24,14 @@
// -----------------------------------------------------------------------
-template <typename TYPE>
-static inline TYPE sqr(TYPE x) {
- return x*x;
-}
+static const float gyroSTDEV = 3.16e-4; // rad/s^3/2
+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 biasSTDEV = 3.16e-5; // rad/s^1/2 (guessed)
-template <typename T>
-static inline T clamp(T v) {
- return v < 0 ? 0 : v;
-}
+static const float FREE_FALL_THRESHOLD = 0.981f;
+
+// -----------------------------------------------------------------------
template <typename TYPE, size_t C, size_t R>
static mat<TYPE, R, R> scaleCovariance(
@@ -71,33 +70,6 @@
return r;
}
-template <typename TYPE>
-static mat<TYPE, 3, 3> MRPsToMatrix(const vec<TYPE, 3>& p) {
- mat<TYPE, 3, 3> res(1);
- const mat<TYPE, 3, 3> px(crossMatrix(p, 0));
- const TYPE ptp(dot_product(p,p));
- const TYPE t = 4/sqr(1+ptp);
- res -= t * (1-ptp) * px;
- res += t * 2 * sqr(px);
- return res;
-}
-
-template <typename TYPE>
-vec<TYPE, 3> matrixToMRPs(const mat<TYPE, 3, 3>& R) {
- // matrix to MRPs
- vec<TYPE, 3> q;
- const float Hx = R[0].x;
- const float My = R[1].y;
- const float Az = R[2].z;
- const float w = 1 / (1 + sqrtf( clamp( Hx + My + Az + 1) * 0.25f ));
- q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ) * w;
- q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ) * w;
- q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ) * w;
- q.x = copysignf(q.x, R[2].y - R[1].z);
- q.y = copysignf(q.y, R[0].z - R[2].x);
- q.z = copysignf(q.z, R[1].x - R[0].y);
- return q;
-}
template<typename TYPE, size_t SIZE>
class Covariance {
@@ -128,11 +100,8 @@
// -----------------------------------------------------------------------
Fusion::Fusion() {
- // process noise covariance matrix
- const float w1 = gyroSTDEV;
- const float w2 = biasSTDEV;
- Q[0] = w1*w1;
- Q[1] = w2*w2;
+ Phi[0][1] = 0;
+ Phi[1][1] = 1;
Ba.x = 0;
Ba.y = 0;
@@ -146,25 +115,46 @@
}
void Fusion::init() {
- // initial estimate: E{ x(t0) }
- x = 0;
-
- // initial covariance: Var{ x(t0) }
- P = 0;
-
mInitState = 0;
+ mGyroRate = 0;
mCount[0] = 0;
mCount[1] = 0;
mCount[2] = 0;
mData = 0;
}
+void Fusion::initFusion(const vec4_t& q, float dT)
+{
+ // initial estimate: E{ x(t0) }
+ x0 = q;
+ x1 = 0;
+
+ // process noise covariance matrix
+ // G = | -1 0 |
+ // | 0 1 |
+
+ const float v = gyroSTDEV;
+ const float u = biasSTDEV;
+ const float q00 = v*v*dT + 0.33333f*(dT*dT*dT)*u*u;
+ const float q10 = 0.5f*(dT*dT) *u*u;
+ const float q01 = q10;
+ const float q11 = u*u*dT;
+ GQGt[0][0] = q00;
+ GQGt[1][0] = -q10;
+ GQGt[0][1] = -q01;
+ GQGt[1][1] = q11;
+
+
+ // initial covariance: Var{ x(t0) }
+ P = 0;
+}
+
bool Fusion::hasEstimate() const {
return (mInitState == (MAG|ACC|GYRO));
}
-bool Fusion::checkInitComplete(int what, const vec3_t& d) {
- if (mInitState == (MAG|ACC|GYRO))
+bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
+ if (hasEstimate())
return true;
if (what == ACC) {
@@ -176,7 +166,8 @@
mCount[1]++;
mInitState |= MAG;
} else if (what == GYRO) {
- mData[2] += d;
+ mGyroRate = dT;
+ mData[2] += d*dT;
mCount[2]++;
if (mCount[2] == 64) {
// 64 samples is good enough to estimate the gyro drift and
@@ -199,37 +190,29 @@
east *= 1/length(east);
vec3_t north(cross_product(up, east));
R << east << north << up;
- x[0] = matrixToMRPs(R);
+ const vec4_t q = matrixToQuat(R);
- // NOTE: we could try to use the average of the gyro data
- // to estimate the initial bias, but this only works if
- // the device is not moving. For now, we don't use that value
- // and start with a bias of 0.
- x[1] = 0;
-
- // initial covariance
- P = 0;
+ initFusion(q, mGyroRate);
}
return false;
}
void Fusion::handleGyro(const vec3_t& w, float dT) {
- const vec3_t wdT(w * dT); // rad/s * s -> rad
- if (!checkInitComplete(GYRO, wdT))
+ if (!checkInitComplete(GYRO, w, dT))
return;
- predict(wdT);
+ predict(w, dT);
}
status_t Fusion::handleAcc(const vec3_t& a) {
- if (length(a) < 0.981f)
+ // ignore acceleration data if we're close to free-fall
+ if (length(a) < FREE_FALL_THRESHOLD)
return BAD_VALUE;
if (!checkInitComplete(ACC, a))
return BAD_VALUE;
- // ignore acceleration data if we're close to free-fall
const float l = 1/length(a);
update(a*l, Ba, accSTDEV*l);
return NO_ERROR;
@@ -251,20 +234,6 @@
const float l = 1 / length(north);
north *= l;
-#if 0
- // in practice the magnetic-field sensor is so wrong
- // that there is no point trying to use it to constantly
- // correct the gyro. instead, we use the mag-sensor only when
- // the device points north (just to give us a reference).
- // We're hoping that it'll actually point north, if it doesn't
- // we'll be offset, but at least the instantaneous posture
- // of the device will be correct.
-
- const float cos_30 = 0.8660254f;
- if (dot_product(north, Bm) < cos_30)
- return BAD_VALUE;
-#endif
-
update(north, Bm, magSTDEV*l);
return NO_ERROR;
}
@@ -273,7 +242,7 @@
if (isnanf(length(v))) {
LOGW("9-axis fusion diverged. reseting state.");
P = 0;
- x[1] = 0;
+ x1 = 0;
mInitState = 0;
mCount[0] = 0;
mCount[1] = 0;
@@ -284,145 +253,89 @@
return true;
}
-vec3_t Fusion::getAttitude() const {
- return x[0];
+vec4_t Fusion::getAttitude() const {
+ return x0;
}
vec3_t Fusion::getBias() const {
- return x[1];
+ return x1;
}
mat33_t Fusion::getRotationMatrix() const {
- return MRPsToMatrix(x[0]);
+ return quatToMatrix(x0);
}
-mat33_t Fusion::getF(const vec3_t& p) {
- const float p0 = p.x;
- const float p1 = p.y;
- const float p2 = p.z;
-
- // f(p, w)
- const float p0p1 = p0*p1;
- const float p0p2 = p0*p2;
- const float p1p2 = p1*p2;
- const float p0p0 = p0*p0;
- const float p1p1 = p1*p1;
- const float p2p2 = p2*p2;
- const float pp = 0.5f * (1 - (p0p0 + p1p1 + p2p2));
-
- mat33_t F;
- F[0][0] = 0.5f*(p0p0 + pp);
- F[0][1] = 0.5f*(p0p1 + p2);
- F[0][2] = 0.5f*(p0p2 - p1);
- F[1][0] = 0.5f*(p0p1 - p2);
- F[1][1] = 0.5f*(p1p1 + pp);
- F[1][2] = 0.5f*(p1p2 + p0);
- F[2][0] = 0.5f*(p0p2 + p1);
- F[2][1] = 0.5f*(p1p2 - p0);
- F[2][2] = 0.5f*(p2p2 + pp);
+mat34_t Fusion::getF(const vec4_t& q) {
+ mat34_t F;
+ F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y;
+ F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x;
+ F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w;
+ F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z;
return F;
}
-mat33_t Fusion::getdFdp(const vec3_t& p, const vec3_t& we) {
+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;
+ const vec4_t dq = getF(q)*((0.5f*dT)*we);
+ x0 = normalize_quat(q + dq);
- // dF = | A = df/dp -F |
- // | 0 0 |
+ // P(k+1) = F*P(k)*Ft + G*Q*Gt
- mat33_t A;
- A[0][0] = A[1][1] = A[2][2] = 0.5f * (p.x*we.x + p.y*we.y + p.z*we.z);
- A[0][1] = 0.5f * (p.y*we.x - p.x*we.y - we.z);
- A[0][2] = 0.5f * (p.z*we.x - p.x*we.z + we.y);
- A[1][2] = 0.5f * (p.z*we.y - p.y*we.z - we.x);
- A[1][0] = -A[0][1];
- A[2][0] = -A[0][2];
- A[2][1] = -A[1][2];
- return A;
-}
+ // Phi = | Phi00 Phi10 |
+ // | 0 1 |
+ const mat33_t I33(1);
+ const mat33_t I33dT(dT);
+ const mat33_t wx(crossMatrix(we, 0));
+ const mat33_t wx2(wx*wx);
+ const float lwedT = length(we)*dT;
+ const float ilwe = 1/length(we);
+ const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
+ const float k1 = sinf(lwedT);
-void Fusion::predict(const vec3_t& w) {
- // f(p, w)
- vec3_t& p(x[0]);
+ Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
+ Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
- // There is a discontinuity at 2.pi, to avoid it we need to switch to
- // the shadow of p when pT.p gets too big.
- const float ptp(dot_product(p,p));
- if (ptp >= 2.0f) {
- p = -p * (1/ptp);
- }
-
- const mat33_t F(getF(p));
-
- // compute w with the bias correction:
- // w_estimated = w - b_estimated
- const vec3_t& b(x[1]);
- const vec3_t we(w - b);
-
- // prediction
- const vec3_t dX(F*we);
-
- if (!checkState(dX))
- return;
-
- p += dX;
-
- const mat33_t A(getdFdp(p, we));
-
- // G = | G0 0 | = | -F 0 |
- // | 0 1 | | 0 1 |
-
- // P += A*P + P*At + F*Q*Ft
- const mat33_t AP(A*transpose(P[0][0]));
- const mat33_t PAt(P[0][0]*transpose(A));
- const mat33_t FPSt(F*transpose(P[1][0]));
- const mat33_t PSFt(P[1][0]*transpose(F));
- const mat33_t FQFt(scaleCovariance(F, Q[0]));
- P[0][0] += AP + PAt - FPSt - PSFt + FQFt;
- P[1][0] += A*P[1][0] - F*P[1][1];
- P[1][1] += Q[1];
+ P = Phi*P*transpose(Phi) + GQGt;
}
void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
- const vec3_t p(x[0]);
+ vec4_t q(x0);
// measured vector in body space: h(p) = A(p)*Bi
- const mat33_t A(MRPsToMatrix(p));
+ const mat33_t A(quatToMatrix(q));
const vec3_t Bb(A*Bi);
// Sensitivity matrix H = dh(p)/dp
// H = [ L 0 ]
- const float ptp(dot_product(p,p));
- const mat33_t px(crossMatrix(p, 0.5f*(ptp-1)));
- const mat33_t ppt(p*transpose(p));
- const mat33_t L((8 / sqr(1+ptp))*crossMatrix(Bb, 0)*(ppt-px));
+ const mat33_t L(crossMatrix(Bb, 0));
- // update...
+ // gain...
+ // K = P*Ht / [H*P*Ht + R]
+ vec<mat33_t, 2> K;
const mat33_t R(sigma*sigma);
const mat33_t S(scaleCovariance(L, P[0][0]) + R);
const mat33_t Si(invert(S));
const mat33_t LtSi(transpose(L)*Si);
-
- vec<mat33_t, 2> K;
K[0] = P[0][0] * LtSi;
K[1] = transpose(P[1][0])*LtSi;
- const vec3_t e(z - Bb);
- const vec3_t K0e(K[0]*e);
- const vec3_t K1e(K[1]*e);
-
- if (!checkState(K0e))
- return;
-
- if (!checkState(K1e))
- return;
-
- x[0] += K0e;
- x[1] += K1e;
-
+ // update...
// P -= K*H*P;
const mat33_t K0L(K[0] * L);
const mat33_t K1L(K[1] * L);
P[0][0] -= K0L*P[0][0];
P[1][1] -= K1L*P[1][0];
P[1][0] -= K0L*P[1][0];
+ P[0][1] = transpose(P[1][0]);
+
+ 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;
}
// -----------------------------------------------------------------------
diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h
index 571a415..556944b 100644
--- a/services/sensorservice/Fusion.h
+++ b/services/sensorservice/Fusion.h
@@ -19,42 +19,39 @@
#include <utils/Errors.h>
-#include "vec.h"
+#include "quat.h"
#include "mat.h"
+#include "vec.h"
namespace android {
+typedef mat<float, 3, 4> mat34_t;
+
class Fusion {
/*
* the state vector is made of two sub-vector containing respectively:
* - modified Rodrigues parameters
* - the estimated gyro bias
*/
- vec<vec3_t, 2> x;
+ quat_t x0;
+ vec3_t x1;
/*
* the predicated covariance matrix is made of 4 3x3 sub-matrices and it
* semi-definite positive.
*
* P = | P00 P10 | = | P00 P10 |
- * | P01 P11 | | P10t Q1 |
+ * | P01 P11 | | P10t P11 |
*
* Since P01 = transpose(P10), the code below never calculates or
- * stores P01. P11 is always equal to Q1, so we don't store it either.
+ * stores P01.
*/
mat<mat33_t, 2, 2> P;
/*
- * the process noise covariance matrix is made of 2 3x3 sub-matrices
- * Q0 encodes the attitude's noise
- * Q1 encodes the bias' noise
+ * the process noise covariance matrix
*/
- vec<mat33_t, 2> Q;
-
- static const float gyroSTDEV = 1.0e-5; // rad/s (measured 1.2e-5)
- 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 biasSTDEV = 2e-9; // rad/s^2 (guessed)
+ mat<mat33_t, 2, 2> GQGt;
public:
Fusion();
@@ -62,23 +59,25 @@
void handleGyro(const vec3_t& w, float dT);
status_t handleAcc(const vec3_t& a);
status_t handleMag(const vec3_t& m);
- vec3_t getAttitude() const;
+ vec4_t getAttitude() const;
vec3_t getBias() const;
mat33_t getRotationMatrix() const;
bool hasEstimate() const;
private:
+ mat<mat33_t, 2, 2> Phi;
vec3_t Ba, Bm;
uint32_t mInitState;
+ float mGyroRate;
vec<vec3_t, 3> mData;
size_t mCount[3];
enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
- bool checkInitComplete(int, const vec3_t&);
+ bool checkInitComplete(int, const vec3_t& w, float d = 0);
+ void initFusion(const vec4_t& q0, float dT);
bool checkState(const vec3_t& v);
- void predict(const vec3_t& w);
+ void predict(const vec3_t& w, float dT);
void update(const vec3_t& z, const vec3_t& Bi, float sigma);
- static mat33_t getF(const vec3_t& p);
- static mat33_t getdFdp(const vec3_t& p, const vec3_t& we);
+ static mat34_t getF(const vec4_t& p);
};
}; // namespace android
diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp
index 541fad2..c57715f 100644
--- a/services/sensorservice/GravitySensor.cpp
+++ b/services/sensorservice/GravitySensor.cpp
@@ -31,10 +31,7 @@
GravitySensor::GravitySensor(sensor_t const* list, size_t count)
: mSensorDevice(SensorDevice::getInstance()),
- mSensorFusion(SensorFusion::getInstance()),
- mAccTime(0),
- mLowPass(M_SQRT1_2, 1.5f),
- mX(mLowPass), mY(mLowPass), mZ(mLowPass)
+ mSensorFusion(SensorFusion::getInstance())
{
for (size_t i=0 ; i<count ; i++) {
if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
@@ -50,30 +47,14 @@
const static double NS2S = 1.0 / 1000000000.0;
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
vec3_t g;
- if (mSensorFusion.hasGyro()) {
- if (!mSensorFusion.hasEstimate())
- return false;
- const mat33_t R(mSensorFusion.getRotationMatrix());
- // 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.
- g = R[2] * GRAVITY_EARTH;
- } else {
- const double now = event.timestamp * NS2S;
- if (mAccTime == 0) {
- g.x = mX.init(event.acceleration.x);
- g.y = mY.init(event.acceleration.y);
- g.z = mZ.init(event.acceleration.z);
- } else {
- double dT = now - mAccTime;
- mLowPass.setSamplingPeriod(dT);
- g.x = mX(event.acceleration.x);
- g.y = mY(event.acceleration.y);
- g.z = mZ(event.acceleration.z);
- }
- g *= (GRAVITY_EARTH / length(g));
- mAccTime = now;
- }
+ if (!mSensorFusion.hasEstimate())
+ return false;
+ const mat33_t R(mSensorFusion.getRotationMatrix());
+ // 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.
+ g = R[2] * GRAVITY_EARTH;
+
*outEvent = event;
outEvent->data[0] = g.x;
outEvent->data[1] = g.y;
@@ -86,42 +67,24 @@
}
status_t GravitySensor::activate(void* ident, bool enabled) {
- status_t err;
- if (mSensorFusion.hasGyro()) {
- err = mSensorFusion.activate(this, enabled);
- } else {
- err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled);
- if (err == NO_ERROR) {
- if (enabled) {
- mAccTime = 0;
- }
- }
- }
- return err;
+ return mSensorFusion.activate(this, enabled);
}
-status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns)
-{
- if (mSensorFusion.hasGyro()) {
- return mSensorFusion.setDelay(this, ns);
- } else {
- return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns);
- }
+status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) {
+ return mSensorFusion.setDelay(this, ns);
}
Sensor GravitySensor::getSensor() const {
sensor_t hwSensor;
hwSensor.name = "Gravity Sensor";
hwSensor.vendor = "Google Inc.";
- hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2;
+ hwSensor.version = 3;
hwSensor.handle = '_grv';
hwSensor.type = SENSOR_TYPE_GRAVITY;
hwSensor.maxRange = GRAVITY_EARTH * 2;
hwSensor.resolution = mAccelerometer.getResolution();
- hwSensor.power = mSensorFusion.hasGyro() ?
- mSensorFusion.getPowerUsage() : mAccelerometer.getPowerUsage();
- hwSensor.minDelay = mSensorFusion.hasGyro() ?
- mSensorFusion.getMinDelay() : mAccelerometer.getMinDelay();
+ hwSensor.power = mSensorFusion.getPowerUsage();
+ hwSensor.minDelay = mSensorFusion.getMinDelay();
Sensor sensor(&hwSensor);
return sensor;
}
diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h
index 0ca3a3c..ac177c4 100644
--- a/services/sensorservice/GravitySensor.h
+++ b/services/sensorservice/GravitySensor.h
@@ -23,7 +23,6 @@
#include <gui/Sensor.h>
#include "SensorInterface.h"
-#include "SecondOrderLowPassFilter.h"
// ---------------------------------------------------------------------------
namespace android {
@@ -36,10 +35,6 @@
SensorDevice& mSensorDevice;
SensorFusion& mSensorFusion;
Sensor mAccelerometer;
- double mAccTime;
-
- SecondOrderLowPassFilter mLowPass;
- CascadedBiquadFilter<float> mX, mY, mZ;
public:
GravitySensor(sensor_t const* list, size_t count);
diff --git a/services/sensorservice/OrientationSensor.cpp b/services/sensorservice/OrientationSensor.cpp
index c9e5080..037adaa 100644
--- a/services/sensorservice/OrientationSensor.cpp
+++ b/services/sensorservice/OrientationSensor.cpp
@@ -50,9 +50,10 @@
g[0] += 360;
*outEvent = event;
- outEvent->data[0] = g.x;
- outEvent->data[1] = g.y;
- outEvent->data[2] = g.z;
+ outEvent->orientation.azimuth = g.x;
+ outEvent->orientation.pitch = g.y;
+ outEvent->orientation.roll = g.z;
+ outEvent->orientation.status = SENSOR_STATUS_ACCURACY_HIGH;
outEvent->sensor = '_ypr';
outEvent->type = SENSOR_TYPE_ORIENTATION;
return true;
diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp
index cba89c9..5ea9568 100644
--- a/services/sensorservice/RotationVectorSensor.cpp
+++ b/services/sensorservice/RotationVectorSensor.cpp
@@ -27,11 +27,6 @@
namespace android {
// ---------------------------------------------------------------------------
-template <typename T>
-static inline T clamp(T v) {
- return v < 0 ? 0 : v;
-}
-
RotationVectorSensor::RotationVectorSensor()
: mSensorDevice(SensorDevice::getInstance()),
mSensorFusion(SensorFusion::getInstance())
@@ -43,29 +38,12 @@
{
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
if (mSensorFusion.hasEstimate()) {
- const mat33_t R(mSensorFusion.getRotationMatrix());
-
- // matrix to rotation vector (normalized quaternion)
- const float Hx = R[0].x;
- const float My = R[1].y;
- const float Az = R[2].z;
-
- float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
- float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
- float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
- float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
- qx = copysignf(qx, R[2].y - R[1].z);
- qy = copysignf(qy, R[0].z - R[2].x);
- qz = copysignf(qz, R[1].x - R[0].y);
-
- // this quaternion is guaranteed to be normalized, by construction
- // of the rotation matrix.
-
+ const vec4_t q(mSensorFusion.getAttitude());
*outEvent = event;
- outEvent->data[0] = qx;
- outEvent->data[1] = qy;
- outEvent->data[2] = qz;
- outEvent->data[3] = qw;
+ 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;
return true;
@@ -86,7 +64,7 @@
sensor_t hwSensor;
hwSensor.name = "Rotation Vector Sensor";
hwSensor.vendor = "Google Inc.";
- hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2;
+ hwSensor.version = 3;
hwSensor.handle = '_rov';
hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR;
hwSensor.maxRange = 1;
@@ -98,5 +76,54 @@
}
// ---------------------------------------------------------------------------
+
+GyroDriftSensor::GyroDriftSensor()
+ : mSensorDevice(SensorDevice::getInstance()),
+ mSensorFusion(SensorFusion::getInstance())
+{
+}
+
+bool GyroDriftSensor::process(sensors_event_t* outEvent,
+ const sensors_event_t& event)
+{
+ if (event.type == SENSOR_TYPE_ACCELEROMETER) {
+ if (mSensorFusion.hasEstimate()) {
+ const vec3_t b(mSensorFusion.getGyroBias());
+ *outEvent = event;
+ outEvent->data[0] = b.x;
+ outEvent->data[1] = b.y;
+ outEvent->data[2] = b.z;
+ outEvent->sensor = '_gbs';
+ outEvent->type = SENSOR_TYPE_ACCELEROMETER;
+ return true;
+ }
+ }
+ return false;
+}
+
+status_t GyroDriftSensor::activate(void* ident, bool enabled) {
+ return mSensorFusion.activate(this, enabled);
+}
+
+status_t GyroDriftSensor::setDelay(void* ident, int handle, int64_t ns) {
+ return mSensorFusion.setDelay(this, ns);
+}
+
+Sensor GyroDriftSensor::getSensor() const {
+ sensor_t hwSensor;
+ hwSensor.name = "Gyroscope Bias (debug)";
+ hwSensor.vendor = "Google Inc.";
+ hwSensor.version = 1;
+ hwSensor.handle = '_gbs';
+ hwSensor.type = SENSOR_TYPE_ACCELEROMETER;
+ hwSensor.maxRange = 1;
+ hwSensor.resolution = 1.0f / (1<<24);
+ hwSensor.power = mSensorFusion.getPowerUsage();
+ hwSensor.minDelay = mSensorFusion.getMinDelay();
+ Sensor sensor(&hwSensor);
+ return sensor;
+}
+
+// ---------------------------------------------------------------------------
}; // namespace android
diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h
index ac76487..bb97fe1 100644
--- a/services/sensorservice/RotationVectorSensor.h
+++ b/services/sensorservice/RotationVectorSensor.h
@@ -24,7 +24,6 @@
#include "SensorDevice.h"
#include "SensorInterface.h"
-#include "SecondOrderLowPassFilter.h"
#include "Fusion.h"
#include "SensorFusion.h"
@@ -47,6 +46,20 @@
virtual bool isVirtual() const { return true; }
};
+class GyroDriftSensor : public SensorInterface {
+ SensorDevice& mSensorDevice;
+ SensorFusion& mSensorFusion;
+
+public:
+ GyroDriftSensor();
+ virtual bool process(sensors_event_t* outEvent,
+ const sensors_event_t& event);
+ virtual status_t activate(void* ident, bool enabled);
+ virtual status_t setDelay(void* ident, int handle, int64_t ns);
+ virtual Sensor getSensor() const;
+ virtual bool isVirtual() const { return true; }
+};
+
// ---------------------------------------------------------------------------
}; // namespace android
diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp
deleted file mode 100644
index c76dd4c..0000000
--- a/services/sensorservice/SecondOrderLowPassFilter.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-/*
- * Copyright (C) 2010 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include <stdint.h>
-#include <sys/types.h>
-#include <math.h>
-
-#include <cutils/log.h>
-
-#include "SecondOrderLowPassFilter.h"
-#include "vec.h"
-
-// ---------------------------------------------------------------------------
-
-namespace android {
-// ---------------------------------------------------------------------------
-
-SecondOrderLowPassFilter::SecondOrderLowPassFilter(float Q, float fc)
- : iQ(1.0f / Q), fc(fc)
-{
-}
-
-void SecondOrderLowPassFilter::setSamplingPeriod(float dT)
-{
- K = tanf(float(M_PI) * fc * dT);
- iD = 1.0f / (K*K + K*iQ + 1);
- a0 = K*K*iD;
- a1 = 2.0f * a0;
- b1 = 2.0f*(K*K - 1)*iD;
- b2 = (K*K - K*iQ + 1)*iD;
-}
-
-// ---------------------------------------------------------------------------
-
-template<typename T>
-BiquadFilter<T>::BiquadFilter(const SecondOrderLowPassFilter& s)
- : s(s)
-{
-}
-
-template<typename T>
-T BiquadFilter<T>::init(const T& x)
-{
- x1 = x2 = x;
- y1 = y2 = x;
- return x;
-}
-
-template<typename T>
-T BiquadFilter<T>::operator()(const T& x)
-{
- T y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2;
- x2 = x1;
- y2 = y1;
- x1 = x;
- y1 = y;
- return y;
-}
-
-// ---------------------------------------------------------------------------
-
-template<typename T>
-CascadedBiquadFilter<T>::CascadedBiquadFilter(const SecondOrderLowPassFilter& s)
- : mA(s), mB(s)
-{
-}
-
-template<typename T>
-T CascadedBiquadFilter<T>::init(const T& x)
-{
- mA.init(x);
- mB.init(x);
- return x;
-}
-
-template<typename T>
-T CascadedBiquadFilter<T>::operator()(const T& x)
-{
- return mB(mA(x));
-}
-
-// ---------------------------------------------------------------------------
-
-template class BiquadFilter<float>;
-template class CascadedBiquadFilter<float>;
-template class BiquadFilter<vec3_t>;
-template class CascadedBiquadFilter<vec3_t>;
-
-// ---------------------------------------------------------------------------
-}; // namespace android
diff --git a/services/sensorservice/SecondOrderLowPassFilter.h b/services/sensorservice/SecondOrderLowPassFilter.h
deleted file mode 100644
index 0cc2446..0000000
--- a/services/sensorservice/SecondOrderLowPassFilter.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * Copyright (C) 2010 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H
-#define ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H
-
-#include <stdint.h>
-#include <sys/types.h>
-
-// ---------------------------------------------------------------------------
-
-namespace android {
-// ---------------------------------------------------------------------------
-
-template<typename T>
-class BiquadFilter;
-
-/*
- * State of a 2nd order low-pass IIR filter
- */
-class SecondOrderLowPassFilter {
- template<typename T>
- friend class BiquadFilter;
- float iQ, fc;
- float K, iD;
- float a0, a1;
- float b1, b2;
-public:
- SecondOrderLowPassFilter(float Q, float fc);
- void setSamplingPeriod(float dT);
-};
-
-/*
- * Implements a Biquad IIR filter
- */
-template<typename T>
-class BiquadFilter {
- T x1, x2;
- T y1, y2;
- const SecondOrderLowPassFilter& s;
-public:
- BiquadFilter(const SecondOrderLowPassFilter& s);
- T init(const T& in);
- T operator()(const T& in);
-};
-
-/*
- * Two cascaded biquad IIR filters
- * (4-poles IIR)
- */
-template<typename T>
-class CascadedBiquadFilter {
- BiquadFilter<T> mA;
- BiquadFilter<T> mB;
-public:
- CascadedBiquadFilter(const SecondOrderLowPassFilter& s);
- T init(const T& in);
- T operator()(const T& in);
-};
-
-// ---------------------------------------------------------------------------
-}; // namespace android
-
-#endif // ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H
diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp
index d4226ec..4ec0c8c 100644
--- a/services/sensorservice/SensorFusion.cpp
+++ b/services/sensorservice/SensorFusion.cpp
@@ -25,9 +25,7 @@
SensorFusion::SensorFusion()
: mSensorDevice(SensorDevice::getInstance()),
- mEnabled(false), mHasGyro(false), mGyroTime(0), mRotationMatrix(1),
- mLowPass(M_SQRT1_2, 1.0f), mAccData(mLowPass),
- mFilteredMag(0.0f), mFilteredAcc(0.0f)
+ mEnabled(false), mGyroTime(0)
{
sensor_t const* list;
size_t count = mSensorDevice.getSensorList(&list);
@@ -42,55 +40,32 @@
mGyro = Sensor(list + i);
// 200 Hz for gyro events is a good compromise between precision
// and power/cpu usage.
- mTargetDelayNs = 1000000000LL/200;
- mGyroRate = 1000000000.0f / mTargetDelayNs;
- mHasGyro = true;
+ mGyroRate = 200;
+ mTargetDelayNs = 1000000000LL/mGyroRate;
}
}
mFusion.init();
- mAccData.init(vec3_t(0.0f));
}
void SensorFusion::process(const sensors_event_t& event) {
-
- if (event.type == SENSOR_TYPE_GYROSCOPE && mHasGyro) {
+ if (event.type == SENSOR_TYPE_GYROSCOPE) {
if (mGyroTime != 0) {
const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;
const float freq = 1 / dT;
- const float alpha = 2 / (2 + dT); // 2s time-constant
- mGyroRate = mGyroRate*alpha + freq*(1 - alpha);
+ if (freq >= 100 && freq<1000) { // filter values obviously wrong
+ const float alpha = 1 / (1 + dT); // 1s time-constant
+ mGyroRate = freq + (mGyroRate - 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);
- if (mHasGyro) {
- mFusion.handleMag(mag);
- } else {
- const float l(length(mag));
- if (l>5 && l<100) {
- mFilteredMag = mag * (1/l);
- }
- }
+ mFusion.handleMag(mag);
} else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
const vec3_t acc(event.data);
- if (mHasGyro) {
- mFusion.handleAcc(acc);
- mRotationMatrix = mFusion.getRotationMatrix();
- } else {
- const float l(length(acc));
- if (l > 0.981f) {
- // remove the linear-acceleration components
- mFilteredAcc = mAccData(acc * (1/l));
- }
- if (length(mFilteredAcc)>0 && length(mFilteredMag)>0) {
- vec3_t up(mFilteredAcc);
- vec3_t east(cross_product(mFilteredMag, up));
- east *= 1/length(east);
- vec3_t north(cross_product(up, east));
- mRotationMatrix << east << north << up;
- }
- }
+ mFusion.handleAcc(acc);
+ mAttitude = mFusion.getAttitude();
}
}
@@ -116,40 +91,31 @@
mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
mSensorDevice.activate(ident, mMag.getHandle(), enabled);
- if (mHasGyro) {
- mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
- }
+ 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(void* ident, int64_t ns) {
- if (mHasGyro) {
- mSensorDevice.setDelay(ident, mAcc.getHandle(), ns);
- mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20));
- mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs);
- } else {
- const static double NS2S = 1.0 / 1000000000.0;
- mSensorDevice.setDelay(ident, mAcc.getHandle(), ns);
- mSensorDevice.setDelay(ident, mMag.getHandle(), max(ns, mMag.getMinDelayNs()));
- mLowPass.setSamplingPeriod(ns*NS2S);
- }
+ mSensorDevice.setDelay(ident, mAcc.getHandle(), ns);
+ mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20));
+ mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs);
return NO_ERROR;
}
float SensorFusion::getPowerUsage() const {
- float power = mAcc.getPowerUsage() + mMag.getPowerUsage();
- if (mHasGyro) {
- power += mGyro.getPowerUsage();
- }
+ float power = mAcc.getPowerUsage() +
+ mMag.getPowerUsage() +
+ mGyro.getPowerUsage();
return power;
}
@@ -159,17 +125,17 @@
void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) {
const Fusion& fusion(mFusion);
- snprintf(buffer, SIZE, "Fusion (%s) %s (%d clients), gyro-rate=%7.2fHz, "
- "MRPS=< %g, %g, %g > (%g), "
- "BIAS=< %g, %g, %g >\n",
- mHasGyro ? "9-axis" : "6-axis",
+ snprintf(buffer, SIZE, "9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, "
+ "q=< %g, %g, %g, %g > (%g), "
+ "b=< %g, %g, %g >\n",
mEnabled ? "enabled" : "disabled",
mClients.size(),
mGyroRate,
fusion.getAttitude().x,
fusion.getAttitude().y,
fusion.getAttitude().z,
- dot_product(fusion.getAttitude(), fusion.getAttitude()),
+ fusion.getAttitude().w,
+ length(fusion.getAttitude()),
fusion.getBias().x,
fusion.getBias().y,
fusion.getBias().z);
diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h
index c7eab12..4c99bcb 100644
--- a/services/sensorservice/SensorFusion.h
+++ b/services/sensorservice/SensorFusion.h
@@ -27,7 +27,6 @@
#include <gui/Sensor.h>
#include "Fusion.h"
-#include "SecondOrderLowPassFilter.h"
// ---------------------------------------------------------------------------
@@ -45,15 +44,10 @@
Sensor mGyro;
Fusion mFusion;
bool mEnabled;
- bool mHasGyro;
float mGyroRate;
nsecs_t mTargetDelayNs;
nsecs_t mGyroTime;
- mat33_t mRotationMatrix;
- SecondOrderLowPassFilter mLowPass;
- BiquadFilter<vec3_t> mAccData;
- vec3_t mFilteredMag;
- vec3_t mFilteredAcc;
+ vec4_t mAttitude;
SortedVector<void*> mClients;
SensorFusion();
@@ -62,9 +56,9 @@
void process(const sensors_event_t& event);
bool isEnabled() const { return mEnabled; }
- bool hasGyro() const { return mHasGyro; }
- bool hasEstimate() const { return !mHasGyro || mFusion.hasEstimate(); }
- mat33_t getRotationMatrix() const { return mRotationMatrix; }
+ 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(); }
float getEstimatedRate() const { return mGyroRate; }
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index 5b86d10..d1b10f7 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -18,6 +18,8 @@
#include <math.h>
#include <sys/types.h>
+#include <cutils/properties.h>
+
#include <utils/SortedVector.h>
#include <utils/KeyedVector.h>
#include <utils/threads.h>
@@ -46,6 +48,16 @@
namespace android {
// ---------------------------------------------------------------------------
+/*
+ * Notes:
+ *
+ * - what about a gyro-corrected magnetic-field sensor?
+ * - option to "hide" the HAL sensors
+ * - run mag sensor from time to time to force calibration
+ * - gravity sensor length is wrong (=> drift in linear-acc sensor)
+ *
+ */
+
SensorService::SensorService()
: mDump("android.permission.DUMP"),
mInitCheck(NO_INIT)
@@ -59,6 +71,7 @@
SensorDevice& dev(SensorDevice::getInstance());
if (dev.initCheck() == NO_ERROR) {
+ bool hasGyro = false;
uint32_t virtualSensorsNeeds =
(1<<SENSOR_TYPE_GRAVITY) |
(1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
@@ -69,6 +82,9 @@
for (int i=0 ; i<count ; i++) {
registerSensor( new HardwareSensor(list[i]) );
switch (list[i].type) {
+ case SENSOR_TYPE_GYROSCOPE:
+ hasGyro = true;
+ break;
case SENSOR_TYPE_GRAVITY:
case SENSOR_TYPE_LINEAR_ACCELERATION:
case SENSOR_TYPE_ROTATION_VECTOR:
@@ -82,21 +98,26 @@
// registered)
const SensorFusion& fusion(SensorFusion::getInstance());
- // Always instantiate Android's virtual sensors. Since they are
- // instantiated behind sensors from the HAL, they won't
- // interfere with applications, unless they looks specifically
- // for them (by name).
+ if (hasGyro) {
+ // Always instantiate Android's virtual sensors. Since they are
+ // instantiated behind sensors from the HAL, they won't
+ // interfere with applications, unless they looks specifically
+ // for them (by name).
- registerVirtualSensor( new RotationVectorSensor() );
- registerVirtualSensor( new GravitySensor(list, count) );
- registerVirtualSensor( new LinearAccelerationSensor(list, count) );
+ registerVirtualSensor( new RotationVectorSensor() );
+ registerVirtualSensor( new GravitySensor(list, count) );
+ registerVirtualSensor( new LinearAccelerationSensor(list, count) );
- // if we have a gyro, we have the option of enabling these
- // "better" orientation and gyro sensors
- if (fusion.hasGyro()) {
- // FIXME: OrientationSensor buggy when not pointing north
+ // these are optional
registerVirtualSensor( new OrientationSensor() );
registerVirtualSensor( new CorrectedGyroSensor(list, count) );
+
+ // virtual debugging sensors...
+ char value[PROPERTY_VALUE_MAX];
+ property_get("debug.sensors", value, "0");
+ if (atoi(value)) {
+ registerVirtualSensor( new GyroDriftSensor() );
+ }
}
run("SensorService", PRIORITY_URGENT_DISPLAY);
diff --git a/services/sensorservice/quat.h b/services/sensorservice/quat.h
new file mode 100644
index 0000000..fea1afe
--- /dev/null
+++ b/services/sensorservice/quat.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2011 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ANDROID_QUAT_H
+#define ANDROID_QUAT_H
+
+#include <math.h>
+
+#include "vec.h"
+#include "mat.h"
+
+// -----------------------------------------------------------------------
+namespace android {
+// -----------------------------------------------------------------------
+
+template <typename TYPE>
+mat<TYPE, 3, 3> quatToMatrix(const vec<TYPE, 4>& q) {
+ mat<TYPE, 3, 3> R;
+ TYPE q0(q.w);
+ TYPE q1(q.x);
+ TYPE q2(q.y);
+ TYPE q3(q.z);
+ TYPE sq_q1 = 2 * q1 * q1;
+ TYPE sq_q2 = 2 * q2 * q2;
+ TYPE sq_q3 = 2 * q3 * q3;
+ TYPE q1_q2 = 2 * q1 * q2;
+ TYPE q3_q0 = 2 * q3 * q0;
+ TYPE q1_q3 = 2 * q1 * q3;
+ TYPE q2_q0 = 2 * q2 * q0;
+ TYPE q2_q3 = 2 * q2 * q3;
+ TYPE q1_q0 = 2 * q1 * q0;
+ R[0][0] = 1 - sq_q2 - sq_q3;
+ R[0][1] = q1_q2 - q3_q0;
+ R[0][2] = q1_q3 + q2_q0;
+ R[1][0] = q1_q2 + q3_q0;
+ R[1][1] = 1 - sq_q1 - sq_q3;
+ R[1][2] = q2_q3 - q1_q0;
+ R[2][0] = q1_q3 - q2_q0;
+ R[2][1] = q2_q3 + q1_q0;
+ R[2][2] = 1 - sq_q1 - sq_q2;
+ return R;
+}
+
+template <typename TYPE>
+vec<TYPE, 4> matrixToQuat(const mat<TYPE, 3, 3>& R) {
+ // matrix to quaternion
+
+ struct {
+ inline TYPE operator()(TYPE v) {
+ return v < 0 ? 0 : v;
+ }
+ } clamp;
+
+ vec<TYPE, 4> q;
+ const float Hx = R[0].x;
+ const float My = R[1].y;
+ const float Az = R[2].z;
+ q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
+ q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
+ q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
+ q.w = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
+ q.x = copysignf(q.x, R[2].y - R[1].z);
+ q.y = copysignf(q.y, R[0].z - R[2].x);
+ q.z = copysignf(q.z, R[1].x - R[0].y);
+ // guaranteed to be unit-quaternion
+ return q;
+}
+
+template <typename TYPE>
+vec<TYPE, 4> normalize_quat(const vec<TYPE, 4>& q) {
+ vec<TYPE, 4> r(q);
+ if (r.w < 0) {
+ r = -r;
+ }
+ return normalize(r);
+}
+
+// -----------------------------------------------------------------------
+
+typedef vec4_t quat_t;
+
+// -----------------------------------------------------------------------
+}; // namespace android
+
+#endif /* ANDROID_QUAT_H */
diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h
index 736ff37..f74ccc5 100644
--- a/services/sensorservice/vec.h
+++ b/services/sensorservice/vec.h
@@ -208,6 +208,15 @@
}
template <
+ template<typename T, size_t S> class V,
+ typename TYPE,
+ size_t SIZE
+>
+V<TYPE, SIZE> PURE normalize(const V<TYPE, SIZE>& v) {
+ return v * (1/length(v));
+}
+
+template <
template<typename T, size_t S> class VLHS,
template<typename T, size_t S> class VRHS,
typename TYPE