summaryrefslogtreecommitdiffstats
path: root/services/sensorservice
diff options
context:
space:
mode:
Diffstat (limited to 'services/sensorservice')
-rw-r--r--services/sensorservice/Android.mk2
-rw-r--r--services/sensorservice/Fusion.cpp65
-rw-r--r--services/sensorservice/SensorDevice.cpp21
-rw-r--r--services/sensorservice/SensorDevice.h2
-rw-r--r--services/sensorservice/SensorInterface.cpp4
-rw-r--r--services/sensorservice/SensorInterface.h2
-rw-r--r--services/sensorservice/SensorService.cpp161
-rw-r--r--services/sensorservice/SensorService.h8
8 files changed, 217 insertions, 48 deletions
diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk
index e0cfaa6..dd698c5 100644
--- a/services/sensorservice/Android.mk
+++ b/services/sensorservice/Android.mk
@@ -20,7 +20,9 @@ LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\"
LOCAL_SHARED_LIBRARIES := \
libcutils \
libhardware \
+ libhardware_legacy \
libutils \
+ liblog \
libbinder \
libui \
libgui
diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
index b88a647..93d6127 100644
--- a/services/sensorservice/Fusion.cpp
+++ b/services/sensorservice/Fusion.cpp
@@ -201,15 +201,15 @@ void Fusion::initFusion(const vec4_t& q, float dT)
// q11 = su^2.dt
//
- // variance of integrated output at 1/dT Hz
- // (random drift)
- const float q00 = gyroVAR * dT;
+ const float dT2 = dT*dT;
+ const float dT3 = dT2*dT;
+
+ // variance of integrated output at 1/dT Hz (random drift)
+ const float q00 = gyroVAR * dT + 0.33333f * biasVAR * dT3;
// variance of drift rate ramp
const float q11 = biasVAR * dT;
-
- const float u = q11 / dT;
- const float q10 = 0.5f*u*dT*dT;
+ const float q10 = 0.5f * biasVAR * dT2;
const float q01 = q10;
GQGt[0][0] = q00; // rad^2
@@ -220,6 +220,22 @@ void Fusion::initFusion(const vec4_t& q, float dT)
// initial covariance: Var{ x(t0) }
// TODO: initialize P correctly
P = 0;
+
+ // it is unclear how to set the initial covariance. It does affect
+ // how quickly the fusion converges. Experimentally it would take
+ // about 10 seconds at 200 Hz to estimate the gyro-drift with an
+ // initial covariance of 0, and about a second with an initial covariance
+ // of about 1 deg/s.
+ const float covv = 0;
+ const float covu = 0.5f * (float(M_PI) / 180);
+ mat33_t& Pv = P[0][0];
+ Pv[0][0] = covv;
+ Pv[1][1] = covv;
+ Pv[2][2] = covv;
+ mat33_t& Pu = P[1][1];
+ Pu[0][0] = covu;
+ Pu[1][1] = covu;
+ Pu[2][2] = covu;
}
bool Fusion::hasEstimate() const {
@@ -357,6 +373,11 @@ mat33_t Fusion::getRotationMatrix() const {
mat34_t Fusion::getF(const vec4_t& q) {
mat34_t F;
+
+ // This is used to compute the derivative of q
+ // F = | [q.xyz]x |
+ // | -q.xyz |
+
F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y;
F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x;
F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w;
@@ -368,10 +389,18 @@ void Fusion::predict(const vec3_t& w, float dT) {
const vec4_t q = x0;
const vec3_t b = x1;
const vec3_t we = w - b;
- const vec4_t dq = getF(q)*((0.5f*dT)*we);
- x0 = normalize_quat(q + dq);
+ // q(k+1) = O(we)*q(k)
+ // --------------------
+ //
+ // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi |
+ // | -psi' cos(0.5*||w||*dT) |
+ //
+ // psi = sin(0.5*||w||*dT)*w / ||w||
+ //
+ //
// P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
+ // ----------------------------------------
//
// G = | -I33 0 |
// | 0 I33 |
@@ -392,13 +421,26 @@ void Fusion::predict(const vec3_t& w, float dT) {
const mat33_t wx(crossMatrix(we, 0));
const mat33_t wx2(wx*wx);
const float lwedT = length(we)*dT;
+ const float hlwedT = 0.5f*lwedT;
const float ilwe = 1/length(we);
const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
const float k1 = sinf(lwedT);
+ const float k2 = cosf(hlwedT);
+ const vec3_t psi(sinf(hlwedT)*ilwe*we);
+ const mat33_t O33(crossMatrix(-psi, k2));
+ mat44_t O;
+ O[0].xyz = O33[0]; O[0].w = -psi.x;
+ O[1].xyz = O33[1]; O[1].w = -psi.y;
+ O[2].xyz = O33[2]; O[2].w = -psi.z;
+ O[3].xyz = psi; O[3].w = k2;
Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
+ x0 = O*q;
+ if (x0.w < 0)
+ x0 = -x0;
+
P = Phi*P*transpose(Phi) + GQGt;
checkState();
@@ -425,7 +467,12 @@ void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
K[1] = transpose(P[1][0])*LtSi;
// update...
- // P -= K*H*P;
+ // P = (I-K*H) * P
+ // P -= K*H*P
+ // | K0 | * | L 0 | * P = | K0*L 0 | * | P00 P10 | = | K0*L*P00 K0*L*P10 |
+ // | K1 | | K1*L 0 | | P01 P11 | | K1*L*P00 K1*L*P10 |
+ // Note: the Joseph form is numerically more stable and given by:
+ // P = (I-KH) * P * (I-KH)' + K*R*R'
const mat33_t K0L(K[0] * L);
const mat33_t K1L(K[1] * L);
P[0][0] -= K0L*P[0][0];
diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp
index a9e3ef4..a12529e 100644
--- a/services/sensorservice/SensorDevice.cpp
+++ b/services/sensorservice/SensorDevice.cpp
@@ -111,6 +111,12 @@ ssize_t SensorDevice::poll(sensors_event_t* buffer, size_t count) {
return c;
}
+void SensorDevice::autoDisable(void *ident, int handle) {
+ Info& info( mActivationCount.editValueFor(handle) );
+ Mutex::Autolock _l(mLock);
+ info.rates.removeItem(ident);
+}
+
status_t SensorDevice::activate(void* ident, int handle, int enabled)
{
if (!mSensorDevice) return NO_INIT;
@@ -159,6 +165,15 @@ status_t SensorDevice::activate(void* ident, int handle, int enabled)
ALOGE_IF(err, "Error %s sensor %d (%s)",
enabled ? "activating" : "disabling",
handle, strerror(-err));
+
+ if (err != NO_ERROR) {
+ // clean-up on failure
+ if (enabled) {
+ // failure when enabling the sensor
+ Mutex::Autolock _l(mLock);
+ info.rates.removeItem(ident);
+ }
+ }
}
{ // scope for the lock
@@ -181,6 +196,12 @@ status_t SensorDevice::setDelay(void* ident, int handle, int64_t ns)
return mSensorDevice->setDelay(mSensorDevice, handle, ns);
}
+int SensorDevice::getHalDeviceVersion() const {
+ if (!mSensorDevice) return -1;
+
+ return mSensorDevice->common.version;
+}
+
// ---------------------------------------------------------------------------
status_t SensorDevice::Info::setDelayForIdent(void* ident, int64_t ns)
diff --git a/services/sensorservice/SensorDevice.h b/services/sensorservice/SensorDevice.h
index 728b6cb..227dab6 100644
--- a/services/sensorservice/SensorDevice.h
+++ b/services/sensorservice/SensorDevice.h
@@ -52,9 +52,11 @@ class SensorDevice : public Singleton<SensorDevice> {
public:
ssize_t getSensorList(sensor_t const** list);
status_t initCheck() const;
+ int getHalDeviceVersion() const;
ssize_t poll(sensors_event_t* buffer, size_t count);
status_t activate(void* ident, int handle, int enabled);
status_t setDelay(void* ident, int handle, int64_t ns);
+ void autoDisable(void *ident, int handle);
void dump(String8& result, char* buffer, size_t SIZE);
};
diff --git a/services/sensorservice/SensorInterface.cpp b/services/sensorservice/SensorInterface.cpp
index 468aa61..b483b75 100644
--- a/services/sensorservice/SensorInterface.cpp
+++ b/services/sensorservice/SensorInterface.cpp
@@ -54,6 +54,10 @@ status_t HardwareSensor::setDelay(void* ident, int handle, int64_t ns) {
return mSensorDevice.setDelay(ident, handle, ns);
}
+void HardwareSensor::autoDisable(void *ident, int handle) {
+ mSensorDevice.autoDisable(ident, handle);
+}
+
Sensor HardwareSensor::getSensor() const {
return mSensor;
}
diff --git a/services/sensorservice/SensorInterface.h b/services/sensorservice/SensorInterface.h
index fb357d7..2e14e57 100644
--- a/services/sensorservice/SensorInterface.h
+++ b/services/sensorservice/SensorInterface.h
@@ -40,6 +40,7 @@ public:
virtual status_t setDelay(void* ident, int handle, int64_t ns) = 0;
virtual Sensor getSensor() const = 0;
virtual bool isVirtual() const = 0;
+ virtual void autoDisable(void *ident, int handle) { }
};
// ---------------------------------------------------------------------------
@@ -61,6 +62,7 @@ public:
virtual status_t setDelay(void* ident, int handle, int64_t ns);
virtual Sensor getSensor() const;
virtual bool isVirtual() const { return false; }
+ virtual void autoDisable(void *ident, int handle);
};
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index e3dcd02..6481584 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -38,6 +38,7 @@
#include <gui/SensorEventQueue.h>
#include <hardware/sensors.h>
+#include <hardware_legacy/power.h>
#include "BatteryService.h"
#include "CorrectedGyroSensor.h"
@@ -60,6 +61,8 @@ namespace android {
*
*/
+const char* SensorService::WAKE_LOCK_NAME = "SensorService";
+
SensorService::SensorService()
: mInitCheck(NO_INIT)
{
@@ -237,6 +240,24 @@ status_t SensorService::dump(int fd, const Vector<String16>& args)
return NO_ERROR;
}
+void SensorService::cleanupAutoDisabledSensor(const sp<SensorEventConnection>& connection,
+ sensors_event_t const* buffer, const int count) {
+ SensorInterface* sensor;
+ status_t err = NO_ERROR;
+ for (int i=0 ; i<count ; i++) {
+ int handle = buffer[i].sensor;
+ if (getSensorType(handle) == SENSOR_TYPE_SIGNIFICANT_MOTION) {
+ if (connection->hasSensor(handle)) {
+ sensor = mSensorMap.valueFor(handle);
+ if (sensor != NULL) {
+ sensor->autoDisable(connection.get(), handle);
+ }
+ cleanupWithoutDisable(connection, handle);
+ }
+ }
+ }
+}
+
bool SensorService::threadLoop()
{
ALOGD("nuSensorService thread starting...");
@@ -249,6 +270,8 @@ bool SensorService::threadLoop()
const size_t vcount = mVirtualSensorList.size();
ssize_t count;
+ bool wakeLockAcquired = false;
+ const int halVersion = device.getHalDeviceVersion();
do {
count = device.poll(buffer, numEventMax);
if (count<0) {
@@ -256,6 +279,17 @@ bool SensorService::threadLoop()
break;
}
+ // Poll has returned. Hold a wakelock.
+ // Todo(): add a flag to the sensors definitions to indicate
+ // the sensors which can wake up the AP
+ for (int i = 0; i < count; i++) {
+ if (getSensorType(buffer[i].sensor) == SENSOR_TYPE_SIGNIFICANT_MOTION) {
+ acquire_wake_lock(PARTIAL_WAKE_LOCK, WAKE_LOCK_NAME);
+ wakeLockAcquired = true;
+ break;
+ }
+ }
+
recordLastValue(buffer, count);
// handle virtual sensors
@@ -298,6 +332,17 @@ bool SensorService::threadLoop()
}
}
+ // handle backward compatibility for RotationVector sensor
+ if (halVersion < SENSORS_DEVICE_API_VERSION_1_0) {
+ for (int i = 0; i < count; i++) {
+ if (getSensorType(buffer[i].sensor) == SENSOR_TYPE_ROTATION_VECTOR) {
+ // All the 4 components of the quaternion should be available
+ // No heading accuracy. Set it to -1
+ buffer[i].data[4] = -1;
+ }
+ }
+ }
+
// send our events to clients...
const SortedVector< wp<SensorEventConnection> > activeConnections(
getActiveConnections());
@@ -307,8 +352,14 @@ bool SensorService::threadLoop()
activeConnections[i].promote());
if (connection != 0) {
connection->sendEvents(buffer, count, scratch);
+ // Some sensors need to be auto disabled after the trigger
+ cleanupAutoDisabledSensor(connection, buffer, count);
}
}
+
+ // We have read the data, upper layers should hold the wakelock.
+ if (wakeLockAcquired) release_wake_lock(WAKE_LOCK_NAME);
+
} while (count >= 0 || Thread::exitPending());
ALOGW("Exiting SensorService::threadLoop => aborting...");
@@ -372,6 +423,18 @@ String8 SensorService::getSensorName(int handle) const {
return result;
}
+int SensorService::getSensorType(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.getType();
+ }
+ }
+ return -1;
+}
+
+
Vector<Sensor> SensorService::getSensorList()
{
char value[PROPERTY_VALUE_MAX];
@@ -431,47 +494,53 @@ status_t SensorService::enable(const sp<SensorEventConnection>& connection,
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 (sensor == NULL) {
+ return BAD_VALUE;
+ }
+
+ Mutex::Autolock _l(mLock);
+ SensorRecord* rec = mActiveSensors.valueFor(handle);
+ if (rec == 0) {
+ rec = new SensorRecord(connection);
+ mActiveSensors.add(handle, rec);
+ if (sensor->isVirtual()) {
+ mActiveVirtualSensors.add(handle, sensor);
}
- if (err == NO_ERROR) {
- // connection now active
- if (connection->addSensor(handle)) {
- BatteryService::enableSensor(connection->getUid(), 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);
+ } 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);
}
- } else {
- ALOGW("sensor %08x already enabled in connection %p (ignoring)",
- handle, connection.get());
}
}
}
+
+ if (connection->addSensor(handle)) {
+ BatteryService::enableSensor(connection->getUid(), 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);
+ }
+ } else {
+ ALOGW("sensor %08x already enabled in connection %p (ignoring)",
+ handle, connection.get());
+ }
+
+ // we are setup, now enable the sensor.
+ status_t err = sensor->activate(connection.get(), true);
+ if (err != NO_ERROR) {
+ // enable has failed, reset our state.
+ cleanupWithoutDisableLocked(connection, handle);
+ }
return err;
}
@@ -481,8 +550,23 @@ status_t SensorService::disable(const sp<SensorEventConnection>& connection,
if (mInitCheck != NO_ERROR)
return mInitCheck;
- status_t err = NO_ERROR;
Mutex::Autolock _l(mLock);
+ status_t err = cleanupWithoutDisableLocked(connection, handle);
+ if (err == NO_ERROR) {
+ SensorInterface* sensor = mSensorMap.valueFor(handle);
+ err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE);
+ }
+ return err;
+}
+
+status_t SensorService::cleanupWithoutDisable(
+ const sp<SensorEventConnection>& connection, int handle) {
+ Mutex::Autolock _l(mLock);
+ return cleanupWithoutDisableLocked(connection, handle);
+}
+
+status_t SensorService::cleanupWithoutDisableLocked(
+ const sp<SensorEventConnection>& connection, int handle) {
SensorRecord* rec = mActiveSensors.valueFor(handle);
if (rec) {
// see if this connection becomes inactive
@@ -498,10 +582,9 @@ status_t SensorService::disable(const sp<SensorEventConnection>& connection,
mActiveVirtualSensors.removeItem(handle);
delete rec;
}
- SensorInterface* sensor = mSensorMap.valueFor(handle);
- err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE);
+ return NO_ERROR;
}
- return err;
+ return BAD_VALUE;
}
status_t SensorService::setEventRate(const sp<SensorEventConnection>& connection,
diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h
index 18591bf..fcdbc7d 100644
--- a/services/sensorservice/SensorService.h
+++ b/services/sensorservice/SensorService.h
@@ -53,6 +53,7 @@ class SensorService :
friend class BinderService<SensorService>;
static const nsecs_t MINIMUM_EVENTS_PERIOD = 1000000; // 1000 Hz
+ static const char* WAKE_LOCK_NAME;
SensorService();
virtual ~SensorService();
@@ -109,10 +110,17 @@ class SensorService :
DefaultKeyedVector<int, SensorInterface*> getActiveVirtualSensors() const;
String8 getSensorName(int handle) const;
+ int getSensorType(int handle) const;
void recordLastValue(sensors_event_t const * buffer, size_t count);
static void sortEventBuffer(sensors_event_t* buffer, size_t count);
void registerSensor(SensorInterface* sensor);
void registerVirtualSensor(SensorInterface* sensor);
+ status_t cleanupWithoutDisable(
+ const sp<SensorEventConnection>& connection, int handle);
+ status_t cleanupWithoutDisableLocked(
+ const sp<SensorEventConnection>& connection, int handle);
+ void cleanupAutoDisabledSensor(const sp<SensorEventConnection>& connection,
+ sensors_event_t const* buffer, const int count);
// constants
Vector<Sensor> mSensorList;
generated by cgit v1.1 at 2025-08-18 10:40:08 +0000