use quaternions instead of MRPs
also use correct time propagation equation
disable the fused sensors when gyro is not present since
they were unusable in practice.
Change-Id: Iad797425784e67dc6c5690e97c71c583418cc5b5
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index 5b86d10..d1b10f7 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -18,6 +18,8 @@
#include <math.h>
#include <sys/types.h>
+#include <cutils/properties.h>
+
#include <utils/SortedVector.h>
#include <utils/KeyedVector.h>
#include <utils/threads.h>
@@ -46,6 +48,16 @@
namespace android {
// ---------------------------------------------------------------------------
+/*
+ * Notes:
+ *
+ * - what about a gyro-corrected magnetic-field sensor?
+ * - option to "hide" the HAL sensors
+ * - 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)
@@ -59,6 +71,7 @@
SensorDevice& dev(SensorDevice::getInstance());
if (dev.initCheck() == NO_ERROR) {
+ bool hasGyro = false;
uint32_t virtualSensorsNeeds =
(1<<SENSOR_TYPE_GRAVITY) |
(1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
@@ -69,6 +82,9 @@
for (int i=0 ; i<count ; i++) {
registerSensor( new HardwareSensor(list[i]) );
switch (list[i].type) {
+ case SENSOR_TYPE_GYROSCOPE:
+ hasGyro = true;
+ break;
case SENSOR_TYPE_GRAVITY:
case SENSOR_TYPE_LINEAR_ACCELERATION:
case SENSOR_TYPE_ROTATION_VECTOR:
@@ -82,21 +98,26 @@
// 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).
+ 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) );
+ 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
+ // 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() );
+ }
}
run("SensorService", PRIORITY_URGENT_DISPLAY);