Sensor: load calibration data at boot

JIRA: MARVIN-79

Load sensors calibration data at boot, and set the corresponding offset of IIO channels
to calibrate raw data of sensors

Change-Id: Iaf5842fd46edd8445a0a2ba03e765fa82a4f0519
Signed-off-by: Fei Li <feix.f.li@intel.com>
Reviewed-on: https://android.intel.com/379867
Reviewed-by: jenkins_ndg <jenkins_ndg@intel.com>
Reviewed-by: Marcenac, GuillaumeX <guillaumex.marcenac@intel.com>
Reviewed-by: Tasayco Loarte, VictorX <victorx.tasayco.loarte@intel.com>
Reviewed-by: Maalem, Saadi <saadi.maalem@intel.com>
Reviewed-by: Yan, Like <like.yan@intel.com>
Tested-by: Yan, Like <like.yan@intel.com>

7 files changed, 173 insertions, 2 deletions

diff --git a/libsensors_iio/src/Accelerometer.cpp b/libsensors_iio/src/Accelerometer.cpp
index 98ed7c6..dc9d8c7 100644
--- a/libsensors_iio/src/Accelerometer.cpp
+++ b/libsensors_iio/src/Accelerometer.cpp
@@ -11,6 +11,7 @@
 #include <assert.h>
 #include <signal.h>
 
+#include "sensor_cal.h"
 #include "Accelerometer.h"
 
 Accelerometer::Accelerometer(HWSensorBaseCommonData *data, const char *name,
@@ -47,7 +48,10 @@ void Accelerometer::ProcessData(SensorBaseData *data)
 	sensor_event.acceleration.x = data->raw[0];
 	sensor_event.acceleration.y = data->raw[1];
 	sensor_event.acceleration.z = data->raw[2];
-	sensor_event.acceleration.status = SENSOR_STATUS_UNRELIABLE;
+	if (accl_cal_data_loaded == true)
+		sensor_event.acceleration.status = SENSOR_STATUS_ACCURACY_LOW;
+	else
+		sensor_event.acceleration.status = SENSOR_STATUS_UNRELIABLE;
 	sensor_event.timestamp = data->timestamp;
 
 	HWSensorBaseWithPollrate::WriteDataToPipe();
diff --git a/libsensors_iio/src/Android.mk b/libsensors_iio/src/Android.mk
index f9fe484..16264c3 100644
--- a/libsensors_iio/src/Android.mk
+++ b/libsensors_iio/src/Android.mk
@@ -77,6 +77,7 @@ endif
 
 
 LOCAL_SRC_FILES := \
+		sensor_cal.c \
 		iio_utils.c \
 		SensorHAL.cpp \
 		CircularBuffer.cpp \
diff --git a/libsensors_iio/src/Gyroscope.cpp b/libsensors_iio/src/Gyroscope.cpp
index aef27ff..20ab7b8 100644
--- a/libsensors_iio/src/Gyroscope.cpp
+++ b/libsensors_iio/src/Gyroscope.cpp
@@ -11,6 +11,7 @@
 #include <assert.h>
 #include <signal.h>
 
+#include "sensor_cal.h"
 #include "Gyroscope.h"
 
 #ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED
@@ -120,7 +121,10 @@ void Gyroscope::ProcessData(SensorBaseData *data)
 
 	sensor_event.gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 #else /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
-	sensor_event.gyro.status = SENSOR_STATUS_UNRELIABLE;
+	if (gyro_cal_data_loaded == true)
+		sensor_event.gyro.status = SENSOR_STATUS_ACCURACY_LOW;
+	else
+		sensor_event.gyro.status = SENSOR_STATUS_UNRELIABLE;
 #endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
 
 	data->processed[0] = data->raw[0] - data->offset[0];
diff --git a/libsensors_iio/src/HWSensorBase.h b/libsensors_iio/src/HWSensorBase.h
index c2b0440..d7fb8a0 100644
--- a/libsensors_iio/src/HWSensorBase.h
+++ b/libsensors_iio/src/HWSensorBase.h
@@ -26,6 +26,7 @@
 extern "C" {
 	#include "iio_utils.h"
 	#include "events.h"
+	#include "sensor_cal.h"
 };
 
 #define HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN	(2)
diff --git a/libsensors_iio/src/SensorHAL.cpp b/libsensors_iio/src/SensorHAL.cpp
index 6cba89d..e25771f 100644
--- a/libsensors_iio/src/SensorHAL.cpp
+++ b/libsensors_iio/src/SensorHAL.cpp
@@ -745,6 +745,9 @@ static int st_hal_open_sensors(const struct hw_module_t *module,
 	hal_data->poll_device.batch = st_hal_dev_batch;
 	hal_data->poll_device.flush = st_hal_dev_flush;
 
+	do_cal_data_loading(ACCEL_SINDEX);
+	do_cal_data_loading(GYRO_SINDEX);
+
 	*device = &hal_data->poll_device.common;
 
 	device_found_num = st_hal_load_iio_devices_data(iio_devices_data);
diff --git a/libsensors_iio/src/sensor_cal.c b/libsensors_iio/src/sensor_cal.c
new file mode 100644
index 0000000..1d14611
--- /dev/null
+++ b/libsensors_iio/src/sensor_cal.c
@@ -0,0 +1,95 @@
+/*
+ * 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.
+ */
+
+#include <cutils/log.h>
+#include "sensor_cal.h"
+
+/* Load sensor calibration data */
+int load_cali_data(const int sindex)
+{
+    int err = 0, num, sum;
+    FILE *filp;
+
+    filp = fopen(sensor_cali_data_path[sindex], "r");
+    if (filp == NULL) {
+        err = -errno;
+        ALOGE("failed to open %s, errno=%d", sensor_cali_data_path[sindex], err);
+        return err;
+    }
+
+    num = fscanf(filp, "%d %d %d %d",
+                    &cal_data[sindex][0], &cal_data[sindex][1],
+                    &cal_data[sindex][2], &sum);
+    if (num != 4) {
+        err = -EINVAL;
+        ALOGE("read %s data failed, num=%d", sensor_cali_data_path[sindex], num);
+        goto out;
+    }
+
+    if ((cal_data[sindex][0] + cal_data[sindex][1] + cal_data[sindex][2]) != sum) {
+        err = -EINVAL;
+        ALOGE("%s check sum error", sensor_cali_data_path[sindex]);
+        goto out;
+    }
+
+out:
+    fclose(filp);
+    return err;
+}
+
+/* Write calibration data to iio channel offset */
+void set_cali_offset(const int sindex)
+{
+#define MAX_BUF_LEN 64
+    int err, j;
+    FILE *filp;
+    char buf[MAX_BUF_LEN];
+
+    for (j = 0; j < (Z_AXIS_INDEX + 1); j++) {
+        err = snprintf(buf, MAX_BUF_LEN, "%s/%s",
+                sensor_sysfs_dir[sindex], sensor_offset[sindex][j]);
+        if (err < 0) {
+            ALOGE("%s/%s snprintf err=%d",
+                sensor_sysfs_dir[sindex], sensor_offset[sindex][j], err);
+            return;
+        }
+        filp = fopen(buf, "w");
+        if (filp == NULL) {
+            ALOGE("failed to open %s, errno=%d", buf, errno);
+            return;
+        }
+        fprintf(filp, "%d", cal_data[sindex][j]);
+        fclose(filp);
+    }
+
+    if (sindex == ACCEL_SINDEX)
+        accl_cal_data_loaded = true;
+    if (sindex == GYRO_SINDEX)
+        gyro_cal_data_loaded = true;
+}
+
+
+void do_cal_data_loading(const int sindex)
+{
+    int err;
+
+    err = load_cali_data(sindex);
+    if (err != 0)
+        return;
+
+    set_cali_offset(sindex);
+
+}
diff --git a/libsensors_iio/src/sensor_cal.h b/libsensors_iio/src/sensor_cal.h
new file mode 100644
index 0000000..3102b9d
--- /dev/null
+++ b/libsensors_iio/src/sensor_cal.h
@@ -0,0 +1,63 @@
+/*
+ * 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 __SENSOR_CAL_H__
+#define __SENSOR_CAL_H__
+
+#include <stdio.h>
+#include <stdbool.h>
+
+enum SENSOR_INDEX {
+    ACCEL_SINDEX = 0,
+    GYRO_SINDEX,
+};
+
+enum SENSOR_AXIS_INDEX {
+    X_AXIS_INDEX = 0,
+    Y_AXIS_INDEX,
+    Z_AXIS_INDEX,
+};
+
+static const char * const sensor_cali_data_path[] = {
+    [ACCEL_SINDEX] = "/config/sensor/accel_cali",
+    [GYRO_SINDEX] = "/config/sensor/gyro_cali",
+};
+
+static const char * const sensor_sysfs_dir[] = {
+    [ACCEL_SINDEX] = "/sys/devices/iio:device1",
+    [GYRO_SINDEX] = "/sys/devices/iio:device2",
+};
+
+static const char * const sensor_offset[][3] = {
+    {
+        [X_AXIS_INDEX] = "in_accel_x_offset",
+        [Y_AXIS_INDEX] = "in_accel_y_offset",
+        [Z_AXIS_INDEX] = "in_accel_z_offset",
+    },
+    {
+        [X_AXIS_INDEX] = "in_anglvel_x_offset",
+        [Y_AXIS_INDEX] = "in_anglvel_y_offset",
+        [Z_AXIS_INDEX] = "in_anglvel_z_offset",
+    },
+};
+
+static bool accl_cal_data_loaded;
+static bool gyro_cal_data_loaded;
+static int cal_data[2][3];
+
+void do_cal_data_loading(const int sindex);
+
+#endif