ADHOC/replicant-frameworks_native
3
0
Fork 0
Code Issues 6 Pull Requests Releases Wiki Activity
eaf2d0bfe3
replicant-frameworks_native/services/sensorservice/SensorService.cpp
Mathias Agopian 010e422301 improve orientation sensor with gyro data 
when we do our own sensor fusion, we also export an
improved orientation sensor and hide the HAL sensor.
The fused orientation sensor is much more precise, fast
and smooth.

Change-Id: I0ea843b47ad9d12f6b22cce51f8629852d423126
2012-06-27 17:07:55 -07:00

620 lines
20 KiB
C++
Raw Blame History

/*
* 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 <cutils/properties.h>
#include <utils/SortedVector.h>
#include <utils/KeyedVector.h>
#include <utils/threads.h>
#include <utils/Atomic.h>
#include <utils/Errors.h>
#include <utils/RefBase.h>
#include <utils/Singleton.h>
#include <utils/String16.h>
#include <binder/BinderService.h>
#include <binder/IServiceManager.h>
#include <gui/ISensorServer.h>
#include <gui/ISensorEventConnection.h>
#include <hardware/sensors.h>
#include "CorrectedGyroSensor.h"
#include "GravitySensor.h"
#include "LinearAccelerationSensor.h"
#include "OrientationSensor.h"
#include "RotationVectorSensor.h"
#include "SensorFusion.h"
#include "SensorService.h"
namespace android {
// ---------------------------------------------------------------------------
/*
* Notes:
*
* - what about a gyro-corrected magnetic-field sensor?
* - run mag sensor from time to time to force calibration
* - gravity sensor length is wrong (=> drift in linear-acc sensor)
*
*/
SensorService::SensorService()
: mDump("android.permission.DUMP"),
mInitCheck(NO_INIT)
{
}
void SensorService::onFirstRef()
{
LOGD("nuSensorService starting...");
SensorDevice& dev(SensorDevice::getInstance());
if (dev.initCheck() == NO_ERROR) {
ssize_t orientationIndex = -1;
bool hasGyro = false;
uint32_t virtualSensorsNeeds =
(1<<SENSOR_TYPE_GRAVITY) |
(1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
(1<<SENSOR_TYPE_ROTATION_VECTOR);
sensor_t const* list;
int count = dev.getSensorList(&list);
mLastEventSeen.setCapacity(count);
for (int i=0 ; i<count ; i++) {
registerSensor( new HardwareSensor(list[i]) );
switch (list[i].type) {
case SENSOR_TYPE_ORIENTATION:
orientationIndex = i;
break;
case SENSOR_TYPE_GYROSCOPE:
hasGyro = true;
break;
case SENSOR_TYPE_GRAVITY:
case SENSOR_TYPE_LINEAR_ACCELERATION:
case SENSOR_TYPE_ROTATION_VECTOR:
virtualSensorsNeeds &= ~(1<<list[i].type);
break;
}
}
// it's safe to instantiate the SensorFusion object here
// (it wants to be instantiated after h/w sensors have been
// registered)
const SensorFusion& fusion(SensorFusion::getInstance());
if (hasGyro) {
// Always instantiate Android's virtual sensors. Since they are
// instantiated behind sensors from the HAL, they won't
// interfere with applications, unless they looks specifically
// for them (by name).
registerVirtualSensor( new RotationVectorSensor() );
registerVirtualSensor( new GravitySensor(list, count) );
registerVirtualSensor( new LinearAccelerationSensor(list, count) );
// these are optional
registerVirtualSensor( new OrientationSensor() );
registerVirtualSensor( new CorrectedGyroSensor(list, count) );
// virtual debugging sensors...
char value[PROPERTY_VALUE_MAX];
property_get("debug.sensors", value, "0");
if (atoi(value)) {
registerVirtualSensor( new GyroDriftSensor() );
}
}
// build the sensor list returned to users
mUserSensorList = mSensorList;
if (hasGyro &&
(virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR))) {
// if we have the fancy sensor fusion, and it's not provided by the
// HAL, use our own (fused) orientation sensor by removing the
// HAL supplied one form the user list.
if (orientationIndex >= 0) {
mUserSensorList.removeItemsAt(orientationIndex);
}
}
run("SensorService", PRIORITY_URGENT_DISPLAY);
mInitCheck = NO_ERROR;
}
}
void SensorService::registerSensor(SensorInterface* s)
{
sensors_event_t event;
memset(&event, 0, sizeof(event));
const Sensor sensor(s->getSensor());
// add to the sensor list (returned to clients)
mSensorList.add(sensor);
// add to our handle->SensorInterface mapping
mSensorMap.add(sensor.getHandle(), s);
// create an entry in the mLastEventSeen array
mLastEventSeen.add(sensor.getHandle(), event);
}
void SensorService::registerVirtualSensor(SensorInterface* s)
{
registerSensor(s);
mVirtualSensorList.add( s );
}
SensorService::~SensorService()
{
for (size_t i=0 ; i<mSensorMap.size() ; i++)
delete mSensorMap.valueAt(i);
}
status_t SensorService::dump(int fd, const Vector<String16>& args)
{
const size_t SIZE = 1024;
char buffer[SIZE];
String8 result;
if (!mDump.checkCalling()) {
snprintf(buffer, SIZE, "Permission Denial: "
"can't dump SurfaceFlinger from pid=%d, uid=%d\n",
IPCThreadState::self()->getCallingPid(),
IPCThreadState::self()->getCallingUid());
result.append(buffer);
} else {
Mutex::Autolock _l(mLock);
snprintf(buffer, SIZE, "Sensor List:\n");
result.append(buffer);
for (size_t i=0 ; i<mSensorList.size() ; i++) {
const Sensor& s(mSensorList[i]);
const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle()));
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(),
s.getMinDelay() ? (1000000.0f / s.getMinDelay()) : 0.0f,
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",
mActiveConnections.size());
result.append(buffer);
snprintf(buffer, SIZE, "Active sensors:\n");
result.append(buffer);
for (size_t i=0 ; i<mActiveSensors.size() ; i++) {
int handle = mActiveSensors.keyAt(i);
snprintf(buffer, SIZE, "%s (handle=0x%08x, connections=%d)\n",
getSensorName(handle).string(),
handle,
mActiveSensors.valueAt(i)->getNumConnections());
result.append(buffer);
}
}
write(fd, result.string(), result.size());
return NO_ERROR;
}
bool SensorService::threadLoop()
{
LOGD("nuSensorService thread starting...");
const size_t numEventMax = 16 * (1 + mVirtualSensorList.size());
sensors_event_t buffer[numEventMax];
sensors_event_t scratch[numEventMax];
SensorDevice& device(SensorDevice::getInstance());
const size_t vcount = mVirtualSensorList.size();
ssize_t count;
do {
count = device.poll(buffer, numEventMax);
if (count<0) {
LOGE("sensor poll failed (%s)", strerror(-count));
break;
}
recordLastValue(buffer, count);
// handle virtual sensors
if (count && vcount) {
sensors_event_t const * const event = buffer;
const DefaultKeyedVector<int, SensorInterface*> 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 ; i<size_t(count) ; i++) {
fusion.process(event[i]);
}
}
for (size_t i=0 ; i<size_t(count) ; i++) {
for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
sensors_event_t out;
if (virtualSensors.valueAt(j)->process(&out, event[i])) {
buffer[count + k] = out;
k++;
}
}
}
if (k) {
// record the last synthesized values
recordLastValue(&buffer[count], k);
count += k;
// sort the buffer by time-stamps
sortEventBuffer(buffer, count);
}
}
}
// send our events to clients...
const SortedVector< wp<SensorEventConnection> > activeConnections(
getActiveConnections());
size_t numConnections = activeConnections.size();
for (size_t i=0 ; i<numConnections ; i++) {
sp<SensorEventConnection> connection(
activeConnections[i].promote());
if (connection != 0) {
connection->sendEvents(buffer, count, scratch);
}
}
} while (count >= 0 || Thread::exitPending());
LOGW("Exiting SensorService::threadLoop!");
return false;
}
void SensorService::recordLastValue(
sensors_event_t const * buffer, size_t count)
{
Mutex::Autolock _l(mLock);
// record the last event for each sensor
int32_t prev = buffer[0].sensor;
for (size_t i=1 ; i<count ; i++) {
// record the last event of each sensor type in this buffer
int32_t curr = buffer[i].sensor;
if (curr != prev) {
mLastEventSeen.editValueFor(prev) = buffer[i-1];
prev = curr;
}
}
mLastEventSeen.editValueFor(prev) = buffer[count-1];
}
void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count)
{
struct compar {
static int cmp(void const* lhs, void const* rhs) {
sensors_event_t const* l = static_cast<sensors_event_t const*>(lhs);
sensors_event_t const* r = static_cast<sensors_event_t const*>(rhs);
return r->timestamp - l->timestamp;
}
};
qsort(buffer, count, sizeof(sensors_event_t), compar::cmp);
}
SortedVector< wp<SensorService::SensorEventConnection> >
SensorService::getActiveConnections() const
{
Mutex::Autolock _l(mLock);
return mActiveConnections;
}
DefaultKeyedVector<int, SensorInterface*>
SensorService::getActiveVirtualSensors() const
{
Mutex::Autolock _l(mLock);
return mActiveVirtualSensors;
}
String8 SensorService::getSensorName(int handle) const {
size_t count = mUserSensorList.size();
for (size_t i=0 ; i<count ; i++) {
const Sensor& sensor(mUserSensorList[i]);
if (sensor.getHandle() == handle) {
return sensor.getName();
}
}
String8 result("unknown");
return result;
}
Vector<Sensor> SensorService::getSensorList()
{
return mUserSensorList;
}
sp<ISensorEventConnection> SensorService::createSensorEventConnection()
{
sp<SensorEventConnection> result(new SensorEventConnection(this));
return result;
}
void SensorService::cleanupConnection(SensorEventConnection* c)
{
Mutex::Autolock _l(mLock);
const wp<SensorEventConnection> connection(c);
size_t size = mActiveSensors.size();
LOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size);
for (size_t i=0 ; i<size ; ) {
int handle = mActiveSensors.keyAt(i);
if (c->hasSensor(handle)) {
LOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle);
SensorInterface* sensor = mSensorMap.valueFor( handle );
LOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle);
if (sensor) {
sensor->activate(c, false);
}
}
SensorRecord* rec = mActiveSensors.valueAt(i);
LOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle);
LOGD_IF(DEBUG_CONNECTIONS,
"removing connection %p for sensor[%d].handle=0x%08x",
c, i, handle);
if (rec && rec->removeConnection(connection)) {
LOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection");
mActiveSensors.removeItemsAt(i, 1);
mActiveVirtualSensors.removeItem(handle);
delete rec;
size--;
} else {
i++;
}
}
mActiveConnections.remove(connection);
}
status_t SensorService::enable(const sp<SensorEventConnection>& connection,
int handle)
{
if (mInitCheck != NO_ERROR)
return mInitCheck;
Mutex::Autolock _l(mLock);
SensorInterface* sensor = mSensorMap.valueFor(handle);
status_t err = sensor ? sensor->activate(connection.get(), true) : status_t(BAD_VALUE);
if (err == NO_ERROR) {
SensorRecord* rec = mActiveSensors.valueFor(handle);
if (rec == 0) {
rec = new SensorRecord(connection);
mActiveSensors.add(handle, rec);
if (sensor->isVirtual()) {
mActiveVirtualSensors.add(handle, sensor);
}
} else {
if (rec->addConnection(connection)) {
// this sensor is already activated, but we are adding a
// connection that uses it. Immediately send down the last
// known value of the requested sensor if it's not a
// "continuous" sensor.
if (sensor->getSensor().getMinDelay() == 0) {
sensors_event_t scratch;
sensors_event_t& event(mLastEventSeen.editValueFor(handle));
if (event.version == sizeof(sensors_event_t)) {
connection->sendEvents(&event, 1);
}
}
}
}
if (err == NO_ERROR) {
// connection now active
if (connection->addSensor(handle)) {
// the sensor was added (which means it wasn't already there)
// so, see if this connection becomes active
if (mActiveConnections.indexOf(connection) < 0) {
mActiveConnections.add(connection);
}
}
}
}
return err;
}
status_t SensorService::disable(const sp<SensorEventConnection>& connection,
int handle)
{
if (mInitCheck != NO_ERROR)
return mInitCheck;
status_t err = NO_ERROR;
Mutex::Autolock _l(mLock);
SensorRecord* rec = mActiveSensors.valueFor(handle);
if (rec) {
// see if this connection becomes inactive
connection->removeSensor(handle);
if (connection->hasAnySensor() == false) {
mActiveConnections.remove(connection);
}
// see if this sensor becomes inactive
if (rec->removeConnection(connection)) {
mActiveSensors.removeItem(handle);
mActiveVirtualSensors.removeItem(handle);
delete rec;
}
SensorInterface* sensor = mSensorMap.valueFor(handle);
err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE);
}
return err;
}
status_t SensorService::setEventRate(const sp<SensorEventConnection>& connection,
int handle, nsecs_t ns)
{
if (mInitCheck != NO_ERROR)
return mInitCheck;
if (ns < 0)
return BAD_VALUE;
if (ns < MINIMUM_EVENTS_PERIOD)
ns = MINIMUM_EVENTS_PERIOD;
SensorInterface* sensor = mSensorMap.valueFor(handle);
if (!sensor) return BAD_VALUE;
return sensor->setDelay(connection.get(), handle, ns);
}
// ---------------------------------------------------------------------------
SensorService::SensorRecord::SensorRecord(
const sp<SensorEventConnection>& connection)
{
mConnections.add(connection);
}
bool SensorService::SensorRecord::addConnection(
const sp<SensorEventConnection>& connection)
{
if (mConnections.indexOf(connection) < 0) {
mConnections.add(connection);
return true;
}
return false;
}
bool SensorService::SensorRecord::removeConnection(
const wp<SensorEventConnection>& connection)
{
ssize_t index = mConnections.indexOf(connection);
if (index >= 0) {
mConnections.removeItemsAt(index, 1);
}
return mConnections.size() ? false : true;
}
// ---------------------------------------------------------------------------
SensorService::SensorEventConnection::SensorEventConnection(
const sp<SensorService>& service)
: mService(service), mChannel(new SensorChannel())
{
}
SensorService::SensorEventConnection::~SensorEventConnection()
{
LOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this);
mService->cleanupConnection(this);
}
void SensorService::SensorEventConnection::onFirstRef()
{
}
bool SensorService::SensorEventConnection::addSensor(int32_t handle) {
Mutex::Autolock _l(mConnectionLock);
if (mSensorInfo.indexOf(handle) <= 0) {
mSensorInfo.add(handle);
return true;
}
return false;
}
bool SensorService::SensorEventConnection::removeSensor(int32_t handle) {
Mutex::Autolock _l(mConnectionLock);
if (mSensorInfo.remove(handle) >= 0) {
return true;
}
return false;
}
bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const {
Mutex::Autolock _l(mConnectionLock);
return mSensorInfo.indexOf(handle) >= 0;
}
bool SensorService::SensorEventConnection::hasAnySensor() const {
Mutex::Autolock _l(mConnectionLock);
return mSensorInfo.size() ? true : false;
}
status_t SensorService::SensorEventConnection::sendEvents(
sensors_event_t const* buffer, size_t numEvents,
sensors_event_t* scratch)
{
// filter out events not for this connection
size_t count = 0;
if (scratch) {
Mutex::Autolock _l(mConnectionLock);
size_t i=0;
while (i<numEvents) {
const int32_t curr = buffer[i].sensor;
if (mSensorInfo.indexOf(curr) >= 0) {
do {
scratch[count++] = buffer[i++];
} while ((i<numEvents) && (buffer[i].sensor == curr));
} else {
i++;
}
}
} else {
scratch = const_cast<sensors_event_t *>(buffer);
count = numEvents;
}
if (count == 0)
return 0;
ssize_t size = mChannel->write(scratch, count*sizeof(sensors_event_t));
if (size == -EAGAIN) {
// the destination doesn't accept events anymore, it's probably
// full. For now, we just drop the events on the floor.
LOGW("dropping %d events on the floor", count);
return size;
}
LOGE_IF(size<0, "dropping %d events on the floor (%s)",
count, strerror(-size));
return size < 0 ? status_t(size) : status_t(NO_ERROR);
}
sp<SensorChannel> SensorService::SensorEventConnection::getSensorChannel() const
{
return mChannel;
}
status_t SensorService::SensorEventConnection::enableDisable(
int handle, bool enabled)
{
status_t err;
if (enabled) {
err = mService->enable(this, handle);
} else {
err = mService->disable(this, handle);
}
return err;
}
status_t SensorService::SensorEventConnection::setEventRate(
int handle, nsecs_t ns)
{
return mService->setEventRate(this, handle, ns);
}
// ---------------------------------------------------------------------------
}; // namespace android
Reference in New Issue View Git Blame Copy Permalink