OOP library

readme.md

@@ -14,47 +14,39 @@ Basic driver for TDK InvenSense ICM-20948. It support 2 modes:
 #include "icm20948.h"
 
 
-Device_TypeDef *module;
-
-void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
-{
-	if(GPIO_Pin == ICM_INT_Pin){
-		module->Read();
-	}
-}
-
-
 int main(void)
 {
   // HAL_Init, etc...
 
-  axises *my_gyro2 = NULL;
-  axises *my_accel2 = NULL;
+  SpiManager manager(&hspi2);
 
-  module = &icmModule;
+  icm20948_Config config2;
+  config2.pinCS = &pinCS;
+  config2.gyro.low_pass_filter = GYRO_lpf_196_6Hz;
+  config2.gyro.sample_rate = GYRO_samplerate_281_3Hz;
+  config2.accel.low_pass_filter = ACCEL_lpf_246Hz;
+  config2.accel.sample_rate = ACCEL_samplerate_281_3Hz;
 
-  McuPin_typeDef pinCS;   // CS for SPI
-  McuPin_typeDef pinINT;  // INT pin from ICM
+  Icm20948 sensor2(&manager, &config2);
 
-  pinCS.port = SPI2_CS_GPIO_Port;
-  pinCS.pin = SPI2_CS_Pin;
-
-  // initialization of module
-  module->Init(&hspi2, &pinCS, NULL);
+  axises *my_gyro2 = NULL;
+  axises *my_accel2 = NULL;
 
   while (1)
   {
     // read raw values
-    module->acc->Read(&my_accel);
-    module->gyro->Read(my_gyro2);
-    module->mag->Read(&my_mag);
-
-    // or unit conversion
-    module->acc->ReadUnit(&my_accel);
-    module->gyro->ReadUnit(&my_gyro);
-    module->mag->ReadUnit(&my_mag);
+    if(sensor2.GetDataStatus() == 1 ){
+      //read data
+      my_accel2 = sensor2.accSensor->Read();
+      my_gyro2 = sensor2.gyroSensor->Read();
+      my_mag2 = sensor2.magSensor->Read();
+
+      // or unit conversion
+      my_accel2 = sensor2.accSensor->ReadUnit();
+      my_gyro2 = sensor2.gyroSensor->ReadUnit();
+      my_mag2 = sensor2.magSensor->ReadUnit();
+    }
 
-    HAL_Delay(10);  // waint 10 ms
   }
 }
 
@@ -84,33 +76,31 @@ int main(void)
 
   // HAL_Init, etc...
 
-  axises *my_gyro2 = NULL;
-  axises *my_accel2 = NULL;
-
-  module = &icmModule;
-
-  McuPin_typeDef pinCS;   // CS for SPI
-  McuPin_typeDef pinINT;  // INT pin from ICM
-
-  pinCS.port = SPI2_CS_GPIO_Port;
-  pinCS.pin = SPI2_CS_Pin;
-
-  pinINT.port = ICM_INT_GPIO_Port;
-  pinINT.pin = ICM_INT_Pin;
-
-  // initialization of module
-  module->Init(&hspi2, &pinCS, &pinINT);
+  axises *my_gyro1 = NULL;
+  axises *my_accel1 = NULL;
+
+  SpiManager manager(&hspi2);
+  
+  icm20948_Config config1;
+  config1.pinCS = &pinCS;
+  config1.pinINT = &pinINT;
+  config1.gyro.low_pass_filter = GYRO_lpf_196_6Hz;
+  config1.gyro.sample_rate = GYRO_samplerate_281_3Hz;
+  config1.accel.low_pass_filter = ACCEL_lpf_246Hz;
+  config1.accel.sample_rate = ACCEL_samplerate_281_3Hz;
+  config1.mag.mode = mag_mode_cont_measurement_100hz;
+  config1.int_source = interrupt_RAW_DATA_0_RDY_EN;
 
   while (1)
   {
-    if(icm_ready == 1){
-      module->Read();
-      icm_ready = 0;
+    if(icm_ready1 == 1){
+      icm_ready1 = 0;
 
-      my_accel2 = module->acc->GetData();
-      my_gyro2 = module->gyro->GetData();
+      sensor1.Read();   // read data from sensors
 
-      // ....
+      my_accel1 = sensor1.accSensor->GetData(); // only return prepared data
+      my_gyro1 = sensor1.gyroSensor->GetData();
+      my_mag1 = sensor1.magSensor->GetData();
     }
   }
 }

src/SpiManager.cpp

@@ -0,0 +1,161 @@
+
+#include "SpiManager.h"
+
+SpiManager::SpiManager(SPI_HandleTypeDef *hspi){
+	_numDevices = 0;
+	_activeDevice = -1;
+	_spi = hspi;
+	_active_bank = ub_NONE;
+}
+
+
+uint8_t SpiManager::addSlave(McuPin_typeDef *pin){
+	_pinCS[_numDevices] = pin;
+	_numDevices++;
+	return _numDevices;
+}
+
+void SpiManager::cs_low(uint8_t index){
+	_pinCS[index]->port->BSRR = (uint32_t)_pinCS[index]->pin << 16U;
+}
+
+
+void SpiManager::cs_high(uint8_t index){
+	//HAL_GPIO_WritePin(_pinCS[index]->port, _pinCS[index]->pin, SET);
+	_pinCS[index]->port->BSRR = _pinCS[index]->pin;
+}
+
+
+
+void SpiManager::select_user_bank(uint8_t sensorNum, 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(sensorNum);
+	HAL_SPI_Transmit(_spi, write_reg, 2, 10);
+	cs_high(sensorNum);
+}
+
+void SpiManager::takeBus(uint8_t index)
+{
+	while(_activeDevice >= 0){
+		; // wait
+	}
+	_activeDevice = index;
+}
+
+void SpiManager::releaseBus()
+{
+	_activeDevice = -1;
+}
+
+uint8_t* SpiManager::read_multiple_reg(uint8_t sensorNum, userbank ub, uint8_t reg, uint8_t len)
+{
+	this->takeBus(sensorNum);
+
+	uint8_t read_reg = READ | reg;
+	this->select_user_bank(sensorNum, ub);
+
+	cs_low(sensorNum);
+	HAL_SPI_Transmit(this->_spi, &read_reg, 1, 1000);
+	HAL_SPI_Receive(this->_spi, this->_reg_val, len, 1000);
+	cs_high(sensorNum);
+
+	this->releaseBus();
+
+	return this->_reg_val;
+}
+
+
+
+uint8_t SpiManager::read_single_reg(uint8_t sensorNum, userbank ub, uint8_t reg)
+{
+	this->takeBus(sensorNum);
+
+	uint8_t read_reg = READ | reg;
+	uint8_t reg_val;
+	select_user_bank(sensorNum, ub);
+
+	cs_low(sensorNum);
+	HAL_SPI_Transmit(_spi, &read_reg, 1, 1000);
+	HAL_SPI_Receive(_spi, &reg_val, 1, 1000);
+	cs_high(sensorNum);
+
+	this->releaseBus();
+
+	return reg_val;
+}
+
+
+void SpiManager::write_single_reg(uint8_t sensorNum, userbank ub, uint8_t reg, uint8_t val)
+{
+	this->takeBus(sensorNum);
+
+	uint8_t write_reg[2];
+	write_reg[0] = WRITE | reg;
+	write_reg[1] = val;
+
+	select_user_bank(sensorNum, ub);
+
+	cs_low(sensorNum);
+	HAL_SPI_Transmit(_spi, write_reg, 2, 1000);
+	cs_high(sensorNum);
+
+	this->releaseBus();
+}
+
+
+void SpiManager::write_multiple_reg(uint8_t sensorNum, userbank ub, uint8_t reg, uint8_t* val, uint8_t len)
+{
+	this->takeBus(sensorNum);
+
+	uint8_t write_reg = WRITE | reg;
+	select_user_bank(sensorNum, ub);
+
+	cs_low(sensorNum);
+	HAL_SPI_Transmit(_spi, &write_reg, 1, 1000);
+	HAL_SPI_Transmit(_spi, val, len, 1000);
+	cs_high(sensorNum);
+
+	this->releaseBus();
+}
+
+
+uint8_t SpiManager::read_single_external_reg(uint8_t sensorNum, uint8_t reg)
+{
+
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_ADDR, READ | MAG_SLAVE_ADDR);
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_REG, reg);
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_CTRL, 0x81);
+
+	HAL_Delay(1);
+	return this->read_single_reg(sensorNum, ub_0, B0_EXT_SLV_SENS_DATA_00);
+}
+
+void SpiManager::write_single_external_reg(uint8_t sensorNum, uint8_t reg, uint8_t val)
+{
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_ADDR, WRITE | MAG_SLAVE_ADDR);
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_REG, reg);
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_DO, val);
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_CTRL, 0x81);
+}
+
+uint8_t* SpiManager::read_multiple_external_reg(uint8_t sensorNum, uint8_t reg, uint8_t len)
+{
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_ADDR, READ | MAG_SLAVE_ADDR);
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_REG, reg);
+	this->write_single_reg(sensorNum, ub_3, B3_I2C_SLV0_CTRL, 0x80 | len);
+
+	HAL_Delay(1);
+	return this->read_multiple_reg(sensorNum, ub_0, B0_EXT_SLV_SENS_DATA_00, len);
+}
+
+
+

src/SpiManager.h

@@ -0,0 +1,231 @@
+/*
+ * @file SpiManager.h
+ *
+ * @date: 8. 1. 2023
+ * @author: juraj
+ */
+
+#ifndef _SPI_MANAGER__H_
+#define _SPI_MANAGER__H_
+
+#include "icm_datatypes.h"
+
+/* Defines */
+#define READ							0x80
+#define WRITE							0x00
+
+typedef enum
+{
+	ub_NONE = -1,
+	ub_0 = 0 << 4,
+	ub_1 = 1 << 4,
+	ub_2 = 2 << 4,
+	ub_3 = 3 << 4
+} userbank;
+
+
+
+
+#define MAX_SLAVES	4
+
+
+
+/* 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.
+
+
+
+class SpiManager{
+
+private:
+	uint8_t _numDevices;
+	volatile int8_t _activeDevice;
+	/** Internal buffer. Size=12B */
+	uint8_t _reg_val[12];
+	SPI_HandleTypeDef *_spi;
+	McuPin_typeDef *_pinCS[MAX_SLAVES];
+	volatile userbank _active_bank;
+
+	void select_user_bank(uint8_t sensorNum, userbank ub);
+	void cs_high(uint8_t index);
+	void cs_low(uint8_t index);
+	void takeBus(uint8_t index);
+	void releaseBus();
+
+
+public:
+    SpiManager(SPI_HandleTypeDef* hspi);
+    uint8_t addSlave(McuPin_typeDef *pin);
+
+    uint8_t* read_multiple_reg(uint8_t index, userbank ub, uint8_t reg, uint8_t len);
+    uint8_t read_single_reg(uint8_t sensorNum, userbank ub, uint8_t reg);
+    void write_single_reg(uint8_t sensorNum, userbank ub, uint8_t reg, uint8_t val);
+    void write_multiple_reg(uint8_t sensorNum, userbank ub, uint8_t reg, uint8_t* val, uint8_t len);
+
+    uint8_t read_single_external_reg(uint8_t sensorNum, uint8_t reg);
+    void write_single_external_reg(uint8_t sensorNum, uint8_t reg, uint8_t val);
+    uint8_t* read_multiple_external_reg(uint8_t sensorNum, uint8_t reg, uint8_t len);
+
+
+};
+
+
+#endif

src/icm20948.c

@@ -1,618 +0,0 @@
-/*
-* @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_acc_get,
-		icm20948_accel_read,
-		icm20948_accel_read_g
-};
-
-Sensor_TypeDef gyroSensor = {
-		icm20948_gyro_get,
-		icm20948_gyro_read,
-		icm20948_gyro_read_dps
-};
-
-Sensor_TypeDef magSensor = {
-		ak09916_mag_get,
-		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());
-
-	ic_accel.type = ICM20948_ID;
-	ic_gyro.type = ICM20948_ID + 1;
-
-	icm20948_device_reset();
-	icm20948_wakeup();
-
-	icm20948_clock_source(1);
-	icm20948_odr_align_enable();
-	
-	icm20948_spi_slave_enable();
-	
-	icm20948_gyro_low_pass_filter(_gyro_low_pass_196_6Hz);
-	icm20948_accel_low_pass_filter(_accel_low_pass_246Hz);
-
-	icm20948_gyro_sample_rate_divider(_gyro_samplerate_281_3Hz);
-	icm20948_accel_sample_rate_divider(_accel_samplerate_562_5Hz);
-
-	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 *ak09916_mag_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(gyro_dlp_cfg config)
-{
-	uint8_t new_val = read_single_icm20948_reg(ub_2, B2_GYRO_CONFIG_1);
-//	new_val |= config << 3;
-	new_val = (new_val & 0x06) | config;	// mask Gyro range settings
-
-	write_single_icm20948_reg(ub_2, B2_GYRO_CONFIG_1, new_val);
-}
-
-/**
- * @brief Configure Low-pass filter:
- * @param config:
- */
-void icm20948_accel_low_pass_filter(accel_dlp_cfg config)
-{
-	uint8_t new_val = read_single_icm20948_reg(ub_2, B2_ACCEL_CONFIG);
-//	new_val |= config << 3;
-	new_val = (new_val & 0x06) | config;	// mask Accel range settings
-
-	write_single_icm20948_reg(ub_2, B2_GYRO_CONFIG_1, new_val);
-}
-
-void icm20948_gyro_sample_rate_divider(gyro_samplerate divider)
-{
-	write_single_icm20948_reg(ub_2, B2_GYRO_SMPLRT_DIV, divider);
-}
-
-void icm20948_accel_sample_rate_divider(accel_samplerate smplrt)
-{
-	uint8_t divider_1 = (uint8_t)(smplrt >> 8);
-	uint8_t divider_2 = (uint8_t)(0x0F & smplrt);
-
-	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.cpp

@@ -0,0 +1,549 @@
+/*
+* @file icm20948.c
+*
+*  Created on: Dec 26, 2020
+*      Author: mokhwasomssi (ANSII C version)
+*
+*  Modified: Juraj Dudak  (C++ version)
+*  Date: 30.12.2022
+*/
+
+
+#include "icm20948.h"
+
+
+Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
+
+	_activeDevice = spi->addSlave(config->pinCS);
+	_pinINT = config->pinINT;
+
+	accSensor = new SensorAccel(spi, _activeDevice);
+	gyroSensor = new SensorGyro(spi, _activeDevice);
+	magSensor = new SensorMag(spi, _activeDevice);
+
+	while(!this->icm20948_who_am_i());
+
+	this->accSensor->data.type = ICM20948_ID;
+	this->gyroSensor->data.type = ICM20948_ID + 1;
+	this->magSensor->data.type = ICM20948_ID + 2;
+
+	this->icm20948_device_reset();
+	this->Wakeup();
+
+	this->icm20948_clock_source(1);
+	this->icm20948_odr_align_enable();
+
+	this->icm20948_spi_slave_enable();
+
+	this->gyroSensor->SetLowPassFilter(config->gyro.low_pass_filter);
+	this->accSensor->SetLowPassFilter(config->accel.low_pass_filter);
+
+	this->gyroSensor->SetSampleRate(config->gyro.sample_rate);
+	this->accSensor->SetSampleRate(config->accel.sample_rate);
+
+
+	if(_pinINT != NULL){
+		this->SetInterruptSource(config->int_source);
+	}
+
+	this->Calibrate();
+
+	this->accSensor->SetRange(_16g);
+	this->gyroSensor->SetRange(_2000dps);
+
+	this->ak09916_init(&config->mag);
+
+}
+
+void Icm20948::Calibrate(void)
+{
+	this->icm20948_gyro_calibration();
+	this->icm20948_accel_calibration();
+}
+
+bool Icm20948::icm20948_who_am_i()
+{
+	uint8_t icm20948_id = _spi->read_single_reg(_activeDevice, ub_0, B0_WHO_AM_I);
+
+	if(icm20948_id == ICM20948_ID)
+		return true;
+	else
+		return false;
+}
+
+void Icm20948::icm20948_device_reset()
+{
+	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, 0x80 | 0x41); // reset, sleep, source PLL auto
+	HAL_Delay(100);
+}
+
+
+void Icm20948::Wakeup()
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
+	new_val &= 0xBF;
+
+	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
+	HAL_Delay(100);
+}
+
+void Icm20948::Sleep()
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
+	new_val |= 0x40;
+
+	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
+	HAL_Delay(100);
+}
+
+void Icm20948::icm20948_spi_slave_enable()
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_USER_CTRL);
+	new_val |= 0x10;
+
+	_spi->write_single_reg(_activeDevice, ub_0, B0_USER_CTRL, new_val);
+}
+
+void Icm20948::icm20948_i2c_master_reset()
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_USER_CTRL);
+	new_val |= 0x02;
+
+	_spi->write_single_reg(_activeDevice, ub_0, B0_USER_CTRL, new_val);
+}
+
+void Icm20948::icm20948_i2c_master_enable()
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_USER_CTRL);
+	new_val |= 0x20;
+
+	_spi->write_single_reg(_activeDevice, ub_0, B0_USER_CTRL, new_val);
+	HAL_Delay(100);
+}
+
+
+void Icm20948::icm20948_i2c_master_clk_frq(uint8_t config)
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_3, B3_I2C_MST_CTRL);
+	new_val |= config;
+
+	_spi->write_single_reg(_activeDevice, ub_3, B3_I2C_MST_CTRL, new_val);
+}
+
+void Icm20948::icm20948_clock_source(uint8_t source)
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
+	new_val |= source;
+
+	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
+}
+
+void Icm20948::icm20948_odr_align_enable()
+{
+	_spi->write_single_reg(_activeDevice, ub_2, B2_ODR_ALIGN_EN, 0x01);
+}
+
+
+void Icm20948::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++)
+	{
+		this->gyroSensor->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;
+
+	_spi->write_multiple_reg(_activeDevice, ub_2, B2_XG_OFFS_USRH, gyro_offset, 6);
+}
+
+void Icm20948::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++)
+	{
+		this->accSensor->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 = _spi->read_multiple_reg(_activeDevice, 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 = _spi->read_multiple_reg(_activeDevice, 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 = _spi->read_multiple_reg(_activeDevice, 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];
+
+	_spi->write_multiple_reg(_activeDevice, ub_1, B1_XA_OFFS_H, &accel_offset[0], 2);
+	_spi->write_multiple_reg(_activeDevice, ub_1, B1_YA_OFFS_H, &accel_offset[2], 2);
+	_spi->write_multiple_reg(_activeDevice, ub_1, B1_ZA_OFFS_H, &accel_offset[4], 2);
+}
+
+void Icm20948::SetInterruptSource(interrupt_source_enum int_source)
+{
+	_spi->write_single_reg(_activeDevice, ub_0, B0_INT_PIN_CFG, (INT_PIN_CFG_2CLEAR));  // ( INT_PIN_CFG_LATCH | ...
+    uint8_t source1 = (uint8_t)(int_source & 0xFF);
+    uint8_t source2 = (uint8_t)((int_source >> 8) & 0xFF);
+	_spi->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE, source1);
+	_spi->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE_1, source2);
+
+}
+
+
+void Icm20948::Read(void)
+{
+	uint8_t* temp =_spi->read_multiple_reg(_activeDevice, ub_0, B0_ACCEL_XOUT_H, 12);
+
+	this->accSensor->data.x = (int16_t)(temp[0] << 8 | temp[1]);
+	this->accSensor->data.y = (int16_t)(temp[2] << 8 | temp[3]);
+	this->accSensor->data.z = (int16_t)(temp[4] << 8 | temp[5]) + this->accSensor->_scaleFactor;
+	// Add scale factor because calibration function offset gravity acceleration.
+
+	this->gyroSensor->data.x = (int16_t)(temp[6] << 8 | temp[7]);
+	this->gyroSensor->data.y = (int16_t)(temp[8] << 8 | temp[9]);
+	this->gyroSensor->data.z = (int16_t)(temp[10] << 8 | temp[11]);
+
+	if(this->_ak09916_enable){
+		if(this->magSensor->Read(&this->magSensor->data) == false){
+			this->magSensor->data.x = 0;
+			this->magSensor->data.y = 0;
+			this->magSensor->data.z = 0;
+		}
+	}
+}
+
+/**
+ * @brief Interrupt status regiter - INT_STATUS_1
+ * @return 1 – Sensor Register Raw Data, from all sensors, is updated and ready to be read.
+ */
+uint8_t Icm20948::GetDataStatus(void)
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_INT_STATUS_1);
+	return new_val & 0x01;
+}
+
+/// ---------------------- MAGNETOEMTER -----------------------------------
+
+
+void Icm20948::ak09916_init(Config_Mag_t *config)
+{
+
+	this->icm20948_i2c_master_reset();
+	this->icm20948_i2c_master_enable();
+	this->icm20948_i2c_master_clk_frq(7);
+
+	while(!this->ak09916_who_am_i());
+
+	this->ak09916_soft_reset();
+	this->ak09916_operation_mode_setting(config->mode);
+
+	if (config->mode == mag_mode_power_down) {
+		this->_ak09916_enable = false;
+	} else {
+		this->_ak09916_enable = true;
+	}
+}
+
+void Icm20948::ak09916_soft_reset()
+{
+	_spi->write_single_external_reg(_activeDevice, MAG_CNTL3, 0x01);
+	HAL_Delay(100);
+}
+
+
+bool Icm20948::ak09916_who_am_i()
+{
+	uint8_t ak09916_id = _spi->read_single_external_reg(_activeDevice, MAG_WIA2);
+
+	if(ak09916_id == AK09916_ID)
+		return true;
+	else
+		return false;
+}
+
+void Icm20948::ak09916_operation_mode_setting(AK09916_operation_mode mode)
+{
+	_spi->write_single_external_reg(_activeDevice, MAG_CNTL2, mode);
+	HAL_Delay(100);
+}
+
+
+
+/* ----------------- SENSORS --------------------
+ * 1) Sensor Accel
+ * 2) Sensor Gyro
+ * 3) Sensor Mag
+ * ---------------------------------------------- */
+
+Sensor::Sensor(SpiManager *spi, int8_t activeDevice){
+	_spi = spi;
+	_activeDevice = activeDevice;
+	_scaleFactor = 1.0f;
+}
+
+
+SensorGyro::SensorGyro(SpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
+
+}
+
+SensorAccel::SensorAccel(SpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
+
+}
+
+SensorMag::SensorMag(SpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
+
+}
+
+void Sensor::SetScaleFactor(float sf){
+	_scaleFactor = sf;
+}
+
+bool SensorGyro::Read(axises* data)
+{
+
+	uint8_t* temp = this->_spi->read_multiple_reg(this->_activeDevice, 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 SensorAccel::Read(axises* data)
+{
+	uint8_t* temp = this->_spi->read_multiple_reg(this->_activeDevice, 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]) + this->_scaleFactor;
+	// Add scale factor because calibraiton function offset gravity acceleration.
+
+	return true;
+}
+
+
+bool SensorMag::Read(axises* data)
+{
+	uint8_t* temp;
+	uint8_t drdy;	// data ready
+	uint8_t hofl;	// overflow
+
+	drdy = this->_spi->read_single_external_reg(this->_activeDevice, MAG_ST1) & 0x01;
+	if(!drdy)	return false;
+
+	temp = this->_spi->read_multiple_external_reg(this->_activeDevice, MAG_HXL, 6);
+
+	hofl = this->_spi->read_single_external_reg(this->_activeDevice, 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 SensorGyro::ReadUnit(axises* data)
+{
+	this->Read(data);
+
+	data->x /= this->_scaleFactor;
+	data->y /= this->_scaleFactor;
+	data->z /= this->_scaleFactor;
+
+	return true;
+}
+
+bool SensorAccel::ReadUnit(axises* data)
+{
+	this->Read(data);
+
+	data->x /= this->_scaleFactor;
+	data->y /= this->_scaleFactor;
+	data->z /= this->_scaleFactor;
+
+	return true;
+}
+
+bool SensorMag::ReadUnit(axises* data)
+{
+	axises temp;
+	bool new_data = this->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;
+}
+
+
+axises *SensorAccel::GetData()
+{
+	return &this->data;
+}
+
+axises *SensorGyro::GetData()
+{
+	return &this->data;
+}
+
+axises *SensorMag::GetData()
+{
+	return &this->data;
+}
+
+void SensorAccel::SetRange(accel_full_scale full_scale)
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_ACCEL_CONFIG);
+	float accel_scale_factor;
+	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;
+	}
+
+	this->SetScaleFactor(accel_scale_factor);
+
+	_spi->write_single_reg(_activeDevice, ub_2, B2_ACCEL_CONFIG, new_val);
+}
+
+void SensorGyro::SetRange(gyro_full_scale full_scale)
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1);
+	float gyro_scale_factor = 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;
+	}
+
+	this->SetScaleFactor(gyro_scale_factor);
+
+	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1, new_val);
+}
+
+void SensorGyro::SetLowPassFilter(gyro_dlp_cfg config)
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1);
+//	new_val |= config << 3;
+	new_val = (new_val & 0x06) | config;	// mask Gyro range settings
+
+	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1, new_val);
+}
+
+/**
+ * @brief Configure Low-pass filter:
+ * @param config:
+ */
+void SensorAccel::SetLowPassFilter(accel_dlp_cfg config)
+{
+	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_ACCEL_CONFIG);
+//	new_val |= config << 3;
+	new_val = (new_val & 0x06) | config;	// mask Accel range settings
+
+	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1, new_val);
+}
+
+
+void SensorGyro::SetSampleRate(gyro_samplerate divider)
+{
+	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_SMPLRT_DIV, divider);
+}
+
+void SensorAccel::SetSampleRate(accel_samplerate smplrt)
+{
+	uint8_t divider_1 = (uint8_t)(smplrt >> 8);
+	uint8_t divider_2 = (uint8_t)(0x0F & smplrt);
+
+	_spi->write_single_reg(_activeDevice, ub_2, B2_ACCEL_SMPLRT_DIV_1, divider_1);
+	_spi->write_single_reg(_activeDevice, ub_2, B2_ACCEL_SMPLRT_DIV_2, divider_2);
+}
+
+

src/icm20948.h

@@ -12,392 +12,105 @@
 #ifndef __ICM20948_H__
 #define	__ICM20948_H__
 
-
-//#include "spi.h"			// header from stm32cubemx code generate
+#include "SpiManager.h"
+#include "icm_datatypes.h"
 #include <stdbool.h>
-#include "main.h"
-
-typedef struct
-{
-	uint8_t type;
-	float x;
-	float y;
-	float z;
-} axises;
-
-typedef struct{
-    GPIO_TypeDef *port;
-    uint16_t pin;
-}McuPin_typeDef;
-
-typedef struct{
-	axises * (*GetData)(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;
-
-/**
- * @brief Acceleroemter settings: register ACCEL_CONFIG_1 (Bank 2).
- */
-typedef enum
-{
-	/** output rate = 4500Hz */
-	_accel_low_pass_OFF = 0,
-	/** output rate = determined by @ref  icm20948_accel_sample_rate_divider function, @ref accel_samplerate respectivelly */
-	_accel_low_pass_246Hz = 0x9,
-	_accel_low_pass_114_4Hz = 0x11,
-	_accel_low_pass_50_4Hz = 0x19,
-	_accel_low_pass_23_9Hz = 0x21,
-	_accel_low_pass_11_5Hz = 0x29,
-	_accel_low_pass_5_7Hz = 0x31,
-	_accel_low_pass_473Hz = 0x39,
-} accel_dlp_cfg;
-
-
-typedef enum
-{
-	_accel_samplerate_562_5Hz = 1,
-	_accel_samplerate_281_3Hz = 3,
-	_accel_samplerate_187_5Hz = 5,
-	_accel_samplerate_140_6Hz = 7,
-	_accel_samplerate_102_3Hz = 10,
-	_accel_samplerate_70_3Hz = 15,
-	_accel_samplerate_48_9Hz = 22,
-	_accel_samplerate_35_2Hz = 31,
-	_accel_samplerate_17_6Hz = 34,
-	_accel_samplerate_8_8Hz = 127,
-	_accel_samplerate_4_4Hz = 255,
-	_accel_samplerate_2_2Hz = 513,
-	_accel_samplerate_1_1Hz = 1022,
-	_accel_samplerate_0_55Hz = 2044,
-	_accel_samplerate_0_27Hz = 4095,
-
-} accel_samplerate;
-
-typedef enum
-{
-	_gyro_samplerate_562_5Hz = 1,
-	_gyro_samplerate_375_0Hz = 2,
-	_gyro_samplerate_281_3Hz = 3,
-	_gyro_samplerate_225_0Hz = 4,
-	_gyro_samplerate_187_5Hz = 5,
-	_gyro_samplerate_140_6Hz = 7,
-	_gyro_samplerate_125_0Hz = 8,
-	_gyro_samplerate_102_3Hz = 10,
-	_gyro_samplerate_70_3Hz = 15,
-	_gyro_samplerate_66_2Hz = 16,
-	_gyro_samplerate_48_9Hz = 22,
-	_gyro_samplerate_35_2Hz = 31,
-	_gyro_samplerate_34_1Hz = 32,
-	_gyro_samplerate_17_6Hz = 63,
-	_gyro_samplerate_17_3Hz = 64,
-	_gyro_samplerate_4_4Hz = 255,
-} gyro_samplerate;
-
-
-/**
- * @brief Gyroscope settings: register GYRO_CONFIG_1 (Bank 2).
- */
-typedef enum
-{
-	/** output rate = 9000Hz */
-	_gyro_low_pass_OFF = 0,
-	/** output rate = determined by @ref  icm20948_gyro_sample_rate_divider function, @ref accel_samplerate respectivelly */
-	_gyro_low_pass_196_6Hz = 0x1,
-	_gyro_low_pass_151_8Hz = 0x09,
-	_gyro_low_pass_119_5Hz = 0x11,
-	_gyro_low_pass_51_2Hz = 0x19,
-	_gyro_low_pass_23_9Hz = 0x21,
-	_gyro_low_pass_11_6Hz = 0x29,
-	_gyro_low_pass_5_7Hz = 0x31,
-	_gyro_low_pass_361_4Hz = 0x39,
-} gyro_dlp_cfg;
-
-
-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);
-
-
-axises *icm20948_acc_get();
-axises *icm20948_gyro_get();
-axises *ak09916_mag_get();
-
-/* 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(gyro_dlp_cfg config);
-void icm20948_accel_low_pass_filter(accel_dlp_cfg config);
-
-// Output Data Rate = 1.125kHz / (1 + divider)
-void icm20948_gyro_sample_rate_divider(gyro_samplerate divider);
-void icm20948_accel_sample_rate_divider(accel_samplerate smplrt);
-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.
+class Sensor{
+protected:
+	int8_t _activeDevice;
+	SpiManager *_spi;
+public:
+	float _scaleFactor;
+	axises data;
+
+	Sensor(SpiManager *spi, int8_t _activeDevice);
+	void SetScaleFactor(float sf);
+	void SetRange();
+	void SetLowPassFilter();
+	void SetSampleRate();
+
+	virtual axises *GetData(void) = 0;
+	virtual bool Read(axises *) = 0;
+	virtual bool ReadUnit(axises *) = 0;
+
+};
+
+class SensorAccel : public Sensor {
+public:
+	SensorAccel(SpiManager *spi, int8_t _activeDevice);
+	axises *GetData(void);
+	bool Read(axises *);
+	bool ReadUnit(axises *);
+
+	void SetRange(accel_full_scale r);
+	void SetLowPassFilter(accel_dlp_cfg config);
+	void SetSampleRate(accel_samplerate divider);
+};
+
+
+class SensorGyro: public Sensor {
+public:
+	SensorGyro(SpiManager *spi, int8_t _activeDevice);
+	axises *GetData(void);
+	bool Read(axises *);
+	bool ReadUnit(axises *);
+
+	void SetRange(gyro_full_scale r);
+	void SetLowPassFilter(gyro_dlp_cfg config);
+	void SetSampleRate(gyro_samplerate divider);
+};
+
+class SensorMag: public Sensor {
+public:
+	SensorMag(SpiManager *spi, int8_t _activeDevice);
+	axises *GetData(void);
+	bool Read(axises *);
+	bool ReadUnit(axises *);
+};
+
+
+
+
+
+class Icm20948{
+private:
+	uint8_t _numDevices;
+	int8_t _activeDevice;
+	SpiManager *_spi;
+	McuPin_typeDef *_pinINT;
+	bool _ak09916_enable;
+
+	bool icm20948_who_am_i();
+	void icm20948_spi_slave_enable();
+	void icm20948_i2c_master_reset();
+	void icm20948_i2c_master_enable();
+	void icm20948_i2c_master_clk_frq(uint8_t config);
+	void icm20948_clock_source(uint8_t source);
+	void icm20948_odr_align_enable();
+	void icm20948_gyro_calibration(void);
+	void icm20948_accel_calibration();
+    void icm20948_device_reset();
+
+    void ak09916_init(Config_Mag_t *config);
+    bool ak09916_who_am_i();
+    void ak09916_operation_mode_setting(AK09916_operation_mode mode);
+    void ak09916_soft_reset();
+
+public:
+	SensorAccel *accSensor;
+	SensorGyro *gyroSensor;
+	SensorMag *magSensor;
+
+    Icm20948(SpiManager *spi, icm20948_Config *config);
+    void SetInterruptSource(interrupt_source_enum int_source);
+    void Calibrate(void);
+    void Read(void);
+	void Wakeup();
+	void Sleep();
+	uint8_t GetDataStatus(void);
+};
 
 
 

src/icm_datatypes.h

@@ -0,0 +1,187 @@
+/*
+ * icm_datatypes.h
+ *
+ *  Created on: 8. 1. 2023
+ *      Author: juraj
+ */
+
+#ifndef ICM_DATATYPES_H_
+#define ICM_DATATYPES_H_
+
+#include "stm32f4xx_hal.h"
+
+typedef struct
+{
+	uint8_t type;
+	float x;
+	float y;
+	float z;
+} axises;
+
+
+typedef enum
+{
+	_250dps,
+	_500dps,
+	_1000dps,
+	_2000dps
+} gyro_full_scale;
+
+typedef enum
+{
+	_2g,
+	_4g,
+	_8g,
+	_16g
+} accel_full_scale;
+
+
+/**
+ * @brief Acceleroemter settings: register ACCEL_CONFIG_1 (Bank 2).
+ */
+typedef enum
+{
+	/** output rate = 4500Hz */
+	ACCEL_low_pass_OFF = 0,
+	/** output rate = determined by @ref  icm20948ACCEL_sample_rate_divider function, @ref accel_samplerate respectivelly */
+	ACCEL_lpf_246Hz = 0x9,
+	ACCEL_lpf_114_4Hz = 0x11,
+	ACCEL_lpf_050_4Hz = 0x19,
+	ACCEL_lpf_023_9Hz = 0x21,
+	ACCEL_lpf_011_5Hz = 0x29,
+	ACCEL_lpf_005_7Hz = 0x31,
+	ACCEL_lpf_473Hz = 0x39,
+} accel_dlp_cfg;
+
+
+typedef enum
+{
+	ACCEL_samplerate_562_5Hz = 1,
+	ACCEL_samplerate_281_3Hz = 3,
+	ACCEL_samplerate_187_5Hz = 5,
+	ACCEL_samplerate_140_6Hz = 7,
+	ACCEL_samplerate_102_3Hz = 10,
+	ACCEL_samplerate_70_3Hz = 15,
+	ACCEL_samplerate_48_9Hz = 22,
+	ACCEL_samplerate_35_2Hz = 31,
+	ACCEL_samplerate_17_6Hz = 34,
+	ACCEL_samplerate_8_8Hz = 127,
+	ACCEL_samplerate_4_4Hz = 255,
+	ACCEL_samplerate_2_2Hz = 513,
+	ACCEL_samplerate_1_1Hz = 1022,
+	ACCEL_samplerate_0_55Hz = 2044,
+	ACCEL_samplerate_0_27Hz = 4095,
+
+} accel_samplerate;
+
+typedef enum
+{
+	GYRO_samplerate_562_5Hz = 1,
+	GYRO_samplerate_375_0Hz = 2,
+	GYRO_samplerate_281_3Hz = 3,
+	GYRO_samplerate_225_0Hz = 4,
+	GYRO_samplerate_187_5Hz = 5,
+	GYRO_samplerate_140_6Hz = 7,
+	GYRO_samplerate_125_0Hz = 8,
+	GYRO_samplerate_102_3Hz = 10,
+	GYRO_samplerate_070_3Hz = 15,
+	GYRO_samplerate_066_2Hz = 16,
+	GYRO_samplerate_048_9Hz = 22,
+	GYRO_samplerate_035_2Hz = 31,
+	GYRO_samplerate_034_1Hz = 32,
+	GYRO_samplerate_017_6Hz = 63,
+	GYRO_samplerate_017_3Hz = 64,
+	GYRO_samplerate_004_4Hz = 255,
+} gyro_samplerate;
+
+
+/**
+ * @brief Gyroscope settings: register GYRO_CONFIG_1 (Bank 2).
+ */
+typedef enum
+{
+	/** output rate = 9000Hz */
+	GYRO_low_pass_OFF = 0,
+	/** output rate = determined by @ref  icm20948_gyro_sample_rate_divider function, @ref accel_samplerate respectivelly */
+	GYRO_lpf_196_6Hz = 0x1,
+	GYRO_lpf_151_8Hz = 0x09,
+	GYRO_lpf_119_5Hz = 0x11,
+	GYRO_lpf_051_2Hz = 0x19,
+	GYRO_lpf_023_9Hz = 0x21,
+	GYRO_lpf_011_6Hz = 0x29,
+	GYRO_lpf_005_7Hz = 0x31,
+	GYRO_lpf_361_4Hz = 0x39,
+} gyro_dlp_cfg;
+
+
+typedef enum
+{
+	mag_mode_power_down = 0,
+	mag_mode_single_measurement = 1,
+	mag_mode_cont_measurement_010hz = 2,
+	mag_mode_cont_measurement_020hz = 4,
+	mag_mode_cont_measurement_050hz = 6,
+	mag_mode_cont_measurement_100hz = 8
+} AK09916_operation_mode;
+
+typedef enum
+{
+	interrupt_disable = 0,
+	/** Enable DMP interrupt to propagate to interrupt pin 1. */
+	interrupt_DMP_INT1_EN = 1 << 1,
+	/** Enable PLL RDY interrupt (PLL RDY means PLL is running and in use as the clock source for the system) to propagate to interrupt pin 1. */
+	interrupt_PLL_RDY_EN = 1 << 2,
+	/** Enable interrupt for wake on motion to propagate to interrupt pin 1. */
+	interrupt_WOM_INT_EN = 1 << 3,
+	/** Enable wake on FSYNC interrupt. */
+	interrupt_REG_WOF_EN = 1 << 7,
+	/** Enable raw data ready interrupt from any sensor to propagate to interrupt pin 1.*/
+	interrupt_RAW_DATA_0_RDY_EN = 1 << 8
+}interrupt_source_enum;
+
+typedef struct
+{
+  uint8_t I2C_MST_INT : 1;
+  uint8_t DMP_INT1 : 1;
+  uint8_t PLL_RDY_INT : 1;
+  uint8_t WOM_INT : 1;
+  uint8_t reserved_0 : 4;
+} ICM_INT_STATUS_t;
+
+
+typedef struct{
+    GPIO_TypeDef *port;
+    uint16_t pin;
+}McuPin_typeDef;
+
+
+typedef struct
+{
+	accel_dlp_cfg low_pass_filter = ACCEL_low_pass_OFF;
+	accel_samplerate sample_rate = ACCEL_samplerate_562_5Hz;
+}Config_Accel_t;
+
+typedef struct
+{
+	gyro_dlp_cfg low_pass_filter = GYRO_low_pass_OFF;
+	gyro_samplerate sample_rate = GYRO_samplerate_375_0Hz;
+}Config_Gyro_t;
+
+typedef struct
+{
+	AK09916_operation_mode mode = mag_mode_power_down;
+}Config_Mag_t;
+
+typedef struct
+{
+	McuPin_typeDef *pinCS = NULL;
+	McuPin_typeDef *pinINT = NULL;
+	McuPin_typeDef *pinLED = NULL;
+	Config_Accel_t accel;
+	Config_Gyro_t gyro;
+	Config_Mag_t mag;
+	interrupt_source_enum int_source = interrupt_disable;
+}icm20948_Config;
+
+
+#endif /* ICM_DATATYPES_H_ */