services/sensorservice/SensorFusion.cpp - platform/frameworks/native - Git at Google

 /*
  * 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
	/*
	* 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