fix gyro->range set method

src/icm20948.cpp

@@ -500,6 +500,7 @@ void SensorAccel::SetRange(accel_full_scale full_scale)
 void SensorGyro::SetRange(gyro_full_scale full_scale)
 {
 	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1);
+	new_val &= 0xF9;	/// mask: xxxx x00x
 	float gyro_scale_factor = 1;
 	switch(full_scale)
 	{
@@ -609,11 +610,15 @@ void SensorAccel::SetLowPassFilter(accel_dlp_cfg config)
 {
 	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_ACCEL_CONFIG);
 	uint8_t accel_fchoice = 1;
+	uint8_t mask = 0xC7;
+//	uint8_t config_val = (uint8_t)config;
 	if (config == ACCEL_lpf_OFF){
 		accel_fchoice = 0;
+//		config_val = 0;
+		mask = 0xC6;
 	}
 
-	new_val = (new_val & 0xC7) | (config<<3) | accel_fchoice;	// mask Accel range settings
+	new_val = (new_val & mask) | (config<<3) | accel_fchoice;	// mask Accel range settings
 
 	_spi->write_single_reg(_activeDevice, ub_2, B2_ACCEL_CONFIG, new_val);
 }
@@ -621,6 +626,11 @@ void SensorAccel::SetLowPassFilter(accel_dlp_cfg config)
 uint8_t SensorAccel::GetLowPassFilter(void)
 {
 	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_ACCEL_CONFIG);
+	if((new_val & 0x01) == 0){
+		//LPF is OFF
+		return 0xFE;
+	}
+
 	new_val = (new_val>>3) & 0x07;
 	if(new_val == 0){ // cofig value 0 and 1 has same LPF frequency: 246Hz
 		new_val = 1;