prvy driver

readme.md

@@ -0,0 +1,2 @@
+
+# ICM 20948 driver for STM32

src/icm20948.c

@@ -0,0 +1,606 @@
+/*
+* @file icm20948.c
+*
+*  Created on: Dec 26, 2020
+*      Author: mokhwasomssi
+*
+*  Modified: Juraj Dudak
+*  Date: 30.12.2022
+*/
+
+
+#include "icm20948.h"
+
+McuPin_typeDef *pin_cs;
+McuPin_typeDef *pin_int;
+SPI_HandleTypeDef *spi;
+
+static float gyro_scale_factor;
+static float accel_scale_factor;
+
+static int8_t active_bank = -1;
+
+/* Static Functions */
+static void     select_user_bank(userbank ub);
+
+static uint8_t  read_single_icm20948_reg(userbank ub, uint8_t reg);
+static void     write_single_icm20948_reg(userbank ub, uint8_t reg, uint8_t val);
+static uint8_t* read_multiple_icm20948_reg(userbank ub, uint8_t reg, uint8_t len);
+static void     write_multiple_icm20948_reg(userbank ub, uint8_t reg, uint8_t* val, uint8_t len);
+
+static uint8_t  read_single_ak09916_reg(uint8_t reg);
+static void     write_single_ak09916_reg(uint8_t reg, uint8_t val);
+static uint8_t* read_multiple_ak09916_reg(uint8_t reg, uint8_t len);
+
+Sensor_TypeDef accSensor = {
+		icm20948_accel_read,
+		icm20948_accel_read_g
+};
+
+Sensor_TypeDef gyroSensor = {
+		icm20948_gyro_read,
+		icm20948_gyro_read_dps
+};
+
+Sensor_TypeDef magSensor = {
+		ak09916_mag_read,
+		ak09916_mag_read_uT
+};
+
+Device_TypeDef icmModule = {
+		icm20948_init,
+		icm20948_read,
+		icm20948_device_reset,
+		icm20948_wakeup,
+		icm20948_sleep,
+		icm20948_calibration,
+};
+
+axises ic_gyro;
+axises ic_accel;
+
+/* Main Functions */
+void icm20948_init(SPI_HandleTypeDef *hspi, McuPin_typeDef *pinCs, McuPin_typeDef *pinInt)
+{
+	pin_cs = pinCs;
+	pin_int = pinInt;
+	spi = hspi;
+	while(!icm20948_who_am_i());
+
+	icm20948_device_reset();
+	icm20948_wakeup();
+
+	icm20948_clock_source(1);
+	icm20948_odr_align_enable();
+	
+	icm20948_spi_slave_enable();
+	
+	icm20948_gyro_low_pass_filter(0);
+	icm20948_accel_low_pass_filter(0);
+
+	icm20948_gyro_sample_rate_divider(0);
+	icm20948_accel_sample_rate_divider(0);
+
+	if(pin_int != NULL){
+		icm20948_set_int();
+	}
+
+	icm20948_gyro_calibration();
+	icm20948_accel_calibration();
+
+	icm20948_gyro_full_scale_select(_2000dps);
+	icm20948_accel_full_scale_select(_16g);
+
+	ak09916_init();
+}
+
+void ak09916_init()
+{
+	icm20948_i2c_master_reset();
+	icm20948_i2c_master_enable();
+	icm20948_i2c_master_clk_frq(7);
+
+	while(!ak09916_who_am_i());
+
+	ak09916_soft_reset();
+	ak09916_operation_mode_setting(continuous_measurement_100hz);
+}
+
+void icm20948_read(void)
+{
+	uint8_t* temp = read_multiple_icm20948_reg(ub_0, B0_ACCEL_XOUT_H, 12);
+
+	ic_accel.x = (int16_t)(temp[0] << 8 | temp[1]);
+	ic_accel.y = (int16_t)(temp[2] << 8 | temp[3]);
+	ic_accel.z = (int16_t)(temp[4] << 8 | temp[5]) + accel_scale_factor;
+	// Add scale factor because calibration function offset gravity acceleration.
+
+	ic_gyro.x = (int16_t)(temp[6] << 8 | temp[7]);
+	ic_gyro.y = (int16_t)(temp[8] << 8 | temp[9]);
+	ic_gyro.z = (int16_t)(temp[10] << 8 | temp[11]);
+}
+
+axises *icm20948_acc_get()
+{
+	return &ic_accel;
+}
+
+axises *icm20948_gyro_get()
+{
+	return &ic_gyro;
+}
+
+axises *icm20948_max_get()
+{
+	return NULL;
+}
+
+
+void icm20948_set_int(void)
+{
+	write_single_icm20948_reg(ub_0, B0_INT_PIN_CFG, ( INT_PIN_CFG_LATCH | INT_PIN_CFG_2CLEAR));
+}
+
+
+bool icm20948_gyro_read(axises* data)
+{
+	uint8_t* temp = read_multiple_icm20948_reg(ub_0, B0_GYRO_XOUT_H, 6);
+
+	data->x = (int16_t)(temp[0] << 8 | temp[1]);
+	data->y = (int16_t)(temp[2] << 8 | temp[3]);
+	data->z = (int16_t)(temp[4] << 8 | temp[5]);
+
+	return true;
+}
+
+bool icm20948_accel_read(axises* data)
+{
+	uint8_t* temp = read_multiple_icm20948_reg(ub_0, B0_ACCEL_XOUT_H, 6);
+
+	data->x = (int16_t)(temp[0] << 8 | temp[1]);
+	data->y = (int16_t)(temp[2] << 8 | temp[3]);
+	data->z = (int16_t)(temp[4] << 8 | temp[5]) + accel_scale_factor; 
+	// Add scale factor because calibraiton function offset gravity acceleration.
+
+	return true;
+}
+
+bool ak09916_mag_read(axises* data)
+{
+	uint8_t* temp;
+	uint8_t drdy;	// data ready
+	uint8_t hofl;	// overflow
+
+	drdy = read_single_ak09916_reg(MAG_ST1) & 0x01;
+	if(!drdy)	return false;
+
+	temp = read_multiple_ak09916_reg(MAG_HXL, 6);
+
+	hofl = read_single_ak09916_reg(MAG_ST2) & 0x08;
+	if(hofl)	return false;
+
+	data->x = (int16_t)(temp[1] << 8 | temp[0]);
+	data->y = (int16_t)(temp[3] << 8 | temp[2]);
+	data->z = (int16_t)(temp[5] << 8 | temp[4]);
+
+	return true;
+}
+
+bool icm20948_gyro_read_dps(axises* data)
+{
+	icm20948_gyro_read(data);
+
+	data->x /= gyro_scale_factor;
+	data->y /= gyro_scale_factor;
+	data->z /= gyro_scale_factor;
+
+	return true;
+}
+
+bool icm20948_accel_read_g(axises* data)
+{
+	icm20948_accel_read(data);
+
+	data->x /= accel_scale_factor;
+	data->y /= accel_scale_factor;
+	data->z /= accel_scale_factor;
+
+	return true;
+}
+
+bool ak09916_mag_read_uT(axises* data)
+{
+	axises temp;
+	bool new_data = ak09916_mag_read(&temp);
+	if(!new_data)	return false;
+
+	data->x = (float)(temp.x * 0.15);
+	data->y = (float)(temp.y * 0.15);
+	data->z = (float)(temp.z * 0.15);
+
+	return true;
+}	
+
+
+/* Sub Functions */
+bool icm20948_who_am_i()
+{
+	uint8_t icm20948_id = read_single_icm20948_reg(ub_0, B0_WHO_AM_I);
+
+	if(icm20948_id == ICM20948_ID)
+		return true;
+	else
+		return false;
+}
+
+bool ak09916_who_am_i()
+{
+	uint8_t ak09916_id = read_single_ak09916_reg(MAG_WIA2);
+
+	if(ak09916_id == AK09916_ID)
+		return true;
+	else
+		return false;
+}
+
+void icm20948_device_reset()
+{
+	write_single_icm20948_reg(ub_0, B0_PWR_MGMT_1, 0x80 | 0x41);
+	HAL_Delay(100);
+}
+
+void ak09916_soft_reset()
+{
+	write_single_ak09916_reg(MAG_CNTL3, 0x01);
+	HAL_Delay(100);
+}
+
+void icm20948_wakeup()
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_0, B0_PWR_MGMT_1);
+	new_val &= 0xBF;
+
+	write_single_icm20948_reg(ub_0, B0_PWR_MGMT_1, new_val);
+	HAL_Delay(100);
+}
+
+void icm20948_sleep()
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_0, B0_PWR_MGMT_1);
+	new_val |= 0x40;
+
+	write_single_icm20948_reg(ub_0, B0_PWR_MGMT_1, new_val);
+	HAL_Delay(100);
+}
+
+void icm20948_spi_slave_enable()
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_0, B0_USER_CTRL);
+	new_val |= 0x10;
+
+	write_single_icm20948_reg(ub_0, B0_USER_CTRL, new_val);
+}
+
+void icm20948_i2c_master_reset()
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_0, B0_USER_CTRL);
+	new_val |= 0x02;
+
+	write_single_icm20948_reg(ub_0, B0_USER_CTRL, new_val);
+}
+
+void icm20948_i2c_master_enable()
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_0, B0_USER_CTRL);
+	new_val |= 0x20;
+
+	write_single_icm20948_reg(ub_0, B0_USER_CTRL, new_val);
+	HAL_Delay(100);
+}
+
+void icm20948_i2c_master_clk_frq(uint8_t config)
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_3, B3_I2C_MST_CTRL);
+	new_val |= config;
+
+	write_single_icm20948_reg(ub_3, B3_I2C_MST_CTRL, new_val);	
+}
+
+void icm20948_clock_source(uint8_t source)
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_0, B0_PWR_MGMT_1);
+	new_val |= source;
+
+	write_single_icm20948_reg(ub_0, B0_PWR_MGMT_1, new_val);
+}
+
+void icm20948_odr_align_enable()
+{
+	write_single_icm20948_reg(ub_2, B2_ODR_ALIGN_EN, 0x01);
+}
+
+void icm20948_gyro_low_pass_filter(uint8_t config)
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_2, B2_GYRO_CONFIG_1);
+	new_val |= config << 3;
+
+	write_single_icm20948_reg(ub_2, B2_GYRO_CONFIG_1, new_val);
+}
+
+void icm20948_accel_low_pass_filter(uint8_t config)
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_2, B2_ACCEL_CONFIG);
+	new_val |= config << 3;
+
+	write_single_icm20948_reg(ub_2, B2_GYRO_CONFIG_1, new_val);
+}
+
+void icm20948_gyro_sample_rate_divider(uint8_t divider)
+{
+	write_single_icm20948_reg(ub_2, B2_GYRO_SMPLRT_DIV, divider);
+}
+
+void icm20948_accel_sample_rate_divider(uint16_t divider)
+{
+	uint8_t divider_1 = (uint8_t)(divider >> 8);
+	uint8_t divider_2 = (uint8_t)(0x0F & divider);
+
+	write_single_icm20948_reg(ub_2, B2_ACCEL_SMPLRT_DIV_1, divider_1);
+	write_single_icm20948_reg(ub_2, B2_ACCEL_SMPLRT_DIV_2, divider_2);
+}
+
+void ak09916_operation_mode_setting(operation_mode mode)
+{
+	write_single_ak09916_reg(MAG_CNTL2, mode);
+	HAL_Delay(100);
+}
+
+void icm20948_calibration(void)
+{
+	icm20948_gyro_calibration();
+	icm20948_accel_calibration();
+}
+
+void icm20948_gyro_calibration(void)
+{
+	axises temp;
+	int32_t gyro_bias[3] = {0};
+	uint8_t gyro_offset[6] = {0};
+
+	for(int i = 0; i < 100; i++)
+	{
+		icm20948_gyro_read(&temp);
+		gyro_bias[0] += temp.x;
+		gyro_bias[1] += temp.y;
+		gyro_bias[2] += temp.z;
+	}
+
+	gyro_bias[0] /= 100;
+	gyro_bias[1] /= 100;
+	gyro_bias[2] /= 100;
+
+	// Construct the gyro biases for push to the hardware gyro bias registers,
+	// which are reset to zero upon device startup.
+	// Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format.
+	// Biases are additive, so change sign on calculated average gyro biases
+	gyro_offset[0] = (-gyro_bias[0] / 4  >> 8) & 0xFF; 
+	gyro_offset[1] = (-gyro_bias[0] / 4)       & 0xFF; 
+	gyro_offset[2] = (-gyro_bias[1] / 4  >> 8) & 0xFF;
+	gyro_offset[3] = (-gyro_bias[1] / 4)       & 0xFF;
+	gyro_offset[4] = (-gyro_bias[2] / 4  >> 8) & 0xFF;
+	gyro_offset[5] = (-gyro_bias[2] / 4)       & 0xFF;
+	
+	write_multiple_icm20948_reg(ub_2, B2_XG_OFFS_USRH, gyro_offset, 6);
+}
+
+void icm20948_accel_calibration()
+{
+	axises temp;
+	uint8_t* temp2;
+	uint8_t* temp3;
+	uint8_t* temp4;
+	
+	int32_t accel_bias[3] = {0};
+	int32_t accel_bias_reg[3] = {0};
+	uint8_t accel_offset[6] = {0};
+
+	for(int i = 0; i < 100; i++)
+	{
+		icm20948_accel_read(&temp);
+		accel_bias[0] += temp.x;
+		accel_bias[1] += temp.y;
+		accel_bias[2] += temp.z;
+	}
+
+	accel_bias[0] /= 100;
+	accel_bias[1] /= 100;
+	accel_bias[2] /= 100;
+
+	uint8_t mask_bit[3] = {0, 0, 0};
+
+	temp2 = read_multiple_icm20948_reg(ub_1, B1_XA_OFFS_H, 2);
+	accel_bias_reg[0] = (int32_t)(temp2[0] << 8 | temp2[1]);
+	mask_bit[0] = temp2[1] & 0x01;
+
+	temp3 = read_multiple_icm20948_reg(ub_1, B1_YA_OFFS_H, 2);
+	accel_bias_reg[1] = (int32_t)(temp3[0] << 8 | temp3[1]);
+	mask_bit[1] = temp3[1] & 0x01;
+
+	temp4 = read_multiple_icm20948_reg(ub_1, B1_ZA_OFFS_H, 2);
+	accel_bias_reg[2] = (int32_t)(temp4[0] << 8 | temp4[1]);
+	mask_bit[2] = temp4[1] & 0x01;
+
+	accel_bias_reg[0] -= (accel_bias[0] / 8);
+	accel_bias_reg[1] -= (accel_bias[1] / 8);
+	accel_bias_reg[2] -= (accel_bias[2] / 8);
+
+	accel_offset[0] = (accel_bias_reg[0] >> 8) & 0xFF;
+  	accel_offset[1] = (accel_bias_reg[0])      & 0xFE;
+	accel_offset[1] = accel_offset[1] | mask_bit[0];
+
+	accel_offset[2] = (accel_bias_reg[1] >> 8) & 0xFF;
+  	accel_offset[3] = (accel_bias_reg[1])      & 0xFE;
+	accel_offset[3] = accel_offset[3] | mask_bit[1];
+
+	accel_offset[4] = (accel_bias_reg[2] >> 8) & 0xFF;
+	accel_offset[5] = (accel_bias_reg[2])      & 0xFE;
+	accel_offset[5] = accel_offset[5] | mask_bit[2];
+	
+	write_multiple_icm20948_reg(ub_1, B1_XA_OFFS_H, &accel_offset[0], 2);
+	write_multiple_icm20948_reg(ub_1, B1_YA_OFFS_H, &accel_offset[2], 2);
+	write_multiple_icm20948_reg(ub_1, B1_ZA_OFFS_H, &accel_offset[4], 2);
+}
+
+void icm20948_gyro_full_scale_select(gyro_full_scale full_scale)
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_2, B2_GYRO_CONFIG_1);
+	
+	switch(full_scale)
+	{
+		case _250dps :
+			new_val |= 0x00;
+			gyro_scale_factor = 131.0;
+			break;
+		case _500dps :
+			new_val |= 0x02;
+			gyro_scale_factor = 65.5;
+			break;
+		case _1000dps :
+			new_val |= 0x04;
+			gyro_scale_factor = 32.8;
+			break;
+		case _2000dps :
+			new_val |= 0x06;
+			gyro_scale_factor = 16.4;
+			break;
+	}
+
+	write_single_icm20948_reg(ub_2, B2_GYRO_CONFIG_1, new_val);
+}
+
+void icm20948_accel_full_scale_select(accel_full_scale full_scale)
+{
+	uint8_t new_val = read_single_icm20948_reg(ub_2, B2_ACCEL_CONFIG);
+	
+	switch(full_scale)
+	{
+		case _2g :
+			new_val |= 0x00;
+			accel_scale_factor = 16384;
+			break;
+		case _4g :
+			new_val |= 0x02;
+			accel_scale_factor = 8192;
+			break;
+		case _8g :
+			new_val |= 0x04;
+			accel_scale_factor = 4096;
+			break;
+		case _16g :
+			new_val |= 0x06;
+			accel_scale_factor = 2048;
+			break;
+	}
+
+	write_single_icm20948_reg(ub_2, B2_ACCEL_CONFIG, new_val);
+}
+
+
+/* Static Functions */
+
+
+static void select_user_bank(userbank ub)
+{
+	if(ub == active_bank){
+		return;
+	}
+	active_bank = ub;
+
+	uint8_t write_reg[2];
+	write_reg[0] = WRITE | REG_BANK_SEL;
+	write_reg[1] = ub;
+
+	cs_low(pin_cs);
+	HAL_SPI_Transmit(spi, write_reg, 2, 10);
+	cs_high(pin_cs);
+}
+
+static uint8_t read_single_icm20948_reg(userbank ub, uint8_t reg)
+{
+	uint8_t read_reg = READ | reg;
+	uint8_t reg_val;
+	select_user_bank(ub);
+
+	cs_low(pin_cs);
+	HAL_SPI_Transmit(spi, &read_reg, 1, 1000);
+	HAL_SPI_Receive(spi, &reg_val, 1, 1000);
+	cs_high(pin_cs);
+
+	return reg_val;
+}
+
+static void write_single_icm20948_reg(userbank ub, uint8_t reg, uint8_t val)
+{
+	uint8_t write_reg[2];
+	write_reg[0] = WRITE | reg;
+	write_reg[1] = val;
+
+	select_user_bank(ub);
+
+	cs_low(pin_cs);
+	HAL_SPI_Transmit(spi, write_reg, 2, 1000);
+	cs_high(pin_cs);
+}
+
+static uint8_t* read_multiple_icm20948_reg(userbank ub, uint8_t reg, uint8_t len)
+{
+	uint8_t read_reg = READ | reg;
+	static uint8_t reg_val[12];
+	select_user_bank(ub);
+
+	cs_low(pin_cs);
+	HAL_SPI_Transmit(spi, &read_reg, 1, 1000);
+	HAL_SPI_Receive(spi, reg_val, len, 1000);
+	cs_high(pin_cs);
+
+	return reg_val;
+}
+
+static void write_multiple_icm20948_reg(userbank ub, uint8_t reg, uint8_t* val, uint8_t len)
+{
+	uint8_t write_reg = WRITE | reg;
+	select_user_bank(ub);
+
+	cs_low(pin_cs);
+	HAL_SPI_Transmit(spi, &write_reg, 1, 1000);
+	HAL_SPI_Transmit(spi, val, len, 1000);
+	cs_high(pin_cs);
+}
+
+static uint8_t read_single_ak09916_reg(uint8_t reg)
+{
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_ADDR, READ | MAG_SLAVE_ADDR);
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_REG, reg);
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_CTRL, 0x81);
+
+	HAL_Delay(1);
+	return read_single_icm20948_reg(ub_0, B0_EXT_SLV_SENS_DATA_00);
+}
+
+static void write_single_ak09916_reg(uint8_t reg, uint8_t val)
+{
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_ADDR, WRITE | MAG_SLAVE_ADDR);
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_REG, reg);
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_DO, val);
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_CTRL, 0x81);
+}
+
+static uint8_t* read_multiple_ak09916_reg(uint8_t reg, uint8_t len)
+{	
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_ADDR, READ | MAG_SLAVE_ADDR);
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_REG, reg);
+	write_single_icm20948_reg(ub_3, B3_I2C_SLV0_CTRL, 0x80 | len);
+
+	HAL_Delay(1);
+	return read_multiple_icm20948_reg(ub_0, B0_EXT_SLV_SENS_DATA_00, len);
+}

src/icm20948.h

@@ -0,0 +1,322 @@
+/*
+* @file icm20948.h
+*
+*  Created on: Dec 26, 2020
+*      Author: mokhwasomssi
+*
+*  Modified: Juraj Dudak
+*  Date: 30.12.2022
+*/
+
+
+#ifndef __ICM20948_H__
+#define	__ICM20948_H__
+
+
+//#include "spi.h"			// header from stm32cubemx code generate
+#include <stdbool.h>
+#include "main.h"
+
+typedef struct
+{
+	float x;
+	float y;
+	float z;
+} axises;
+
+typedef struct{
+    GPIO_TypeDef *port;
+    uint16_t pin;
+}McuPin_typeDef;
+
+typedef struct{
+	axises * (*Get)(void);
+	bool (*Read)(axises *);
+	bool (*ReadUnit)(axises *);
+}Sensor_TypeDef;
+
+typedef struct{
+	void (*Init)(SPI_HandleTypeDef *, McuPin_typeDef *, McuPin_typeDef*);
+	void (*Read)(void);
+	void (*Reset)(void);
+	void (*WakeUp)(void);
+	void (*Sleep)(void);
+	void (*Calibrate)(void);
+	Sensor_TypeDef *acc;
+	Sensor_TypeDef *gyro;
+	Sensor_TypeDef *mag;
+}Device_TypeDef;
+
+
+
+/* Defines */
+#define READ							0x80
+#define WRITE							0x00
+
+
+
+#define cs_high(pin_cs)  (HAL_GPIO_WritePin(pin_cs->port, pin_cs->pin, SET))
+#define cs_low(pin_cs)  (HAL_GPIO_WritePin(pin_cs->port, pin_cs->pin, RESET))
+//static void cs_high()
+//{
+//	HAL_GPIO_WritePin(ICM20948_SPI_CS_PIN_PORT, ICM20948_SPI_CS_PIN_NUMBER, SET);
+//}
+
+/* Typedefs */
+typedef enum
+{
+	ub_0 = 0 << 4,
+	ub_1 = 1 << 4,
+	ub_2 = 2 << 4,
+	ub_3 = 3 << 4
+} userbank;
+
+typedef enum
+{
+	_250dps,
+	_500dps,
+	_1000dps,
+	_2000dps
+} gyro_full_scale;
+
+typedef enum
+{
+	_2g,
+	_4g,
+	_8g,
+	_16g
+} accel_full_scale;
+
+
+typedef enum
+{
+	power_down_mode = 0,
+	single_measurement_mode = 1,
+	continuous_measurement_10hz = 2,
+	continuous_measurement_20hz = 4,
+	continuous_measurement_50hz = 6,
+	continuous_measurement_100hz = 8
+} operation_mode;
+
+
+/* Main Functions */
+
+// sensor init function.
+// if sensor id is wrong, it is stuck in while.
+void icm20948_init(SPI_HandleTypeDef *, McuPin_typeDef *, McuPin_typeDef *);
+void ak09916_init();
+void icm20948_set_int(void);
+void icm20948_read(void);
+
+// 16 bits ADC value. raw data.
+bool icm20948_gyro_read(axises* data);
+bool icm20948_accel_read(axises* data);
+bool ak09916_mag_read(axises* data); 
+
+// Convert 16 bits ADC value to their unit.
+bool icm20948_gyro_read_dps(axises* data);
+bool icm20948_accel_read_g(axises* data);
+bool ak09916_mag_read_uT(axises* data);
+
+
+/* Sub Functions */
+bool icm20948_who_am_i();
+bool ak09916_who_am_i();
+
+
+void icm20948_device_reset();
+void ak09916_soft_reset();
+
+void icm20948_wakeup();
+void icm20948_sleep();
+
+void icm20948_spi_slave_enable();
+
+void icm20948_i2c_master_reset();
+void icm20948_i2c_master_enable();
+void icm20948_i2c_master_clk_frq(uint8_t config); // 0 - 15
+
+void icm20948_clock_source(uint8_t source);
+void icm20948_odr_align_enable();
+
+void icm20948_gyro_low_pass_filter(uint8_t config); // 0 - 7
+void icm20948_accel_low_pass_filter(uint8_t config); // 0 - 7
+
+// Output Data Rate = 1.125kHz / (1 + divider)
+void icm20948_gyro_sample_rate_divider(uint8_t divider);
+void icm20948_accel_sample_rate_divider(uint16_t divider);
+void ak09916_operation_mode_setting(operation_mode mode);
+
+// Calibration before select full scale.
+void icm20948_calibration(void);
+void icm20948_gyro_calibration();
+void icm20948_accel_calibration();
+
+void icm20948_gyro_full_scale_select(gyro_full_scale full_scale);
+void icm20948_accel_full_scale_select(accel_full_scale full_scale);
+
+
+/* ICM-20948 Registers */
+#define ICM20948_ID						0xEA
+#define REG_BANK_SEL					0x7F
+
+// USER BANK 0
+#define B0_WHO_AM_I						0x00		
+#define B0_USER_CTRL					0x03
+#define B0_LP_CONFIG					0x05
+#define B0_PWR_MGMT_1					0x06
+#define B0_PWR_MGMT_2					0x07
+#define B0_INT_PIN_CFG					0x0F		
+#define B0_INT_ENABLE					0x10
+#define B0_INT_ENABLE_1					0x11
+#define B0_INT_ENABLE_2					0x12
+#define B0_INT_ENABLE_3					0x13
+#define B0_I2C_MST_STATUS				0x17		
+#define B0_INT_STATUS					0x19		
+#define B0_INT_STATUS_1					0x1A
+#define B0_INT_STATUS_2					0x1B
+#define B0_INT_STATUS_3					0x1C
+#define B0_DELAY_TIMEH					0x28
+#define B0_DELAY_TIMEL					0x29
+#define B0_ACCEL_XOUT_H					0x2D		
+#define B0_ACCEL_XOUT_L					0x2E		
+#define B0_ACCEL_YOUT_H					0x2F		
+#define B0_ACCEL_YOUT_L					0x30		
+#define B0_ACCEL_ZOUT_H					0x31		
+#define B0_ACCEL_ZOUT_L					0x32	
+#define B0_GYRO_XOUT_H					0x33	
+#define B0_GYRO_XOUT_L					0x34
+#define B0_GYRO_YOUT_H					0x35
+#define B0_GYRO_YOUT_L					0x36
+#define B0_GYRO_ZOUT_H					0x37
+#define B0_GYRO_ZOUT_L					0x38
+#define B0_TEMP_OUT_H					0x39		
+#define B0_TEMP_OUT_L					0x3A
+#define B0_EXT_SLV_SENS_DATA_00			0x3B
+#define B0_EXT_SLV_SENS_DATA_01			0x3C
+#define B0_EXT_SLV_SENS_DATA_02			0x3D
+#define B0_EXT_SLV_SENS_DATA_03			0x3E
+#define B0_EXT_SLV_SENS_DATA_04			0x3F
+#define B0_EXT_SLV_SENS_DATA_05			0x40
+#define B0_EXT_SLV_SENS_DATA_06			0x41
+#define B0_EXT_SLV_SENS_DATA_07			0x42
+#define B0_EXT_SLV_SENS_DATA_08			0x43
+#define B0_EXT_SLV_SENS_DATA_09			0x44
+#define B0_EXT_SLV_SENS_DATA_10			0x45
+#define B0_EXT_SLV_SENS_DATA_11			0x46
+#define B0_EXT_SLV_SENS_DATA_12			0x47
+#define B0_EXT_SLV_SENS_DATA_13			0x48
+#define B0_EXT_SLV_SENS_DATA_14			0x49
+#define B0_EXT_SLV_SENS_DATA_15			0x4A
+#define B0_EXT_SLV_SENS_DATA_16			0x4B
+#define B0_EXT_SLV_SENS_DATA_17			0x4C
+#define B0_EXT_SLV_SENS_DATA_18			0x4D
+#define B0_EXT_SLV_SENS_DATA_19			0x4E
+#define B0_EXT_SLV_SENS_DATA_20			0x4F
+#define B0_EXT_SLV_SENS_DATA_21			0x50
+#define B0_EXT_SLV_SENS_DATA_22			0x51
+#define B0_EXT_SLV_SENS_DATA_23			0x52
+#define B0_FIFO_EN_1					0x66	
+#define B0_FIFO_EN_2					0x67
+#define B0_FIFO_RST						0x68
+#define B0_FIFO_MODE					0x69
+#define B0_FIFO_COUNTH					0X70
+#define B0_FIFO_COUNTL					0X71
+#define B0_FIFO_R_W						0x72
+#define B0_DATA_RDY_STATUS				0x74
+#define B0_FIFO_CFG						0x76	
+
+// USER BANK 1
+#define B1_SELF_TEST_X_GYRO				0x02	
+#define B1_SELF_TEST_Y_GYRO				0x03
+#define B1_SELF_TEST_Z_GYRO				0x04
+#define B1_SELF_TEST_X_ACCEL			0x0E	
+#define B1_SELF_TEST_Y_ACCEL			0x0F
+#define B1_SELF_TEST_Z_ACCEL			0x10
+#define B1_XA_OFFS_H					0x14	
+#define B1_XA_OFFS_L					0x15
+#define B1_YA_OFFS_H					0x17
+#define B1_YA_OFFS_L					0x18
+#define B1_ZA_OFFS_H					0x1A
+#define B1_ZA_OFFS_L					0x1B
+#define B1_TIMEBASE_CORRECTION_PLL		0x28	
+
+// USER BANK 2
+#define B2_GYRO_SMPLRT_DIV				0x00	
+#define B2_GYRO_CONFIG_1				0x01	
+#define B2_GYRO_CONFIG_2				0x02
+#define B2_XG_OFFS_USRH					0x03	
+#define B2_XG_OFFS_USRL 				0x04
+#define B2_YG_OFFS_USRH					0x05
+#define B2_YG_OFFS_USRL					0x06
+#define B2_ZG_OFFS_USRH					0x07
+#define B2_ZG_OFFS_USRL					0x08
+#define B2_ODR_ALIGN_EN					0x09	
+#define B2_ACCEL_SMPLRT_DIV_1			0x10	
+#define B2_ACCEL_SMPLRT_DIV_2			0x11		
+#define B2_ACCEL_INTEL_CTRL				0x12		
+#define B2_ACCEL_WOM_THR				0x13
+#define B2_ACCEL_CONFIG					0x14
+#define B2_ACCEL_CONFIG_2				0x15
+#define B2_FSYNC_CONFIG					0x52
+#define B2_TEMP_CONFIG					0x53
+#define B2_MOD_CTRL_USR					0X54
+
+// USER BANK 3
+#define B3_I2C_MST_ODR_CONFIG			0x00
+#define B3_I2C_MST_CTRL					0x01
+#define B3_I2C_MST_DELAY_CTRL			0x02	
+#define B3_I2C_SLV0_ADDR				0x03
+#define B3_I2C_SLV0_REG					0x04		
+#define B3_I2C_SLV0_CTRL				0x05
+#define B3_I2C_SLV0_DO					0x06
+#define B3_I2C_SLV1_ADDR				0x07		
+#define B3_I2C_SLV1_REG					0x08		
+#define B3_I2C_SLV1_CTRL				0x09
+#define B3_I2C_SLV1_DO					0x0A
+#define B3_I2C_SLV2_ADDR				0x0B		
+#define B3_I2C_SLV2_REG					0x0C		
+#define B3_I2C_SLV2_CTRL				0x0D
+#define B3_I2C_SLV2_DO					0x0E
+#define B3_I2C_SLV3_ADDR				0x0F		
+#define B3_I2C_SLV3_REG					0x10		
+#define B3_I2C_SLV3_CTRL				0x11
+#define B3_I2C_SLV3_DO					0x12
+#define B3_I2C_SLV4_ADDR				0x13	
+#define B3_I2C_SLV4_REG					0x14		
+#define B3_I2C_SLV4_CTRL				0x15
+#define B3_I2C_SLV4_DO					0x16
+#define B3_I2C_SLV4_DI					0x17
+	
+
+/* AK09916 Registers */
+#define AK09916_ID						0x09
+#define MAG_SLAVE_ADDR                  0x0C
+
+#define MAG_WIA2						0x01
+#define MAG_ST1							0x10
+#define MAG_HXL							0x11
+#define MAG_HXH							0x12
+#define MAG_HYL							0x13
+#define MAG_HYH							0x14
+#define MAG_HZL							0x15
+#define MAG_HZH							0x16
+#define MAG_ST2							0x18
+#define MAG_CNTL2						0x31
+#define MAG_CNTL3						0x32
+#define MAG_TS1							0x33
+#define MAG_TS2							0x34
+
+
+// B0_INT_PIN_CFG params
+#define INT_PIN_CFG_ACTL		(1<<7)  // The logic level for INT1 pin is active low.
+#define INT_PIN_CFG_OPEN		(1<<6)  // INT1 pin is configured as open drain.
+#define INT_PIN_CFG_LATCH		(1<<5)  // INT1 pin level held until interrupt status is cleared.
+#define INT_PIN_CFG_2CLEAR		(1<<4)		//Interrupt status in INT_STATUS is cleared (set to 0) if any read operation isperformed.
+#define INT_PIN_CFG_FSYNC		(1<<3)  // The logic level for the FSYNC pin as an interrupt to the ICM-20948 is active low.
+#define INT_PIN_CFG_FSYNC_EN	(1<<2)  // This enables the FSYNC pin to be used as an interrupt. A transition to the active level described by the ACTL_FSYNC bit will cause an interrupt. The status of the interrupt is read in the I2C Master Status register PASS_THROUGH bit.
+#define INT_PIN_CFG_BYPASS		(1<<1)  // When asserted, the I2C_MASTER interface pins (ES_CL and ES_DA) will go into ‘bypass mode’ when the I2C master interface is disabled.
+
+
+
+#endif	/* __ICM20948_H__ */