From aedbd2bfdcf85c9d2fdb4b95098c2c501048ef3f Mon Sep 17 00:00:00 2001 From: Saadi Maalem Date: Wed, 29 Jul 2015 15:09:14 +0800 Subject: sensor: add Gravity and Linear Accelerometer virtual sensors support JIRA: MARVIN-522 Change-Id: I8ea95dfd8c2fff3ed4df5c59d808d45fccb238d0 Signed-off-by: Fei Li Reviewed-on: https://android.intel.com/394732 Reviewed-by: Maalem, Saadi Reviewed-by: Tasayco Loarte, VictorX --- libsensors_iio/.config | 18 ++++++++--- libsensors_iio/Android.mk | 10 ++++-- libsensors_iio/Kconfig | 4 +-- libsensors_iio/Kconfig_menu/Kconfig_enable_sensors | 6 ++-- libsensors_iio/configuration.h | 11 +++++-- libsensors_iio/lib/vSensorFusion/vSensorAPI.h | 35 +++++++++++++++++++++ libsensors_iio/lib/vSensorFusion/vSensorFusion.a | Bin 0 -> 7616 bytes libsensors_iio/src/Android.mk | 4 +-- libsensors_iio/src/SWAccelGyroFusion6X.cpp | 26 ++++++++++++--- libsensors_iio/src/SWAccelGyroFusion6X.h | 4 ++- libsensors_iio/src/SWGravity.cpp | 6 ++-- libsensors_iio/src/SWLinearAccel.cpp | 6 ++-- libsensors_iio/src/SensorHAL.h | 4 ++- libsensors_iio/src/android_L_defconfig | 1 + 14 files changed, 105 insertions(+), 30 deletions(-) create mode 100644 libsensors_iio/lib/vSensorFusion/vSensorAPI.h create mode 100644 libsensors_iio/lib/vSensorFusion/vSensorFusion.a diff --git a/libsensors_iio/.config b/libsensors_iio/.config index c4a62b0..7c85cc0 100644 --- a/libsensors_iio/.config +++ b/libsensors_iio/.config @@ -3,7 +3,7 @@ # SensorHAL IIO Configuration # CONFIG_ST_HAL_ANDROID_VERSION=1 -# CONFIG_ST_HAL_HAS_6AX_FUSION is not set +CONFIG_ST_HAL_HAS_6AX_FUSION=y # CONFIG_ST_HAL_HAS_9AX_FUSION is not set # CONFIG_ST_HAL_HAS_GEOMAG_FUSION is not set # CONFIG_ST_HAL_HAS_MAGN_CALIB is not set @@ -22,12 +22,15 @@ CONFIG_ST_HAL_SIGN_MOTION_ENABLED=y CONFIG_ST_HAL_STEP_DETECTOR_ENABLED=y CONFIG_ST_HAL_STEP_COUNTER_ENABLED=y # CONFIG_ST_HAL_TILT_ENABLED is not set -CONFIG_ST_HAL_PRESSURE_ENABLED=y +# CONFIG_ST_HAL_PRESSURE_ENABLED is not set CONFIG_ST_HAL_ORIENTATION_DISABLED=y -CONFIG_ST_HAL_GRAVITY_DISABLED=y -CONFIG_ST_HAL_LINEAR_DISABLED=y +# CONFIG_ST_HAL_GRAVITY_DISABLED is not set +CONFIG_ST_HAL_GRAVITY_AP_ENABLED=y +# CONFIG_ST_HAL_LINEAR_DISABLED is not set +CONFIG_ST_HAL_LINEAR_AP_ENABLED=y CONFIG_ST_HAL_ROT_VECTOR_DISABLED=y CONFIG_ST_HAL_GAME_ROT_VECTOR_DISABLED=y +# CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED is not set CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_DISABLED=y # @@ -42,6 +45,13 @@ CONFIG_ST_HAL_MAGN_RANGE=1000 CONFIG_ST_HAL_GYRO_ROT_MATRIX="1,0,0,0,1,0,0,0,-1" CONFIG_ST_HAL_GYRO_RANGE=35 +# +# SensorFusion configuration +# +CONFIG_ST_HAL_MIN_FUSION_POLLRATE=50 +CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_ACCEL="86999e-9" +CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_GYRO="114999e-9" + # # IIO supported sensors # diff --git a/libsensors_iio/Android.mk b/libsensors_iio/Android.mk index 48d97bf..8bb9e95 100644 --- a/libsensors_iio/Android.mk +++ b/libsensors_iio/Android.mk @@ -55,9 +55,15 @@ export ST_HAL_HAS_GEOMAG_LIB=n endif ifneq ("$(wildcard $(CURRENT_DIRECTORY)/lib/iNemoEngine_SensorFusion/iNemoEngine_SensorFusion*)","") -export ST_HAL_HAS_9X_6X_LIB=y +export ST_HAL_HAS_9X_LIB=y else -export ST_HAL_HAS_9X_6X_LIB=n +export ST_HAL_HAS_9X_LIB=n +endif + +ifneq ("$(wildcard $(CURRENT_DIRECTORY)/lib/vSensorFusion/vSensorFusion*)","") +export ST_HAL_HAS_6X_LIB=y +else +export ST_HAL_HAS_6X_LIB=n endif ifneq ("$(wildcard $(CURRENT_DIRECTORY)/lib/STCompass/STCompass*)","") diff --git a/libsensors_iio/Kconfig b/libsensors_iio/Kconfig index 8619323..243d81e 100644 --- a/libsensors_iio/Kconfig +++ b/libsensors_iio/Kconfig @@ -27,11 +27,11 @@ endif config ST_HAL_HAS_6AX_FUSION_CHECK bool - option env="ST_HAL_HAS_9X_6X_LIB" + option env="ST_HAL_HAS_6X_LIB" config ST_HAL_HAS_9AX_FUSION_CHECK bool - option env="ST_HAL_HAS_9X_6X_LIB" + option env="ST_HAL_HAS_9X_LIB" config ST_HAL_HAS_GEOMAG_FUSION_CHECK bool diff --git a/libsensors_iio/Kconfig_menu/Kconfig_enable_sensors b/libsensors_iio/Kconfig_menu/Kconfig_enable_sensors index a36cbe3..740f88a 100644 --- a/libsensors_iio/Kconfig_menu/Kconfig_enable_sensors +++ b/libsensors_iio/Kconfig_menu/Kconfig_enable_sensors @@ -122,9 +122,8 @@ choice config ST_HAL_GRAVITY_DISABLED bool "Disabled" config ST_HAL_GRAVITY_AP_ENABLED - depends on ST_HAL_HAS_9AX_FUSION + depends on ST_HAL_HAS_6AX_FUSION depends on ST_HAL_ACCEL_ENABLED - depends on ST_HAL_MAGN_ENABLED depends on ST_HAL_GYRO_ENABLED bool "Software by Application Processor Library" endchoice @@ -138,9 +137,8 @@ choice config ST_HAL_LINEAR_DISABLED bool "Disabled" config ST_HAL_LINEAR_AP_ENABLED - depends on ST_HAL_HAS_9AX_FUSION + depends on ST_HAL_HAS_6AX_FUSION depends on ST_HAL_ACCEL_ENABLED - depends on ST_HAL_MAGN_ENABLED depends on ST_HAL_GYRO_ENABLED bool "Software by Application Processor Library" endchoice diff --git a/libsensors_iio/configuration.h b/libsensors_iio/configuration.h index a7bf75f..beead98 100644 --- a/libsensors_iio/configuration.h +++ b/libsensors_iio/configuration.h @@ -6,6 +6,7 @@ #define CONFIG_ST_HAL_ANDROID_VERSION 1 +#define CONFIG_ST_HAL_HAS_6AX_FUSION 1 #define CONFIG_ST_HAL_ACCEL_ENABLED 1 @@ -16,10 +17,9 @@ #define CONFIG_ST_HAL_SIGN_MOTION_ENABLED 1 #define CONFIG_ST_HAL_STEP_DETECTOR_ENABLED 1 #define CONFIG_ST_HAL_STEP_COUNTER_ENABLED 1 -#define CONFIG_ST_HAL_PRESSURE_ENABLED 1 #define CONFIG_ST_HAL_ORIENTATION_DISABLED 1 -#define CONFIG_ST_HAL_GRAVITY_DISABLED 1 -#define CONFIG_ST_HAL_LINEAR_DISABLED 1 +#define CONFIG_ST_HAL_GRAVITY_AP_ENABLED 1 +#define CONFIG_ST_HAL_LINEAR_AP_ENABLED 1 #define CONFIG_ST_HAL_ROT_VECTOR_DISABLED 1 #define CONFIG_ST_HAL_GAME_ROT_VECTOR_DISABLED 1 #define CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_DISABLED 1 @@ -35,6 +35,11 @@ #define CONFIG_ST_HAL_GYRO_RANGE 35 +#define CONFIG_ST_HAL_MIN_FUSION_POLLRATE 50 +#define CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_ACCEL 86999e-9 +#define CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_GYRO 114999e-9 + + #define CONFIG_ST_HAL_LSM6DS3_ENABLED 1 #endif /* __ST_SENSOR_HAL_CONFIG_H */ diff --git a/libsensors_iio/lib/vSensorFusion/vSensorAPI.h b/libsensors_iio/lib/vSensorFusion/vSensorAPI.h new file mode 100644 index 0000000..1a51303 --- /dev/null +++ b/libsensors_iio/lib/vSensorFusion/vSensorAPI.h @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2015 Intel Corp + * + * 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. + */ + +#ifndef __VSENSOR_API_H__ +#define __VSENSOR_API_H__ + +#include +#include + +static int init_flag; +static float grav[3]; +static float lin_acc[3]; + +void vSensor_API_Initialization_6X(void *); +void vSensor_API_enable_6X(bool enable); +int vSensor_API_Run_6X(float *accel, + float *gyro, long long timestamp); +int vSensor_API_Get_Quaternion_6X(float *rotation_vector); +int vSensor_API_Get_LinAcc_6X(float *lin_accel); +int vSensor_API_Get_Gravity_6X(float *garvity); + +#endif /* __VSENSOR_API_H__ */ diff --git a/libsensors_iio/lib/vSensorFusion/vSensorFusion.a b/libsensors_iio/lib/vSensorFusion/vSensorFusion.a new file mode 100644 index 0000000..ff5c763 Binary files /dev/null and b/libsensors_iio/lib/vSensorFusion/vSensorFusion.a differ diff --git a/libsensors_iio/src/Android.mk b/libsensors_iio/src/Android.mk index 91fbe61..39eca67 100644 --- a/libsensors_iio/src/Android.mk +++ b/libsensors_iio/src/Android.mk @@ -46,8 +46,8 @@ endif ifdef CONFIG_ST_HAL_HAS_6AX_FUSION -LOCAL_C_INCLUDES += $(LOCAL_PATH)/../lib/iNemoEngine_SensorFusion -LOCAL_STATIC_LIBRARIES += iNemoEngine_SensorFusion +LOCAL_C_INCLUDES += $(LOCAL_PATH)/../lib/vSensorFusion +LOCAL_LDFLAGS += $(LOCAL_PATH)/../lib/vSensorFusion/vSensorFusion.a else ifdef CONFIG_ST_HAL_HAS_9AX_FUSION LOCAL_C_INCLUDES += $(LOCAL_PATH)/../lib/iNemoEngine_SensorFusion diff --git a/libsensors_iio/src/SWAccelGyroFusion6X.cpp b/libsensors_iio/src/SWAccelGyroFusion6X.cpp index f03c950..83f8bd5 100644 --- a/libsensors_iio/src/SWAccelGyroFusion6X.cpp +++ b/libsensors_iio/src/SWAccelGyroFusion6X.cpp @@ -14,7 +14,7 @@ #include "SWAccelGyroFusion6X.h" extern "C" { - #include "iNemoEngineAPI.h" + #include "vSensorAPI.h" } SWAccelGyroFusion6X::SWAccelGyroFusion6X(const char *name, int handle, int pipe_data_fd) : @@ -30,7 +30,7 @@ SWAccelGyroFusion6X::SWAccelGyroFusion6X(const char *name, int handle, int pipe_ type_dependencies[SENSOR_BASE_DEPENDENCY_1] = SENSOR_TYPE_GYROSCOPE; type_sensor_need_trigger = SENSOR_TYPE_GYROSCOPE; - iNemoEngine_API_Initialization_6X(NULL); + vSensor_API_Initialization_6X(NULL); } SWAccelGyroFusion6X::~SWAccelGyroFusion6X() @@ -51,7 +51,7 @@ int SWAccelGyroFusion6X::Enable(int handle, bool enable) if ((GetStatus() && !old_status) || (!GetStatus() && old_status)) { sensor_event.timestamp = 0; - iNemoEngine_API_enable_6X(enable); + vSensor_API_enable_6X(enable); } return 0; @@ -84,6 +84,12 @@ void SWAccelGyroFusion6X::SplitAndProcessData(SensorBaseData data[ST_ACCEL_GYRO_ case SENSOR_TYPE_GAME_ROTATION_VECTOR: id = ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID; break; + case SENSOR_TYPE_LINEAR_ACCELERATION: + id = ST_ACCEL_GYRO_LINEAR_ACCEL_OUT_ID; + break; + case SENSOR_TYPE_GRAVITY: + id = ST_ACCEL_GYRO_GRAVITY_OUT_ID; + break; default: continue; } @@ -126,15 +132,25 @@ void SWAccelGyroFusion6X::TriggerEventReceived() } while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0)); if (err >= 0) - iNemoEngine_API_Run_6X(accel_data.raw, gyro_data.processed, gyro_data.timestamp); + vSensor_API_Run_6X(accel_data.raw, gyro_data.processed, gyro_data.timestamp); sensor_event.timestamp = gyro_data.timestamp; - err = iNemoEngine_API_Get_Quaternion_6X(outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].processed); + err = vSensor_API_Get_Quaternion_6X(outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].processed); + if (err < 0) + return; + + err = vSensor_API_Get_LinAcc_6X(outdata[ST_ACCEL_GYRO_LINEAR_ACCEL_OUT_ID].processed); + if (err < 0) + return; + + err = vSensor_API_Get_Gravity_6X(outdata[ST_ACCEL_GYRO_GRAVITY_OUT_ID].processed); if (err < 0) return; outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].timestamp = sensor_event.timestamp; + outdata[ST_ACCEL_GYRO_LINEAR_ACCEL_OUT_ID].timestamp = sensor_event.timestamp; + outdata[ST_ACCEL_GYRO_GRAVITY_OUT_ID].timestamp = sensor_event.timestamp; SplitAndProcessData(outdata); } while (data_remaining_gyro > 0); diff --git a/libsensors_iio/src/SWAccelGyroFusion6X.h b/libsensors_iio/src/SWAccelGyroFusion6X.h index 1e2fdd8..601191e 100644 --- a/libsensors_iio/src/SWAccelGyroFusion6X.h +++ b/libsensors_iio/src/SWAccelGyroFusion6X.h @@ -21,7 +21,9 @@ #include "SWSensorBase.h" #define ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID (0) -#define ST_ACCEL_GYRO_MAX_OUT_ID (1) +#define ST_ACCEL_GYRO_LINEAR_ACCEL_OUT_ID (1) +#define ST_ACCEL_GYRO_GRAVITY_OUT_ID (2) +#define ST_ACCEL_GYRO_MAX_OUT_ID (3) class SWAccelGyroFusion6X : public SWSensorBaseWithPollrate { protected: diff --git a/libsensors_iio/src/SWGravity.cpp b/libsensors_iio/src/SWGravity.cpp index 4bb8d35..34362aa 100644 --- a/libsensors_iio/src/SWGravity.cpp +++ b/libsensors_iio/src/SWGravity.cpp @@ -20,10 +20,10 @@ SWGravity::SWGravity(const char *name, int handle, int pipe_data_fd) : sensor_t_data.stringType = SENSOR_STRING_TYPE_GRAVITY; sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; - sensor_t_data.maxRange = CONFIG_ST_HAL_ACCEL_RANGE; + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X; + type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X; - type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; - type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; + num_data_axis = SENSOR_BASE_4AXIS; } SWGravity::~SWGravity() diff --git a/libsensors_iio/src/SWLinearAccel.cpp b/libsensors_iio/src/SWLinearAccel.cpp index 111d822..2afa018 100644 --- a/libsensors_iio/src/SWLinearAccel.cpp +++ b/libsensors_iio/src/SWLinearAccel.cpp @@ -20,10 +20,10 @@ SWLinearAccel::SWLinearAccel(const char *name, int handle, int pipe_data_fd) : sensor_t_data.stringType = SENSOR_STRING_TYPE_LINEAR_ACCELERATION; sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; - sensor_t_data.maxRange = CONFIG_ST_HAL_ACCEL_RANGE; + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X; + type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X; - type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; - type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; + num_data_axis = SENSOR_BASE_4AXIS; } SWLinearAccel::~SWLinearAccel() diff --git a/libsensors_iio/src/SensorHAL.h b/libsensors_iio/src/SensorHAL.h index 99d4646..9f5a557 100644 --- a/libsensors_iio/src/SensorHAL.h +++ b/libsensors_iio/src/SensorHAL.h @@ -67,7 +67,9 @@ #endif /* CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED */ #if defined(CONFIG_ST_HAL_HAS_6AX_FUSION) && \ - (defined(CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED)) + (defined(CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED) || \ + defined(CONFIG_ST_HAL_GRAVITY_AP_ENABLED) || \ + defined(CONFIG_ST_HAL_LINEAR_AP_ENABLED)) #define ST_HAL_NEEDS_6AX_FUSION 1 #endif /* CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED */ diff --git a/libsensors_iio/src/android_L_defconfig b/libsensors_iio/src/android_L_defconfig index 2add11a..297ebb9 100644 --- a/libsensors_iio/src/android_L_defconfig +++ b/libsensors_iio/src/android_L_defconfig @@ -53,6 +53,7 @@ CONFIG_ST_HAL_RELATIVE_TILT_DISABLED=y # # Common configuration # +CONFIG_ST_HAL_MIN_FUSION_POLLRATE=50 CONFIG_ST_HAL_MAX_SAMPLING_FREQUENCY=200 CONFIG_ST_HAL_DEBUG_LEVEL=0 CONFIG_ST_HAL_ACCEL_ROT_MATRIX="1,0,0,0,1,0,0,0,1" -- cgit v1.2.3