diff options
Diffstat (limited to 'services')
23 files changed, 2158 insertions, 190 deletions
diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk index c50e4a1..57a3b15 100644 --- a/services/sensorservice/Android.mk +++ b/services/sensorservice/Android.mk @@ -2,13 +2,18 @@ LOCAL_PATH:= $(call my-dir) include $(CLEAR_VARS) LOCAL_SRC_FILES:= \ - GravitySensor.cpp \ - LinearAccelerationSensor.cpp \ - RotationVectorSensor.cpp \ - SensorService.cpp \ - SensorInterface.cpp \ + CorrectedGyroSensor.cpp \ + Fusion.cpp \ + GravitySensor.cpp \ + LinearAccelerationSensor.cpp \ + OrientationSensor.cpp \ + RotationVectorSensor.cpp \ + SecondOrderLowPassFilter.cpp \ SensorDevice.cpp \ - SecondOrderLowPassFilter.cpp + SensorFusion.cpp \ + SensorInterface.cpp \ + SensorService.cpp \ + LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\" diff --git a/services/sensorservice/CorrectedGyroSensor.cpp b/services/sensorservice/CorrectedGyroSensor.cpp new file mode 100644 index 0000000..9b75b70 --- /dev/null +++ b/services/sensorservice/CorrectedGyroSensor.cpp @@ -0,0 +1,86 @@ +/* + * 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. + */ + +#include <stdint.h> +#include <math.h> +#include <sys/types.h> + +#include <utils/Errors.h> + +#include <hardware/sensors.h> + +#include "CorrectedGyroSensor.h" +#include "SensorDevice.h" +#include "SensorFusion.h" + +namespace android { +// --------------------------------------------------------------------------- + +CorrectedGyroSensor::CorrectedGyroSensor(sensor_t const* list, size_t count) + : mSensorDevice(SensorDevice::getInstance()), + mSensorFusion(SensorFusion::getInstance()) +{ + for (size_t i=0 ; i<count ; i++) { + if (list[i].type == SENSOR_TYPE_GYROSCOPE) { + mGyro = Sensor(list + i); + break; + } + } +} + +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()); + *outEvent = event; + outEvent->data[0] -= bias.x; + outEvent->data[1] -= bias.y; + outEvent->data[2] -= bias.z; + outEvent->sensor = '_cgy'; + return true; + } + return false; +} + +status_t CorrectedGyroSensor::activate(void* ident, bool enabled) { + mSensorDevice.activate(this, mGyro.getHandle(), enabled); + return mSensorFusion.activate(this, enabled); +} + +status_t CorrectedGyroSensor::setDelay(void* ident, int handle, int64_t ns) { + mSensorDevice.setDelay(this, mGyro.getHandle(), ns); + return mSensorFusion.setDelay(this, ns); +} + +Sensor CorrectedGyroSensor::getSensor() const { + sensor_t hwSensor; + hwSensor.name = "Corrected Gyroscope Sensor"; + hwSensor.vendor = "Google Inc."; + hwSensor.version = 1; + hwSensor.handle = '_cgy'; + hwSensor.type = SENSOR_TYPE_GYROSCOPE; + hwSensor.maxRange = mGyro.getMaxValue(); + hwSensor.resolution = mGyro.getResolution(); + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mGyro.getMinDelay(); + Sensor sensor(&hwSensor); + return sensor; +} + +// --------------------------------------------------------------------------- +}; // namespace android + diff --git a/services/sensorservice/CorrectedGyroSensor.h b/services/sensorservice/CorrectedGyroSensor.h new file mode 100644 index 0000000..3c49c08 --- /dev/null +++ b/services/sensorservice/CorrectedGyroSensor.h @@ -0,0 +1,52 @@ +/* + * 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_CORRECTED_GYRO_SENSOR_H +#define ANDROID_CORRECTED_GYRO_SENSOR_H + +#include <stdint.h> +#include <sys/types.h> + +#include <gui/Sensor.h> + +#include "SensorInterface.h" + +// --------------------------------------------------------------------------- +namespace android { +// --------------------------------------------------------------------------- + +class SensorDevice; +class SensorFusion; + +class CorrectedGyroSensor : public SensorInterface { + SensorDevice& mSensorDevice; + SensorFusion& mSensorFusion; + Sensor mGyro; + +public: + CorrectedGyroSensor(sensor_t const* list, size_t count); + 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 + +#endif // ANDROID_CORRECTED_GYRO_SENSOR_H diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp new file mode 100644 index 0000000..56ac9f9 --- /dev/null +++ b/services/sensorservice/Fusion.cpp @@ -0,0 +1,431 @@ +/* + * 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. + */ + +#include <stdio.h> + +#include <utils/Log.h> + +#include "Fusion.h" + +namespace android { + +// ----------------------------------------------------------------------- + +template <typename TYPE> +static inline TYPE sqr(TYPE x) { + return x*x; +} + +template <typename T> +static inline T clamp(T v) { + return v < 0 ? 0 : v; +} + +template <typename TYPE, size_t C, size_t R> +static mat<TYPE, R, R> scaleCovariance( + const mat<TYPE, C, R>& A, + const mat<TYPE, C, C>& P) { + // A*P*transpose(A); + mat<TYPE, R, R> APAt; + for (size_t r=0 ; r<R ; r++) { + for (size_t j=r ; j<R ; j++) { + double apat(0); + for (size_t c=0 ; c<C ; c++) { + double v(A[c][r]*P[c][c]*0.5); + for (size_t k=c+1 ; k<C ; k++) + v += A[k][r] * P[c][k]; + apat += 2 * v * A[c][j]; + } + APAt[j][r] = apat; + APAt[r][j] = apat; + } + } + return APAt; +} + +template <typename TYPE, typename OTHER_TYPE> +static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) { + mat<TYPE, 3, 3> r; + r[0][0] = diag; + r[1][1] = diag; + r[2][2] = diag; + r[0][1] = p.z; + r[1][0] =-p.z; + r[0][2] =-p.y; + r[2][0] = p.y; + r[1][2] = p.x; + r[2][1] =-p.x; + 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 { + mat<TYPE, SIZE, SIZE> mSumXX; + vec<TYPE, SIZE> mSumX; + size_t mN; +public: + Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { } + void update(const vec<TYPE, SIZE>& x) { + mSumXX += x*transpose(x); + mSumX += x; + mN++; + } + mat<TYPE, SIZE, SIZE> operator()() const { + const float N = 1.0f / mN; + return mSumXX*N - (mSumX*transpose(mSumX))*(N*N); + } + void reset() { + mN = 0; + mSumXX = 0; + mSumX = 0; + } + size_t getCount() const { + return mN; + } +}; + +// ----------------------------------------------------------------------- + +Fusion::Fusion() { + // process noise covariance matrix + const float w1 = gyroSTDEV; + const float w2 = biasSTDEV; + Q[0] = w1*w1; + Q[1] = w2*w2; + + Ba.x = 0; + Ba.y = 0; + Ba.z = 1; + + Bm.x = 0; + Bm.y = 1; + Bm.z = 0; + + init(); +} + +void Fusion::init() { + // initial estimate: E{ x(t0) } + x = 0; + + // initial covariance: Var{ x(t0) } + P = 0; + + mInitState = 0; + mCount[0] = 0; + mCount[1] = 0; + mCount[2] = 0; + mData = 0; +} + +bool Fusion::hasEstimate() const { + return (mInitState == (MAG|ACC|GYRO)); +} + +bool Fusion::checkInitComplete(int what, const vec3_t& d) { + if (mInitState == (MAG|ACC|GYRO)) + return true; + + if (what == ACC) { + mData[0] += d * (1/length(d)); + mCount[0]++; + mInitState |= ACC; + } else if (what == MAG) { + mData[1] += d * (1/length(d)); + mCount[1]++; + mInitState |= MAG; + } else if (what == GYRO) { + mData[2] += d; + mCount[2]++; + if (mCount[2] == 64) { + // 64 samples is good enough to estimate the gyro drift and + // doesn't take too much time. + mInitState |= GYRO; + } + } + + if (mInitState == (MAG|ACC|GYRO)) { + // Average all the values we collected so far + mData[0] *= 1.0f/mCount[0]; + mData[1] *= 1.0f/mCount[1]; + mData[2] *= 1.0f/mCount[2]; + + // calculate the MRPs from the data collection, this gives us + // a rough estimate of our initial state + mat33_t R; + vec3_t up(mData[0]); + vec3_t east(cross_product(mData[1], up)); + east *= 1/length(east); + vec3_t north(cross_product(up, east)); + R << east << north << up; + x[0] = matrixToMRPs(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; + } + + 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)) + return; + + predict(wdT); +} + +status_t Fusion::handleAcc(const vec3_t& a) { + if (length(a) < 0.981f) + 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; +} + +status_t Fusion::handleMag(const vec3_t& m) { + // the geomagnetic-field should be between 30uT and 60uT + // reject obviously wrong magnetic-fields + if (length(m) > 100) + return BAD_VALUE; + + if (!checkInitComplete(MAG, m)) + return BAD_VALUE; + + const vec3_t up( getRotationMatrix() * Ba ); + const vec3_t east( cross_product(m, up) ); + vec3_t north( cross_product(up, east) ); + + 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; +} + +bool Fusion::checkState(const vec3_t& v) { + if (isnanf(length(v))) { + LOGW("9-axis fusion diverged. reseting state."); + P = 0; + x[1] = 0; + mInitState = 0; + mCount[0] = 0; + mCount[1] = 0; + mCount[2] = 0; + mData = 0; + return false; + } + return true; +} + +vec3_t Fusion::getAttitude() const { + return x[0]; +} + +vec3_t Fusion::getBias() const { + return x[1]; +} + +mat33_t Fusion::getRotationMatrix() const { + return MRPsToMatrix(x[0]); +} + +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); + return F; +} + +mat33_t Fusion::getdFdp(const vec3_t& p, const vec3_t& we) { + + // dF = | A = df/dp -F | + // | 0 0 | + + 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; +} + +void Fusion::predict(const vec3_t& w) { + // f(p, w) + vec3_t& p(x[0]); + + // 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]; +} + +void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { + const vec3_t p(x[0]); + // measured vector in body space: h(p) = A(p)*Bi + const mat33_t A(MRPsToMatrix(p)); + 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)); + + // update... + 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; + + // 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]; +} + +// ----------------------------------------------------------------------- + +}; // namespace android + diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h new file mode 100644 index 0000000..571a415 --- /dev/null +++ b/services/sensorservice/Fusion.h @@ -0,0 +1,86 @@ +/* + * 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_FUSION_H +#define ANDROID_FUSION_H + +#include <utils/Errors.h> + +#include "vec.h" +#include "mat.h" + +namespace android { + +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; + + /* + * 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 | + * + * 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. + */ + 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 + */ + 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) + +public: + Fusion(); + void init(); + 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; + vec3_t getBias() const; + mat33_t getRotationMatrix() const; + bool hasEstimate() const; + +private: + vec3_t Ba, Bm; + uint32_t mInitState; + vec<vec3_t, 3> mData; + size_t mCount[3]; + enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; + bool checkInitComplete(int, const vec3_t&); + bool checkState(const vec3_t& v); + void predict(const vec3_t& w); + 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); +}; + +}; // namespace android + +#endif // ANDROID_FUSION_H diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp index 5c6aa99..541fad2 100644 --- a/services/sensorservice/GravitySensor.cpp +++ b/services/sensorservice/GravitySensor.cpp @@ -23,16 +23,18 @@ #include <hardware/sensors.h> #include "GravitySensor.h" +#include "SensorDevice.h" +#include "SensorFusion.h" 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) - { for (size_t i=0 ; i<count ; i++) { if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { @@ -47,35 +49,52 @@ bool GravitySensor::process(sensors_event_t* outEvent, { const static double NS2S = 1.0 / 1000000000.0; if (event.type == SENSOR_TYPE_ACCELEROMETER) { - float x, y, z; - const double now = event.timestamp * NS2S; - if (mAccTime == 0) { - x = mX.init(event.acceleration.x); - y = mY.init(event.acceleration.y); - z = mZ.init(event.acceleration.z); + 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 { - double dT = now - mAccTime; - mLowPass.setSamplingPeriod(dT); - x = mX(event.acceleration.x); - y = mY(event.acceleration.y); - z = mZ(event.acceleration.z); + 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; } - mAccTime = now; *outEvent = event; - outEvent->data[0] = x; - outEvent->data[1] = y; - outEvent->data[2] = z; + outEvent->data[0] = g.x; + outEvent->data[1] = g.y; + outEvent->data[2] = g.z; outEvent->sensor = '_grv'; outEvent->type = SENSOR_TYPE_GRAVITY; return true; } return false; } + status_t GravitySensor::activate(void* ident, bool enabled) { - status_t err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled); - if (err == NO_ERROR) { - if (enabled) { - mAccTime = 0; + 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; @@ -83,20 +102,26 @@ status_t GravitySensor::activate(void* ident, bool enabled) { status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) { - return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns); + if (mSensorFusion.hasGyro()) { + return mSensorFusion.setDelay(this, ns); + } else { + return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns); + } } Sensor GravitySensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Gravity Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = 1; + hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; hwSensor.handle = '_grv'; hwSensor.type = SENSOR_TYPE_GRAVITY; - hwSensor.maxRange = mAccelerometer.getMaxValue(); + hwSensor.maxRange = GRAVITY_EARTH * 2; hwSensor.resolution = mAccelerometer.getResolution(); - hwSensor.power = mAccelerometer.getPowerUsage(); - hwSensor.minDelay = mAccelerometer.getMinDelay(); + hwSensor.power = mSensorFusion.hasGyro() ? + mSensorFusion.getPowerUsage() : mAccelerometer.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.hasGyro() ? + mSensorFusion.getMinDelay() : mAccelerometer.getMinDelay(); Sensor sensor(&hwSensor); return sensor; } diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h index decfbb8..0ca3a3c 100644 --- a/services/sensorservice/GravitySensor.h +++ b/services/sensorservice/GravitySensor.h @@ -22,7 +22,6 @@ #include <gui/Sensor.h> -#include "SensorDevice.h" #include "SensorInterface.h" #include "SecondOrderLowPassFilter.h" @@ -30,13 +29,17 @@ namespace android { // --------------------------------------------------------------------------- +class SensorDevice; +class SensorFusion; + class GravitySensor : public SensorInterface { SensorDevice& mSensorDevice; + SensorFusion& mSensorFusion; Sensor mAccelerometer; double mAccTime; SecondOrderLowPassFilter mLowPass; - CascadedBiquadFilter mX, mY, mZ; + CascadedBiquadFilter<float> mX, mY, mZ; public: GravitySensor(sensor_t const* list, size_t count); diff --git a/services/sensorservice/LinearAccelerationSensor.cpp b/services/sensorservice/LinearAccelerationSensor.cpp index 9425a92..f0054f2 100644 --- a/services/sensorservice/LinearAccelerationSensor.cpp +++ b/services/sensorservice/LinearAccelerationSensor.cpp @@ -23,6 +23,8 @@ #include <hardware/sensors.h> #include "LinearAccelerationSensor.h" +#include "SensorDevice.h" +#include "SensorFusion.h" namespace android { // --------------------------------------------------------------------------- @@ -31,34 +33,29 @@ LinearAccelerationSensor::LinearAccelerationSensor(sensor_t const* list, size_t : mSensorDevice(SensorDevice::getInstance()), mGravitySensor(list, count) { - mData[0] = mData[1] = mData[2] = 0; } bool LinearAccelerationSensor::process(sensors_event_t* outEvent, const sensors_event_t& event) { bool result = mGravitySensor.process(outEvent, event); - if (result) { - if (event.type == SENSOR_TYPE_ACCELEROMETER) { - mData[0] = event.acceleration.x; - mData[1] = event.acceleration.y; - mData[2] = event.acceleration.z; - } - outEvent->data[0] = mData[0] - outEvent->data[0]; - outEvent->data[1] = mData[1] - outEvent->data[1]; - outEvent->data[2] = mData[2] - outEvent->data[2]; + if (result && event.type == SENSOR_TYPE_ACCELEROMETER) { + outEvent->data[0] = event.acceleration.x - outEvent->data[0]; + outEvent->data[1] = event.acceleration.y - outEvent->data[1]; + outEvent->data[2] = event.acceleration.z - outEvent->data[2]; outEvent->sensor = '_lin'; outEvent->type = SENSOR_TYPE_LINEAR_ACCELERATION; + return true; } - return result; + return false; } status_t LinearAccelerationSensor::activate(void* ident, bool enabled) { - return mGravitySensor.activate(ident, enabled); + return mGravitySensor.activate(this, enabled); } status_t LinearAccelerationSensor::setDelay(void* ident, int handle, int64_t ns) { - return mGravitySensor.setDelay(ident, handle, ns); + return mGravitySensor.setDelay(this, handle, ns); } Sensor LinearAccelerationSensor::getSensor() const { @@ -66,7 +63,7 @@ Sensor LinearAccelerationSensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Linear Acceleration Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = 1; + hwSensor.version = gsensor.getVersion(); hwSensor.handle = '_lin'; hwSensor.type = SENSOR_TYPE_LINEAR_ACCELERATION; hwSensor.maxRange = gsensor.getMaxValue(); diff --git a/services/sensorservice/LinearAccelerationSensor.h b/services/sensorservice/LinearAccelerationSensor.h index c577086..5deb24f 100644 --- a/services/sensorservice/LinearAccelerationSensor.h +++ b/services/sensorservice/LinearAccelerationSensor.h @@ -22,19 +22,19 @@ #include <gui/Sensor.h> -#include "SensorDevice.h" #include "SensorInterface.h" #include "GravitySensor.h" // --------------------------------------------------------------------------- - namespace android { // --------------------------------------------------------------------------- +class SensorDevice; +class SensorFusion; + class LinearAccelerationSensor : public SensorInterface { SensorDevice& mSensorDevice; GravitySensor mGravitySensor; - float mData[3]; virtual bool process(sensors_event_t* outEvent, const sensors_event_t& event); diff --git a/services/sensorservice/OrientationSensor.cpp b/services/sensorservice/OrientationSensor.cpp new file mode 100644 index 0000000..c9e5080 --- /dev/null +++ b/services/sensorservice/OrientationSensor.cpp @@ -0,0 +1,89 @@ +/* + * 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. + */ + +#include <stdint.h> +#include <math.h> +#include <sys/types.h> + +#include <utils/Errors.h> + +#include <hardware/sensors.h> + +#include "OrientationSensor.h" +#include "SensorDevice.h" +#include "SensorFusion.h" + +namespace android { +// --------------------------------------------------------------------------- + +OrientationSensor::OrientationSensor() + : mSensorDevice(SensorDevice::getInstance()), + mSensorFusion(SensorFusion::getInstance()) +{ +} + +bool OrientationSensor::process(sensors_event_t* outEvent, + const sensors_event_t& event) +{ + if (event.type == SENSOR_TYPE_ACCELEROMETER) { + if (mSensorFusion.hasEstimate()) { + vec3_t g; + const float rad2deg = 180 / M_PI; + const mat33_t R(mSensorFusion.getRotationMatrix()); + g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; + g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; + g[2] = asinf ( R[2][0]) * rad2deg; + if (g[0] < 0) + g[0] += 360; + + *outEvent = event; + outEvent->data[0] = g.x; + outEvent->data[1] = g.y; + outEvent->data[2] = g.z; + outEvent->sensor = '_ypr'; + outEvent->type = SENSOR_TYPE_ORIENTATION; + return true; + } + } + return false; +} + +status_t OrientationSensor::activate(void* ident, bool enabled) { + return mSensorFusion.activate(this, enabled); +} + +status_t OrientationSensor::setDelay(void* ident, int handle, int64_t ns) { + return mSensorFusion.setDelay(this, ns); +} + +Sensor OrientationSensor::getSensor() const { + sensor_t hwSensor; + hwSensor.name = "Orientation Sensor"; + hwSensor.vendor = "Google Inc."; + hwSensor.version = 1; + hwSensor.handle = '_ypr'; + hwSensor.type = SENSOR_TYPE_ORIENTATION; + hwSensor.maxRange = 360.0f; + hwSensor.resolution = 1.0f/256.0f; // FIXME: real value here + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.getMinDelay(); + Sensor sensor(&hwSensor); + return sensor; +} + +// --------------------------------------------------------------------------- +}; // namespace android + diff --git a/services/sensorservice/OrientationSensor.h b/services/sensorservice/OrientationSensor.h new file mode 100644 index 0000000..855949d --- /dev/null +++ b/services/sensorservice/OrientationSensor.h @@ -0,0 +1,51 @@ +/* + * 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_ORIENTATION_SENSOR_H +#define ANDROID_ORIENTATION_SENSOR_H + +#include <stdint.h> +#include <sys/types.h> + +#include <gui/Sensor.h> + +#include "SensorInterface.h" + +// --------------------------------------------------------------------------- +namespace android { +// --------------------------------------------------------------------------- + +class SensorDevice; +class SensorFusion; + +class OrientationSensor : public SensorInterface { + SensorDevice& mSensorDevice; + SensorFusion& mSensorFusion; + +public: + OrientationSensor(); + 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 + +#endif // ANDROID_ORIENTATION_SENSOR_H diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp index 3abfc12..cba89c9 100644 --- a/services/sensorservice/RotationVectorSensor.cpp +++ b/services/sensorservice/RotationVectorSensor.cpp @@ -32,134 +32,67 @@ static inline T clamp(T v) { return v < 0 ? 0 : v; } -RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count) +RotationVectorSensor::RotationVectorSensor() : mSensorDevice(SensorDevice::getInstance()), - mALowPass(M_SQRT1_2, 1.5f), - mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass), - mMLowPass(M_SQRT1_2, 1.5f), - mMX(mMLowPass), mMY(mMLowPass), mMZ(mMLowPass) + mSensorFusion(SensorFusion::getInstance()) { - for (size_t i=0 ; i<count ; i++) { - if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { - mAcc = Sensor(list + i); - } - if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { - mMag = Sensor(list + i); - } - } - memset(mMagData, 0, sizeof(mMagData)); } bool RotationVectorSensor::process(sensors_event_t* outEvent, const sensors_event_t& event) { - const static double NS2S = 1.0 / 1000000000.0; - if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { - const double now = event.timestamp * NS2S; - if (mMagTime == 0) { - mMagData[0] = mMX.init(event.magnetic.x); - mMagData[1] = mMY.init(event.magnetic.y); - mMagData[2] = mMZ.init(event.magnetic.z); - } else { - double dT = now - mMagTime; - mMLowPass.setSamplingPeriod(dT); - mMagData[0] = mMX(event.magnetic.x); - mMagData[1] = mMY(event.magnetic.y); - mMagData[2] = mMZ(event.magnetic.z); - } - mMagTime = now; - } if (event.type == SENSOR_TYPE_ACCELEROMETER) { - const double now = event.timestamp * NS2S; - float Ax, Ay, Az; - if (mAccTime == 0) { - Ax = mAX.init(event.acceleration.x); - Ay = mAY.init(event.acceleration.y); - Az = mAZ.init(event.acceleration.z); - } else { - double dT = now - mAccTime; - mALowPass.setSamplingPeriod(dT); - Ax = mAX(event.acceleration.x); - Ay = mAY(event.acceleration.y); - Az = mAZ(event.acceleration.z); - } - mAccTime = now; - const float Ex = mMagData[0]; - const float Ey = mMagData[1]; - const float Ez = mMagData[2]; - float Hx = Ey*Az - Ez*Ay; - float Hy = Ez*Ax - Ex*Az; - float Hz = Ex*Ay - Ey*Ax; - const float normH = sqrtf(Hx*Hx + Hy*Hy + Hz*Hz); - if (normH < 0.1f) { - // device is close to free fall (or in space?), or close to - // magnetic north pole. Typical values are > 100. - return false; + 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. + + *outEvent = event; + outEvent->data[0] = qx; + outEvent->data[1] = qy; + outEvent->data[2] = qz; + outEvent->data[3] = qw; + outEvent->sensor = '_rov'; + outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; + return true; } - const float invH = 1.0f / normH; - const float invA = 1.0f / sqrtf(Ax*Ax + Ay*Ay + Az*Az); - Hx *= invH; - Hy *= invH; - Hz *= invH; - Ax *= invA; - Ay *= invA; - Az *= invA; - const float Mx = Ay*Hz - Az*Hy; - const float My = Az*Hx - Ax*Hz; - const float Mz = Ax*Hy - Ay*Hx; - - // matrix to rotation vector (normalized quaternion) - 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, Ay - Mz); - qy = copysignf(qy, Hz - Ax); - qz = copysignf(qz, Mx - Hy); - - // this quaternion is guaranteed to be normalized, by construction - // of the rotation matrix. - - *outEvent = event; - outEvent->data[0] = qx; - outEvent->data[1] = qy; - outEvent->data[2] = qz; - outEvent->data[3] = qw; - outEvent->sensor = '_rov'; - outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; - return true; } return false; } status_t RotationVectorSensor::activate(void* ident, bool enabled) { - mSensorDevice.activate(this, mAcc.getHandle(), enabled); - mSensorDevice.activate(this, mMag.getHandle(), enabled); - if (enabled) { - mMagTime = 0; - mAccTime = 0; - } - return NO_ERROR; + return mSensorFusion.activate(this, enabled); } -status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) -{ - mSensorDevice.setDelay(this, mAcc.getHandle(), ns); - mSensorDevice.setDelay(this, mMag.getHandle(), ns); - return NO_ERROR; +status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) { + return mSensorFusion.setDelay(this, ns); } Sensor RotationVectorSensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Rotation Vector Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = 1; + hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; hwSensor.handle = '_rov'; hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR; hwSensor.maxRange = 1; hwSensor.resolution = 1.0f / (1<<24); - hwSensor.power = mAcc.getPowerUsage() + mMag.getPowerUsage(); - hwSensor.minDelay = mAcc.getMinDelay(); + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.getMinDelay(); Sensor sensor(&hwSensor); return sensor; } diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h index 17699f8..ac76487 100644 --- a/services/sensorservice/RotationVectorSensor.h +++ b/services/sensorservice/RotationVectorSensor.h @@ -26,24 +26,19 @@ #include "SensorInterface.h" #include "SecondOrderLowPassFilter.h" +#include "Fusion.h" +#include "SensorFusion.h" + // --------------------------------------------------------------------------- namespace android { // --------------------------------------------------------------------------- class RotationVectorSensor : public SensorInterface { SensorDevice& mSensorDevice; - Sensor mAcc; - Sensor mMag; - float mMagData[3]; - double mAccTime; - double mMagTime; - SecondOrderLowPassFilter mALowPass; - CascadedBiquadFilter mAX, mAY, mAZ; - SecondOrderLowPassFilter mMLowPass; - CascadedBiquadFilter mMX, mMY, mMZ; + SensorFusion& mSensorFusion; public: - RotationVectorSensor(sensor_t const* list, size_t count); + RotationVectorSensor(); virtual bool process(sensors_event_t* outEvent, const sensors_event_t& event); virtual status_t activate(void* ident, bool enabled); diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp index eeb6d1e..c76dd4c 100644 --- a/services/sensorservice/SecondOrderLowPassFilter.cpp +++ b/services/sensorservice/SecondOrderLowPassFilter.cpp @@ -21,6 +21,7 @@ #include <cutils/log.h> #include "SecondOrderLowPassFilter.h" +#include "vec.h" // --------------------------------------------------------------------------- @@ -44,21 +45,24 @@ void SecondOrderLowPassFilter::setSamplingPeriod(float dT) // --------------------------------------------------------------------------- -BiquadFilter::BiquadFilter(const SecondOrderLowPassFilter& s) +template<typename T> +BiquadFilter<T>::BiquadFilter(const SecondOrderLowPassFilter& s) : s(s) { } -float BiquadFilter::init(float x) +template<typename T> +T BiquadFilter<T>::init(const T& x) { x1 = x2 = x; y1 = y2 = x; return x; } -float BiquadFilter::operator()(float x) +template<typename T> +T BiquadFilter<T>::operator()(const T& x) { - float y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2; + T y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2; x2 = x1; y2 = y1; x1 = x; @@ -68,22 +72,32 @@ float BiquadFilter::operator()(float x) // --------------------------------------------------------------------------- -CascadedBiquadFilter::CascadedBiquadFilter(const SecondOrderLowPassFilter& s) +template<typename T> +CascadedBiquadFilter<T>::CascadedBiquadFilter(const SecondOrderLowPassFilter& s) : mA(s), mB(s) { } -float CascadedBiquadFilter::init(float x) +template<typename T> +T CascadedBiquadFilter<T>::init(const T& x) { mA.init(x); mB.init(x); return x; } -float CascadedBiquadFilter::operator()(float 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 index 85698ca..0cc2446 100644 --- a/services/sensorservice/SecondOrderLowPassFilter.h +++ b/services/sensorservice/SecondOrderLowPassFilter.h @@ -25,12 +25,14 @@ 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; @@ -44,27 +46,29 @@ public: /* * Implements a Biquad IIR filter */ +template<typename T> class BiquadFilter { - float x1, x2; - float y1, y2; + T x1, x2; + T y1, y2; const SecondOrderLowPassFilter& s; public: BiquadFilter(const SecondOrderLowPassFilter& s); - float init(float in); - float operator()(float in); + T init(const T& in); + T operator()(const T& in); }; /* * Two cascaded biquad IIR filters * (4-poles IIR) */ +template<typename T> class CascadedBiquadFilter { - BiquadFilter mA; - BiquadFilter mB; + BiquadFilter<T> mA; + BiquadFilter<T> mB; public: CascadedBiquadFilter(const SecondOrderLowPassFilter& s); - float init(float in); - float operator()(float in); + T init(const T& in); + T operator()(const T& in); }; // --------------------------------------------------------------------------- diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp index b3c8ef5..38d498c 100644 --- a/services/sensorservice/SensorDevice.cpp +++ b/services/sensorservice/SensorDevice.cpp @@ -251,6 +251,9 @@ status_t SensorDevice::setDelay(void* ident, int handle, int64_t ns) } } } + + //LOGD("setDelay: ident=%p, handle=%d, ns=%lld", ident, handle, ns); + return mSensorDevice->setDelay(mSensorDevice, handle, ns); } diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp new file mode 100644 index 0000000..d4226ec --- /dev/null +++ b/services/sensorservice/SensorFusion.cpp @@ -0,0 +1,180 @@ +/* + * 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. + */ + +#include "SensorDevice.h" +#include "SensorFusion.h" +#include "SensorService.h" + +namespace android { +// --------------------------------------------------------------------------- + +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) +{ + sensor_t const* list; + size_t count = mSensorDevice.getSensorList(&list); + for (size_t i=0 ; i<count ; i++) { + if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { + mAcc = Sensor(list + i); + } + if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { + mMag = Sensor(list + i); + } + if (list[i].type == SENSOR_TYPE_GYROSCOPE) { + 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; + } + } + mFusion.init(); + mAccData.init(vec3_t(0.0f)); +} + +void SensorFusion::process(const sensors_event_t& event) { + + if (event.type == SENSOR_TYPE_GYROSCOPE && mHasGyro) { + 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); + } + 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); + } + } + } 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; + } + } + } +} + +template <typename T> inline T min(T a, T b) { return a<b ? a : b; } +template <typename T> inline T max(T a, T b) { return a>b ? a : b; } + +status_t SensorFusion::activate(void* ident, bool enabled) { + + LOGD_IF(DEBUG_CONNECTIONS, + "SensorFusion::activate(ident=%p, enabled=%d)", + ident, enabled); + + const ssize_t idx = mClients.indexOf(ident); + if (enabled) { + if (idx < 0) { + mClients.add(ident); + } + } else { + if (idx >= 0) { + mClients.removeItemsAt(idx); + } + } + + mSensorDevice.activate(ident, mAcc.getHandle(), enabled); + mSensorDevice.activate(ident, mMag.getHandle(), enabled); + if (mHasGyro) { + mSensorDevice.activate(ident, mGyro.getHandle(), enabled); + } + + const bool newState = mClients.size() != 0; + if (newState != mEnabled) { + mEnabled = newState; + if (newState) { + mFusion.init(); + } + } + 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); + } + return NO_ERROR; +} + + +float SensorFusion::getPowerUsage() const { + float power = mAcc.getPowerUsage() + mMag.getPowerUsage(); + if (mHasGyro) { + power += mGyro.getPowerUsage(); + } + return power; +} + +int32_t SensorFusion::getMinDelay() const { + return mAcc.getMinDelay(); +} + +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", + mEnabled ? "enabled" : "disabled", + mClients.size(), + mGyroRate, + fusion.getAttitude().x, + fusion.getAttitude().y, + fusion.getAttitude().z, + dot_product(fusion.getAttitude(), fusion.getAttitude()), + fusion.getBias().x, + fusion.getBias().y, + fusion.getBias().z); + result.append(buffer); +} + +// --------------------------------------------------------------------------- +}; // namespace android diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h new file mode 100644 index 0000000..c7eab12 --- /dev/null +++ b/services/sensorservice/SensorFusion.h @@ -0,0 +1,84 @@ +/* + * 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_SENSOR_FUSION_H +#define ANDROID_SENSOR_FUSION_H + +#include <stdint.h> +#include <sys/types.h> + +#include <utils/SortedVector.h> +#include <utils/Singleton.h> +#include <utils/String8.h> + +#include <gui/Sensor.h> + +#include "Fusion.h" +#include "SecondOrderLowPassFilter.h" + +// --------------------------------------------------------------------------- + +namespace android { +// --------------------------------------------------------------------------- + +class SensorDevice; + +class SensorFusion : public Singleton<SensorFusion> { + friend class Singleton<SensorFusion>; + + SensorDevice& mSensorDevice; + Sensor mAcc; + Sensor mMag; + 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; + SortedVector<void*> mClients; + + SensorFusion(); + +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; } + vec3_t getGyroBias() const { return mFusion.getBias(); } + float getEstimatedRate() const { return mGyroRate; } + + status_t activate(void* ident, bool enabled); + status_t setDelay(void* ident, int64_t ns); + + float getPowerUsage() const; + int32_t getMinDelay() const; + + void dump(String8& result, char* buffer, size_t SIZE); +}; + + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_SENSOR_FUSION_H diff --git a/services/sensorservice/SensorInterface.h b/services/sensorservice/SensorInterface.h index 084f2f5..fb357d7 100644 --- a/services/sensorservice/SensorInterface.h +++ b/services/sensorservice/SensorInterface.h @@ -20,8 +20,6 @@ #include <stdint.h> #include <sys/types.h> -#include <utils/Singleton.h> - #include <gui/Sensor.h> #include "SensorDevice.h" diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index f1db2f5..5b86d10 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -35,10 +35,13 @@ #include <hardware/sensors.h> -#include "SensorService.h" +#include "CorrectedGyroSensor.h" #include "GravitySensor.h" #include "LinearAccelerationSensor.h" +#include "OrientationSensor.h" #include "RotationVectorSensor.h" +#include "SensorFusion.h" +#include "SensorService.h" namespace android { // --------------------------------------------------------------------------- @@ -74,14 +77,26 @@ void SensorService::onFirstRef() } } - if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) { - registerVirtualSensor( new GravitySensor(list, count) ); - } - if (virtualSensorsNeeds & (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) { - registerVirtualSensor( new LinearAccelerationSensor(list, count) ); - } - if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) { - registerVirtualSensor( new RotationVectorSensor(list, count) ); + // it's safe to instantiate the SensorFusion object here + // (it wants to be instantiated after h/w sensors have been + // 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). + + 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 + registerVirtualSensor( new OrientationSensor() ); + registerVirtualSensor( new CorrectedGyroSensor(list, count) ); } run("SensorService", PRIORITY_URGENT_DISPLAY); @@ -133,7 +148,9 @@ status_t SensorService::dump(int fd, const Vector<String16>& args) for (size_t i=0 ; i<mSensorList.size() ; i++) { const Sensor& s(mSensorList[i]); const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle())); - snprintf(buffer, SIZE, "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | last=<%5.1f,%5.1f,%5.1f>\n", + snprintf(buffer, SIZE, + "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | " + "last=<%5.1f,%5.1f,%5.1f>\n", s.getName().string(), s.getVendor().string(), s.getHandle(), @@ -141,6 +158,7 @@ status_t SensorService::dump(int fd, const Vector<String16>& args) e.data[0], e.data[1], e.data[2]); result.append(buffer); } + SensorFusion::getInstance().dump(result, buffer, SIZE); SensorDevice::getInstance().dump(result, buffer, SIZE); snprintf(buffer, SIZE, "%d active connections\n", @@ -183,13 +201,19 @@ bool SensorService::threadLoop() // handle virtual sensors if (count && vcount) { + sensors_event_t const * const event = buffer; const DefaultKeyedVector<int, SensorInterface*> virtualSensors( getActiveVirtualSensors()); const size_t activeVirtualSensorCount = virtualSensors.size(); if (activeVirtualSensorCount) { size_t k = 0; + SensorFusion& fusion(SensorFusion::getInstance()); + if (fusion.isEnabled()) { + for (size_t i=0 ; i<size_t(count) ; i++) { + fusion.process(event[i]); + } + } for (size_t i=0 ; i<size_t(count) ; i++) { - sensors_event_t const * const event = buffer; for (size_t j=0 ; j<activeVirtualSensorCount ; j++) { sensors_event_t out; if (virtualSensors.valueAt(j)->process(&out, event[i])) { diff --git a/services/sensorservice/mat.h b/services/sensorservice/mat.h new file mode 100644 index 0000000..1302ca3 --- /dev/null +++ b/services/sensorservice/mat.h @@ -0,0 +1,370 @@ +/* + * 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_MAT_H +#define ANDROID_MAT_H + +#include "vec.h" +#include "traits.h" + +// ----------------------------------------------------------------------- + +namespace android { + +template <typename TYPE, size_t C, size_t R> +class mat; + +namespace helpers { + +template <typename TYPE, size_t C, size_t R> +mat<TYPE, C, R>& doAssign( + mat<TYPE, C, R>& lhs, + typename TypeTraits<TYPE>::ParameterType rhs) { + for (size_t i=0 ; i<C ; i++) + for (size_t j=0 ; j<R ; j++) + lhs[i][j] = (i==j) ? rhs : 0; + return lhs; +} + +template <typename TYPE, size_t C, size_t R, size_t D> +mat<TYPE, C, R> PURE doMul( + const mat<TYPE, D, R>& lhs, + const mat<TYPE, C, D>& rhs) +{ + mat<TYPE, C, R> res; + for (size_t c=0 ; c<C ; c++) { + for (size_t r=0 ; r<R ; r++) { + TYPE v(0); + for (size_t k=0 ; k<D ; k++) { + v += lhs[k][r] * rhs[c][k]; + } + res[c][r] = v; + } + } + return res; +} + +template <typename TYPE, size_t R, size_t D> +vec<TYPE, R> PURE doMul( + const mat<TYPE, D, R>& lhs, + const vec<TYPE, D>& rhs) +{ + vec<TYPE, R> res; + for (size_t r=0 ; r<R ; r++) { + TYPE v(0); + for (size_t k=0 ; k<D ; k++) { + v += lhs[k][r] * rhs[k]; + } + res[r] = v; + } + return res; +} + +template <typename TYPE, size_t C, size_t R> +mat<TYPE, C, R> PURE doMul( + const vec<TYPE, R>& lhs, + const mat<TYPE, C, 1>& rhs) +{ + mat<TYPE, C, R> res; + for (size_t c=0 ; c<C ; c++) { + for (size_t r=0 ; r<R ; r++) { + res[c][r] = lhs[r] * rhs[c][0]; + } + } + return res; +} + +template <typename TYPE, size_t C, size_t R> +mat<TYPE, C, R> PURE doMul( + const mat<TYPE, C, R>& rhs, + typename TypeTraits<TYPE>::ParameterType v) +{ + mat<TYPE, C, R> res; + for (size_t c=0 ; c<C ; c++) { + for (size_t r=0 ; r<R ; r++) { + res[c][r] = rhs[c][r] * v; + } + } + return res; +} + +template <typename TYPE, size_t C, size_t R> +mat<TYPE, C, R> PURE doMul( + typename TypeTraits<TYPE>::ParameterType v, + const mat<TYPE, C, R>& rhs) +{ + mat<TYPE, C, R> res; + for (size_t c=0 ; c<C ; c++) { + for (size_t r=0 ; r<R ; r++) { + res[c][r] = v * rhs[c][r]; + } + } + return res; +} + + +}; // namespace helpers + +// ----------------------------------------------------------------------- + +template <typename TYPE, size_t C, size_t R> +class mat : public vec< vec<TYPE, R>, C > { + typedef typename TypeTraits<TYPE>::ParameterType pTYPE; + typedef vec< vec<TYPE, R>, C > base; +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + size_type size() const { return R*C; } + enum { ROWS = R, COLS = C }; + + + // ----------------------------------------------------------------------- + // default constructors + + mat() { } + mat(const mat& rhs) : base(rhs) { } + mat(const base& rhs) : base(rhs) { } + + // ----------------------------------------------------------------------- + // conversion constructors + + // sets the diagonal to the value, off-diagonal to zero + mat(pTYPE rhs) { + helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // Assignment + + mat& operator=(const mat& rhs) { + base::operator=(rhs); + return *this; + } + + mat& operator=(const base& rhs) { + base::operator=(rhs); + return *this; + } + + mat& operator=(pTYPE rhs) { + return helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // non-member function declaration and definition + + friend inline mat PURE operator + (const mat& lhs, const mat& rhs) { + return helpers::doAdd( + static_cast<const base&>(lhs), + static_cast<const base&>(rhs)); + } + friend inline mat PURE operator - (const mat& lhs, const mat& rhs) { + return helpers::doSub( + static_cast<const base&>(lhs), + static_cast<const base&>(rhs)); + } + + // matrix*matrix + template <size_t D> + friend mat PURE operator * ( + const mat<TYPE, D, R>& lhs, + const mat<TYPE, C, D>& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*vector + friend vec<TYPE, R> PURE operator * ( + const mat& lhs, const vec<TYPE, C>& rhs) { + return helpers::doMul(lhs, rhs); + } + + // vector*matrix + friend mat PURE operator * ( + const vec<TYPE, R>& lhs, const mat<TYPE, C, 1>& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*scalar + friend inline mat PURE operator * (const mat& lhs, pTYPE v) { + return helpers::doMul(lhs, v); + } + + // scalar*matrix + friend inline mat PURE operator * (pTYPE v, const mat& rhs) { + return helpers::doMul(v, rhs); + } + + // ----------------------------------------------------------------------- + // streaming operator to set the columns of the matrix: + // example: + // mat33_t m; + // m << v0 << v1 << v2; + + // column_builder<> stores the matrix and knows which column to set + template<size_t PREV_COLUMN> + struct column_builder { + mat& matrix; + column_builder(mat& matrix) : matrix(matrix) { } + }; + + // operator << is not a method of column_builder<> so we can + // overload it for unauthorized values (partial specialization + // not allowed in class-scope). + // we just set the column and return the next column_builder<> + template<size_t PREV_COLUMN> + friend column_builder<PREV_COLUMN+1> operator << ( + const column_builder<PREV_COLUMN>& lhs, + const vec<TYPE, R>& rhs) { + lhs.matrix[PREV_COLUMN+1] = rhs; + return column_builder<PREV_COLUMN+1>(lhs.matrix); + } + + // we return void here so we get a compile-time error if the + // user tries to set too many columns + friend void operator << ( + const column_builder<C-2>& lhs, + const vec<TYPE, R>& rhs) { + lhs.matrix[C-1] = rhs; + } + + // this is where the process starts. we set the first columns and + // return the next column_builder<> + column_builder<0> operator << (const vec<TYPE, R>& rhs) { + (*this)[0] = rhs; + return column_builder<0>(*this); + } +}; + +// Specialize column matrix so they're exactly equivalent to a vector +template <typename TYPE, size_t R> +class mat<TYPE, 1, R> : public vec<TYPE, R> { + typedef vec<TYPE, R> base; +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + size_type size() const { return R; } + enum { ROWS = R, COLS = 1 }; + + mat() { } + mat(const base& rhs) : base(rhs) { } + mat(const mat& rhs) : base(rhs) { } + mat(const TYPE& rhs) { helpers::doAssign(*this, rhs); } + mat& operator=(const mat& rhs) { base::operator=(rhs); return *this; } + mat& operator=(const base& rhs) { base::operator=(rhs); return *this; } + mat& operator=(const TYPE& rhs) { return helpers::doAssign(*this, rhs); } + // we only have one column, so ignore the index + const base& operator[](size_t) const { return *this; } + base& operator[](size_t) { return *this; } + void operator << (const vec<TYPE, R>& rhs) { base::operator[](0) = rhs; } +}; + +// ----------------------------------------------------------------------- +// matrix functions + +// transpose. this handles matrices of matrices +inline int PURE transpose(int v) { return v; } +inline float PURE transpose(float v) { return v; } +inline double PURE transpose(double v) { return v; } + +// Transpose a matrix +template <typename TYPE, size_t C, size_t R> +mat<TYPE, R, C> PURE transpose(const mat<TYPE, C, R>& m) { + mat<TYPE, R, C> r; + for (size_t i=0 ; i<R ; i++) + for (size_t j=0 ; j<C ; j++) + r[i][j] = transpose(m[j][i]); + return r; +} + +// Transpose a vector +template < + template<typename T, size_t S> class VEC, + typename TYPE, + size_t SIZE +> +mat<TYPE, SIZE, 1> PURE transpose(const VEC<TYPE, SIZE>& v) { + mat<TYPE, SIZE, 1> r; + for (size_t i=0 ; i<SIZE ; i++) + r[i][0] = transpose(v[i]); + return r; +} + +// ----------------------------------------------------------------------- +// "dumb" matrix inversion +template<typename T, size_t N> +mat<T, N, N> PURE invert(const mat<T, N, N>& src) { + T t; + size_t swap; + mat<T, N, N> tmp(src); + mat<T, N, N> inverse(1); + + for (size_t i=0 ; i<N ; i++) { + // look for largest element in column + swap = i; + for (size_t j=i+1 ; j<N ; j++) { + if (fabs(tmp[j][i]) > fabs(tmp[i][i])) { + swap = j; + } + } + + if (swap != i) { + /* swap rows. */ + for (size_t k=0 ; k<N ; k++) { + t = tmp[i][k]; + tmp[i][k] = tmp[swap][k]; + tmp[swap][k] = t; + + t = inverse[i][k]; + inverse[i][k] = inverse[swap][k]; + inverse[swap][k] = t; + } + } + + t = 1 / tmp[i][i]; + for (size_t k=0 ; k<N ; k++) { + tmp[i][k] *= t; + inverse[i][k] *= t; + } + for (size_t j=0 ; j<N ; j++) { + if (j != i) { + t = tmp[j][i]; + for (size_t k=0 ; k<N ; k++) { + tmp[j][k] -= tmp[i][k] * t; + inverse[j][k] -= inverse[i][k] * t; + } + } + } + } + return inverse; +} + +// ----------------------------------------------------------------------- + +typedef mat<float, 2, 2> mat22_t; +typedef mat<float, 3, 3> mat33_t; +typedef mat<float, 4, 4> mat44_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_MAT_H */ diff --git a/services/sensorservice/traits.h b/services/sensorservice/traits.h new file mode 100644 index 0000000..da4c599 --- /dev/null +++ b/services/sensorservice/traits.h @@ -0,0 +1,118 @@ +/* + * 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_TRAITS_H +#define ANDROID_TRAITS_H + +// ----------------------------------------------------------------------- +// Typelists + +namespace android { + +// end-of-list marker +class NullType {}; + +// type-list node +template <typename T, typename U> +struct TypeList { + typedef T Head; + typedef U Tail; +}; + +// helpers to build typelists +#define TYPELIST_1(T1) TypeList<T1, NullType> +#define TYPELIST_2(T1, T2) TypeList<T1, TYPELIST_1(T2)> +#define TYPELIST_3(T1, T2, T3) TypeList<T1, TYPELIST_2(T2, T3)> +#define TYPELIST_4(T1, T2, T3, T4) TypeList<T1, TYPELIST_3(T2, T3, T4)> + +// typelists algorithms +namespace TL { +template <typename TList, typename T> struct IndexOf; + +template <typename T> +struct IndexOf<NullType, T> { + enum { value = -1 }; +}; + +template <typename T, typename Tail> +struct IndexOf<TypeList<T, Tail>, T> { + enum { value = 0 }; +}; + +template <typename Head, typename Tail, typename T> +struct IndexOf<TypeList<Head, Tail>, T> { +private: + enum { temp = IndexOf<Tail, T>::value }; +public: + enum { value = temp == -1 ? -1 : 1 + temp }; +}; + +}; // namespace TL + +// type selection based on a boolean +template <bool flag, typename T, typename U> +struct Select { + typedef T Result; +}; +template <typename T, typename U> +struct Select<false, T, U> { + typedef U Result; +}; + +// ----------------------------------------------------------------------- +// Type traits + +template <typename T> +class TypeTraits { + typedef TYPELIST_4( + unsigned char, unsigned short, + unsigned int, unsigned long int) UnsignedInts; + + typedef TYPELIST_4( + signed char, signed short, + signed int, signed long int) SignedInts; + + typedef TYPELIST_1( + bool) OtherInts; + + typedef TYPELIST_3( + float, double, long double) Floats; + + template<typename U> struct PointerTraits { + enum { result = false }; + typedef NullType PointeeType; + }; + template<typename U> struct PointerTraits<U*> { + enum { result = true }; + typedef U PointeeType; + }; + +public: + enum { isStdUnsignedInt = TL::IndexOf<UnsignedInts, T>::value >= 0 }; + enum { isStdSignedInt = TL::IndexOf<SignedInts, T>::value >= 0 }; + enum { isStdIntegral = TL::IndexOf<OtherInts, T>::value >= 0 || isStdUnsignedInt || isStdSignedInt }; + enum { isStdFloat = TL::IndexOf<Floats, T>::value >= 0 }; + enum { isPointer = PointerTraits<T>::result }; + enum { isStdArith = isStdIntegral || isStdFloat }; + + // best parameter type for given type + typedef typename Select<isStdArith || isPointer, T, const T&>::Result ParameterType; +}; + +// ----------------------------------------------------------------------- +}; // namespace android + +#endif /* ANDROID_TRAITS_H */ diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h new file mode 100644 index 0000000..736ff37 --- /dev/null +++ b/services/sensorservice/vec.h @@ -0,0 +1,420 @@ +/* + * 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_VEC_H +#define ANDROID_VEC_H + +#include <math.h> + +#include <stdint.h> +#include <stddef.h> + +#include "traits.h" + +// ----------------------------------------------------------------------- + +#define PURE __attribute__((pure)) + +namespace android { + +// ----------------------------------------------------------------------- +// non-inline helpers + +template <typename TYPE, size_t SIZE> +class vec; + +template <typename TYPE, size_t SIZE> +class vbase; + +namespace helpers { + +template <typename T> inline T min(T a, T b) { return a<b ? a : b; } +template <typename T> inline T max(T a, T b) { return a>b ? a : b; } + +template < template<typename T, size_t S> class VEC, + typename TYPE, size_t SIZE, size_t S> +vec<TYPE, SIZE>& doAssign( + vec<TYPE, SIZE>& lhs, const VEC<TYPE, S>& rhs) { + const size_t minSize = min(SIZE, S); + const size_t maxSize = max(SIZE, S); + for (size_t i=0 ; i<minSize ; i++) + lhs[i] = rhs[i]; + for (size_t i=minSize ; i<maxSize ; i++) + lhs[i] = 0; + return lhs; +} + + +template < + template<typename T, size_t S> class VLHS, + template<typename T, size_t S> class VRHS, + typename TYPE, + size_t SIZE +> +VLHS<TYPE, SIZE> PURE doAdd( + const VLHS<TYPE, SIZE>& lhs, + const VRHS<TYPE, SIZE>& rhs) { + VLHS<TYPE, SIZE> r; + for (size_t i=0 ; i<SIZE ; i++) + r[i] = lhs[i] + rhs[i]; + return r; +} + +template < + template<typename T, size_t S> class VLHS, + template<typename T, size_t S> class VRHS, + typename TYPE, + size_t SIZE +> +VLHS<TYPE, SIZE> PURE doSub( + const VLHS<TYPE, SIZE>& lhs, + const VRHS<TYPE, SIZE>& rhs) { + VLHS<TYPE, SIZE> r; + for (size_t i=0 ; i<SIZE ; i++) + r[i] = lhs[i] - rhs[i]; + return r; +} + +template < + template<typename T, size_t S> class VEC, + typename TYPE, + size_t SIZE +> +VEC<TYPE, SIZE> PURE doMulScalar( + const VEC<TYPE, SIZE>& lhs, + typename TypeTraits<TYPE>::ParameterType rhs) { + VEC<TYPE, SIZE> r; + for (size_t i=0 ; i<SIZE ; i++) + r[i] = lhs[i] * rhs; + return r; +} + +template < + template<typename T, size_t S> class VEC, + typename TYPE, + size_t SIZE +> +VEC<TYPE, SIZE> PURE doScalarMul( + typename TypeTraits<TYPE>::ParameterType lhs, + const VEC<TYPE, SIZE>& rhs) { + VEC<TYPE, SIZE> r; + for (size_t i=0 ; i<SIZE ; i++) + r[i] = lhs * rhs[i]; + return r; +} + +}; // namespace helpers + +// ----------------------------------------------------------------------- +// Below we define the mathematical operators for vectors. +// We use template template arguments so we can generically +// handle the case where the right-hand-size and left-hand-side are +// different vector types (but with same value_type and size). +// This is needed for performance when using ".xy{z}" element access +// on vec<>. Without this, an extra conversion to vec<> would be needed. +// +// example: +// vec4_t a; +// vec3_t b; +// vec3_t c = a.xyz + b; +// +// "a.xyz + b" is a mixed-operation between a vbase<> and a vec<>, requiring +// a conversion of vbase<> to vec<>. The template gunk below avoids this, +// by allowing the addition on these different vector types directly +// + +template < + template<typename T, size_t S> class VLHS, + template<typename T, size_t S> class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS<TYPE, SIZE> PURE operator + ( + const VLHS<TYPE, SIZE>& lhs, + const VRHS<TYPE, SIZE>& rhs) { + return helpers::doAdd(lhs, rhs); +} + +template < + template<typename T, size_t S> class VLHS, + template<typename T, size_t S> class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS<TYPE, SIZE> PURE operator - ( + const VLHS<TYPE, SIZE>& lhs, + const VRHS<TYPE, SIZE>& rhs) { + return helpers::doSub(lhs, rhs); +} + +template < + template<typename T, size_t S> class VEC, + typename TYPE, + size_t SIZE +> +inline VEC<TYPE, SIZE> PURE operator * ( + const VEC<TYPE, SIZE>& lhs, + typename TypeTraits<TYPE>::ParameterType rhs) { + return helpers::doMulScalar(lhs, rhs); +} + +template < + template<typename T, size_t S> class VEC, + typename TYPE, + size_t SIZE +> +inline VEC<TYPE, SIZE> PURE operator * ( + typename TypeTraits<TYPE>::ParameterType lhs, + const VEC<TYPE, SIZE>& rhs) { + return helpers::doScalarMul(lhs, rhs); +} + + +template < + template<typename T, size_t S> class VLHS, + template<typename T, size_t S> class VRHS, + typename TYPE, + size_t SIZE +> +TYPE PURE dot_product( + const VLHS<TYPE, SIZE>& lhs, + const VRHS<TYPE, SIZE>& rhs) { + TYPE r(0); + for (size_t i=0 ; i<SIZE ; i++) + r += lhs[i] * rhs[i]; + return r; +} + +template < + template<typename T, size_t S> class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length(const V<TYPE, SIZE>& v) { + return sqrt(dot_product(v, v)); +} + +template < + template<typename T, size_t S> class VLHS, + template<typename T, size_t S> class VRHS, + typename TYPE +> +VLHS<TYPE, 3> PURE cross_product( + const VLHS<TYPE, 3>& u, + const VRHS<TYPE, 3>& v) { + VLHS<TYPE, 3> r; + r.x = u.y*v.z - u.z*v.y; + r.y = u.z*v.x - u.x*v.z; + r.z = u.x*v.y - u.y*v.x; + return r; +} + + +template <typename TYPE, size_t SIZE> +vec<TYPE, SIZE> PURE operator - (const vec<TYPE, SIZE>& lhs) { + vec<TYPE, SIZE> r; + for (size_t i=0 ; i<SIZE ; i++) + r[i] = -lhs[i]; + return r; +} + +// ----------------------------------------------------------------------- + +// This our basic vector type, it just implements the data storage +// and accessors. + +template <typename TYPE, size_t SIZE> +struct vbase { + TYPE v[SIZE]; + inline const TYPE& operator[](size_t i) const { return v[i]; } + inline TYPE& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase<float, 2> { + union { + float v[2]; + struct { float x, y; }; + struct { float s, t; }; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase<float, 3> { + union { + float v[3]; + struct { float x, y, z; }; + struct { float s, t, r; }; + vbase<float, 2> xy; + vbase<float, 2> st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase<float, 4> { + union { + float v[4]; + struct { float x, y, z, w; }; + struct { float s, t, r, q; }; + vbase<float, 3> xyz; + vbase<float, 3> str; + vbase<float, 2> xy; + vbase<float, 2> st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; + +// ----------------------------------------------------------------------- + +template <typename TYPE, size_t SIZE> +class vec : public vbase<TYPE, SIZE> +{ + typedef typename TypeTraits<TYPE>::ParameterType pTYPE; + typedef vbase<TYPE, SIZE> base; + +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + + typedef TYPE* iterator; + typedef TYPE const* const_iterator; + iterator begin() { return base::v; } + iterator end() { return base::v + SIZE; } + const_iterator begin() const { return base::v; } + const_iterator end() const { return base::v + SIZE; } + size_type size() const { return SIZE; } + + // ----------------------------------------------------------------------- + // default constructors + + vec() { } + vec(const vec& rhs) : base(rhs) { } + vec(const base& rhs) : base(rhs) { } + + // ----------------------------------------------------------------------- + // conversion constructors + + vec(pTYPE rhs) { + for (size_t i=0 ; i<SIZE ; i++) + base::operator[](i) = rhs; + } + + template < template<typename T, size_t S> class VEC, size_t S> + explicit vec(const VEC<TYPE, S>& rhs) { + helpers::doAssign(*this, rhs); + } + + explicit vec(TYPE const* array) { + for (size_t i=0 ; i<SIZE ; i++) + base::operator[](i) = array[i]; + } + + // ----------------------------------------------------------------------- + // Assignment + + vec& operator = (const vec& rhs) { + base::operator=(rhs); + return *this; + } + + vec& operator = (const base& rhs) { + base::operator=(rhs); + return *this; + } + + vec& operator = (pTYPE rhs) { + for (size_t i=0 ; i<SIZE ; i++) + base::operator[](i) = rhs; + return *this; + } + + template < template<typename T, size_t S> class VEC, size_t S> + vec& operator = (const VEC<TYPE, S>& rhs) { + return helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // operation-assignment + + vec& operator += (const vec& rhs); + vec& operator -= (const vec& rhs); + vec& operator *= (pTYPE rhs); + + // ----------------------------------------------------------------------- + // non-member function declaration and definition + // NOTE: we declare the non-member function as friend inside the class + // so that they are known to the compiler when the class is instantiated. + // This helps the compiler doing template argument deduction when the + // passed types are not identical. Essentially this helps with + // type conversion so that you can multiply a vec<float> by an scalar int + // (for instance). + + friend inline vec PURE operator + (const vec& lhs, const vec& rhs) { + return helpers::doAdd(lhs, rhs); + } + friend inline vec PURE operator - (const vec& lhs, const vec& rhs) { + return helpers::doSub(lhs, rhs); + } + friend inline vec PURE operator * (const vec& lhs, pTYPE v) { + return helpers::doMulScalar(lhs, v); + } + friend inline vec PURE operator * (pTYPE v, const vec& rhs) { + return helpers::doScalarMul(v, rhs); + } + friend inline TYPE PURE dot_product(const vec& lhs, const vec& rhs) { + return android::dot_product(lhs, rhs); + } +}; + +// ----------------------------------------------------------------------- + +template <typename TYPE, size_t SIZE> +vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator += (const vec<TYPE, SIZE>& rhs) { + vec<TYPE, SIZE>& lhs(*this); + for (size_t i=0 ; i<SIZE ; i++) + lhs[i] += rhs[i]; + return lhs; +} + +template <typename TYPE, size_t SIZE> +vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator -= (const vec<TYPE, SIZE>& rhs) { + vec<TYPE, SIZE>& lhs(*this); + for (size_t i=0 ; i<SIZE ; i++) + lhs[i] -= rhs[i]; + return lhs; +} + +template <typename TYPE, size_t SIZE> +vec<TYPE, SIZE>& vec<TYPE, SIZE>::operator *= (vec<TYPE, SIZE>::pTYPE rhs) { + vec<TYPE, SIZE>& lhs(*this); + for (size_t i=0 ; i<SIZE ; i++) + lhs[i] *= rhs; + return lhs; +} + +// ----------------------------------------------------------------------- + +typedef vec<float, 2> vec2_t; +typedef vec<float, 3> vec3_t; +typedef vec<float, 4> vec4_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_VEC_H */ |