diff options
author | Mathias Agopian <mathias@google.com> | 2011-05-17 22:54:42 -0700 |
---|---|---|
committer | Mathias Agopian <mathias@google.com> | 2011-05-27 17:04:55 -0700 |
commit | 73e0bc805a143d8cc2202fccb73230459edc6869 (patch) | |
tree | 59aeea0bcd592df160cf08606fa03cd5fa0a4211 /services/sensorservice/RotationVectorSensor.cpp | |
parent | 48a542aabe2d58a0c3e7bac7f384d9951e5a26ed (diff) | |
download | frameworks_base-73e0bc805a143d8cc2202fccb73230459edc6869.zip frameworks_base-73e0bc805a143d8cc2202fccb73230459edc6869.tar.gz frameworks_base-73e0bc805a143d8cc2202fccb73230459edc6869.tar.bz2 |
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
Diffstat (limited to 'services/sensorservice/RotationVectorSensor.cpp')
-rw-r--r-- | services/sensorservice/RotationVectorSensor.cpp | 137 |
1 files changed, 35 insertions, 102 deletions
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; } |