accel: add calibration and unit

Change-Id: I51fe95095381879c72c120a867048c8d35fe4e36
Signed-off-by: taemin.yeom <taemin.yeom@samsung.com>

diff --git a/src/accelerometer/accel_device.cpp b/src/accelerometer/accel_device.cpp
index e431b5d..6bb83ff 100644 (file)
--- a/src/accelerometer/accel_device.cpp
+++ b/src/accelerometer/accel_device.cpp
@@ -29,33 +29,30 @@
 
 #define MODEL_NAME "LSM9DS1"
 #define VENDOR "ST Microelectronics"
-#define RESOLUTION 16
-#define RAW_DATA_UNIT 0.122
+
 #define MIN_INTERVAL 10
 #define MAX_BATCH_COUNT 0
 
 #define SENSOR_NAME "SENSOR_ACCELEROMETER"
-#define SENSOR_TYPE_ACCEL              "ACCEL"
+#define SENSOR_TYPE_ACCEL "ACCEL"
 
 #define INPUT_NAME     "accelerometer_sensor"
 #define ACCEL_SENSORHUB_POLL_NODE_NAME "accel_poll_delay"
 
 #define GRAVITY 9.80665
-#define G_TO_MG 1000
-#define RAW_DATA_TO_G_UNIT(X) (((float)(X))/((float)G_TO_MG))
-#define RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(X) (GRAVITY * (RAW_DATA_TO_G_UNIT(X)))
-
-#define MIN_RANGE(RES) (-((1 << (RES))/2))
-#define MAX_RANGE(RES) (((1 << (RES))/2)-1)
 
 #define MAX_ID 0x3
 
-#define I2C_BUS_ADDRESS     1
-#define I2C_SLAVE_ADDRESS   0x6A
+#define I2C_BUS_ADDRESS 1
+#define I2C_SLAVE_ADDRESS 0x6A
 
 #define CTRL_REG5_XL 0x1F
 #define CTRL_REG6_XL 0x20
 
+#define CTRL_REG9 0x23
+#define FIFO_CTRL 0x2E
+#define FIFO_SRC 0x2F
+
 #define OUT_X_L_XL 0x28
 #define OUT_X_H_XL 0x29
 #define OUT_Y_L_XL 0x2A
@@ -70,9 +67,9 @@ static sensor_info_t sensor_info = {
        event_type: (SENSOR_DEVICE_ACCELEROMETER << SENSOR_EVENT_SHIFT) | RAW_DATA_EVENT,
        model_name: MODEL_NAME,
        vendor: VENDOR,
-       min_range: MIN_RANGE(RESOLUTION) * RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
-       max_range: MAX_RANGE(RESOLUTION) * RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
-       resolution: RAW_DATA_TO_METRE_PER_SECOND_SQUARED_UNIT(RAW_DATA_UNIT),
+       min_range: -19.6,
+       max_range: 19.6,
+       resolution: (0.000061 * GRAVITY),
        min_interval: MIN_INTERVAL,
        max_batch_count: MAX_BATCH_COUNT,
        wakeup_supported: false
@@ -145,8 +142,42 @@ bool accel_device::enable(uint32_t id)
 {
        retvm_if(id == 0 || id > MAX_ID, false, "%s:Invalid ID Received", SENSOR_NAME);
 
+       //turn on the sensor
+       set_fullscale();
        peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG5_XL, 0b00111000);
-       peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG6_XL, 0b01000000);
+
+       //set FIFO
+       uint8_t temp;
+       peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG9, &temp);
+       temp |= 0b00000010;
+       peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG9, temp);
+       peripheral_i2c_write_register_byte(m_i2c_handle, FIFO_CTRL, 0b00111111);
+
+       uint16_t samples = 0;
+       while (samples < 0x1F){
+               peripheral_i2c_read_register_word(m_i2c_handle, FIFO_SRC, &samples);
+               samples &= 0x3F;
+       }
+
+       //calibration
+       for (int i = 0; i < samples; ++i){
+               update_value_i2c();
+               bias[0] += m_x;
+               bias[1] += m_y;
+               bias[2] += m_z - (int16_t)(1./res);
+       }
+
+       bias[0] /= samples;
+       bias[1] /= samples;
+       bias[2] /= samples;
+       res *= GRAVITY;
+
+       //disable FIFO
+       peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG9, &temp);
+       temp &= ~(0b00000010);
+       peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG9, temp);
+       peripheral_i2c_write_register_byte(m_i2c_handle, FIFO_CTRL, 0);
+
        _I("Enable accelerometer sensor");
 
        m_update_event.start();
@@ -188,19 +219,15 @@ bool accel_device::set_interval(uint32_t id, unsigned long val)
 
 bool accel_device::update_value_i2c(void)
 {
-       uint8_t temp[6];
+       uint16_t temp[3];
        int x, y, z;
-       peripheral_i2c_read_register_byte(m_i2c_handle, OUT_X_L_XL, &temp[0]);
-       peripheral_i2c_read_register_byte(m_i2c_handle, OUT_X_H_XL, &temp[1]);
-       peripheral_i2c_read_register_byte(m_i2c_handle, OUT_Y_L_XL, &temp[2]);
-       peripheral_i2c_read_register_byte(m_i2c_handle, OUT_Y_H_XL, &temp[3]);
-       peripheral_i2c_read_register_byte(m_i2c_handle, OUT_Z_L_XL, &temp[4]);
-       peripheral_i2c_read_register_byte(m_i2c_handle, OUT_Z_H_XL, &temp[5]);
-
-       //high 8bits and low 8bits
-       x = (temp[1] << 8) | temp[0];
-       y = (temp[3] << 8) | temp[2];
-       z = (temp[5] << 8) | temp[4];
+       peripheral_i2c_read_register_word(m_i2c_handle, OUT_X_L_XL, &temp[0]);
+       peripheral_i2c_read_register_word(m_i2c_handle, OUT_Y_L_XL, &temp[1]);
+       peripheral_i2c_read_register_word(m_i2c_handle, OUT_Z_L_XL, &temp[2]);
+
+       x = temp[0];
+       y = temp[1];
+       z = temp[2];
 
        if ((x == m_x) && (y == m_y) && (z == m_z))
                return false;
@@ -246,10 +273,30 @@ int accel_device::get_data(uint32_t id, sensor_data_t **data, int *length)
        sensor_data->values[1] = m_y;
        sensor_data->values[2] = m_z;
 
-       //raw_to_base(sensor_data);
+       raw_to_base(sensor_data);
 
        *data = sensor_data;
        *length = sizeof(sensor_data_t);
 
        return 0;
 }
+
+void accel_device::set_fullscale(void)
+{
+       uint8_t temp;
+
+       //set fullscale ±2g == ±19.8m/s^2
+       peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG6_XL, &temp);
+       temp &= 0b11100111;
+       peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG6_XL, temp);
+
+       //unit is 0.061mg == 0.0005978m/s^2
+       res = 0.000061;
+}
+
+void accel_device::raw_to_base(sensor_data_t *data)
+{
+       data->values[0] = (data->values[0] - bias[0]) * res;
+       data->values[1] = (data->values[1] - bias[1]) * res;
+       data->values[2] = (data->values[2] - bias[2]) * res;
+}

diff --git a/src/accelerometer/accel_device.h b/src/accelerometer/accel_device.h
index 29f85c4..d9d1896 100644 (file)
--- a/src/accelerometer/accel_device.h
+++ b/src/accelerometer/accel_device.h
@@ -46,6 +46,9 @@ private:
        int m_x;
        int m_y;
        int m_z;
+       int bias[3];
+       float res;
+
        unsigned long m_polling_interval;
        unsigned long long m_fired_time;
        bool m_sensorhub_controlled;
@@ -58,5 +61,7 @@ private:
        sensor_update_event m_update_event;
 
        bool update_value_i2c(void);
+       void set_fullscale(void);
+       void raw_to_base(sensor_data_t *data);
 };
 #endif /* _ACCEL_DEVICE_H_ */