Fix calibration error in accel and gyro

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

diff --git a/src/accelerometer/accel_device.cpp b/src/accelerometer/accel_device.cpp
index 39898b8..4ca33d9 100644 (file)
--- a/src/accelerometer/accel_device.cpp
+++ b/src/accelerometer/accel_device.cpp
@@ -100,8 +100,6 @@ accel_device::accel_device()
                throw ENXIO;
        }
 
-       peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG5_XL, 0b00111000);
-       peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG6_XL, 0b01000000);
        update_value = [=]() {
                return this->update_value_i2c();
        };
@@ -145,25 +143,13 @@ bool accel_device::enable(uint32_t id)
        set_fullscale();
        peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG5_XL, 0b00111000);
 
-       //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 < 3; i++)
-               bias[i] = 0;
+       bias[0] = bias[1] = bias[2] = 0;
 
+       int samples = 50;
        for (int i = 0; i < samples; ++i){
-               update_value_i2c();
+               while (!update_value_i2c()) ;
+
                bias[0] += m_x;
                bias[1] += m_y;
                bias[2] += m_z - (int16_t)(1./res);
@@ -174,12 +160,6 @@ bool accel_device::enable(uint32_t id)
        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();
@@ -221,10 +201,10 @@ bool accel_device::set_interval(uint32_t id, unsigned long val)
 
 bool accel_device::update_value_i2c(void)
 {
-       uint16_t temp[3];
-       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]);
+       int16_t temp[3];
+       peripheral_i2c_read_register_word(m_i2c_handle, OUT_X_L_XL, (uint16_t*) &temp[0]);
+       peripheral_i2c_read_register_word(m_i2c_handle, OUT_Y_L_XL, (uint16_t*) &temp[1]);
+       peripheral_i2c_read_register_word(m_i2c_handle, OUT_Z_L_XL, (uint16_t*) &temp[2]);
 
        if ((temp[0] == m_x) && (temp[1] == m_y) && (temp[2] == m_z))
                return false;
@@ -285,6 +265,7 @@ void accel_device::set_fullscale(void)
        //set fullscale ±2g == ±19.8m/s^2
        peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG6_XL, &temp);
        temp &= 0b11100111;
+       temp |= 0b01000000;
        peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG6_XL, temp);
 
        //unit is 0.061mg == 0.0005978m/s^2

diff --git a/src/gyroscope/gyro_device.cpp b/src/gyroscope/gyro_device.cpp
index 560cf9c..f884433 100644 (file)
--- a/src/gyroscope/gyro_device.cpp
+++ b/src/gyroscope/gyro_device.cpp
@@ -140,40 +140,25 @@ bool gyro_device::enable(uint32_t id)
        //turn on the sensor
        set_fullscale();
        peripheral_i2c_read_register_byte(m_i2c_handle, CTRL_REG1_G, &temp);
-       temp |= 0b00100000;
+       temp |= 0b01000000;
        peripheral_i2c_write_register_byte(m_i2c_handle, CTRL_REG1_G, temp);
 
-       //set 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, 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 < 3; ++i)
-               bias[i] = 0;
+       bias[0] = bias[1] = bias[2] = 0;
 
+       int samples = 50;
        for (int i = 0; i < samples; ++i){
-               update_value_i2c();
+               while (!update_value_i2c()) ;
+
                bias[0] += m_x;
                bias[1] += m_y;
                bias[2] += m_z;
        }
+
        bias[0] /= samples;
        bias[1] /= samples;
        bias[2] /= samples;
 
-       //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 gyroscope sensor");
 
        m_update_event.start();
@@ -210,10 +195,10 @@ bool gyro_device::set_interval(uint32_t id, unsigned long val)
 
 bool gyro_device::update_value_i2c(void)
 {
-       uint16_t temp[3];
-       peripheral_i2c_read_register_word(m_i2c_handle, OUT_X_L_G, &temp[0]);
-       peripheral_i2c_read_register_word(m_i2c_handle, OUT_Y_L_G, &temp[1]);
-       peripheral_i2c_read_register_word(m_i2c_handle, OUT_Z_L_G, &temp[2]);
+       int16_t temp[3];
+       peripheral_i2c_read_register_word(m_i2c_handle, OUT_X_L_G, (uint16_t*) &temp[0]);
+       peripheral_i2c_read_register_word(m_i2c_handle, OUT_Y_L_G, (uint16_t*) &temp[1]);
+       peripheral_i2c_read_register_word(m_i2c_handle, OUT_Z_L_G, (uint16_t*) &temp[2]);
 
        if ((temp[0] == m_x) && (temp[1] == m_y) && (temp[2] == m_z))
                return false;