fixed write to EEPROM

src/icm20948.cpp

@@ -29,24 +29,6 @@ uint16_t ACCEL_SampleRate_Table[15] = {
 	4095,
 };
 
-uint16_t GYRO_SampleRate_Table[16] = {
-	1,
-	2,
-	3,
-	4,
-	5,
-	7,
-	8,
-	10,
-	15,
-	16,
-	22,
-	31,
-	32,
-	63,
-	64,
-	255,
-};
 
 Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
 
@@ -63,9 +45,8 @@ Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
 	this->gyroSensor->data.type = ICM20948_GYRO + _activeDevice;
 	this->magSensor->data.type = ICM20948_MAG + _activeDevice;
 
-	this->Reset(1);
+	this->Reset();
 	this->Wakeup();
-	//this->Calibrate(config);
 
 	this->SetInterruptSource(config->int_source);
 
@@ -114,19 +95,16 @@ bool Icm20948::icm20948_who_am_i()
 		return false;
 }
 
-void Icm20948::Reset(uint8_t toSleep)
+void Icm20948::Reset(void)
 {
-	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, 0x80 | toSleep == 1 ? 0x41 : 0x01); // reset, sleep, source PLL auto
+	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1, 0xC1); // reset - 0x80, sleep 0x40, source PLL auto 0x01
 	HAL_Delay(100);
 
 	this->Wakeup();
 
-	//if(toSleep == 0)	// reset without sleep - set internal settings
-	{
-		this->icm20948_clock_source(1);
-		this->icm20948_odr_align_enable();
-		this->icm20948_spi_slave_enable();
-	}
+	this->icm20948_clock_source(1);
+	this->icm20948_odr_align_enable();
+	this->icm20948_spi_slave_enable();
 
 }
 
@@ -551,7 +529,12 @@ void SensorGyro::SetRange(gyro_full_scale full_scale)
 uint8_t SensorGyro::GetLowPassFilter(void)
 {
 	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1);
-	return (new_val>>3) & 0x07;
+	if( (new_val & 0x01) == 0x01)
+	{
+		return (new_val>>3) & 0x07;
+	}
+	return 0xFE;
+
 }
 
 void SensorGyro::SetLowPassFilter(gyro_dlp_cfg config)
@@ -559,12 +542,14 @@ void SensorGyro::SetLowPassFilter(gyro_dlp_cfg config)
 	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1);
 	uint8_t gyro_fchoice = 1;
 	uint8_t config_val = (uint8_t)config;
+	uint8_t mask = 0xC7;
 	if (config == GYRO_low_pass_OFF){
 		gyro_fchoice = 0;
 		config_val = 0;
+		mask = 0xC6;
 	}
 
-	new_val = (new_val & 0xC7) | (config_val<<3) | gyro_fchoice;	// mask Gyro range settings
+	new_val = (new_val & mask) | (config_val<<3) | gyro_fchoice;	// mask Gyro range settings
 
 	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1, new_val);
 }
@@ -646,26 +631,14 @@ uint8_t SensorAccel::GetLowPassFilter(void)
 
 uint8_t SensorGyro::SetSampleRate(gyro_samplerate divider)
 {
-
-	if(divider < 0 || divider >= 16){
-		return 0;
-	}
-
-	uint8_t sr = GYRO_SampleRate_Table[divider];
-	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_SMPLRT_DIV, sr);
+	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_SMPLRT_DIV, divider);
 	return 1;
 }
 
 uint8_t SensorGyro::GetSampleRate(void)
 {
 	uint8_t value = _spi->read_single_reg(_activeDevice, ub_2, B2_GYRO_SMPLRT_DIV);
-	uint8_t sr_index = 0;
-	for(uint8_t i=0 ; i < 16 ; i++){
-		if(value == GYRO_SampleRate_Table[i]){
-			sr_index = i;
-		}
-	}
-	return sr_index;
+	return value;
 }
 
 uint8_t SensorAccel::SetSampleRate(accel_samplerate smplrt)

src/icm20948.h

@@ -136,7 +136,7 @@ public:
 	uint8_t GetDataStatus(void);
 	uint8_t GetIndex(void);
 	uint8_t GetIndexZeroBased(void);
-	void Reset(uint8_t toSleep);
+	void Reset(void);
 
 	void Stop(void);
 	void Start(void);

src/icm_datatypes.h

@@ -89,22 +89,22 @@ typedef enum
 
 typedef enum
 {
-	GYRO_samplerate_562_5Hz = 0,
-	GYRO_samplerate_375_0Hz = 1,
-	GYRO_samplerate_281_3Hz = 2,
-	GYRO_samplerate_225_0Hz = 3,
-	GYRO_samplerate_187_5Hz = 4,
-	GYRO_samplerate_140_6Hz = 5,
-	GYRO_samplerate_125_0Hz = 6,
-	GYRO_samplerate_102_3Hz = 7,
-	GYRO_samplerate_070_3Hz = 8,
-	GYRO_samplerate_066_2Hz = 9,
-	GYRO_samplerate_048_9Hz = 10,
-	GYRO_samplerate_035_2Hz = 11,
-	GYRO_samplerate_034_1Hz = 12,
-	GYRO_samplerate_017_6Hz = 13,
-	GYRO_samplerate_017_3Hz = 14,
-	GYRO_samplerate_004_4Hz = 15,
+	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;