From 73e0bc805a143d8cc2202fccb73230459edc6869 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Tue, 17 May 2011 22:54:42 -0700 Subject: 9-axis sensor fusion with Kalman filter Add support for 9-axis gravity and linear-acceleration sensors virtual orientation sensor using 9-axis fusion Change-Id: I6717539373fce781c10e97b6fa59f68a831a592f --- include/gui/Sensor.h | 6 +- libs/gui/Sensor.cpp | 8 + services/sensorservice/Android.mk | 17 +- services/sensorservice/CorrectedGyroSensor.cpp | 86 ++++ services/sensorservice/CorrectedGyroSensor.h | 52 +++ services/sensorservice/Fusion.cpp | 431 +++++++++++++++++++++ services/sensorservice/Fusion.h | 86 ++++ services/sensorservice/GravitySensor.cpp | 75 ++-- services/sensorservice/GravitySensor.h | 7 +- .../sensorservice/LinearAccelerationSensor.cpp | 25 +- services/sensorservice/LinearAccelerationSensor.h | 6 +- services/sensorservice/OrientationSensor.cpp | 89 +++++ services/sensorservice/OrientationSensor.h | 51 +++ services/sensorservice/RotationVectorSensor.cpp | 137 ++----- services/sensorservice/RotationVectorSensor.h | 15 +- .../sensorservice/SecondOrderLowPassFilter.cpp | 28 +- services/sensorservice/SecondOrderLowPassFilter.h | 20 +- services/sensorservice/SensorDevice.cpp | 3 + services/sensorservice/SensorFusion.cpp | 180 +++++++++ services/sensorservice/SensorFusion.h | 84 ++++ services/sensorservice/SensorInterface.h | 2 - services/sensorservice/SensorService.cpp | 46 ++- services/sensorservice/mat.h | 370 ++++++++++++++++++ services/sensorservice/traits.h | 118 ++++++ services/sensorservice/vec.h | 420 ++++++++++++++++++++ 25 files changed, 2171 insertions(+), 191 deletions(-) create mode 100644 services/sensorservice/CorrectedGyroSensor.cpp create mode 100644 services/sensorservice/CorrectedGyroSensor.h create mode 100644 services/sensorservice/Fusion.cpp create mode 100644 services/sensorservice/Fusion.h create mode 100644 services/sensorservice/OrientationSensor.cpp create mode 100644 services/sensorservice/OrientationSensor.h create mode 100644 services/sensorservice/SensorFusion.cpp create mode 100644 services/sensorservice/SensorFusion.h create mode 100644 services/sensorservice/mat.h create mode 100644 services/sensorservice/traits.h create mode 100644 services/sensorservice/vec.h diff --git a/include/gui/Sensor.h b/include/gui/Sensor.h index 2de07b1..e59757a 100644 --- a/include/gui/Sensor.h +++ b/include/gui/Sensor.h @@ -21,8 +21,9 @@ #include #include -#include #include +#include +#include #include @@ -64,6 +65,8 @@ public: float getResolution() const; float getPowerUsage() const; int32_t getMinDelay() const; + nsecs_t getMinDelayNs() const; + int32_t getVersion() const; // Flattenable interface virtual size_t getFlattenedSize() const; @@ -83,6 +86,7 @@ private: float mResolution; float mPower; int32_t mMinDelay; + int32_t mVersion; }; // ---------------------------------------------------------------------------- diff --git a/libs/gui/Sensor.cpp b/libs/gui/Sensor.cpp index b1f37ff..f9a2c04 100644 --- a/libs/gui/Sensor.cpp +++ b/libs/gui/Sensor.cpp @@ -89,6 +89,14 @@ int32_t Sensor::getMinDelay() const { return mMinDelay; } +nsecs_t Sensor::getMinDelayNs() const { + return getMinDelay() * 1000; +} + +int32_t Sensor::getVersion() const { + return mVersion; +} + size_t Sensor::getFlattenedSize() const { return sizeof(int32_t) + ((mName.length() + 3) & ~3) + 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 +#include +#include + +#include + +#include + +#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 ; idata[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 +#include + +#include + +#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 + +#include + +#include "Fusion.h" + +namespace android { + +// ----------------------------------------------------------------------- + +template +static inline TYPE sqr(TYPE x) { + return x*x; +} + +template +static inline T clamp(T v) { + return v < 0 ? 0 : v; +} + +template +static mat scaleCovariance( + const mat& A, + const mat& P) { + // A*P*transpose(A); + mat APAt; + for (size_t r=0 ; r +static mat crossMatrix(const vec& p, OTHER_TYPE diag) { + mat 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 +static mat MRPsToMatrix(const vec& p) { + mat res(1); + const mat 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 +vec matrixToMRPs(const mat& R) { + // matrix to MRPs + vec 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 +class Covariance { + mat mSumXX; + vec mSumX; + size_t mN; +public: + Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { } + void update(const vec& x) { + mSumXX += x*transpose(x); + mSumX += x; + mN++; + } + mat 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 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 + +#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 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 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 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 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 #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 ; idata[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 -#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 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 #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 -#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 +#include +#include + +#include + +#include + +#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 +#include + +#include + +#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 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 #include "SecondOrderLowPassFilter.h" +#include "vec.h" // --------------------------------------------------------------------------- @@ -44,21 +45,24 @@ void SecondOrderLowPassFilter::setSamplingPeriod(float dT) // --------------------------------------------------------------------------- -BiquadFilter::BiquadFilter(const SecondOrderLowPassFilter& s) +template +BiquadFilter::BiquadFilter(const SecondOrderLowPassFilter& s) : s(s) { } -float BiquadFilter::init(float x) +template +T BiquadFilter::init(const T& x) { x1 = x2 = x; y1 = y2 = x; return x; } -float BiquadFilter::operator()(float x) +template +T BiquadFilter::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 +CascadedBiquadFilter::CascadedBiquadFilter(const SecondOrderLowPassFilter& s) : mA(s), mB(s) { } -float CascadedBiquadFilter::init(float x) +template +T CascadedBiquadFilter::init(const T& x) { mA.init(x); mB.init(x); return x; } -float CascadedBiquadFilter::operator()(float x) +template +T CascadedBiquadFilter::operator()(const T& x) { return mB(mA(x)); } // --------------------------------------------------------------------------- + +template class BiquadFilter; +template class CascadedBiquadFilter; +template class BiquadFilter; +template class CascadedBiquadFilter; + +// --------------------------------------------------------------------------- }; // 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 class BiquadFilter; /* * State of a 2nd order low-pass IIR filter */ class SecondOrderLowPassFilter { + template friend class BiquadFilter; float iQ, fc; float K, iD; @@ -44,27 +46,29 @@ public: /* * Implements a Biquad IIR filter */ +template 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 class CascadedBiquadFilter { - BiquadFilter mA; - BiquadFilter mB; + BiquadFilter mA; + BiquadFilter 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 ; i5 && 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 inline T min(T a, T b) { return a 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 +#include + +#include +#include +#include + +#include + +#include "Fusion.h" +#include "SecondOrderLowPassFilter.h" + +// --------------------------------------------------------------------------- + +namespace android { +// --------------------------------------------------------------------------- + +class SensorDevice; + +class SensorFusion : public Singleton { + friend class Singleton; + + 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 mAccData; + vec3_t mFilteredMag; + vec3_t mFilteredAcc; + SortedVector 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 #include -#include - #include #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 -#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<& args) for (size_t i=0 ; i\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& 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 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 ; iprocess(&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 +class mat; + +namespace helpers { + +template +mat& doAssign( + mat& lhs, + typename TypeTraits::ParameterType rhs) { + for (size_t i=0 ; i +mat PURE doMul( + const mat& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +vec PURE doMul( + const mat& lhs, + const vec& rhs) +{ + vec res; + for (size_t r=0 ; r +mat PURE doMul( + const vec& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + const mat& rhs, + typename TypeTraits::ParameterType v) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + typename TypeTraits::ParameterType v, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +class mat : public vec< vec, C > { + typedef typename TypeTraits::ParameterType pTYPE; + typedef vec< vec, 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(lhs), + static_cast(rhs)); + } + friend inline mat PURE operator - (const mat& lhs, const mat& rhs) { + return helpers::doSub( + static_cast(lhs), + static_cast(rhs)); + } + + // matrix*matrix + template + friend mat PURE operator * ( + const mat& lhs, + const mat& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*vector + friend vec PURE operator * ( + const mat& lhs, const vec& rhs) { + return helpers::doMul(lhs, rhs); + } + + // vector*matrix + friend mat PURE operator * ( + const vec& lhs, const mat& 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 + 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 + friend column_builder operator << ( + const column_builder& lhs, + const vec& rhs) { + lhs.matrix[PREV_COLUMN+1] = rhs; + return column_builder(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& lhs, + const vec& 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& rhs) { + (*this)[0] = rhs; + return column_builder<0>(*this); + } +}; + +// Specialize column matrix so they're exactly equivalent to a vector +template +class mat : public vec { + typedef vec 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& 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 +mat PURE transpose(const mat& m) { + mat r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +mat PURE transpose(const VEC& v) { + mat r; + for (size_t i=0 ; i +mat PURE invert(const mat& src) { + T t; + size_t swap; + mat tmp(src); + mat inverse(1); + + for (size_t i=0 ; i fabs(tmp[i][i])) { + swap = j; + } + } + + if (swap != i) { + /* swap rows. */ + for (size_t k=0 ; k mat22_t; +typedef mat mat33_t; +typedef mat 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 +struct TypeList { + typedef T Head; + typedef U Tail; +}; + +// helpers to build typelists +#define TYPELIST_1(T1) TypeList +#define TYPELIST_2(T1, T2) TypeList +#define TYPELIST_3(T1, T2, T3) TypeList +#define TYPELIST_4(T1, T2, T3, T4) TypeList + +// typelists algorithms +namespace TL { +template struct IndexOf; + +template +struct IndexOf { + enum { value = -1 }; +}; + +template +struct IndexOf, T> { + enum { value = 0 }; +}; + +template +struct IndexOf, T> { +private: + enum { temp = IndexOf::value }; +public: + enum { value = temp == -1 ? -1 : 1 + temp }; +}; + +}; // namespace TL + +// type selection based on a boolean +template +struct Select { + typedef T Result; +}; +template +struct Select { + typedef U Result; +}; + +// ----------------------------------------------------------------------- +// Type traits + +template +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 struct PointerTraits { + enum { result = false }; + typedef NullType PointeeType; + }; + template struct PointerTraits { + enum { result = true }; + typedef U PointeeType; + }; + +public: + enum { isStdUnsignedInt = TL::IndexOf::value >= 0 }; + enum { isStdSignedInt = TL::IndexOf::value >= 0 }; + enum { isStdIntegral = TL::IndexOf::value >= 0 || isStdUnsignedInt || isStdSignedInt }; + enum { isStdFloat = TL::IndexOf::value >= 0 }; + enum { isPointer = PointerTraits::result }; + enum { isStdArith = isStdIntegral || isStdFloat }; + + // best parameter type for given type + typedef typename Select::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 + +#include +#include + +#include "traits.h" + +// ----------------------------------------------------------------------- + +#define PURE __attribute__((pure)) + +namespace android { + +// ----------------------------------------------------------------------- +// non-inline helpers + +template +class vec; + +template +class vbase; + +namespace helpers { + +template inline T min(T a, T b) { return a inline T max(T a, T b) { return a>b ? a : b; } + +template < template class VEC, + typename TYPE, size_t SIZE, size_t S> +vec& doAssign( + vec& lhs, const VEC& rhs) { + const size_t minSize = min(SIZE, S); + const size_t maxSize = max(SIZE, S); + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doAdd( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doSub( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doMulScalar( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + VEC r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doScalarMul( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + VEC r; + for (size_t i=0 ; i. 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 class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator + ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doAdd(lhs, rhs); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator - ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doSub(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + return helpers::doMulScalar(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + return helpers::doScalarMul(lhs, rhs); +} + + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +TYPE PURE dot_product( + const VLHS& lhs, + const VRHS& rhs) { + TYPE r(0); + for (size_t i=0 ; i class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length(const V& v) { + return sqrt(dot_product(v, v)); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE +> +VLHS PURE cross_product( + const VLHS& u, + const VRHS& v) { + VLHS 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 +vec PURE operator - (const vec& lhs) { + vec r; + for (size_t i=0 ; i +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 { + 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 { + union { + float v[3]; + struct { float x, y, z; }; + struct { float s, t, r; }; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[4]; + struct { float x, y, z, w; }; + struct { float s, t, r, q; }; + vbase xyz; + vbase str; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; + +// ----------------------------------------------------------------------- + +template +class vec : public vbase +{ + typedef typename TypeTraits::ParameterType pTYPE; + typedef vbase 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 class VEC, size_t S> + explicit vec(const VEC& rhs) { + helpers::doAssign(*this, rhs); + } + + explicit vec(TYPE const* array) { + for (size_t i=0 ; i class VEC, size_t S> + vec& operator = (const VEC& 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 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 +vec& vec::operator += (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator -= (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator *= (vec::pTYPE rhs) { + vec& lhs(*this); + for (size_t i=0 ; i vec2_t; +typedef vec vec3_t; +typedef vec vec4_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_VEC_H */ -- cgit v1.1