SensorService.cpp revision 010e42230135815907e76e5d7e5f30edf9e1799d
1/* 2 * Copyright (C) 2010 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17#include <stdint.h> 18#include <math.h> 19#include <sys/types.h> 20 21#include <cutils/properties.h> 22 23#include <utils/SortedVector.h> 24#include <utils/KeyedVector.h> 25#include <utils/threads.h> 26#include <utils/Atomic.h> 27#include <utils/Errors.h> 28#include <utils/RefBase.h> 29#include <utils/Singleton.h> 30#include <utils/String16.h> 31 32#include <binder/BinderService.h> 33#include <binder/IServiceManager.h> 34 35#include <gui/ISensorServer.h> 36#include <gui/ISensorEventConnection.h> 37 38#include <hardware/sensors.h> 39 40#include "CorrectedGyroSensor.h" 41#include "GravitySensor.h" 42#include "LinearAccelerationSensor.h" 43#include "OrientationSensor.h" 44#include "RotationVectorSensor.h" 45#include "SensorFusion.h" 46#include "SensorService.h" 47 48namespace android { 49// --------------------------------------------------------------------------- 50 51/* 52 * Notes: 53 * 54 * - what about a gyro-corrected magnetic-field sensor? 55 * - run mag sensor from time to time to force calibration 56 * - gravity sensor length is wrong (=> drift in linear-acc sensor) 57 * 58 */ 59 60SensorService::SensorService() 61 : mDump("android.permission.DUMP"), 62 mInitCheck(NO_INIT) 63{ 64} 65 66void SensorService::onFirstRef() 67{ 68 LOGD("nuSensorService starting..."); 69 70 SensorDevice& dev(SensorDevice::getInstance()); 71 72 if (dev.initCheck() == NO_ERROR) { 73 ssize_t orientationIndex = -1; 74 bool hasGyro = false; 75 uint32_t virtualSensorsNeeds = 76 (1<<SENSOR_TYPE_GRAVITY) | 77 (1<<SENSOR_TYPE_LINEAR_ACCELERATION) | 78 (1<<SENSOR_TYPE_ROTATION_VECTOR); 79 sensor_t const* list; 80 int count = dev.getSensorList(&list); 81 mLastEventSeen.setCapacity(count); 82 for (int i=0 ; i<count ; i++) { 83 registerSensor( new HardwareSensor(list[i]) ); 84 switch (list[i].type) { 85 case SENSOR_TYPE_ORIENTATION: 86 orientationIndex = i; 87 break; 88 case SENSOR_TYPE_GYROSCOPE: 89 hasGyro = true; 90 break; 91 case SENSOR_TYPE_GRAVITY: 92 case SENSOR_TYPE_LINEAR_ACCELERATION: 93 case SENSOR_TYPE_ROTATION_VECTOR: 94 virtualSensorsNeeds &= ~(1<<list[i].type); 95 break; 96 } 97 } 98 99 // it's safe to instantiate the SensorFusion object here 100 // (it wants to be instantiated after h/w sensors have been 101 // registered) 102 const SensorFusion& fusion(SensorFusion::getInstance()); 103 104 if (hasGyro) { 105 // Always instantiate Android's virtual sensors. Since they are 106 // instantiated behind sensors from the HAL, they won't 107 // interfere with applications, unless they looks specifically 108 // for them (by name). 109 110 registerVirtualSensor( new RotationVectorSensor() ); 111 registerVirtualSensor( new GravitySensor(list, count) ); 112 registerVirtualSensor( new LinearAccelerationSensor(list, count) ); 113 114 // these are optional 115 registerVirtualSensor( new OrientationSensor() ); 116 registerVirtualSensor( new CorrectedGyroSensor(list, count) ); 117 118 // virtual debugging sensors... 119 char value[PROPERTY_VALUE_MAX]; 120 property_get("debug.sensors", value, "0"); 121 if (atoi(value)) { 122 registerVirtualSensor( new GyroDriftSensor() ); 123 } 124 } 125 126 // build the sensor list returned to users 127 mUserSensorList = mSensorList; 128 if (hasGyro && 129 (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR))) { 130 // if we have the fancy sensor fusion, and it's not provided by the 131 // HAL, use our own (fused) orientation sensor by removing the 132 // HAL supplied one form the user list. 133 if (orientationIndex >= 0) { 134 mUserSensorList.removeItemsAt(orientationIndex); 135 } 136 } 137 138 run("SensorService", PRIORITY_URGENT_DISPLAY); 139 mInitCheck = NO_ERROR; 140 } 141} 142 143void SensorService::registerSensor(SensorInterface* s) 144{ 145 sensors_event_t event; 146 memset(&event, 0, sizeof(event)); 147 148 const Sensor sensor(s->getSensor()); 149 // add to the sensor list (returned to clients) 150 mSensorList.add(sensor); 151 // add to our handle->SensorInterface mapping 152 mSensorMap.add(sensor.getHandle(), s); 153 // create an entry in the mLastEventSeen array 154 mLastEventSeen.add(sensor.getHandle(), event); 155} 156 157void SensorService::registerVirtualSensor(SensorInterface* s) 158{ 159 registerSensor(s); 160 mVirtualSensorList.add( s ); 161} 162 163SensorService::~SensorService() 164{ 165 for (size_t i=0 ; i<mSensorMap.size() ; i++) 166 delete mSensorMap.valueAt(i); 167} 168 169status_t SensorService::dump(int fd, const Vector<String16>& args) 170{ 171 const size_t SIZE = 1024; 172 char buffer[SIZE]; 173 String8 result; 174 if (!mDump.checkCalling()) { 175 snprintf(buffer, SIZE, "Permission Denial: " 176 "can't dump SurfaceFlinger from pid=%d, uid=%d\n", 177 IPCThreadState::self()->getCallingPid(), 178 IPCThreadState::self()->getCallingUid()); 179 result.append(buffer); 180 } else { 181 Mutex::Autolock _l(mLock); 182 snprintf(buffer, SIZE, "Sensor List:\n"); 183 result.append(buffer); 184 for (size_t i=0 ; i<mSensorList.size() ; i++) { 185 const Sensor& s(mSensorList[i]); 186 const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle())); 187 snprintf(buffer, SIZE, 188 "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | " 189 "last=<%5.1f,%5.1f,%5.1f>\n", 190 s.getName().string(), 191 s.getVendor().string(), 192 s.getHandle(), 193 s.getMinDelay() ? (1000000.0f / s.getMinDelay()) : 0.0f, 194 e.data[0], e.data[1], e.data[2]); 195 result.append(buffer); 196 } 197 SensorFusion::getInstance().dump(result, buffer, SIZE); 198 SensorDevice::getInstance().dump(result, buffer, SIZE); 199 200 snprintf(buffer, SIZE, "%d active connections\n", 201 mActiveConnections.size()); 202 result.append(buffer); 203 snprintf(buffer, SIZE, "Active sensors:\n"); 204 result.append(buffer); 205 for (size_t i=0 ; i<mActiveSensors.size() ; i++) { 206 int handle = mActiveSensors.keyAt(i); 207 snprintf(buffer, SIZE, "%s (handle=0x%08x, connections=%d)\n", 208 getSensorName(handle).string(), 209 handle, 210 mActiveSensors.valueAt(i)->getNumConnections()); 211 result.append(buffer); 212 } 213 } 214 write(fd, result.string(), result.size()); 215 return NO_ERROR; 216} 217 218bool SensorService::threadLoop() 219{ 220 LOGD("nuSensorService thread starting..."); 221 222 const size_t numEventMax = 16 * (1 + mVirtualSensorList.size()); 223 sensors_event_t buffer[numEventMax]; 224 sensors_event_t scratch[numEventMax]; 225 SensorDevice& device(SensorDevice::getInstance()); 226 const size_t vcount = mVirtualSensorList.size(); 227 228 ssize_t count; 229 do { 230 count = device.poll(buffer, numEventMax); 231 if (count<0) { 232 LOGE("sensor poll failed (%s)", strerror(-count)); 233 break; 234 } 235 236 recordLastValue(buffer, count); 237 238 // handle virtual sensors 239 if (count && vcount) { 240 sensors_event_t const * const event = buffer; 241 const DefaultKeyedVector<int, SensorInterface*> virtualSensors( 242 getActiveVirtualSensors()); 243 const size_t activeVirtualSensorCount = virtualSensors.size(); 244 if (activeVirtualSensorCount) { 245 size_t k = 0; 246 SensorFusion& fusion(SensorFusion::getInstance()); 247 if (fusion.isEnabled()) { 248 for (size_t i=0 ; i<size_t(count) ; i++) { 249 fusion.process(event[i]); 250 } 251 } 252 for (size_t i=0 ; i<size_t(count) ; i++) { 253 for (size_t j=0 ; j<activeVirtualSensorCount ; j++) { 254 sensors_event_t out; 255 if (virtualSensors.valueAt(j)->process(&out, event[i])) { 256 buffer[count + k] = out; 257 k++; 258 } 259 } 260 } 261 if (k) { 262 // record the last synthesized values 263 recordLastValue(&buffer[count], k); 264 count += k; 265 // sort the buffer by time-stamps 266 sortEventBuffer(buffer, count); 267 } 268 } 269 } 270 271 // send our events to clients... 272 const SortedVector< wp<SensorEventConnection> > activeConnections( 273 getActiveConnections()); 274 size_t numConnections = activeConnections.size(); 275 for (size_t i=0 ; i<numConnections ; i++) { 276 sp<SensorEventConnection> connection( 277 activeConnections[i].promote()); 278 if (connection != 0) { 279 connection->sendEvents(buffer, count, scratch); 280 } 281 } 282 } while (count >= 0 || Thread::exitPending()); 283 284 LOGW("Exiting SensorService::threadLoop!"); 285 return false; 286} 287 288void SensorService::recordLastValue( 289 sensors_event_t const * buffer, size_t count) 290{ 291 Mutex::Autolock _l(mLock); 292 293 // record the last event for each sensor 294 int32_t prev = buffer[0].sensor; 295 for (size_t i=1 ; i<count ; i++) { 296 // record the last event of each sensor type in this buffer 297 int32_t curr = buffer[i].sensor; 298 if (curr != prev) { 299 mLastEventSeen.editValueFor(prev) = buffer[i-1]; 300 prev = curr; 301 } 302 } 303 mLastEventSeen.editValueFor(prev) = buffer[count-1]; 304} 305 306void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count) 307{ 308 struct compar { 309 static int cmp(void const* lhs, void const* rhs) { 310 sensors_event_t const* l = static_cast<sensors_event_t const*>(lhs); 311 sensors_event_t const* r = static_cast<sensors_event_t const*>(rhs); 312 return r->timestamp - l->timestamp; 313 } 314 }; 315 qsort(buffer, count, sizeof(sensors_event_t), compar::cmp); 316} 317 318SortedVector< wp<SensorService::SensorEventConnection> > 319SensorService::getActiveConnections() const 320{ 321 Mutex::Autolock _l(mLock); 322 return mActiveConnections; 323} 324 325DefaultKeyedVector<int, SensorInterface*> 326SensorService::getActiveVirtualSensors() const 327{ 328 Mutex::Autolock _l(mLock); 329 return mActiveVirtualSensors; 330} 331 332String8 SensorService::getSensorName(int handle) const { 333 size_t count = mUserSensorList.size(); 334 for (size_t i=0 ; i<count ; i++) { 335 const Sensor& sensor(mUserSensorList[i]); 336 if (sensor.getHandle() == handle) { 337 return sensor.getName(); 338 } 339 } 340 String8 result("unknown"); 341 return result; 342} 343 344Vector<Sensor> SensorService::getSensorList() 345{ 346 return mUserSensorList; 347} 348 349sp<ISensorEventConnection> SensorService::createSensorEventConnection() 350{ 351 sp<SensorEventConnection> result(new SensorEventConnection(this)); 352 return result; 353} 354 355void SensorService::cleanupConnection(SensorEventConnection* c) 356{ 357 Mutex::Autolock _l(mLock); 358 const wp<SensorEventConnection> connection(c); 359 size_t size = mActiveSensors.size(); 360 LOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size); 361 for (size_t i=0 ; i<size ; ) { 362 int handle = mActiveSensors.keyAt(i); 363 if (c->hasSensor(handle)) { 364 LOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle); 365 SensorInterface* sensor = mSensorMap.valueFor( handle ); 366 LOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle); 367 if (sensor) { 368 sensor->activate(c, false); 369 } 370 } 371 SensorRecord* rec = mActiveSensors.valueAt(i); 372 LOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle); 373 LOGD_IF(DEBUG_CONNECTIONS, 374 "removing connection %p for sensor[%d].handle=0x%08x", 375 c, i, handle); 376 377 if (rec && rec->removeConnection(connection)) { 378 LOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection"); 379 mActiveSensors.removeItemsAt(i, 1); 380 mActiveVirtualSensors.removeItem(handle); 381 delete rec; 382 size--; 383 } else { 384 i++; 385 } 386 } 387 mActiveConnections.remove(connection); 388} 389 390status_t SensorService::enable(const sp<SensorEventConnection>& connection, 391 int handle) 392{ 393 if (mInitCheck != NO_ERROR) 394 return mInitCheck; 395 396 Mutex::Autolock _l(mLock); 397 SensorInterface* sensor = mSensorMap.valueFor(handle); 398 status_t err = sensor ? sensor->activate(connection.get(), true) : status_t(BAD_VALUE); 399 if (err == NO_ERROR) { 400 SensorRecord* rec = mActiveSensors.valueFor(handle); 401 if (rec == 0) { 402 rec = new SensorRecord(connection); 403 mActiveSensors.add(handle, rec); 404 if (sensor->isVirtual()) { 405 mActiveVirtualSensors.add(handle, sensor); 406 } 407 } else { 408 if (rec->addConnection(connection)) { 409 // this sensor is already activated, but we are adding a 410 // connection that uses it. Immediately send down the last 411 // known value of the requested sensor if it's not a 412 // "continuous" sensor. 413 if (sensor->getSensor().getMinDelay() == 0) { 414 sensors_event_t scratch; 415 sensors_event_t& event(mLastEventSeen.editValueFor(handle)); 416 if (event.version == sizeof(sensors_event_t)) { 417 connection->sendEvents(&event, 1); 418 } 419 } 420 } 421 } 422 if (err == NO_ERROR) { 423 // connection now active 424 if (connection->addSensor(handle)) { 425 // the sensor was added (which means it wasn't already there) 426 // so, see if this connection becomes active 427 if (mActiveConnections.indexOf(connection) < 0) { 428 mActiveConnections.add(connection); 429 } 430 } 431 } 432 } 433 return err; 434} 435 436status_t SensorService::disable(const sp<SensorEventConnection>& connection, 437 int handle) 438{ 439 if (mInitCheck != NO_ERROR) 440 return mInitCheck; 441 442 status_t err = NO_ERROR; 443 Mutex::Autolock _l(mLock); 444 SensorRecord* rec = mActiveSensors.valueFor(handle); 445 if (rec) { 446 // see if this connection becomes inactive 447 connection->removeSensor(handle); 448 if (connection->hasAnySensor() == false) { 449 mActiveConnections.remove(connection); 450 } 451 // see if this sensor becomes inactive 452 if (rec->removeConnection(connection)) { 453 mActiveSensors.removeItem(handle); 454 mActiveVirtualSensors.removeItem(handle); 455 delete rec; 456 } 457 SensorInterface* sensor = mSensorMap.valueFor(handle); 458 err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE); 459 } 460 return err; 461} 462 463status_t SensorService::setEventRate(const sp<SensorEventConnection>& connection, 464 int handle, nsecs_t ns) 465{ 466 if (mInitCheck != NO_ERROR) 467 return mInitCheck; 468 469 if (ns < 0) 470 return BAD_VALUE; 471 472 if (ns < MINIMUM_EVENTS_PERIOD) 473 ns = MINIMUM_EVENTS_PERIOD; 474 475 SensorInterface* sensor = mSensorMap.valueFor(handle); 476 if (!sensor) return BAD_VALUE; 477 return sensor->setDelay(connection.get(), handle, ns); 478} 479 480// --------------------------------------------------------------------------- 481 482SensorService::SensorRecord::SensorRecord( 483 const sp<SensorEventConnection>& connection) 484{ 485 mConnections.add(connection); 486} 487 488bool SensorService::SensorRecord::addConnection( 489 const sp<SensorEventConnection>& connection) 490{ 491 if (mConnections.indexOf(connection) < 0) { 492 mConnections.add(connection); 493 return true; 494 } 495 return false; 496} 497 498bool SensorService::SensorRecord::removeConnection( 499 const wp<SensorEventConnection>& connection) 500{ 501 ssize_t index = mConnections.indexOf(connection); 502 if (index >= 0) { 503 mConnections.removeItemsAt(index, 1); 504 } 505 return mConnections.size() ? false : true; 506} 507 508// --------------------------------------------------------------------------- 509 510SensorService::SensorEventConnection::SensorEventConnection( 511 const sp<SensorService>& service) 512 : mService(service), mChannel(new SensorChannel()) 513{ 514} 515 516SensorService::SensorEventConnection::~SensorEventConnection() 517{ 518 LOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this); 519 mService->cleanupConnection(this); 520} 521 522void SensorService::SensorEventConnection::onFirstRef() 523{ 524} 525 526bool SensorService::SensorEventConnection::addSensor(int32_t handle) { 527 Mutex::Autolock _l(mConnectionLock); 528 if (mSensorInfo.indexOf(handle) <= 0) { 529 mSensorInfo.add(handle); 530 return true; 531 } 532 return false; 533} 534 535bool SensorService::SensorEventConnection::removeSensor(int32_t handle) { 536 Mutex::Autolock _l(mConnectionLock); 537 if (mSensorInfo.remove(handle) >= 0) { 538 return true; 539 } 540 return false; 541} 542 543bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const { 544 Mutex::Autolock _l(mConnectionLock); 545 return mSensorInfo.indexOf(handle) >= 0; 546} 547 548bool SensorService::SensorEventConnection::hasAnySensor() const { 549 Mutex::Autolock _l(mConnectionLock); 550 return mSensorInfo.size() ? true : false; 551} 552 553status_t SensorService::SensorEventConnection::sendEvents( 554 sensors_event_t const* buffer, size_t numEvents, 555 sensors_event_t* scratch) 556{ 557 // filter out events not for this connection 558 size_t count = 0; 559 if (scratch) { 560 Mutex::Autolock _l(mConnectionLock); 561 size_t i=0; 562 while (i<numEvents) { 563 const int32_t curr = buffer[i].sensor; 564 if (mSensorInfo.indexOf(curr) >= 0) { 565 do { 566 scratch[count++] = buffer[i++]; 567 } while ((i<numEvents) && (buffer[i].sensor == curr)); 568 } else { 569 i++; 570 } 571 } 572 } else { 573 scratch = const_cast<sensors_event_t *>(buffer); 574 count = numEvents; 575 } 576 577 if (count == 0) 578 return 0; 579 580 ssize_t size = mChannel->write(scratch, count*sizeof(sensors_event_t)); 581 if (size == -EAGAIN) { 582 // the destination doesn't accept events anymore, it's probably 583 // full. For now, we just drop the events on the floor. 584 LOGW("dropping %d events on the floor", count); 585 return size; 586 } 587 588 LOGE_IF(size<0, "dropping %d events on the floor (%s)", 589 count, strerror(-size)); 590 591 return size < 0 ? status_t(size) : status_t(NO_ERROR); 592} 593 594sp<SensorChannel> SensorService::SensorEventConnection::getSensorChannel() const 595{ 596 return mChannel; 597} 598 599status_t SensorService::SensorEventConnection::enableDisable( 600 int handle, bool enabled) 601{ 602 status_t err; 603 if (enabled) { 604 err = mService->enable(this, handle); 605 } else { 606 err = mService->disable(this, handle); 607 } 608 return err; 609} 610 611status_t SensorService::SensorEventConnection::setEventRate( 612 int handle, nsecs_t ns) 613{ 614 return mService->setEventRate(this, handle, ns); 615} 616 617// --------------------------------------------------------------------------- 618}; // namespace android 619 620