9-axis sensor fusion with Kalman filter
Add support for 9-axis gravity and linear-acceleration sensors
virtual orientation sensor using 9-axis fusion
Change-Id: I6717539373fce781c10e97b6fa59f68a831a592f
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index f1db2f5..5b86d10 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -35,10 +35,13 @@
#include <hardware/sensors.h>
-#include "SensorService.h"
+#include "CorrectedGyroSensor.h"
#include "GravitySensor.h"
#include "LinearAccelerationSensor.h"
+#include "OrientationSensor.h"
#include "RotationVectorSensor.h"
+#include "SensorFusion.h"
+#include "SensorService.h"
namespace android {
// ---------------------------------------------------------------------------
@@ -74,14 +77,26 @@
}
}
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
- registerVirtualSensor( new GravitySensor(list, count) );
- }
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
- registerVirtualSensor( new LinearAccelerationSensor(list, count) );
- }
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
- registerVirtualSensor( new RotationVectorSensor(list, count) );
+ // 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());
+
+ // 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) );
+
+ // if we have a gyro, we have the option of enabling these
+ // "better" orientation and gyro sensors
+ if (fusion.hasGyro()) {
+ // FIXME: OrientationSensor buggy when not pointing north
+ registerVirtualSensor( new OrientationSensor() );
+ registerVirtualSensor( new CorrectedGyroSensor(list, count) );
}
run("SensorService", PRIORITY_URGENT_DISPLAY);
@@ -133,7 +148,9 @@
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",
+ 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(),
@@ -141,6 +158,7 @@
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",
@@ -183,13 +201,19 @@
// 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++) {
- sensors_event_t const * const event = buffer;
for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
sensors_event_t out;
if (virtualSensors.valueAt(j)->process(&out, event[i])) {