sensor: add Gravity and Linear Accelerometer virtual sensors support

JIRA: MARVIN-522

Change-Id: I8ea95dfd8c2fff3ed4df5c59d808d45fccb238d0
Signed-off-by: Fei Li <feix.f.li@intel.com>
Reviewed-on: https://android.intel.com/394732
Reviewed-by: Maalem, Saadi <saadi.maalem@intel.com>
Reviewed-by: Tasayco Loarte, VictorX <victorx.tasayco.loarte@intel.com>

14 files changed, 105 insertions, 30 deletions

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
 
 #
@@ -43,6 +46,13 @@ 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
 #
 CONFIG_ST_HAL_LSM6DS3_ENABLED=y
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 <sys/types.h>
+#include <stdbool.h>
+
+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
--- /dev/null
+++ b/libsensors_iio/lib/vSensorFusion/vSensorFusion.a
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"