resolve merge conflicts of 13c5c34 to mnc-dev am: 79b74d787d
am: abe293b950
Change-Id: I7f8ce6dbb313d592b97b41a982e629680eba36c4
diff --git a/cmds/atrace/atrace.cpp b/cmds/atrace/atrace.cpp
index 26c5b4a..9a5a81e 100644
--- a/cmds/atrace/atrace.cpp
+++ b/cmds/atrace/atrace.cpp
@@ -140,6 +140,15 @@
{ "regulators", "Voltage and Current Regulators", 0, {
{ REQ, "/sys/kernel/debug/tracing/events/regulator/enable" },
} },
+ { "binder_driver", "Binder Kernel driver", 0, {
+ { REQ, "/sys/kernel/debug/tracing/events/binder/binder_transaction/enable" },
+ { REQ, "/sys/kernel/debug/tracing/events/binder/binder_transaction_received/enable" },
+ } },
+ { "binder_lock", "Binder global lock trace", 0, {
+ { REQ, "/sys/kernel/debug/tracing/events/binder/binder_lock/enable" },
+ { REQ, "/sys/kernel/debug/tracing/events/binder/binder_locked/enable" },
+ { REQ, "/sys/kernel/debug/tracing/events/binder/binder_unlock/enable" },
+ } },
};
/* Command line options */
diff --git a/cmds/dumpstate/dumpstate.c b/cmds/dumpstate/dumpstate.c
index 792f015..b88b605 100644
--- a/cmds/dumpstate/dumpstate.c
+++ b/cmds/dumpstate/dumpstate.c
@@ -47,6 +47,7 @@
#define PSTORE_LAST_KMSG "/sys/fs/pstore/console-ramoops"
+#define RAFT_DIR "/data/misc/raft/"
#define TOMBSTONE_DIR "/data/tombstones"
#define TOMBSTONE_FILE_PREFIX TOMBSTONE_DIR "/tombstone_"
/* Can accomodate a tombstone number up to 9999. */
@@ -350,6 +351,8 @@
run_command("LOG STATISTICS", 10, "logcat", "-b", "all", "-S", NULL);
+ run_command("RAFT LOGS", 600, SU_PATH, "root", "logcompressor", "-r", RAFT_DIR, NULL);
+
/* show the traces we collected in main(), if that was done */
if (dump_traces_path != NULL) {
dump_file("VM TRACES JUST NOW", dump_traces_path);
diff --git a/cmds/installd/commands.cpp b/cmds/installd/commands.cpp
index d4aa7d3..c05a3d3 100644
--- a/cmds/installd/commands.cpp
+++ b/cmds/installd/commands.cpp
@@ -770,24 +770,11 @@
dex2oat_compiler_filter_flag, NULL) > 0;
char dex2oat_threads_buf[PROPERTY_VALUE_MAX];
- bool have_dex2oat_threads_flag = false;
- if (!post_bootcomplete) {
- have_dex2oat_threads_flag = property_get("dalvik.vm.boot-dex2oat-threads",
- dex2oat_threads_buf,
- NULL) > 0;
- // If there's no boot property, fall back to the image property.
- if (!have_dex2oat_threads_flag) {
- have_dex2oat_threads_flag = property_get("dalvik.vm.image-dex2oat-threads",
- dex2oat_threads_buf,
- NULL) > 0;
- }
- // If there's neither, fall back to the default property.
- }
- if (!have_dex2oat_threads_flag) {
- have_dex2oat_threads_flag = property_get("dalvik.vm.dex2oat-threads",
- dex2oat_threads_buf,
- NULL) > 0;
- }
+ bool have_dex2oat_threads_flag = property_get(post_bootcomplete
+ ? "dalvik.vm.dex2oat-threads"
+ : "dalvik.vm.boot-dex2oat-threads",
+ dex2oat_threads_buf,
+ NULL) > 0;
char dex2oat_threads_arg[PROPERTY_VALUE_MAX + 2];
if (have_dex2oat_threads_flag) {
sprintf(dex2oat_threads_arg, "-j%s", dex2oat_threads_buf);
diff --git a/data/etc/android.hardware.sensor.ambient_temperature.xml b/data/etc/android.hardware.sensor.ambient_temperature.xml
new file mode 100644
index 0000000..8ad140e
--- /dev/null
+++ b/data/etc/android.hardware.sensor.ambient_temperature.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Copyright (C) 2009 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.
+-->
+
+<!-- Feature for devices with an ambient temperature sensor. -->
+<permissions>
+ <feature name="android.hardware.sensor.ambient_temperature" />
+</permissions>
diff --git a/data/etc/android.hardware.sensor.relative_humidity.xml b/data/etc/android.hardware.sensor.relative_humidity.xml
new file mode 100644
index 0000000..b9432d3
--- /dev/null
+++ b/data/etc/android.hardware.sensor.relative_humidity.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Copyright (C) 2009 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.
+-->
+
+<!-- Feature for devices with a relative humidity sensor. -->
+<permissions>
+ <feature name="android.hardware.sensor.relative_humidity" />
+</permissions>
diff --git a/data/etc/wearable_core_hardware.xml b/data/etc/wearable_core_hardware.xml
index 346abad..7c9e8ef 100644
--- a/data/etc/wearable_core_hardware.xml
+++ b/data/etc/wearable_core_hardware.xml
@@ -22,7 +22,8 @@
-->
<permissions>
<feature name="android.hardware.location" />
- <feature name="android.hardware.sensor.compass" />
+ <!-- devices supporting compass/magnitometer sensor must include
+ android.hardware.sensor.compass.xml -->
<feature name="android.hardware.sensor.accelerometer" />
<feature name="android.hardware.bluetooth" />
<feature name="android.hardware.touchscreen" />
diff --git a/include/android/keycodes.h b/include/android/keycodes.h
index 15bb786..421abe5 100644
--- a/include/android/keycodes.h
+++ b/include/android/keycodes.h
@@ -722,7 +722,22 @@
AKEYCODE_NAVIGATE_PREVIOUS = 260,
AKEYCODE_NAVIGATE_NEXT = 261,
AKEYCODE_NAVIGATE_IN = 262,
- AKEYCODE_NAVIGATE_OUT = 263
+ AKEYCODE_NAVIGATE_OUT = 263,
+ /** Primary stem key for Wear
+ * Main power/reset button on watch. */
+ AKEYCODE_STEM_PRIMARY = 264,
+ /** Generic stem key 1 for Wear */
+ AKEYCODE_STEM_1 = 265,
+ /** Generic stem key 2 for Wear */
+ AKEYCODE_STEM_2 = 266,
+ /** Generic stem key 3 for Wear */
+ AKEYCODE_STEM_3 = 267,
+ AKEYCODE_MEDIA_SKIP_FORWARD = 272,
+ AKEYCODE_MEDIA_SKIP_BACKWARD = 273,
+ AKEYCODE_MEDIA_STEP_FORWARD = 274,
+ AKEYCODE_MEDIA_STEP_BACKWARD = 275,
+ /** Put device to sleep unless a wakelock is held. */
+ AKEYCODE_SOFT_SLEEP = 276
// NOTE: If you add a new keycode here you must also add it to several other files.
// Refer to frameworks/base/core/java/android/view/KeyEvent.java for the full list.
diff --git a/include/gui/SensorManager.h b/include/gui/SensorManager.h
index 0cff46c..f039caf 100644
--- a/include/gui/SensorManager.h
+++ b/include/gui/SensorManager.h
@@ -22,9 +22,11 @@
#include <stdint.h>
#include <sys/types.h>
+#include <binder/BinderService.h>
#include <binder/IBinder.h>
#include <binder/IPCThreadState.h>
#include <binder/IServiceManager.h>
+#include <binder/PermissionCache.h>
#include <utils/Errors.h>
#include <utils/RefBase.h>
@@ -54,7 +56,8 @@
static SensorManager& getInstanceForPackage(const String16& packageName);
~SensorManager();
- ssize_t getSensorList(Sensor const* const** list) const;
+ ssize_t getSensorList(Sensor const* const** list);
+ ssize_t getAvailableSensorList(Sensor const* const** list);
Sensor const* getDefaultSensor(int type);
sp<SensorEventQueue> createEventQueue(String8 packageName = String8(""), int mode = 0);
bool isDataInjectionEnabled();
@@ -64,18 +67,27 @@
void sensorManagerDied();
SensorManager(const String16& opPackageName);
- status_t assertStateLocked() const;
+ status_t assertStateLocked();
+ void updateAvailableSensorList();
private:
static Mutex sLock;
static std::map<String16, SensorManager*> sPackageInstances;
- mutable Mutex mLock;
- mutable sp<ISensorServer> mSensorServer;
- mutable Sensor const** mSensorList;
- mutable Vector<Sensor> mSensors;
- mutable sp<IBinder::DeathRecipient> mDeathObserver;
+ Mutex mLock;
+ sp<ISensorServer> mSensorServer;
+
+ // for Java API
+ Sensor const** mSensorList;
+
+ // for NDK API
+ Sensor const** mAvailableSensorList;
+ ssize_t mNumAvailableSensor;
+
+ Vector<Sensor> mSensors;
+ sp<IBinder::DeathRecipient> mDeathObserver;
const String16 mOpPackageName;
+ bool mBodyPermission;
};
// ----------------------------------------------------------------------------
diff --git a/include/input/InputEventLabels.h b/include/input/InputEventLabels.h
index 3962001..f0a6238 100644
--- a/include/input/InputEventLabels.h
+++ b/include/input/InputEventLabels.h
@@ -303,6 +303,15 @@
DEFINE_KEYCODE(NAVIGATE_NEXT),
DEFINE_KEYCODE(NAVIGATE_IN),
DEFINE_KEYCODE(NAVIGATE_OUT),
+ DEFINE_KEYCODE(STEM_PRIMARY),
+ DEFINE_KEYCODE(STEM_1),
+ DEFINE_KEYCODE(STEM_2),
+ DEFINE_KEYCODE(STEM_3),
+ DEFINE_KEYCODE(MEDIA_SKIP_FORWARD),
+ DEFINE_KEYCODE(MEDIA_SKIP_BACKWARD),
+ DEFINE_KEYCODE(MEDIA_STEP_FORWARD),
+ DEFINE_KEYCODE(MEDIA_STEP_BACKWARD),
+ DEFINE_KEYCODE(SOFT_SLEEP),
{ NULL, 0 }
};
diff --git a/libs/binder/Parcel.cpp b/libs/binder/Parcel.cpp
index ef39801..c7987df 100644
--- a/libs/binder/Parcel.cpp
+++ b/libs/binder/Parcel.cpp
@@ -17,40 +17,42 @@
#define LOG_TAG "Parcel"
//#define LOG_NDEBUG 0
-#include <binder/Parcel.h>
+#include <errno.h>
+#include <inttypes.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
-#include <binder/IPCThreadState.h>
#include <binder/Binder.h>
#include <binder/BpBinder.h>
+#include <binder/IPCThreadState.h>
+#include <binder/Parcel.h>
#include <binder/ProcessState.h>
#include <binder/TextOutput.h>
-#include <errno.h>
+#include <cutils/ashmem.h>
#include <utils/Debug.h>
+#include <utils/Flattenable.h>
#include <utils/Log.h>
+#include <utils/misc.h>
#include <utils/String8.h>
#include <utils/String16.h>
-#include <utils/misc.h>
-#include <utils/Flattenable.h>
-#include <cutils/ashmem.h>
#include <private/binder/binder_module.h>
#include <private/binder/Static.h>
-#include <inttypes.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdint.h>
-#include <sys/mman.h>
-
#ifndef INT32_MAX
#define INT32_MAX ((int32_t)(2147483647))
#endif
#define LOG_REFS(...)
-//#define LOG_REFS(...) ALOG(LOG_DEBUG, "Parcel", __VA_ARGS__)
+//#define LOG_REFS(...) ALOG(LOG_DEBUG, LOG_TAG, __VA_ARGS__)
#define LOG_ALLOC(...)
-//#define LOG_ALLOC(...) ALOG(LOG_DEBUG, "Parcel", __VA_ARGS__)
+//#define LOG_ALLOC(...) ALOG(LOG_DEBUG, LOG_TAG, __VA_ARGS__)
// ---------------------------------------------------------------------------
@@ -123,8 +125,10 @@
return;
}
case BINDER_TYPE_FD: {
- if (obj.cookie != 0) {
- if (outAshmemSize != NULL) {
+ if ((obj.cookie != 0) && (outAshmemSize != NULL)) {
+ struct stat st;
+ int ret = fstat(obj.handle, &st);
+ if (!ret && S_ISCHR(st.st_mode)) {
// If we own an ashmem fd, keep track of how much memory it refers to.
int size = ashmem_get_size_region(obj.handle);
if (size > 0) {
@@ -173,15 +177,19 @@
return;
}
case BINDER_TYPE_FD: {
- if (outAshmemSize != NULL) {
- if (obj.cookie != 0) {
- int size = ashmem_get_size_region(obj.handle);
- if (size > 0) {
- *outAshmemSize -= size;
+ if (obj.cookie != 0) { // owned
+ if (outAshmemSize != NULL) {
+ struct stat st;
+ int ret = fstat(obj.handle, &st);
+ if (!ret && S_ISCHR(st.st_mode)) {
+ int size = ashmem_get_size_region(obj.handle);
+ if (size > 0) {
+ *outAshmemSize -= size;
+ }
}
-
- close(obj.handle);
}
+
+ close(obj.handle);
}
return;
}
diff --git a/libs/gui/SensorManager.cpp b/libs/gui/SensorManager.cpp
index 33608b5..4277032 100644
--- a/libs/gui/SensorManager.cpp
+++ b/libs/gui/SensorManager.cpp
@@ -89,7 +89,8 @@
}
SensorManager::SensorManager(const String16& opPackageName)
- : mSensorList(0), mOpPackageName(opPackageName)
+ : mSensorList(NULL), mAvailableSensorList(NULL), mNumAvailableSensor(0),
+ mOpPackageName(opPackageName), mBodyPermission(false)
{
// okay we're not locked here, but it's not needed during construction
assertStateLocked();
@@ -98,6 +99,9 @@
SensorManager::~SensorManager()
{
free(mSensorList);
+ if (mAvailableSensorList) {
+ free(mAvailableSensorList);
+ }
}
void SensorManager::sensorManagerDied()
@@ -106,10 +110,14 @@
mSensorServer.clear();
free(mSensorList);
mSensorList = NULL;
+ if (mAvailableSensorList) {
+ free(mAvailableSensorList);
+ mAvailableSensorList = NULL;
+ }
mSensors.clear();
}
-status_t SensorManager::assertStateLocked() const {
+status_t SensorManager::assertStateLocked() {
bool initSensorManager = false;
if (mSensorServer == NULL) {
initSensorManager = true;
@@ -159,13 +167,14 @@
for (size_t i=0 ; i<count ; i++) {
mSensorList[i] = mSensors.array() + i;
}
+
+ updateAvailableSensorList();
}
return NO_ERROR;
}
-ssize_t SensorManager::getSensorList(Sensor const* const** list) const
-{
+ssize_t SensorManager::getSensorList(Sensor const* const** list) {
Mutex::Autolock _l(mLock);
status_t err = assertStateLocked();
if (err < 0) {
@@ -175,10 +184,76 @@
return static_cast<ssize_t>(mSensors.size());
}
-Sensor const* SensorManager::getDefaultSensor(int type)
-{
+void SensorManager::updateAvailableSensorList() {
+ const int uid = static_cast<int>(IPCThreadState::self()->getCallingUid());
+ const int pid = static_cast<int>(IPCThreadState::self()->getCallingPid());
+ const String16 BODY_SENSOR_PERMISSION("android.permission.BODY_SENSORS");
+ const String8 BODY_SENSOR_PERMISSION8("android.permission.BODY_SENSORS");
+
+ bool bodySensorPermission = false;
+
+ sp<IBinder> binder = defaultServiceManager()->getService(String16("permission"));
+ if (binder != NULL) {
+ bodySensorPermission = interface_cast<IPermissionController>(binder)->
+ checkPermission(BODY_SENSOR_PERMISSION, pid, uid);
+ }
+
+ // only update if app got BODY_SENSORS permission after last call or the sensor list has not
+ // been populated.
+ //
+ // it is not possible for the reverse transition, as the app will be killed when permission is
+ // revoked.
+ if ( (bodySensorPermission && !mBodyPermission) || mAvailableSensorList == NULL) {
+
+ // allocate only when necessary
+ if (mAvailableSensorList == NULL) {
+ // allocate a list big enough to fit all sensors (including those requires permission
+ // that the app do not have;
+ mAvailableSensorList =
+ static_cast<Sensor const**>(malloc(mSensors.size() * sizeof(Sensor*)));
+
+ // first populate all sensors that do not need body sensor permission
+ ssize_t& n = mNumAvailableSensor;
+ for (size_t i = 0; i < mSensors.size() ; i++) {
+ if (mSensors[i].getRequiredPermission() != BODY_SENSOR_PERMISSION8) {
+ mAvailableSensorList[n++] = mSensors.array() + i;
+ }
+ }
+ }
+
+ if (bodySensorPermission) {
+ // if the app just got the sensor permission back, fill the sensor at the end of list
+ ssize_t& n = mNumAvailableSensor;
+ for (size_t i = 0; i < mSensors.size() ; i++) {
+ if (mSensors[i].getRequiredPermission() == BODY_SENSOR_PERMISSION8) {
+ mAvailableSensorList[n++] = mSensors.array() + i;
+ }
+ }
+ }
+
+ mBodyPermission = bodySensorPermission;
+ }
+}
+
+ssize_t SensorManager::getAvailableSensorList(Sensor const* const** list) {
+ Mutex::Autolock _l(mLock);
+ status_t err = assertStateLocked();
+ if (err < 0) {
+ return static_cast<ssize_t>(err);
+ }
+
+ updateAvailableSensorList();
+
+ *list = mAvailableSensorList;
+ return mNumAvailableSensor;
+}
+
+Sensor const* SensorManager::getDefaultSensor(int type) {
Mutex::Autolock _l(mLock);
if (assertStateLocked() == NO_ERROR) {
+
+ updateAvailableSensorList();
+
bool wakeUpSensor = false;
// For the following sensor types, return a wake-up sensor. These types are by default
// defined as wake-up sensors. For the rest of the sensor types defined in sensors.h return
@@ -192,9 +267,9 @@
// in the future it will make sense to let the SensorService make
// that decision.
for (size_t i=0 ; i<mSensors.size() ; i++) {
- if (mSensorList[i]->getType() == type &&
- mSensorList[i]->isWakeUpSensor() == wakeUpSensor) {
- return mSensorList[i];
+ if (mAvailableSensorList[i]->getType() == type &&
+ mAvailableSensorList[i]->isWakeUpSensor() == wakeUpSensor) {
+ return mAvailableSensorList[i];
}
}
}
diff --git a/services/sensorservice/CorrectedGyroSensor.cpp b/services/sensorservice/CorrectedGyroSensor.cpp
index b07d544..7b1f346 100644
--- a/services/sensorservice/CorrectedGyroSensor.cpp
+++ b/services/sensorservice/CorrectedGyroSensor.cpp
@@ -58,12 +58,12 @@
status_t CorrectedGyroSensor::activate(void* ident, bool enabled) {
mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
}
status_t CorrectedGyroSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
mSensorDevice.setDelay(ident, mGyro.getHandle(), ns);
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
}
Sensor CorrectedGyroSensor::getSensor() const {
diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp
index 4f63c31..359d289 100644
--- a/services/sensorservice/Fusion.cpp
+++ b/services/sensorservice/Fusion.cpp
@@ -24,28 +24,44 @@
// -----------------------------------------------------------------------
+/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/
+
+/* Note:
+ * If a platform uses software fusion, it is necessary to tune the following
+ * parameters to fit the hardware sensors prior to release.
+ *
+ * The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode.
+ * The GEOMAG_ parameters will be used in FUSION_NOGYRO mode.
+ */
+
/*
- * gyroVAR gives the measured variance of the gyro's output per
+ * GYRO_VAR gives the measured variance of the gyro's output per
* Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,
* which is independent of the sampling frequency.
*
* The variance of gyro's output at a given sampling period can be
* calculated as:
- * variance(T) = gyroVAR / T
+ * variance(T) = GYRO_VAR / T
*
* The variance of the INTEGRATED OUTPUT at a given sampling period can be
* calculated as:
- * variance_integrate_output(T) = gyroVAR * T
- *
+ * variance_integrate_output(T) = GYRO_VAR * T
*/
-static const float gyroVAR = 1e-7; // (rad/s)^2 / Hz
-static const float biasVAR = 1e-8; // (rad/s)^2 / s (guessed)
+static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz
+static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed)
+static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz
+static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed)
/*
* Standard deviations of accelerometer and magnetometer
*/
-static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
-static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5)
+static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)
+static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
+static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
+static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
+
+
+/* ====================== END FUSION SENSOR PARAMETER ========================*/
static const float SYMMETRY_TOLERANCE = 1e-10f;
@@ -54,7 +70,8 @@
* ill-conditioning and div by zeros.
* Threshhold: 10% of g, in m/s^2
*/
-static const float FREE_FALL_THRESHOLD = 0.981f;
+static const float NOMINAL_GRAVITY = 9.81f;
+static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY);
static const float FREE_FALL_THRESHOLD_SQ =
FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD;
@@ -87,6 +104,9 @@
static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;
+static const float W_EPS = 1e-4f;
+static const float SQRT_3 = 1.732f;
+static const float WVEC_EPS = 1e-4f/SQRT_3;
// -----------------------------------------------------------------------
template <typename TYPE, size_t C, size_t R>
@@ -173,7 +193,7 @@
init();
}
-void Fusion::init() {
+void Fusion::init(int mode) {
mInitState = 0;
mGyroRate = 0;
@@ -183,6 +203,19 @@
mCount[2] = 0;
mData = 0;
+ mMode = mode;
+
+ if (mMode != FUSION_NOGYRO) { //normal or game rotation
+ mParam.gyroVar = DEFAULT_GYRO_VAR;
+ mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR;
+ mParam.accStdev = DEFAULT_ACC_STDEV;
+ mParam.magStdev = DEFAULT_MAG_STDEV;
+ } else {
+ mParam.gyroVar = GEOMAG_GYRO_VAR;
+ mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR;
+ mParam.accStdev = GEOMAG_ACC_STDEV;
+ mParam.magStdev = GEOMAG_MAG_STDEV;
+ }
}
void Fusion::initFusion(const vec4_t& q, float dT)
@@ -205,11 +238,11 @@
const float dT3 = dT2*dT;
// variance of integrated output at 1/dT Hz (random drift)
- const float q00 = gyroVAR * dT + 0.33333f * biasVAR * dT3;
+ const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;
// variance of drift rate ramp
- const float q11 = biasVAR * dT;
- const float q10 = 0.5f * biasVAR * dT2;
+ const float q11 = mParam.gyroBiasVar * dT;
+ const float q10 = 0.5f * mParam.gyroBiasVar * dT2;
const float q01 = q10;
GQGt[0][0] = q00; // rad^2
@@ -223,7 +256,9 @@
}
bool Fusion::hasEstimate() const {
- return (mInitState == (MAG|ACC|GYRO));
+ return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) &&
+ ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) &&
+ (mInitState & ACC);
}
bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
@@ -234,6 +269,9 @@
mData[0] += d * (1/length(d));
mCount[0]++;
mInitState |= ACC;
+ if (mMode == FUSION_NOGYRO ) {
+ mGyroRate = dT;
+ }
} else if (what == MAG) {
mData[1] += d * (1/length(d));
mCount[1]++;
@@ -242,25 +280,29 @@
mGyroRate = dT;
mData[2] += d*dT;
mCount[2]++;
- if (mCount[2] == 64) {
- // 64 samples is good enough to estimate the gyro drift and
- // doesn't take too much time.
- mInitState |= GYRO;
- }
+ mInitState |= GYRO;
}
- if (mInitState == (MAG|ACC|GYRO)) {
+ if (hasEstimate()) {
// Average all the values we collected so far
mData[0] *= 1.0f/mCount[0];
- mData[1] *= 1.0f/mCount[1];
+ if (mMode != FUSION_NOMAG) {
+ mData[1] *= 1.0f/mCount[1];
+ }
mData[2] *= 1.0f/mCount[2];
// calculate the MRPs from the data collection, this gives us
// a rough estimate of our initial state
mat33_t R;
- vec3_t up(mData[0]);
- vec3_t east(cross_product(mData[1], up));
- east *= 1/length(east);
+ vec3_t up(mData[0]);
+ vec3_t east;
+
+ if (mMode != FUSION_NOMAG) {
+ east = normalize(cross_product(mData[1], up));
+ } else {
+ east = getOrthogonal(up);
+ }
+
vec3_t north(cross_product(up, east));
R << east << north << up;
const vec4_t q = matrixToQuat(R);
@@ -278,21 +320,43 @@
predict(w, dT);
}
-status_t Fusion::handleAcc(const vec3_t& a) {
+status_t Fusion::handleAcc(const vec3_t& a, float dT) {
+ if (!checkInitComplete(ACC, a, dT))
+ return BAD_VALUE;
+
// ignore acceleration data if we're close to free-fall
- if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) {
+ const float l = length(a);
+ if (l < FREE_FALL_THRESHOLD) {
return BAD_VALUE;
}
- if (!checkInitComplete(ACC, a))
- return BAD_VALUE;
+ const float l_inv = 1.0f/l;
- const float l = 1/length(a);
- update(a*l, Ba, accSTDEV*l);
+ if ( mMode == FUSION_NOGYRO ) {
+ //geo mag
+ vec3_t w_dummy;
+ w_dummy = x1; //bias
+ predict(w_dummy, dT);
+ }
+
+ if ( mMode == FUSION_NOMAG) {
+ vec3_t m;
+ m = getRotationMatrix()*Bm;
+ update(m, Bm, mParam.magStdev);
+ }
+
+ vec3_t unityA = a * l_inv;
+ const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY));
+ const float p = l_inv * mParam.accStdev*expf(d);
+
+ update(unityA, Ba, p);
return NO_ERROR;
}
status_t Fusion::handleMag(const vec3_t& m) {
+ if (!checkInitComplete(MAG, m))
+ return BAD_VALUE;
+
// the geomagnetic-field should be between 30uT and 60uT
// reject if too large to avoid spurious magnetic sources
const float magFieldSq = length_squared(m);
@@ -304,9 +368,6 @@
return BAD_VALUE;
}
- if (!checkInitComplete(MAG, m))
- return BAD_VALUE;
-
// Orthogonalize the magnetic field to the gravity field, mapping it into
// tangent to Earth.
const vec3_t up( getRotationMatrix() * Ba );
@@ -324,10 +385,10 @@
// then pass it in as the update.
vec3_t north( cross_product(up, east) );
- const float l = 1 / length(north);
- north *= l;
+ const float l_inv = 1 / length(north);
+ north *= l_inv;
- update(north, Bm, magSTDEV*l);
+ update(north, Bm, mParam.magStdev*l_inv);
return NO_ERROR;
}
@@ -372,8 +433,11 @@
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;
+ vec3_t we = w - b;
+ if (length(we) < WVEC_EPS) {
+ we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS;
+ }
// q(k+1) = O(we)*q(k)
// --------------------
//
@@ -406,7 +470,7 @@
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 ilwe = 1.f/length(we);
const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
const float k1 = sinf(lwedT);
const float k2 = cosf(hlwedT);
@@ -422,6 +486,7 @@
Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
x0 = O*q;
+
if (x0.w < 0)
x0 = -x0;
@@ -466,15 +531,37 @@
const vec3_t e(z - Bb);
const vec3_t dq(K[0]*e);
- const vec3_t db(K[1]*e);
q += getF(q)*(0.5f*dq);
x0 = normalize_quat(q);
- x1 += db;
+
+ if (mMode != FUSION_NOMAG) {
+ const vec3_t db(K[1]*e);
+ x1 += db;
+ }
checkState();
}
+vec3_t Fusion::getOrthogonal(const vec3_t &v) {
+ vec3_t w;
+ if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) {
+ w[0]=0.f;
+ w[1] = v[2];
+ w[2] = -v[1];
+ } else if (fabsf(v[1]) <= fabsf(v[2])) {
+ w[0] = v[2];
+ w[1] = 0.f;
+ w[2] = -v[0];
+ }else {
+ w[0] = v[1];
+ w[1] = -v[0];
+ w[2] = 0.f;
+ }
+ return normalize(w);
+}
+
+
// -----------------------------------------------------------------------
}; // namespace android
diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h
index 7062999..602779f 100644
--- a/services/sensorservice/Fusion.h
+++ b/services/sensorservice/Fusion.h
@@ -27,6 +27,13 @@
typedef mat<float, 3, 4> mat34_t;
+enum FUSION_MODE{
+ FUSION_9AXIS, // use accel gyro mag
+ FUSION_NOMAG, // use accel gyro (game rotation, gravity)
+ FUSION_NOGYRO, // use accel mag (geomag rotation)
+ NUM_FUSION_MODE
+};
+
class Fusion {
/*
* the state vector is made of two sub-vector containing respectively:
@@ -55,9 +62,9 @@
public:
Fusion();
- void init();
+ void init(int mode = FUSION_9AXIS);
void handleGyro(const vec3_t& w, float dT);
- status_t handleAcc(const vec3_t& a);
+ status_t handleAcc(const vec3_t& a, float dT);
status_t handleMag(const vec3_t& m);
vec4_t getAttitude() const;
vec3_t getBias() const;
@@ -65,12 +72,21 @@
bool hasEstimate() const;
private:
+ struct Parameter {
+ float gyroVar;
+ float gyroBiasVar;
+ float accStdev;
+ float magStdev;
+ } mParam;
+
mat<mat33_t, 2, 2> Phi;
vec3_t Ba, Bm;
uint32_t mInitState;
float mGyroRate;
vec<vec3_t, 3> mData;
size_t mCount[3];
+ int mMode;
+
enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
bool checkInitComplete(int, const vec3_t& w, float d = 0);
void initFusion(const vec4_t& q0, float dT);
@@ -78,6 +94,7 @@
void predict(const vec3_t& w, float dT);
void update(const vec3_t& z, const vec3_t& Bi, float sigma);
static mat34_t getF(const vec4_t& p);
+ static vec3_t getOrthogonal(const vec3_t &v);
};
}; // namespace android
diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp
index 3cb3745..f8279d2 100644
--- a/services/sensorservice/GravitySensor.cpp
+++ b/services/sensorservice/GravitySensor.cpp
@@ -47,9 +47,9 @@
const static double NS2S = 1.0 / 1000000000.0;
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
vec3_t g;
- if (!mSensorFusion.hasEstimate())
+ if (!mSensorFusion.hasEstimate(FUSION_NOMAG))
return false;
- const mat33_t R(mSensorFusion.getRotationMatrix());
+ const mat33_t R(mSensorFusion.getRotationMatrix(FUSION_NOMAG));
// FIXME: we need to estimate the length of gravity because
// the accelerometer may have a small scaling error. This
// translates to an offset in the linear-acceleration sensor.
@@ -67,11 +67,11 @@
}
status_t GravitySensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_NOMAG, ident, enabled);
}
status_t GravitySensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_NOMAG, ident, ns);
}
Sensor GravitySensor::getSensor() const {
diff --git a/services/sensorservice/OrientationSensor.cpp b/services/sensorservice/OrientationSensor.cpp
index 6d85cca..d55f336 100644
--- a/services/sensorservice/OrientationSensor.cpp
+++ b/services/sensorservice/OrientationSensor.cpp
@@ -66,11 +66,11 @@
}
status_t OrientationSensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
}
status_t OrientationSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
}
Sensor OrientationSensor::getSensor() const {
diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp
index cb305eb..238845b 100644
--- a/services/sensorservice/RotationVectorSensor.cpp
+++ b/services/sensorservice/RotationVectorSensor.cpp
@@ -27,9 +27,10 @@
namespace android {
// ---------------------------------------------------------------------------
-RotationVectorSensor::RotationVectorSensor()
+RotationVectorSensor::RotationVectorSensor(int mode)
: mSensorDevice(SensorDevice::getInstance()),
- mSensorFusion(SensorFusion::getInstance())
+ mSensorFusion(SensorFusion::getInstance()),
+ mMode(mode)
{
}
@@ -37,15 +38,15 @@
const sensors_event_t& event)
{
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
- if (mSensorFusion.hasEstimate()) {
- const vec4_t q(mSensorFusion.getAttitude());
+ if (mSensorFusion.hasEstimate(mMode)) {
+ const vec4_t q(mSensorFusion.getAttitude(mMode));
*outEvent = event;
outEvent->data[0] = q.x;
outEvent->data[1] = q.y;
outEvent->data[2] = q.z;
outEvent->data[3] = q.w;
- outEvent->sensor = '_rov';
- outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
+ outEvent->sensor = getSensorToken();
+ outEvent->type = getSensorType();
return true;
}
}
@@ -53,20 +54,20 @@
}
status_t RotationVectorSensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(mMode, ident, enabled);
}
status_t RotationVectorSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(mMode, ident, ns);
}
Sensor RotationVectorSensor::getSensor() const {
sensor_t hwSensor;
- hwSensor.name = "Rotation Vector Sensor";
+ hwSensor.name = getSensorName();
hwSensor.vendor = "AOSP";
hwSensor.version = 3;
- hwSensor.handle = '_rov';
- hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR;
+ hwSensor.handle = getSensorToken();
+ hwSensor.type = getSensorType();
hwSensor.maxRange = 1;
hwSensor.resolution = 1.0f / (1<<24);
hwSensor.power = mSensorFusion.getPowerUsage();
@@ -75,6 +76,48 @@
return sensor;
}
+int RotationVectorSensor::getSensorType() const {
+ switch(mMode) {
+ case FUSION_9AXIS:
+ return SENSOR_TYPE_ROTATION_VECTOR;
+ case FUSION_NOMAG:
+ return SENSOR_TYPE_GAME_ROTATION_VECTOR;
+ case FUSION_NOGYRO:
+ return SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
+ default:
+ assert(0);
+ return 0;
+ }
+}
+
+const char* RotationVectorSensor::getSensorName() const {
+ switch(mMode) {
+ case FUSION_9AXIS:
+ return "Rotation Vector Sensor";
+ case FUSION_NOMAG:
+ return "Game Rotation Vector Sensor";
+ case FUSION_NOGYRO:
+ return "GeoMag Rotation Vector Sensor";
+ default:
+ assert(0);
+ return NULL;
+ }
+}
+
+int RotationVectorSensor::getSensorToken() const {
+ switch(mMode) {
+ case FUSION_9AXIS:
+ return '_rov';
+ case FUSION_NOMAG:
+ return '_gar';
+ case FUSION_NOGYRO:
+ return '_geo';
+ default:
+ assert(0);
+ return 0;
+ }
+}
+
// ---------------------------------------------------------------------------
GyroDriftSensor::GyroDriftSensor()
@@ -102,11 +145,11 @@
}
status_t GyroDriftSensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
}
status_t GyroDriftSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
}
Sensor GyroDriftSensor::getSensor() const {
diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h
index bb97fe1..1fc316b 100644
--- a/services/sensorservice/RotationVectorSensor.h
+++ b/services/sensorservice/RotationVectorSensor.h
@@ -35,9 +35,14 @@
class RotationVectorSensor : public SensorInterface {
SensorDevice& mSensorDevice;
SensorFusion& mSensorFusion;
+ int mMode;
+
+ int getSensorType() const;
+ const char* getSensorName() const ;
+ int getSensorToken() const ;
public:
- RotationVectorSensor();
+ RotationVectorSensor(int mode = FUSION_9AXIS);
virtual bool process(sensors_event_t* outEvent,
const sensors_event_t& event);
virtual status_t activate(void* ident, bool enabled);
@@ -46,6 +51,16 @@
virtual bool isVirtual() const { return true; }
};
+class GameRotationVectorSensor : public RotationVectorSensor {
+public:
+ GameRotationVectorSensor() : RotationVectorSensor(FUSION_NOMAG) {}
+};
+
+class GeoMagRotationVectorSensor : public RotationVectorSensor {
+public:
+ GeoMagRotationVectorSensor() : RotationVectorSensor(FUSION_NOGYRO) {}
+};
+
class GyroDriftSensor : public SensorInterface {
SensorDevice& mSensorDevice;
SensorFusion& mSensorFusion;
diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp
index 6d93009..9863f62 100644
--- a/services/sensorservice/SensorFusion.cpp
+++ b/services/sensorservice/SensorFusion.cpp
@@ -25,11 +25,17 @@
SensorFusion::SensorFusion()
: mSensorDevice(SensorDevice::getInstance()),
- mEnabled(false), mGyroTime(0)
+ mAttitude(mAttitudes[FUSION_9AXIS]),
+ mGyroTime(0), mAccTime(0)
{
sensor_t const* list;
Sensor uncalibratedGyro;
ssize_t count = mSensorDevice.getSensorList(&list);
+
+ mEnabled[FUSION_9AXIS] = false;
+ mEnabled[FUSION_NOMAG] = false;
+ mEnabled[FUSION_NOGYRO] = false;
+
if (count > 0) {
for (size_t i=0 ; i<size_t(count) ; i++) {
if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
@@ -55,81 +61,121 @@
// and power/cpu usage.
mEstimatedGyroRate = 200;
mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;
- mFusion.init();
+
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ mFusions[i].init(i);
+ }
}
}
void SensorFusion::process(const sensors_event_t& event) {
+
if (event.type == mGyro.getType()) {
- if (mGyroTime != 0) {
- const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;
- mFusion.handleGyro(vec3_t(event.data), dT);
+ float dT;
+ if ( event.timestamp - mGyroTime> 0 &&
+ event.timestamp - mGyroTime< (int64_t)(5e7) ) { //0.05sec
+
+ dT = (event.timestamp - mGyroTime) / 1000000000.0f;
// here we estimate the gyro rate (useful for debugging)
const float freq = 1 / dT;
if (freq >= 100 && freq<1000) { // filter values obviously wrong
const float alpha = 1 / (1 + dT); // 1s time-constant
mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;
}
+
+ const vec3_t gyro(event.data);
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ if (mEnabled[i]) {
+ // fusion in no gyro mode will ignore
+ mFusions[i].handleGyro(gyro, dT);
+ }
+ }
}
mGyroTime = event.timestamp;
} else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
const vec3_t mag(event.data);
- mFusion.handleMag(mag);
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ if (mEnabled[i]) {
+ mFusions[i].handleMag(mag);// fusion in no mag mode will ignore
+ }
+ }
} else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
- const vec3_t acc(event.data);
- mFusion.handleAcc(acc);
- mAttitude = mFusion.getAttitude();
+ float dT;
+ if ( event.timestamp - mAccTime> 0 &&
+ event.timestamp - mAccTime< (int64_t)(1e8) ) { //0.1sec
+ dT = (event.timestamp - mAccTime) / 1000000000.0f;
+
+ const vec3_t acc(event.data);
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ if (mEnabled[i]) {
+ mFusions[i].handleAcc(acc, dT);
+ mAttitudes[i] = mFusions[i].getAttitude();
+ }
+ }
+ }
+ mAccTime = event.timestamp;
}
}
template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
-status_t SensorFusion::activate(void* ident, bool enabled) {
+status_t SensorFusion::activate(int mode, void* ident, bool enabled) {
ALOGD_IF(DEBUG_CONNECTIONS,
- "SensorFusion::activate(ident=%p, enabled=%d)",
- ident, enabled);
+ "SensorFusion::activate(mode=%d, ident=%p, enabled=%d)",
+ mode, ident, enabled);
- const ssize_t idx = mClients.indexOf(ident);
+ const ssize_t idx = mClients[mode].indexOf(ident);
if (enabled) {
if (idx < 0) {
- mClients.add(ident);
+ mClients[mode].add(ident);
}
} else {
if (idx >= 0) {
- mClients.removeItemsAt(idx);
+ mClients[mode].removeItemsAt(idx);
+ }
+ }
+
+ const bool newState = mClients[mode].size() != 0;
+ if (newState != mEnabled[mode]) {
+ mEnabled[mode] = newState;
+ if (newState) {
+ mFusions[mode].init(mode);
}
}
mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
- mSensorDevice.activate(ident, mMag.getHandle(), enabled);
- mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
+ if (mode != FUSION_NOMAG) {
+ mSensorDevice.activate(ident, mMag.getHandle(), enabled);
+ }
+ if (mode != FUSION_NOGYRO) {
+ mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
+ }
- const bool newState = mClients.size() != 0;
- if (newState != mEnabled) {
- mEnabled = newState;
- if (newState) {
- mFusion.init();
- mGyroTime = 0;
- }
+ return NO_ERROR;
+}
+
+status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) {
+ // Call batch with timeout zero instead of setDelay().
+ if (ns > (int64_t)5e7) {
+ ns = (int64_t)(5e7);
+ }
+ mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
+ if (mode != FUSION_NOMAG) {
+ mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(20), 0);
+ }
+ if (mode != FUSION_NOGYRO) {
+ mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
}
return NO_ERROR;
}
-status_t SensorFusion::setDelay(void* ident, int64_t ns) {
- // Call batch with timeout zero instead of setDelay().
- mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
- mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(20), 0);
- mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
- return NO_ERROR;
-}
-
-float SensorFusion::getPowerUsage() const {
+float SensorFusion::getPowerUsage(int mode) const {
float power = mAcc.getPowerUsage() +
- mMag.getPowerUsage() +
- mGyro.getPowerUsage();
+ ((mode != FUSION_NOMAG) ? mMag.getPowerUsage() : 0) +
+ ((mode != FUSION_NOGYRO) ? mGyro.getPowerUsage() : 0);
return power;
}
@@ -138,21 +184,55 @@
}
void SensorFusion::dump(String8& result) {
- const Fusion& fusion(mFusion);
+ const Fusion& fusion_9axis(mFusions[FUSION_9AXIS]);
result.appendFormat("9-axis fusion %s (%zd clients), gyro-rate=%7.2fHz, "
"q=< %g, %g, %g, %g > (%g), "
"b=< %g, %g, %g >\n",
- mEnabled ? "enabled" : "disabled",
- mClients.size(),
+ mEnabled[FUSION_9AXIS] ? "enabled" : "disabled",
+ mClients[FUSION_9AXIS].size(),
mEstimatedGyroRate,
- fusion.getAttitude().x,
- fusion.getAttitude().y,
- fusion.getAttitude().z,
- fusion.getAttitude().w,
- length(fusion.getAttitude()),
- fusion.getBias().x,
- fusion.getBias().y,
- fusion.getBias().z);
+ fusion_9axis.getAttitude().x,
+ fusion_9axis.getAttitude().y,
+ fusion_9axis.getAttitude().z,
+ fusion_9axis.getAttitude().w,
+ length(fusion_9axis.getAttitude()),
+ fusion_9axis.getBias().x,
+ fusion_9axis.getBias().y,
+ fusion_9axis.getBias().z);
+
+ const Fusion& fusion_nomag(mFusions[FUSION_NOMAG]);
+ result.appendFormat("game fusion(no mag) %s (%zd clients), "
+ "gyro-rate=%7.2fHz, "
+ "q=< %g, %g, %g, %g > (%g), "
+ "b=< %g, %g, %g >\n",
+ mEnabled[FUSION_NOMAG] ? "enabled" : "disabled",
+ mClients[FUSION_NOMAG].size(),
+ mEstimatedGyroRate,
+ fusion_nomag.getAttitude().x,
+ fusion_nomag.getAttitude().y,
+ fusion_nomag.getAttitude().z,
+ fusion_nomag.getAttitude().w,
+ length(fusion_nomag.getAttitude()),
+ fusion_nomag.getBias().x,
+ fusion_nomag.getBias().y,
+ fusion_nomag.getBias().z);
+
+ const Fusion& fusion_nogyro(mFusions[FUSION_NOGYRO]);
+ result.appendFormat("geomag fusion (no gyro) %s (%zd clients), "
+ "gyro-rate=%7.2fHz, "
+ "q=< %g, %g, %g, %g > (%g), "
+ "b=< %g, %g, %g >\n",
+ mEnabled[FUSION_NOGYRO] ? "enabled" : "disabled",
+ mClients[FUSION_NOGYRO].size(),
+ mEstimatedGyroRate,
+ fusion_nogyro.getAttitude().x,
+ fusion_nogyro.getAttitude().y,
+ fusion_nogyro.getAttitude().z,
+ fusion_nogyro.getAttitude().w,
+ length(fusion_nogyro.getAttitude()),
+ fusion_nogyro.getBias().x,
+ fusion_nogyro.getBias().y,
+ fusion_nogyro.getBias().z);
}
// ---------------------------------------------------------------------------
diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h
index 432adbc..ad636d5 100644
--- a/services/sensorservice/SensorFusion.h
+++ b/services/sensorservice/SensorFusion.h
@@ -42,30 +42,52 @@
Sensor mAcc;
Sensor mMag;
Sensor mGyro;
- Fusion mFusion;
- bool mEnabled;
+
+ Fusion mFusions[NUM_FUSION_MODE]; // normal, no_mag, no_gyro
+
+ bool mEnabled[NUM_FUSION_MODE];
+
+ vec4_t &mAttitude;
+ vec4_t mAttitudes[NUM_FUSION_MODE];
+
+ SortedVector<void*> mClients[3];
+
float mEstimatedGyroRate;
nsecs_t mTargetDelayNs;
+
nsecs_t mGyroTime;
- vec4_t mAttitude;
- SortedVector<void*> mClients;
+ nsecs_t mAccTime;
SensorFusion();
public:
void process(const sensors_event_t& event);
- bool isEnabled() const { return mEnabled; }
- bool hasEstimate() const { return mFusion.hasEstimate(); }
- mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); }
- vec4_t getAttitude() const { return mAttitude; }
- vec3_t getGyroBias() const { return mFusion.getBias(); }
+ bool isEnabled() const {
+ return mEnabled[FUSION_9AXIS] ||
+ mEnabled[FUSION_NOMAG] ||
+ mEnabled[FUSION_NOGYRO];
+ }
+
+ bool hasEstimate(int mode = FUSION_9AXIS) const {
+ return mFusions[mode].hasEstimate();
+ }
+
+ mat33_t getRotationMatrix(int mode = FUSION_9AXIS) const {
+ return mFusions[mode].getRotationMatrix();
+ }
+
+ vec4_t getAttitude(int mode = FUSION_9AXIS) const {
+ return mAttitudes[mode];
+ }
+
+ vec3_t getGyroBias() const { return mFusions[FUSION_9AXIS].getBias(); }
float getEstimatedRate() const { return mEstimatedGyroRate; }
- status_t activate(void* ident, bool enabled);
- status_t setDelay(void* ident, int64_t ns);
+ status_t activate(int mode, void* ident, bool enabled);
+ status_t setDelay(int mode, void* ident, int64_t ns);
- float getPowerUsage() const;
+ float getPowerUsage(int mode=FUSION_9AXIS) const;
int32_t getMinDelay() const;
void dump(String8& result);
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
old mode 100644
new mode 100755
index fd72b23..c73af3a
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -88,11 +88,14 @@
uint32_t virtualSensorsNeeds =
(1<<SENSOR_TYPE_GRAVITY) |
(1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
- (1<<SENSOR_TYPE_ROTATION_VECTOR);
+ (1<<SENSOR_TYPE_ROTATION_VECTOR) |
+ (1<<SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR) |
+ (1<<SENSOR_TYPE_GAME_ROTATION_VECTOR);
mLastEventSeen.setCapacity(count);
for (ssize_t i=0 ; i<count ; i++) {
- registerSensor( new HardwareSensor(list[i]) );
+ bool useThisSensor=true;
+
switch (list[i].type) {
case SENSOR_TYPE_ACCELEROMETER:
hasAccel = true;
@@ -110,9 +113,18 @@
case SENSOR_TYPE_GRAVITY:
case SENSOR_TYPE_LINEAR_ACCELERATION:
case SENSOR_TYPE_ROTATION_VECTOR:
- virtualSensorsNeeds &= ~(1<<list[i].type);
+ case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR:
+ case SENSOR_TYPE_GAME_ROTATION_VECTOR:
+ if (IGNORE_HARDWARE_FUSION) {
+ useThisSensor = false;
+ } else {
+ virtualSensorsNeeds &= ~(1<<list[i].type);
+ }
break;
}
+ if (useThisSensor) {
+ registerSensor( new HardwareSensor(list[i]) );
+ }
}
// it's safe to instantiate the SensorFusion object here
@@ -124,26 +136,15 @@
mUserSensorList = mSensorList;
if (hasGyro && hasAccel && hasMag) {
- Sensor aSensor;
-
// Add Android virtual sensors if they're not already
// available in the HAL
+ Sensor aSensor;
aSensor = registerVirtualSensor( new RotationVectorSensor() );
if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
mUserSensorList.add(aSensor);
}
- aSensor = registerVirtualSensor( new GravitySensor(list, count) );
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
- mUserSensorList.add(aSensor);
- }
-
- aSensor = registerVirtualSensor( new LinearAccelerationSensor(list, count) );
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
- mUserSensorList.add(aSensor);
- }
-
aSensor = registerVirtualSensor( new OrientationSensor() );
if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
// if we are doing our own rotation-vector, also add
@@ -151,11 +152,46 @@
mUserSensorList.replaceAt(aSensor, orientationIndex);
}
+ aSensor = registerVirtualSensor(
+ new LinearAccelerationSensor(list, count) );
+ if (virtualSensorsNeeds &
+ (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
+ mUserSensorList.add(aSensor);
+ }
+
// virtual debugging sensors are not added to mUserSensorList
registerVirtualSensor( new CorrectedGyroSensor(list, count) );
registerVirtualSensor( new GyroDriftSensor() );
}
+ if (hasAccel && hasGyro) {
+ Sensor aSensor;
+
+ aSensor = registerVirtualSensor(
+ new GravitySensor(list, count) );
+ if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
+ mUserSensorList.add(aSensor);
+ }
+
+ aSensor = registerVirtualSensor(
+ new GameRotationVectorSensor() );
+ if (virtualSensorsNeeds &
+ (1<<SENSOR_TYPE_GAME_ROTATION_VECTOR)) {
+ mUserSensorList.add(aSensor);
+ }
+ }
+
+ if (hasAccel && hasMag) {
+ Sensor aSensor;
+
+ aSensor = registerVirtualSensor(
+ new GeoMagRotationVectorSensor() );
+ if (virtualSensorsNeeds &
+ (1<<SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR)) {
+ mUserSensorList.add(aSensor);
+ }
+ }
+
// debugging sensor list
mUserSensorListDebug = mSensorList;
@@ -723,14 +759,7 @@
Vector<Sensor> accessibleSensorList;
for (size_t i = 0; i < initialSensorList.size(); i++) {
Sensor sensor = initialSensorList[i];
- if (canAccessSensor(sensor, "getSensorList", opPackageName)) {
- accessibleSensorList.add(sensor);
- } else {
- ALOGI("Skipped sensor %s because it requires permission %s and app op %d",
- sensor.getName().string(),
- sensor.getRequiredPermission().string(),
- sensor.getRequiredAppOp());
- }
+ accessibleSensorList.add(sensor);
}
return accessibleSensorList;
}
@@ -1166,6 +1195,9 @@
}
int SensorService::getNumEventsForSensorType(int sensor_event_type) {
+ if (sensor_event_type >= SENSOR_TYPE_DEVICE_PRIVATE_BASE) {
+ return 16;
+ }
switch (sensor_event_type) {
case SENSOR_TYPE_ROTATION_VECTOR:
case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR:
@@ -1623,21 +1655,26 @@
reAllocateCacheLocked(scratch, count);
return status_t(NO_ERROR);
}
- // Some events need to be dropped.
- int remaningCacheSize = mMaxCacheSize - mCacheSize;
- if (remaningCacheSize != 0) {
- memcpy(&mEventCache[mCacheSize], scratch,
- remaningCacheSize * sizeof(sensors_event_t));
- }
- int numEventsDropped = count - remaningCacheSize;
- countFlushCompleteEventsLocked(mEventCache, numEventsDropped);
- // Drop the first "numEventsDropped" in the cache.
- memmove(mEventCache, &mEventCache[numEventsDropped],
+ if (count >= mMaxCacheSize) {
+ //count is larger than MaxCacheSize,
+ //so copy the end MaxCacheSize data in scratch buffer to cache
+ countFlushCompleteEventsLocked(mEventCache, mCacheSize);
+ countFlushCompleteEventsLocked(scratch, count - mMaxCacheSize);
+ memcpy(&mEventCache[0], &scratch[count-mMaxCacheSize],
+ mMaxCacheSize * sizeof(sensors_event_t));
+ mCacheSize = mMaxCacheSize;
+ } else {
+ //count is smaller than MaxCacheSize, so numEventsDropped data in cache
+ //should be dropped, then copy the scratch to the end of cache
+ int numEventsDropped = count + mCacheSize - mMaxCacheSize;
+ countFlushCompleteEventsLocked(mEventCache, numEventsDropped);
+ memmove(mEventCache, &mEventCache[numEventsDropped],
(mCacheSize - numEventsDropped) * sizeof(sensors_event_t));
- // Copy the remainingEvents in scratch buffer to the end of cache.
- memcpy(&mEventCache[mCacheSize - numEventsDropped], scratch + remaningCacheSize,
- numEventsDropped * sizeof(sensors_event_t));
+ memcpy(&mEventCache[mCacheSize - numEventsDropped], scratch,
+ count * sizeof(sensors_event_t));
+ mCacheSize = mMaxCacheSize;
+ }
}
return status_t(NO_ERROR);
}
@@ -1656,6 +1693,7 @@
reinterpret_cast<ASensorEvent const*>(scratch), count);
if (size < 0) {
// Write error, copy events to local cache.
+ ALOGE("write failed, size=%d err=%d:%s ", size, errno, strerror(errno));
if (index_wake_up_event >= 0) {
// If there was a wake_up sensor_event, reset the flag.
scratch[index_wake_up_event].flags &= ~WAKE_UP_SENSOR_EVENT_NEEDS_ACK;
@@ -1671,8 +1709,21 @@
mEventCache = new sensors_event_t[mMaxCacheSize];
mCacheSize = 0;
}
- memcpy(&mEventCache[mCacheSize], scratch, count * sizeof(sensors_event_t));
- mCacheSize += count;
+ //here mCacheSize must be 0 because if (mCacheSize != 0) has been executed above
+ if (count <= mMaxCacheSize) {
+ //count is no more than free size of cache,
+ //copy the scratch to the end of cache
+ memcpy(&mEventCache[0], scratch, count * sizeof(sensors_event_t));
+ mCacheSize = count;
+ } else {
+ //count is larger than free size of cache,
+ //so just copy the free size of scratch to the end of cache
+ ALOGI("count > MaxCache count=%d mMaxCacheSize=%d", count, mMaxCacheSize);
+ countFlushCompleteEventsLocked(scratch, count - mMaxCacheSize);
+ memcpy(&mEventCache[0], &scratch[count - mMaxCacheSize],
+ mMaxCacheSize * sizeof(sensors_event_t));
+ mCacheSize = mMaxCacheSize;
+ }
// Add this file descriptor to the looper to get a callback when this fd is available for
// writing.
@@ -1725,6 +1776,7 @@
}
ssize_t size = SensorEventQueue::write(mChannel, &flushCompleteEvent, 1);
if (size < 0) {
+ ALOGE("write failed, size=%d err=%d:%s ", size, errno, strerror(errno));
if (wakeUpSensor) --mWakeLockRefCount;
return;
}
@@ -1760,6 +1812,7 @@
reinterpret_cast<ASensorEvent const*>(mEventCache + numEventsSent),
numEventsToWrite);
if (size < 0) {
+ ALOGE("write failed, size=%d err=%d:%s ", size, errno, strerror(errno));
if (index_wake_up_event >= 0) {
// If there was a wake_up sensor_event, reset the flag.
mEventCache[index_wake_up_event + numEventsSent].flags &=
diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h
index 9a573ae..7d81d6e 100644
--- a/services/sensorservice/SensorService.h
+++ b/services/sensorservice/SensorService.h
@@ -46,7 +46,7 @@
#endif
// ---------------------------------------------------------------------------
-
+#define IGNORE_HARDWARE_FUSION false
#define DEBUG_CONNECTIONS false
// Max size is 100 KB which is enough to accept a batch of about 1000 events.
#define MAX_SOCKET_BUFFER_SIZE_BATCHED 100 * 1024
