You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
280 lines
9.7 KiB
280 lines
9.7 KiB
/*
|
|
* 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"
|
|
|
|
#include <android/util/ProtoOutputStream.h>
|
|
#include <frameworks/base/core/proto/android/service/sensor_service.proto.h>
|
|
|
|
namespace android {
|
|
// ---------------------------------------------------------------------------
|
|
|
|
ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion)
|
|
|
|
SensorFusion::SensorFusion()
|
|
: mSensorDevice(SensorDevice::getInstance()),
|
|
mAttitude(mAttitudes[FUSION_9AXIS]),
|
|
mGyroTime(0), mAccTime(0)
|
|
{
|
|
sensor_t const* list;
|
|
Sensor uncalibratedGyro;
|
|
ssize_t count = mSensorDevice.getSensorList(&list);
|
|
|
|
mEnabled[FUSION_9AXIS] = false;
|
|
mEnabled[FUSION_NOMAG] = false;
|
|
mEnabled[FUSION_NOGYRO] = false;
|
|
|
|
if (count > 0) {
|
|
for (size_t i=0 ; i<size_t(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);
|
|
}
|
|
if (list[i].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
|
|
uncalibratedGyro = Sensor(list + i);
|
|
}
|
|
}
|
|
|
|
// Use the uncalibrated gyroscope for sensor fusion when available
|
|
if (uncalibratedGyro.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
|
|
mGyro = uncalibratedGyro;
|
|
}
|
|
|
|
// 200 Hz for gyro events is a good compromise between precision
|
|
// and power/cpu usage.
|
|
mEstimatedGyroRate = 200;
|
|
mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;
|
|
|
|
for (int i = 0; i<NUM_FUSION_MODE; ++i) {
|
|
mFusions[i].init(i);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SensorFusion::process(const sensors_event_t& event) {
|
|
|
|
if (event.type == mGyro.getType()) {
|
|
float dT;
|
|
if ( event.timestamp - mGyroTime> 0 &&
|
|
event.timestamp - mGyroTime< (int64_t)(5e7) ) { //0.05sec
|
|
|
|
dT = (event.timestamp - mGyroTime) / 1000000000.0f;
|
|
// here we estimate the gyro rate (useful for debugging)
|
|
const float freq = 1 / dT;
|
|
if (freq >= 100 && freq<1000) { // filter values obviously wrong
|
|
const float alpha = 1 / (1 + dT); // 1s time-constant
|
|
mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;
|
|
}
|
|
|
|
const vec3_t gyro(event.data);
|
|
for (int i = 0; i<NUM_FUSION_MODE; ++i) {
|
|
if (mEnabled[i]) {
|
|
// fusion in no gyro mode will ignore
|
|
mFusions[i].handleGyro(gyro, dT);
|
|
}
|
|
}
|
|
}
|
|
mGyroTime = event.timestamp;
|
|
} else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
|
|
const vec3_t mag(event.data);
|
|
for (int i = 0; i<NUM_FUSION_MODE; ++i) {
|
|
if (mEnabled[i]) {
|
|
mFusions[i].handleMag(mag);// fusion in no mag mode will ignore
|
|
}
|
|
}
|
|
} else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
|
|
float dT;
|
|
if ( event.timestamp - mAccTime> 0 &&
|
|
event.timestamp - mAccTime< (int64_t)(1e8) ) { //0.1sec
|
|
dT = (event.timestamp - mAccTime) / 1000000000.0f;
|
|
|
|
const vec3_t acc(event.data);
|
|
for (int i = 0; i<NUM_FUSION_MODE; ++i) {
|
|
if (mEnabled[i]) {
|
|
mFusions[i].handleAcc(acc, dT);
|
|
mAttitudes[i] = mFusions[i].getAttitude();
|
|
}
|
|
}
|
|
}
|
|
mAccTime = event.timestamp;
|
|
}
|
|
}
|
|
|
|
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(int mode, void* ident, bool enabled) {
|
|
|
|
ALOGD_IF(DEBUG_CONNECTIONS,
|
|
"SensorFusion::activate(mode=%d, ident=%p, enabled=%d)",
|
|
mode, ident, enabled);
|
|
|
|
const ssize_t idx = mClients[mode].indexOf(ident);
|
|
if (enabled) {
|
|
if (idx < 0) {
|
|
mClients[mode].add(ident);
|
|
}
|
|
} else {
|
|
if (idx >= 0) {
|
|
mClients[mode].removeItemsAt(idx);
|
|
}
|
|
}
|
|
|
|
const bool newState = mClients[mode].size() != 0;
|
|
if (newState != mEnabled[mode]) {
|
|
mEnabled[mode] = newState;
|
|
if (newState) {
|
|
mFusions[mode].init(mode);
|
|
}
|
|
}
|
|
|
|
mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
|
|
if (mode != FUSION_NOMAG) {
|
|
mSensorDevice.activate(ident, mMag.getHandle(), enabled);
|
|
}
|
|
if (mode != FUSION_NOGYRO) {
|
|
mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
|
|
}
|
|
|
|
return NO_ERROR;
|
|
}
|
|
|
|
status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) {
|
|
// Call batch with timeout zero instead of setDelay().
|
|
if (ns > (int64_t)5e7) {
|
|
ns = (int64_t)(5e7);
|
|
}
|
|
mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
|
|
if (mode != FUSION_NOMAG) {
|
|
mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(10), 0);
|
|
}
|
|
if (mode != FUSION_NOGYRO) {
|
|
mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
|
|
}
|
|
return NO_ERROR;
|
|
}
|
|
|
|
|
|
float SensorFusion::getPowerUsage(int mode) const {
|
|
float power = mAcc.getPowerUsage() +
|
|
((mode != FUSION_NOMAG) ? mMag.getPowerUsage() : 0) +
|
|
((mode != FUSION_NOGYRO) ? mGyro.getPowerUsage() : 0);
|
|
return power;
|
|
}
|
|
|
|
int32_t SensorFusion::getMinDelay() const {
|
|
return mAcc.getMinDelay();
|
|
}
|
|
|
|
void SensorFusion::dump(String8& result) const {
|
|
const Fusion& fusion_9axis(mFusions[FUSION_9AXIS]);
|
|
result.appendFormat("9-axis fusion %s (%zd clients), gyro-rate=%7.2fHz, "
|
|
"q=< %g, %g, %g, %g > (%g), "
|
|
"b=< %g, %g, %g >\n",
|
|
mEnabled[FUSION_9AXIS] ? "enabled" : "disabled",
|
|
mClients[FUSION_9AXIS].size(),
|
|
mEstimatedGyroRate,
|
|
fusion_9axis.getAttitude().x,
|
|
fusion_9axis.getAttitude().y,
|
|
fusion_9axis.getAttitude().z,
|
|
fusion_9axis.getAttitude().w,
|
|
length(fusion_9axis.getAttitude()),
|
|
fusion_9axis.getBias().x,
|
|
fusion_9axis.getBias().y,
|
|
fusion_9axis.getBias().z);
|
|
|
|
const Fusion& fusion_nomag(mFusions[FUSION_NOMAG]);
|
|
result.appendFormat("game fusion(no mag) %s (%zd clients), "
|
|
"gyro-rate=%7.2fHz, "
|
|
"q=< %g, %g, %g, %g > (%g), "
|
|
"b=< %g, %g, %g >\n",
|
|
mEnabled[FUSION_NOMAG] ? "enabled" : "disabled",
|
|
mClients[FUSION_NOMAG].size(),
|
|
mEstimatedGyroRate,
|
|
fusion_nomag.getAttitude().x,
|
|
fusion_nomag.getAttitude().y,
|
|
fusion_nomag.getAttitude().z,
|
|
fusion_nomag.getAttitude().w,
|
|
length(fusion_nomag.getAttitude()),
|
|
fusion_nomag.getBias().x,
|
|
fusion_nomag.getBias().y,
|
|
fusion_nomag.getBias().z);
|
|
|
|
const Fusion& fusion_nogyro(mFusions[FUSION_NOGYRO]);
|
|
result.appendFormat("geomag fusion (no gyro) %s (%zd clients), "
|
|
"gyro-rate=%7.2fHz, "
|
|
"q=< %g, %g, %g, %g > (%g), "
|
|
"b=< %g, %g, %g >\n",
|
|
mEnabled[FUSION_NOGYRO] ? "enabled" : "disabled",
|
|
mClients[FUSION_NOGYRO].size(),
|
|
mEstimatedGyroRate,
|
|
fusion_nogyro.getAttitude().x,
|
|
fusion_nogyro.getAttitude().y,
|
|
fusion_nogyro.getAttitude().z,
|
|
fusion_nogyro.getAttitude().w,
|
|
length(fusion_nogyro.getAttitude()),
|
|
fusion_nogyro.getBias().x,
|
|
fusion_nogyro.getBias().y,
|
|
fusion_nogyro.getBias().z);
|
|
}
|
|
|
|
void SensorFusion::dumpFusion(FUSION_MODE mode, util::ProtoOutputStream* proto) const {
|
|
using namespace service::SensorFusionProto::FusionProto;
|
|
const Fusion& fusion(mFusions[mode]);
|
|
proto->write(ENABLED, mEnabled[mode]);
|
|
proto->write(NUM_CLIENTS, (int)mClients[mode].size());
|
|
proto->write(ESTIMATED_GYRO_RATE, mEstimatedGyroRate);
|
|
proto->write(ATTITUDE_X, fusion.getAttitude().x);
|
|
proto->write(ATTITUDE_Y, fusion.getAttitude().y);
|
|
proto->write(ATTITUDE_Z, fusion.getAttitude().z);
|
|
proto->write(ATTITUDE_W, fusion.getAttitude().w);
|
|
proto->write(ATTITUDE_LENGTH, length(fusion.getAttitude()));
|
|
proto->write(BIAS_X, fusion.getBias().x);
|
|
proto->write(BIAS_Y, fusion.getBias().y);
|
|
proto->write(BIAS_Z, fusion.getBias().z);
|
|
}
|
|
|
|
/**
|
|
* Dump debugging information as android.service.SensorFusionProto protobuf message using
|
|
* ProtoOutputStream.
|
|
*
|
|
* See proto definition and some notes about ProtoOutputStream in
|
|
* frameworks/base/core/proto/android/service/sensor_service.proto
|
|
*/
|
|
void SensorFusion::dump(util::ProtoOutputStream* proto) const {
|
|
uint64_t token = proto->start(service::SensorFusionProto::FUSION_9AXIS);
|
|
dumpFusion(FUSION_9AXIS, proto);
|
|
proto->end(token);
|
|
|
|
token = proto->start(service::SensorFusionProto::FUSION_NOMAG);
|
|
dumpFusion(FUSION_NOMAG, proto);
|
|
proto->end(token);
|
|
|
|
token = proto->start(service::SensorFusionProto::FUSION_NOGYRO);
|
|
dumpFusion(FUSION_NOGYRO, proto);
|
|
proto->end(token);
|
|
}
|
|
|
|
// ---------------------------------------------------------------------------
|
|
}; // namespace android
|