better init procedure

src/icm20948.cpp

@@ -67,6 +67,7 @@ Icm20948::Icm20948(IcmSpiManager *spi, icm20948_Config *config){
 	this->ak09916_init(&config->mag);
 
 	_sensor_ready = true;
+	this->Stop();
 
 }
 uint8_t Icm20948::IsPresent(void){
@@ -110,10 +111,8 @@ void Icm20948::Reset(void)
 {
 	ASSERT_SENSOR;
 
-	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, 0xC1); // reset - 0x80, sleep 0x40, source PLL auto 0x01
-	HAL_Delay(100);
-
-	this->Wakeup();
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, 0x81); // reset - 0x80, not sleep 0x40, source PLL auto 0x01
+	HAL_Delay(10);  // min 2 ms delay have to be placed here
 
 	this->icm20948_clock_source(1);
 	this->icm20948_odr_align_enable();
@@ -126,7 +125,7 @@ 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 = 0x01;
+	uint8_t new_val = 0x01; // Auto selects the best available clock source
 
 	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, new_val);
 	HAL_Delay(50);
@@ -145,14 +144,14 @@ bool Icm20948::IsReady(void)
 
 void Icm20948::Stop(void)
 {
-//	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_2, 0x3F);	// vypne vsetky senzory
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_2, 0x3F);	// vypne vsetky senzory
 	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE_1, 0x0);
 	_sensor_ready = false;
 }
 
 void Icm20948::Start(void)
 {
-//	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_2, 0x0);
+	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_2, 0x0);
 	_spi_manager->write_single_reg(_activeDevice, ub_0, B0_INT_ENABLE_1, 0x1);
 	_sensor_ready = true;
 }