accel: add calibration and unit

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

2 files changed, 80 insertions, 28 deletions

diff --git a/src/accelerometer/accel_device.cpp b/src/accelerometer/accel_device.cpp
index e431b5d..6bb83ff 100644
--- 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
--- 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_ */