ddf1ceb647
- upadte documentation for rotation vector - update method dealing with rotation vector to deal with 4 components - virtual rotation-vector sensor reports all four components - improve SensorManager documentation layout Whent he 4-th component of the rotation-vector is present, we can save a square-root when computing the quaternion or rotation matrix from it. Change-Id: Ia84d278dd5f0909fab1c5ba050f8df2679e2c7c8
178 lines
5.7 KiB
C++
178 lines
5.7 KiB
C++
/*
|
|
* Copyright (C) 2010 The Android Open Source Project
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#include <stdint.h>
|
|
#include <math.h>
|
|
#include <sys/types.h>
|
|
|
|
#include <utils/Errors.h>
|
|
|
|
#include <hardware/sensors.h>
|
|
|
|
#include "RotationVectorSensor.h"
|
|
|
|
namespace android {
|
|
// ---------------------------------------------------------------------------
|
|
|
|
template <typename T>
|
|
static inline T clamp(T v) {
|
|
return v < 0 ? 0 : v;
|
|
}
|
|
|
|
RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count)
|
|
: mSensorDevice(SensorDevice::getInstance()),
|
|
mEnabled(false),
|
|
mALowPass(M_SQRT1_2, 5.0f),
|
|
mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass),
|
|
mMLowPass(M_SQRT1_2, 2.5f),
|
|
mMX(mMLowPass), mMY(mMLowPass), mMZ(mMLowPass)
|
|
{
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
|
|
bool RotationVectorSensor::isEnabled() const {
|
|
return mEnabled;
|
|
}
|
|
|
|
status_t RotationVectorSensor::activate(void* ident, bool enabled) {
|
|
if (mEnabled != enabled) {
|
|
mSensorDevice.activate(this, mAcc.getHandle(), enabled);
|
|
mSensorDevice.activate(this, mMag.getHandle(), enabled);
|
|
mEnabled = enabled;
|
|
if (enabled) {
|
|
mMagTime = 0;
|
|
mAccTime = 0;
|
|
}
|
|
}
|
|
return NO_ERROR;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
Sensor RotationVectorSensor::getSensor() const {
|
|
sensor_t hwSensor;
|
|
hwSensor.name = "Rotation Vector Sensor";
|
|
hwSensor.vendor = "Google Inc.";
|
|
hwSensor.version = 1;
|
|
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();
|
|
Sensor sensor(&hwSensor);
|
|
return sensor;
|
|
}
|
|
|
|
// ---------------------------------------------------------------------------
|
|
}; // namespace android
|
|
|