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
This commit is contained in:
Mathias Agopian
parent
984826cc15
commit
3301542828
|
|
@ -8,7 +8,6 @@ LOCAL_SRC_FILES:= \
|
|||
LinearAccelerationSensor.cpp \
|
||||
OrientationSensor.cpp \
|
||||
RotationVectorSensor.cpp \
|
||||
SecondOrderLowPassFilter.cpp \
|
||||
SensorDevice.cpp \
|
||||
SensorFusion.cpp \
|
||||
SensorInterface.cpp \
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@ bool CorrectedGyroSensor::process(sensors_event_t* outEvent,
|
|||
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;
|
||||
|
|
|
|||
|
|
@ -24,15 +24,14 @@ namespace android {
|
|||
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
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 @@ static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
|
|||
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 @@ public:
|
|||
// -----------------------------------------------------------------------
|
||||
|
||||
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 @@ Fusion::Fusion() {
|
|||
}
|
||||
|
||||
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 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) {
|
|||
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 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) {
|
|||
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 @@ status_t Fusion::handleMag(const vec3_t& m) {
|
|||
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 @@ bool Fusion::checkState(const vec3_t& v) {
|
|||
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 @@ bool Fusion::checkState(const vec3_t& v) {
|
|||
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;
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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 @@ public:
|
|||
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
|
||||
|
|
|
|||
|
|
@ -31,10 +31,7 @@ namespace android {
|
|||
|
||||
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 @@ bool GravitySensor::process(sensors_event_t* outEvent,
|
|||
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 @@ bool GravitySensor::process(sensors_event_t* outEvent,
|
|||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,7 +23,6 @@
|
|||
#include <gui/Sensor.h>
|
||||
|
||||
#include "SensorInterface.h"
|
||||
#include "SecondOrderLowPassFilter.h"
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
namespace android {
|
||||
|
|
@ -36,10 +35,6 @@ class GravitySensor : public SensorInterface {
|
|||
SensorDevice& mSensorDevice;
|
||||
SensorFusion& mSensorFusion;
|
||||
Sensor mAccelerometer;
|
||||
double mAccTime;
|
||||
|
||||
SecondOrderLowPassFilter mLowPass;
|
||||
CascadedBiquadFilter<float> mX, mY, mZ;
|
||||
|
||||
public:
|
||||
GravitySensor(sensor_t const* list, size_t count);
|
||||
|
|
|
|||
|
|
@ -50,9 +50,10 @@ bool OrientationSensor::process(sensors_event_t* outEvent,
|
|||
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;
|
||||
|
|
|
|||
|
|
@ -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 @@ bool RotationVectorSensor::process(sensors_event_t* outEvent,
|
|||
{
|
||||
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 RotationVectorSensor::getSensor() const {
|
|||
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;
|
||||
|
|
@ -97,6 +75,55 @@ Sensor RotationVectorSensor::getSensor() const {
|
|||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -24,7 +24,6 @@
|
|||
|
||||
#include "SensorDevice.h"
|
||||
#include "SensorInterface.h"
|
||||
#include "SecondOrderLowPassFilter.h"
|
||||
|
||||
#include "Fusion.h"
|
||||
#include "SensorFusion.h"
|
||||
|
|
@ -47,6 +46,20 @@ public:
|
|||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -25,9 +25,7 @@ ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion)
|
|||
|
||||
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 @@ SensorFusion::SensorFusion()
|
|||
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 @@ status_t SensorFusion::activate(void* ident, bool enabled) {
|
|||
|
||||
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 @@ int32_t SensorFusion::getMinDelay() const {
|
|||
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -27,7 +27,6 @@
|
|||
#include <gui/Sensor.h>
|
||||
|
||||
#include "Fusion.h"
|
||||
#include "SecondOrderLowPassFilter.h"
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
|
|
@ -45,15 +44,10 @@ class SensorFusion : public Singleton<SensorFusion> {
|
|||
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 @@ public:
|
|||
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; }
|
||||
|
||||
|
|
|
|||
|
|
@ -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 @@ void SensorService::onFirstRef()
|
|||
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 @@ void SensorService::onFirstRef()
|
|||
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 @@ void SensorService::onFirstRef()
|
|||
// 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);
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
@ -207,6 +207,15 @@ TYPE PURE length(const V<TYPE, SIZE>& 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<typename T, size_t S> class VLHS,
|
||||
template<typename T, size_t S> class VRHS,
|
||||
|
|
|
|||
Loading…
Reference in New Issue