ADHOC/replicant-frameworks_native
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

use quaternions instead of MRPs

Browse Source 
also use correct time propagation equation
disable the fused sensors when gyro is not present since
they were unusable in practice.

Change-Id: Iad797425784e67dc6c5690e97c71c583418cc5b5
This commit is contained in:
Mathias Agopian 2011-05-27 18:18:13 -07:00
parent 984826cc15
commit 3301542828
16 changed files with 366 additions and 548 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
services/sensorservice/Android.mk
View File

@ -8,7 +8,6 @@ LOCAL_SRC_FILES:= \
LinearAccelerationSensor.cpp \ LinearAccelerationSensor.cpp \
OrientationSensor.cpp \ OrientationSensor.cpp \
RotationVectorSensor.cpp \ RotationVectorSensor.cpp \
SecondOrderLowPassFilter.cpp \
SensorDevice.cpp \ SensorDevice.cpp \
SensorFusion.cpp \ SensorFusion.cpp \
SensorInterface.cpp \ SensorInterface.cpp \

2
services/sensorservice/CorrectedGyroSensor.cpp
View File

@ -45,7 +45,7 @@ bool CorrectedGyroSensor::process(sensors_event_t* outEvent,
const sensors_event_t& event) const sensors_event_t& event)
{ {
if (event.type == SENSOR_TYPE_GYROSCOPE) { if (event.type == SENSOR_TYPE_GYROSCOPE) {
const vec3_t bias(mSensorFusion.getGyroBias() * mSensorFusion.getEstimatedRate()); const vec3_t bias(mSensorFusion.getGyroBias());
*outEvent = event; *outEvent = event;
outEvent->data[0] -= bias.x; outEvent->data[0] -= bias.x;
outEvent->data[1] -= bias.y; outEvent->data[1] -= bias.y;

273
services/sensorservice/Fusion.cpp
View File

@ -24,15 +24,14 @@ namespace android {
// ----------------------------------------------------------------------- // -----------------------------------------------------------------------
template <typename TYPE> static const float gyroSTDEV = 3.16e-4; // rad/s^3/2
static inline TYPE sqr(TYPE x) { static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
return x*x; 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 const float FREE_FALL_THRESHOLD = 0.981f;
static inline T clamp(T v) {
return v < 0 ? 0 : v; // -----------------------------------------------------------------------
}
template <typename TYPE, size_t C, size_t R> template <typename TYPE, size_t C, size_t R>
static mat<TYPE, R, R> scaleCovariance( static mat<TYPE, R, R> scaleCovariance(
@ -71,33 +70,6 @@ static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
return r; 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> template<typename TYPE, size_t SIZE>
class Covariance { class Covariance {
@ -128,11 +100,8 @@ public:
// ----------------------------------------------------------------------- // -----------------------------------------------------------------------
Fusion::Fusion() { Fusion::Fusion() {
// process noise covariance matrix Phi[0][1] = 0;
const float w1 = gyroSTDEV; Phi[1][1] = 1;
const float w2 = biasSTDEV;
Q[0] = w1*w1;
Q[1] = w2*w2;
Ba.x = 0; Ba.x = 0;
Ba.y = 0; Ba.y = 0;
@ -146,25 +115,46 @@ Fusion::Fusion() {
} }
void Fusion::init() { void Fusion::init() {
// initial estimate: E{ x(t0) }
x = 0;
// initial covariance: Var{ x(t0) }
P = 0;
mInitState = 0; mInitState = 0;
mGyroRate = 0;
mCount[0] = 0; mCount[0] = 0;
mCount[1] = 0; mCount[1] = 0;
mCount[2] = 0; mCount[2] = 0;
mData = 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 { bool Fusion::hasEstimate() const {
return (mInitState == (MAG|ACC|GYRO)); return (mInitState == (MAG|ACC|GYRO));
} }
bool Fusion::checkInitComplete(int what, const vec3_t& d) { bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
if (mInitState == (MAG|ACC|GYRO)) if (hasEstimate())
return true; return true;
if (what == ACC) { if (what == ACC) {
@ -176,7 +166,8 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) {
mCount[1]++; mCount[1]++;
mInitState |= MAG; mInitState |= MAG;
} else if (what == GYRO) { } else if (what == GYRO) {
mData[2] += d; mGyroRate = dT;
mData[2] += d*dT;
mCount[2]++; mCount[2]++;
if (mCount[2] == 64) { if (mCount[2] == 64) {
// 64 samples is good enough to estimate the gyro drift and // 64 samples is good enough to estimate the gyro drift and
@ -199,37 +190,29 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) {
east *= 1/length(east); east *= 1/length(east);
vec3_t north(cross_product(up, east)); vec3_t north(cross_product(up, east));
R << east << north << up; 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 initFusion(q, mGyroRate);
// 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;
} }
return false; return false;
} }
void Fusion::handleGyro(const vec3_t& w, float dT) { void Fusion::handleGyro(const vec3_t& w, float dT) {
const vec3_t wdT(w * dT); // rad/s * s -> rad if (!checkInitComplete(GYRO, w, dT))
if (!checkInitComplete(GYRO, wdT))
return; return;
predict(wdT); predict(w, dT);
} }
status_t Fusion::handleAcc(const vec3_t& a) { 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; return BAD_VALUE;
if (!checkInitComplete(ACC, a)) if (!checkInitComplete(ACC, a))
return BAD_VALUE; return BAD_VALUE;
// ignore acceleration data if we're close to free-fall
const float l = 1/length(a); const float l = 1/length(a);
update(a*l, Ba, accSTDEV*l); update(a*l, Ba, accSTDEV*l);
return NO_ERROR; return NO_ERROR;
@ -251,20 +234,6 @@ status_t Fusion::handleMag(const vec3_t& m) {
const float l = 1 / length(north); const float l = 1 / length(north);
north *= l; 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); update(north, Bm, magSTDEV*l);
return NO_ERROR; return NO_ERROR;
} }
@ -273,7 +242,7 @@ bool Fusion::checkState(const vec3_t& v) {
if (isnanf(length(v))) { if (isnanf(length(v))) {
LOGW("9-axis fusion diverged. reseting state."); LOGW("9-axis fusion diverged. reseting state.");
P = 0; P = 0;
x[1] = 0; x1 = 0;
mInitState = 0; mInitState = 0;
mCount[0] = 0; mCount[0] = 0;
mCount[1] = 0; mCount[1] = 0;
@ -284,145 +253,89 @@ bool Fusion::checkState(const vec3_t& v) {
return true; return true;
} }
vec3_t Fusion::getAttitude() const { vec4_t Fusion::getAttitude() const {
return x[0]; return x0;
} }
vec3_t Fusion::getBias() const { vec3_t Fusion::getBias() const {
return x[1]; return x1;
} }
mat33_t Fusion::getRotationMatrix() const { mat33_t Fusion::getRotationMatrix() const {
return MRPsToMatrix(x[0]); return quatToMatrix(x0);
} }
mat33_t Fusion::getF(const vec3_t& p) { mat34_t Fusion::getF(const vec4_t& q) {
const float p0 = p.x; mat34_t F;
const float p1 = p.y; F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y;
const float p2 = p.z; 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(p, w) F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z;
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);
return F; 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 | // P(k+1) = F*P(k)*Ft + G*Q*Gt
// | 0 0 |
mat33_t A; // Phi = | Phi00 Phi10 |
A[0][0] = A[1][1] = A[2][2] = 0.5f * (p.x*we.x + p.y*we.y + p.z*we.z); // | 0 1 |
A[0][1] = 0.5f * (p.y*we.x - p.x*we.y - we.z); const mat33_t I33(1);
A[0][2] = 0.5f * (p.z*we.x - p.x*we.z + we.y); const mat33_t I33dT(dT);
A[1][2] = 0.5f * (p.z*we.y - p.y*we.z - we.x); const mat33_t wx(crossMatrix(we, 0));
A[1][0] = -A[0][1]; const mat33_t wx2(wx*wx);
A[2][0] = -A[0][2]; const float lwedT = length(we)*dT;
A[2][1] = -A[1][2]; const float ilwe = 1/length(we);
return A; const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
} const float k1 = sinf(lwedT);
void Fusion::predict(const vec3_t& w) { Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
// f(p, w) Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
vec3_t& p(x[0]);
// There is a discontinuity at 2.pi, to avoid it we need to switch to P = Phi*P*transpose(Phi) + GQGt;
// 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];
} }
void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { 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 // 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); const vec3_t Bb(A*Bi);
// Sensitivity matrix H = dh(p)/dp // Sensitivity matrix H = dh(p)/dp
// H = [ L 0 ] // H = [ L 0 ]
const float ptp(dot_product(p,p)); const mat33_t L(crossMatrix(Bb, 0));
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));
// update... // gain...
// K = P*Ht / [H*P*Ht + R]
vec<mat33_t, 2> K;
const mat33_t R(sigma*sigma); const mat33_t R(sigma*sigma);
const mat33_t S(scaleCovariance(L, P[0][0]) + R); const mat33_t S(scaleCovariance(L, P[0][0]) + R);
const mat33_t Si(invert(S)); const mat33_t Si(invert(S));
const mat33_t LtSi(transpose(L)*Si); const mat33_t LtSi(transpose(L)*Si);
vec<mat33_t, 2> K;
K[0] = P[0][0] * LtSi; K[0] = P[0][0] * LtSi;
K[1] = transpose(P[1][0])*LtSi; K[1] = transpose(P[1][0])*LtSi;
const vec3_t e(z - Bb); // update...
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;
// P -= K*H*P; // P -= K*H*P;
const mat33_t K0L(K[0] * L); const mat33_t K0L(K[0] * L);
const mat33_t K1L(K[1] * L); const mat33_t K1L(K[1] * L);
P[0][0] -= K0L*P[0][0]; P[0][0] -= K0L*P[0][0];
P[1][1] -= K1L*P[1][0]; P[1][1] -= K1L*P[1][0];
P[1][0] -= K0L*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;
} }
// ----------------------------------------------------------------------- // -----------------------------------------------------------------------

35
services/sensorservice/Fusion.h
View File

@ -19,42 +19,39 @@
#include <utils/Errors.h> #include <utils/Errors.h>
#include "vec.h" #include "quat.h"
#include "mat.h" #include "mat.h"
#include "vec.h"
namespace android { namespace android {
typedef mat<float, 3, 4> mat34_t;
class Fusion { class Fusion {
/* /*
* the state vector is made of two sub-vector containing respectively: * the state vector is made of two sub-vector containing respectively:
* - modified Rodrigues parameters * - modified Rodrigues parameters
* - the estimated gyro bias * - 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 * the predicated covariance matrix is made of 4 3x3 sub-matrices and it
* semi-definite positive. * semi-definite positive.
* *
* P = | P00 P10 | = | P00 P10 | * P = | P00 P10 | = | P00 P10 |
* | P01 P11 | | P10t Q1 | * | P01 P11 | | P10t P11 |
* *
* Since P01 = transpose(P10), the code below never calculates or * 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; mat<mat33_t, 2, 2> P;
/* /*
* the process noise covariance matrix is made of 2 3x3 sub-matrices * the process noise covariance matrix
* Q0 encodes the attitude's noise
* Q1 encodes the bias' noise
*/ */
vec<mat33_t, 2> Q; mat<mat33_t, 2, 2> GQGt;
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)
public: public:
Fusion(); Fusion();
@ -62,23 +59,25 @@ public:
void handleGyro(const vec3_t& w, float dT); void handleGyro(const vec3_t& w, float dT);
status_t handleAcc(const vec3_t& a); status_t handleAcc(const vec3_t& a);
status_t handleMag(const vec3_t& m); status_t handleMag(const vec3_t& m);
vec3_t getAttitude() const; vec4_t getAttitude() const;
vec3_t getBias() const; vec3_t getBias() const;
mat33_t getRotationMatrix() const; mat33_t getRotationMatrix() const;
bool hasEstimate() const; bool hasEstimate() const;
private: private:
mat<mat33_t, 2, 2> Phi;
vec3_t Ba, Bm; vec3_t Ba, Bm;
uint32_t mInitState; uint32_t mInitState;
float mGyroRate;
vec<vec3_t, 3> mData; vec<vec3_t, 3> mData;
size_t mCount[3]; size_t mCount[3];
enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; 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); 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); void update(const vec3_t& z, const vec3_t& Bi, float sigma);
static mat33_t getF(const vec3_t& p); static mat34_t getF(const vec4_t& p);
static mat33_t getdFdp(const vec3_t& p, const vec3_t& we);
}; };
}; // namespace android }; // namespace android

51
services/sensorservice/GravitySensor.cpp
View File

@ -31,10 +31,7 @@ namespace android {
GravitySensor::GravitySensor(sensor_t const* list, size_t count) GravitySensor::GravitySensor(sensor_t const* list, size_t count)
: mSensorDevice(SensorDevice::getInstance()), : mSensorDevice(SensorDevice::getInstance()),
mSensorFusion(SensorFusion::getInstance()), mSensorFusion(SensorFusion::getInstance())
mAccTime(0),
mLowPass(M_SQRT1_2, 1.5f),
mX(mLowPass), mY(mLowPass), mZ(mLowPass)
{ {
for (size_t i=0 ; i<count ; i++) { for (size_t i=0 ; i<count ; i++) {
if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
@ -50,7 +47,6 @@ bool GravitySensor::process(sensors_event_t* outEvent,
const static double NS2S = 1.0 / 1000000000.0; const static double NS2S = 1.0 / 1000000000.0;
if (event.type == SENSOR_TYPE_ACCELEROMETER) { if (event.type == SENSOR_TYPE_ACCELEROMETER) {
vec3_t g; vec3_t g;
if (mSensorFusion.hasGyro()) {
if (!mSensorFusion.hasEstimate()) if (!mSensorFusion.hasEstimate())
return false; return false;
const mat33_t R(mSensorFusion.getRotationMatrix()); const mat33_t R(mSensorFusion.getRotationMatrix());
@ -58,22 +54,7 @@ bool GravitySensor::process(sensors_event_t* outEvent,
// the accelerometer may have a small scaling error. This // the accelerometer may have a small scaling error. This
// translates to an offset in the linear-acceleration sensor. // translates to an offset in the linear-acceleration sensor.
g = R[2] * GRAVITY_EARTH; 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;
}
*outEvent = event; *outEvent = event;
outEvent->data[0] = g.x; outEvent->data[0] = g.x;
outEvent->data[1] = g.y; outEvent->data[1] = g.y;
@ -86,42 +67,24 @@ bool GravitySensor::process(sensors_event_t* outEvent,
} }
status_t GravitySensor::activate(void* ident, bool enabled) { status_t GravitySensor::activate(void* ident, bool enabled) {
status_t err; return mSensorFusion.activate(this, enabled);
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;
} }
status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) {
{
if (mSensorFusion.hasGyro()) {
return mSensorFusion.setDelay(this, ns); return mSensorFusion.setDelay(this, ns);
} else {
return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns);
}
} }
Sensor GravitySensor::getSensor() const { Sensor GravitySensor::getSensor() const {
sensor_t hwSensor; sensor_t hwSensor;
hwSensor.name = "Gravity Sensor"; hwSensor.name = "Gravity Sensor";
hwSensor.vendor = "Google Inc."; hwSensor.vendor = "Google Inc.";
hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; hwSensor.version = 3;
hwSensor.handle = '_grv'; hwSensor.handle = '_grv';
hwSensor.type = SENSOR_TYPE_GRAVITY; hwSensor.type = SENSOR_TYPE_GRAVITY;
hwSensor.maxRange = GRAVITY_EARTH * 2; hwSensor.maxRange = GRAVITY_EARTH * 2;
hwSensor.resolution = mAccelerometer.getResolution(); hwSensor.resolution = mAccelerometer.getResolution();
hwSensor.power = mSensorFusion.hasGyro() ? hwSensor.power = mSensorFusion.getPowerUsage();
mSensorFusion.getPowerUsage() : mAccelerometer.getPowerUsage(); hwSensor.minDelay = mSensorFusion.getMinDelay();
hwSensor.minDelay = mSensorFusion.hasGyro() ?
mSensorFusion.getMinDelay() : mAccelerometer.getMinDelay();
Sensor sensor(&hwSensor); Sensor sensor(&hwSensor);
return sensor; return sensor;
} }

5
services/sensorservice/GravitySensor.h
View File

@ -23,7 +23,6 @@
#include <gui/Sensor.h> #include <gui/Sensor.h>
#include "SensorInterface.h" #include "SensorInterface.h"
#include "SecondOrderLowPassFilter.h"
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
namespace android { namespace android {
@ -36,10 +35,6 @@ class GravitySensor : public SensorInterface {
SensorDevice& mSensorDevice; SensorDevice& mSensorDevice;
SensorFusion& mSensorFusion; SensorFusion& mSensorFusion;
Sensor mAccelerometer; Sensor mAccelerometer;
double mAccTime;
SecondOrderLowPassFilter mLowPass;
CascadedBiquadFilter<float> mX, mY, mZ;
public: public:
GravitySensor(sensor_t const* list, size_t count); GravitySensor(sensor_t const* list, size_t count);

7
services/sensorservice/OrientationSensor.cpp
View File

@ -50,9 +50,10 @@ bool OrientationSensor::process(sensors_event_t* outEvent,
g[0] += 360; g[0] += 360;
*outEvent = event; *outEvent = event;
outEvent->data[0] = g.x; outEvent->orientation.azimuth = g.x;
outEvent->data[1] = g.y; outEvent->orientation.pitch = g.y;
outEvent->data[2] = g.z; outEvent->orientation.roll = g.z;
outEvent->orientation.status = SENSOR_STATUS_ACCURACY_HIGH;
outEvent->sensor = '_ypr'; outEvent->sensor = '_ypr';
outEvent->type = SENSOR_TYPE_ORIENTATION; outEvent->type = SENSOR_TYPE_ORIENTATION;
return true; return true;

83
services/sensorservice/RotationVectorSensor.cpp
View File

@ -27,11 +27,6 @@
namespace android { namespace android {
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
template <typename T>
static inline T clamp(T v) {
return v < 0 ? 0 : v;
}
RotationVectorSensor::RotationVectorSensor() RotationVectorSensor::RotationVectorSensor()
: mSensorDevice(SensorDevice::getInstance()), : mSensorDevice(SensorDevice::getInstance()),
mSensorFusion(SensorFusion::getInstance()) mSensorFusion(SensorFusion::getInstance())
@ -43,29 +38,12 @@ bool RotationVectorSensor::process(sensors_event_t* outEvent,
{ {
if (event.type == SENSOR_TYPE_ACCELEROMETER) { if (event.type == SENSOR_TYPE_ACCELEROMETER) {
if (mSensorFusion.hasEstimate()) { if (mSensorFusion.hasEstimate()) {
const mat33_t R(mSensorFusion.getRotationMatrix()); const vec4_t q(mSensorFusion.getAttitude());
// 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.
*outEvent = event; *outEvent = event;
outEvent->data[0] = qx; outEvent->data[0] = q.x;
outEvent->data[1] = qy; outEvent->data[1] = q.y;
outEvent->data[2] = qz; outEvent->data[2] = q.z;
outEvent->data[3] = qw; outEvent->data[3] = q.w;
outEvent->sensor = '_rov'; outEvent->sensor = '_rov';
outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
return true; return true;
@ -86,7 +64,7 @@ Sensor RotationVectorSensor::getSensor() const {
sensor_t hwSensor; sensor_t hwSensor;
hwSensor.name = "Rotation Vector Sensor"; hwSensor.name = "Rotation Vector Sensor";
hwSensor.vendor = "Google Inc."; hwSensor.vendor = "Google Inc.";
hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; hwSensor.version = 3;
hwSensor.handle = '_rov'; hwSensor.handle = '_rov';
hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR; hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR;
hwSensor.maxRange = 1; hwSensor.maxRange = 1;
@ -97,6 +75,55 @@ Sensor RotationVectorSensor::getSensor() const {
return sensor; return sensor;
} }
// ---------------------------------------------------------------------------
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 }; // namespace android

15
services/sensorservice/RotationVectorSensor.h
View File

@ -24,7 +24,6 @@
#include "SensorDevice.h" #include "SensorDevice.h"
#include "SensorInterface.h" #include "SensorInterface.h"
#include "SecondOrderLowPassFilter.h"
#include "Fusion.h" #include "Fusion.h"
#include "SensorFusion.h" #include "SensorFusion.h"
@ -47,6 +46,20 @@ public:
virtual bool isVirtual() const { return true; } 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 }; // namespace android

103
services/sensorservice/SecondOrderLowPassFilter.cpp
View File

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

77
services/sensorservice/SecondOrderLowPassFilter.h
View File

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

70
services/sensorservice/SensorFusion.cpp
View File

@ -25,9 +25,7 @@ ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion)
SensorFusion::SensorFusion() SensorFusion::SensorFusion()
: mSensorDevice(SensorDevice::getInstance()), : mSensorDevice(SensorDevice::getInstance()),
mEnabled(false), mHasGyro(false), mGyroTime(0), mRotationMatrix(1), mEnabled(false), mGyroTime(0)
mLowPass(M_SQRT1_2, 1.0f), mAccData(mLowPass),
mFilteredMag(0.0f), mFilteredAcc(0.0f)
{ {
sensor_t const* list; sensor_t const* list;
size_t count = mSensorDevice.getSensorList(&list); size_t count = mSensorDevice.getSensorList(&list);
@ -42,55 +40,32 @@ SensorFusion::SensorFusion()
mGyro = Sensor(list + i); mGyro = Sensor(list + i);
// 200 Hz for gyro events is a good compromise between precision // 200 Hz for gyro events is a good compromise between precision
// and power/cpu usage. // and power/cpu usage.
mTargetDelayNs = 1000000000LL/200; mGyroRate = 200;
mGyroRate = 1000000000.0f / mTargetDelayNs; mTargetDelayNs = 1000000000LL/mGyroRate;
mHasGyro = true;
} }
} }
mFusion.init(); mFusion.init();
mAccData.init(vec3_t(0.0f));
} }
void SensorFusion::process(const sensors_event_t& event) { void SensorFusion::process(const sensors_event_t& event) {
if (event.type == SENSOR_TYPE_GYROSCOPE) {
if (event.type == SENSOR_TYPE_GYROSCOPE && mHasGyro) {
if (mGyroTime != 0) { if (mGyroTime != 0) {
const float dT = (event.timestamp - mGyroTime) / 1000000000.0f; const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;
const float freq = 1 / dT; const float freq = 1 / dT;
const float alpha = 2 / (2 + dT); // 2s time-constant if (freq >= 100 && freq<1000) { // filter values obviously wrong
mGyroRate = mGyroRate*alpha + freq*(1 - alpha); const float alpha = 1 / (1 + dT); // 1s time-constant
mGyroRate = freq + (mGyroRate - freq)*alpha;
}
} }
mGyroTime = event.timestamp; mGyroTime = event.timestamp;
mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate); mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate);
} else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
const vec3_t mag(event.data); const vec3_t mag(event.data);
if (mHasGyro) {
mFusion.handleMag(mag); mFusion.handleMag(mag);
} else {
const float l(length(mag));
if (l>5 && l<100) {
mFilteredMag = mag * (1/l);
}
}
} else if (event.type == SENSOR_TYPE_ACCELEROMETER) { } else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
const vec3_t acc(event.data); const vec3_t acc(event.data);
if (mHasGyro) {
mFusion.handleAcc(acc); mFusion.handleAcc(acc);
mRotationMatrix = mFusion.getRotationMatrix(); mAttitude = mFusion.getAttitude();
} 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;
}
}
} }
} }
@ -116,40 +91,31 @@ status_t SensorFusion::activate(void* ident, bool enabled) {
mSensorDevice.activate(ident, mAcc.getHandle(), enabled); mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
mSensorDevice.activate(ident, mMag.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; const bool newState = mClients.size() != 0;
if (newState != mEnabled) { if (newState != mEnabled) {
mEnabled = newState; mEnabled = newState;
if (newState) { if (newState) {
mFusion.init(); mFusion.init();
mGyroTime = 0;
} }
} }
return NO_ERROR; return NO_ERROR;
} }
status_t SensorFusion::setDelay(void* ident, int64_t ns) { status_t SensorFusion::setDelay(void* ident, int64_t ns) {
if (mHasGyro) {
mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); mSensorDevice.setDelay(ident, mAcc.getHandle(), ns);
mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20));
mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); 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);
}
return NO_ERROR; return NO_ERROR;
} }
float SensorFusion::getPowerUsage() const { float SensorFusion::getPowerUsage() const {
float power = mAcc.getPowerUsage() + mMag.getPowerUsage(); float power = mAcc.getPowerUsage() +
if (mHasGyro) { mMag.getPowerUsage() +
power += mGyro.getPowerUsage(); mGyro.getPowerUsage();
}
return power; return power;
} }
@ -159,17 +125,17 @@ int32_t SensorFusion::getMinDelay() const {
void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) { void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) {
const Fusion& fusion(mFusion); const Fusion& fusion(mFusion);
snprintf(buffer, SIZE, "Fusion (%s) %s (%d clients), gyro-rate=%7.2fHz, " snprintf(buffer, SIZE, "9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, "
"MRPS=< %g, %g, %g > (%g), " "q=< %g, %g, %g, %g > (%g), "
"BIAS=< %g, %g, %g >\n", "b=< %g, %g, %g >\n",
mHasGyro ? "9-axis" : "6-axis",
mEnabled ? "enabled" : "disabled", mEnabled ? "enabled" : "disabled",
mClients.size(), mClients.size(),
mGyroRate, mGyroRate,
fusion.getAttitude().x, fusion.getAttitude().x,
fusion.getAttitude().y, fusion.getAttitude().y,
fusion.getAttitude().z, fusion.getAttitude().z,
dot_product(fusion.getAttitude(), fusion.getAttitude()), fusion.getAttitude().w,
length(fusion.getAttitude()),
fusion.getBias().x, fusion.getBias().x,
fusion.getBias().y, fusion.getBias().y,
fusion.getBias().z); fusion.getBias().z);

14
services/sensorservice/SensorFusion.h
View File

@ -27,7 +27,6 @@
#include <gui/Sensor.h> #include <gui/Sensor.h>
#include "Fusion.h" #include "Fusion.h"
#include "SecondOrderLowPassFilter.h"
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
@ -45,15 +44,10 @@ class SensorFusion : public Singleton<SensorFusion> {
Sensor mGyro; Sensor mGyro;
Fusion mFusion; Fusion mFusion;
bool mEnabled; bool mEnabled;
bool mHasGyro;
float mGyroRate; float mGyroRate;
nsecs_t mTargetDelayNs; nsecs_t mTargetDelayNs;
nsecs_t mGyroTime; nsecs_t mGyroTime;
mat33_t mRotationMatrix; vec4_t mAttitude;
SecondOrderLowPassFilter mLowPass;
BiquadFilter<vec3_t> mAccData;
vec3_t mFilteredMag;
vec3_t mFilteredAcc;
SortedVector<void*> mClients; SortedVector<void*> mClients;
SensorFusion(); SensorFusion();
@ -62,9 +56,9 @@ public:
void process(const sensors_event_t& event); void process(const sensors_event_t& event);
bool isEnabled() const { return mEnabled; } bool isEnabled() const { return mEnabled; }
bool hasGyro() const { return mHasGyro; } bool hasEstimate() const { return mFusion.hasEstimate(); }
bool hasEstimate() const { return !mHasGyro || mFusion.hasEstimate(); } mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); }
mat33_t getRotationMatrix() const { return mRotationMatrix; } vec4_t getAttitude() const { return mAttitude; }
vec3_t getGyroBias() const { return mFusion.getBias(); } vec3_t getGyroBias() const { return mFusion.getBias(); }
float getEstimatedRate() const { return mGyroRate; } float getEstimatedRate() const { return mGyroRate; }

29
services/sensorservice/SensorService.cpp
View File

@ -18,6 +18,8 @@
#include <math.h> #include <math.h>
#include <sys/types.h> #include <sys/types.h>
#include <cutils/properties.h>
#include <utils/SortedVector.h> #include <utils/SortedVector.h>
#include <utils/KeyedVector.h> #include <utils/KeyedVector.h>
#include <utils/threads.h> #include <utils/threads.h>
@ -46,6 +48,16 @@
namespace android { 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() SensorService::SensorService()
: mDump("android.permission.DUMP"), : mDump("android.permission.DUMP"),
mInitCheck(NO_INIT) mInitCheck(NO_INIT)
@ -59,6 +71,7 @@ void SensorService::onFirstRef()
SensorDevice& dev(SensorDevice::getInstance()); SensorDevice& dev(SensorDevice::getInstance());
if (dev.initCheck() == NO_ERROR) { if (dev.initCheck() == NO_ERROR) {
bool hasGyro = false;
uint32_t virtualSensorsNeeds = uint32_t virtualSensorsNeeds =
(1<<SENSOR_TYPE_GRAVITY) | (1<<SENSOR_TYPE_GRAVITY) |
(1<<SENSOR_TYPE_LINEAR_ACCELERATION) | (1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
@ -69,6 +82,9 @@ void SensorService::onFirstRef()
for (int i=0 ; i<count ; i++) { for (int i=0 ; i<count ; i++) {
registerSensor( new HardwareSensor(list[i]) ); registerSensor( new HardwareSensor(list[i]) );
switch (list[i].type) { switch (list[i].type) {
case SENSOR_TYPE_GYROSCOPE:
hasGyro = true;
break;
case SENSOR_TYPE_GRAVITY: case SENSOR_TYPE_GRAVITY:
case SENSOR_TYPE_LINEAR_ACCELERATION: case SENSOR_TYPE_LINEAR_ACCELERATION:
case SENSOR_TYPE_ROTATION_VECTOR: case SENSOR_TYPE_ROTATION_VECTOR:
@ -82,6 +98,7 @@ void SensorService::onFirstRef()
// registered) // registered)
const SensorFusion& fusion(SensorFusion::getInstance()); const SensorFusion& fusion(SensorFusion::getInstance());
if (hasGyro) {
// Always instantiate Android's virtual sensors. Since they are // Always instantiate Android's virtual sensors. Since they are
// instantiated behind sensors from the HAL, they won't // instantiated behind sensors from the HAL, they won't
// interfere with applications, unless they looks specifically // interfere with applications, unless they looks specifically
@ -91,12 +108,16 @@ void SensorService::onFirstRef()
registerVirtualSensor( new GravitySensor(list, count) ); registerVirtualSensor( new GravitySensor(list, count) );
registerVirtualSensor( new LinearAccelerationSensor(list, count) ); registerVirtualSensor( new LinearAccelerationSensor(list, count) );
// if we have a gyro, we have the option of enabling these // these are optional
// "better" orientation and gyro sensors
if (fusion.hasGyro()) {
// FIXME: OrientationSensor buggy when not pointing north
registerVirtualSensor( new OrientationSensor() ); registerVirtualSensor( new OrientationSensor() );
registerVirtualSensor( new CorrectedGyroSensor(list, count) ); 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); run("SensorService", PRIORITY_URGENT_DISPLAY);

98
services/sensorservice/quat.h Normal file
View File

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

9
services/sensorservice/vec.h
View File

@ -207,6 +207,15 @@ TYPE PURE length(const V<TYPE, SIZE>& v) {
return sqrt(dot_product(v, v)); return sqrt(dot_product(v, v));
} }
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 <
template<typename T, size_t S> class VLHS, template<typename T, size_t S> class VLHS,
template<typename T, size_t S> class VRHS, template<typename T, size_t S> class VRHS,