From f75e2d216835d8354543b80a213011d721478826 Mon Sep 17 00:00:00 2001 From: Fei Li Date: Mon, 15 Jun 2015 13:38:41 +0800 Subject: 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 Reviewed-on: https://android.intel.com/379867 Reviewed-by: jenkins_ndg Reviewed-by: Marcenac, GuillaumeX Reviewed-by: Tasayco Loarte, VictorX Reviewed-by: Maalem, Saadi Reviewed-by: Yan, Like Tested-by: Yan, Like --- libsensors_iio/src/Accelerometer.cpp | 6 ++- libsensors_iio/src/Android.mk | 1 + libsensors_iio/src/Gyroscope.cpp | 6 ++- libsensors_iio/src/HWSensorBase.h | 1 + libsensors_iio/src/SensorHAL.cpp | 3 ++ libsensors_iio/src/sensor_cal.c | 95 ++++++++++++++++++++++++++++++++++++ libsensors_iio/src/sensor_cal.h | 63 ++++++++++++++++++++++++ 7 files changed, 173 insertions(+), 2 deletions(-) create mode 100644 libsensors_iio/src/sensor_cal.c create mode 100644 libsensors_iio/src/sensor_cal.h (limited to 'libsensors_iio') 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 #include +#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 #include +#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 +#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 +#include + +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 -- cgit v1.2.3