udpate init procedure

src/IcmSpiManager.cpp

@@ -26,13 +26,13 @@ IcmSpiManager* IcmSpiManager::getInstance(){
 	return this;
 }
 
-void IcmSpiManager::cs_low(uint8_t index)
+inline void IcmSpiManager::cs_low(uint8_t index)
 {
 	_pinCS[index]->port->BRR = (uint32_t)_pinCS[index]->pin;
 }
 
 
-void IcmSpiManager::cs_high(uint8_t index)
+inline void IcmSpiManager::cs_high(uint8_t index)
 {
 	_pinCS[index]->port->BSRR = _pinCS[index]->pin;
 }

src/icm20948.cpp

@@ -66,7 +66,6 @@ Icm20948::Icm20948(IcmSpiManager *spi, icm20948_Config *config){
 
 	this->ak09916_init(&config->mag);
 
-
 	_sensor_ready = true;
 
 }
@@ -78,9 +77,6 @@ void Icm20948::Calibrate(icm20948_Config *config)
 {
 	this->SetInterruptSource(config->int_source);
 
-	this->accSensor->Calibrate();
-	this->gyroSensor->Calibrate();
-
 	if (config != NULL)
 	{
 		this->gyroSensor->SetLowPassFilter(config->gyro.low_pass_filter);
@@ -94,6 +90,10 @@ void Icm20948::Calibrate(icm20948_Config *config)
 
 		this->ak09916_init(&config->mag);
 	}
+
+	this->accSensor->Calibrate();
+	this->gyroSensor->Calibrate();
+
 }
 
 bool Icm20948::icm20948_who_am_i()
@@ -124,8 +124,9 @@ void Icm20948::Reset(void)
 
 void Icm20948::Wakeup(void)
 {
-	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
-	new_val &= 0xBF;
+//	uint8_t new_val = _spi_manager->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
+//	new_val &= 0xBF;
+	uint8_t new_val = 0x01;
 
 	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
 	HAL_Delay(50);
@@ -729,6 +730,7 @@ void SensorGyro::Calibrate(void)
 		gyro_bias[0] += temp.x;
 		gyro_bias[1] += temp.y;
 		gyro_bias[2] += temp.z;
+		HAL_Delay(2);
 	}
 
 	gyro_bias[0] /= 100;
@@ -781,8 +783,9 @@ void SensorAccel::Calibrate()
 			accel_bias[0] += temp.x;
 			accel_bias[1] += temp.y;
 			accel_bias[2] += temp.z;
+			HAL_Delay(2);
 		}
-		HAL_Delay(25);
+		HAL_Delay(5);
 	}
 
 	accel_bias[0] /= 300;

src/icm_datatypes.h

@@ -27,6 +27,15 @@
 
 #define GET2BYTES(ptr,offset) 		((((uint16_t)*(ptr+(offset)))<<8) + *(ptr+(offset)+1))
 
+
+typedef enum
+{
+	sensorAll = 0,
+	sensorAccIndex = ICM20948_ACCEL >> 4,
+	sensorGyroIndex = ICM20948_GYRO >> 4,
+	sensorMagIndex =  ICM20948_MAG >> 4,
+}Icm20948_sensorIndex;
+
 typedef struct
 {
 	uint8_t type;