correct settings of base parameters

src/icm20948.cpp

@@ -11,6 +11,42 @@
 
 #include "icm20948.h"
 
+uint16_t ACCEL_SampleRate_Table[15] = {
+	1,
+	3,
+	5,
+	7,
+	10,
+	15,
+	22,
+	31,
+	34,
+	127,
+	255,
+	513,
+	1022,
+	2044,
+	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){
 
@@ -53,6 +89,7 @@ Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
 	this->gyroSensor->SetRange(_2000dps);
 
 	this->ak09916_init(&config->mag);
+	_sensor_ready = true;
 
 }
 
@@ -94,6 +131,23 @@ uint8_t Icm20948::IsSleep(void)
 	return new_val & 0x40;
 }
 
+bool Icm20948::IsReady(void)
+{
+	return _sensor_ready;
+}
+
+void Icm20948::Stop(void)
+{
+	_sensor_ready = false;
+	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_2, 0x3F);
+}
+
+void Icm20948::Start(void)
+{
+	_sensor_ready = true;
+	_spi->write_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_2, 0x0);
+}
+
 void Icm20948::Sleep(void)
 {
 	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_0, B0_PWR_MGMT_1);
@@ -472,6 +526,7 @@ axises *SensorMag::GetData()
 void SensorAccel::SetRange(accel_full_scale full_scale)
 {
 	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_ACCEL_CONFIG);
+	new_val = new_val & 0xF9;	// remova ACCEL_FS_SEL bites [1-2]
 	float accel_scale_factor;
 	switch(full_scale)
 	{
@@ -530,18 +585,71 @@ 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>>1) & 0x03;
+	return (new_val>>3) & 0x07;
 }
 
 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
+	uint8_t gyro_fchoice = 1;
+	uint8_t config_val = (uint8_t)config;
+	if (config == GYRO_low_pass_OFF){
+		gyro_fchoice = 0;
+		config_val = 0;
+	}
+
+	new_val = (new_val & 0xC7) | (config_val<<3) | gyro_fchoice;	// mask Gyro range settings
 
 	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1, new_val);
 }
 
+
+uint8_t SensorAccel::CheckLowPassInputValue(uint8_t value)
+{
+	uint8_t valid = 0;
+
+	    switch(value) {
+	        case ACCEL_lpf_246Hz:
+	        case ACCEL_lpf_114_4Hz:
+	        case ACCEL_lpf_050_4Hz:
+	        case ACCEL_lpf_023_9Hz:
+	        case ACCEL_lpf_011_5Hz:
+	        case ACCEL_lpf_005_7Hz:
+	        case ACCEL_lpf_473Hz:
+	        case ACCEL_lpf_OFF:
+	            valid = 1;
+	            break;
+	        default:
+	        	valid = 0;
+	    };
+
+	    return valid;
+}
+
+uint8_t SensorGyro::CheckLowPassInputValue(uint8_t value)
+{
+	uint8_t valid = 0;
+
+	    switch(value) {
+	        case GYRO_low_pass_OFF:
+	        case GYRO_lpf_005_7Hz:
+	        case GYRO_lpf_011_6Hz:
+	        case GYRO_lpf_023_9Hz:
+	        case GYRO_lpf_051_2Hz:
+	        case GYRO_lpf_119_5Hz:
+	        case GYRO_lpf_151_8Hz:
+	        case GYRO_lpf_196_6Hz:
+	        case GYRO_lpf_361_4Hz:
+	            valid = 1;
+	            break;
+	        default:
+	        	valid = 0;
+	    };
+
+	    return valid;
+}
+
+
 /**
  * @brief Configure Low-pass filter:
  * @param config:
@@ -549,41 +657,76 @@ void SensorGyro::SetLowPassFilter(gyro_dlp_cfg 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
+	uint8_t accel_fchoice = 1;
+	if (config == ACCEL_lpf_OFF){
+		accel_fchoice = 0;
+	}
 
-	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1, new_val);
+	new_val = (new_val & 0xC7) | (config<<3) | accel_fchoice;	// mask Accel range settings
+
+	_spi->write_single_reg(_activeDevice, ub_2, B2_ACCEL_CONFIG, new_val);
 }
 
 uint8_t SensorAccel::GetLowPassFilter(void)
 {
 	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_ACCEL_CONFIG);
-	return (new_val>>1) & 0x03 ;
+	new_val = (new_val>>3) & 0x07;
+	if(new_val == 0){ // cofig value 0 and 1 has same LPF frequency: 246Hz
+		new_val = 1;
+	}
+
+	return new_val;
 }
 
-void SensorGyro::SetSampleRate(gyro_samplerate divider)
+uint8_t SensorGyro::SetSampleRate(gyro_samplerate divider)
 {
-	_spi->write_single_reg(_activeDevice, ub_2, B2_GYRO_SMPLRT_DIV, 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);
+	return 1;
 }
 
 uint8_t SensorGyro::GetSampleRate(void)
 {
-	return _spi->read_single_reg(_activeDevice, ub_2, B2_GYRO_SMPLRT_DIV);
+	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;
 }
 
-void SensorAccel::SetSampleRate(accel_samplerate smplrt)
+uint8_t SensorAccel::SetSampleRate(accel_samplerate smplrt)
 {
-	uint8_t divider_1 = (uint8_t)(smplrt >> 8);
-	uint8_t divider_2 = (uint8_t)(0x0F & smplrt);
+	if(smplrt < 0 || smplrt >= 15){
+		return 0;
+	}
 
-	_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);
+	uint16_t sr = ACCEL_SampleRate_Table[smplrt];
+	uint8_t data[2] = {(uint8_t)((sr>> 8) & 0x0F), (uint8_t)(sr & 0xFF)}; // MSB, LSB
+
+	_spi->write_multiple_reg(_activeDevice, ub_2, B2_ACCEL_SMPLRT_DIV_1, data, 2);
+
+	return 1;
 }
 
 uint8_t SensorAccel::GetSampleRate(void)
 {
 	uint8_t d1 =  _spi->read_single_reg(_activeDevice, ub_2, B2_ACCEL_SMPLRT_DIV_1);
 	uint8_t d2 =  _spi->read_single_reg(_activeDevice, ub_2, B2_ACCEL_SMPLRT_DIV_2);
-	return (d1<<8 | d2);
+	uint16_t value = (d1<<8 | d2);
+	uint8_t sr_index = 0;
+	for(uint8_t i=0 ; i < 15 ; i++){
+		if(value == ACCEL_SampleRate_Table[i]){
+			sr_index = i;
+		}
+	}
+	return sr_index;
 }
 

src/icm20948.h

@@ -29,9 +29,11 @@ public:
 	void SetScaleFactor(float sf);
 	void SetRange();
 	void SetLowPassFilter();
-	void SetSampleRate();
+	uint8_t SetSampleRate();
 	uint8_t GetLowPassFilter(void);
 	uint8_t GetSampleRate(void);
+	uint8_t CheckLowPassInputValue(uint8_t);
+
 
 	virtual axises *GetData(void) = 0;
 	virtual bool Read(axises *) = 0;
@@ -48,7 +50,8 @@ public:
 
 	void SetRange(accel_full_scale r);
 	void SetLowPassFilter(accel_dlp_cfg config);
-	void SetSampleRate(accel_samplerate divider);
+	uint8_t SetSampleRate(accel_samplerate divider);
+	uint8_t CheckLowPassInputValue(uint8_t);
 
 	uint8_t GetLowPassFilter(void);
 	uint8_t GetSampleRate(void);
@@ -64,7 +67,8 @@ public:
 
 	void SetRange(gyro_full_scale r);
 	void SetLowPassFilter(gyro_dlp_cfg config);
-	void SetSampleRate(gyro_samplerate divider);
+	uint8_t SetSampleRate(gyro_samplerate divider);
+	uint8_t CheckLowPassInputValue(uint8_t);
 
 	uint8_t GetLowPassFilter(void);
 	uint8_t GetSampleRate(void);
@@ -89,6 +93,7 @@ private:
 	SpiManager *_spi;
 	McuPin_typeDef *_pinINT;
 	bool _ak09916_enable;
+	bool _sensor_ready;
 
 	bool icm20948_who_am_i();
 	void icm20948_spi_slave_enable();
@@ -118,7 +123,11 @@ public:
 	void Wakeup(void);
 	void Sleep(void);
 	uint8_t IsSleep(void);
+	bool IsReady(void);
 	uint8_t GetDataStatus(void);
+
+	void Stop(void);
+	void Start(void);
 };
 
 

src/icm_datatypes.h

@@ -48,56 +48,58 @@ typedef enum
 typedef enum
 {
 	/** output rate = 4500Hz */
-	ACCEL_low_pass_OFF = 0,
+	ACCEL_lpf_OFF = 0xFE,
 	/** 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_lpf_246Hz = 0x0,
+	ACCEL_lpf_246Hz = 0x1,
+	ACCEL_lpf_114_4Hz = 0x2,
+	ACCEL_lpf_050_4Hz = 0x3,
+	ACCEL_lpf_023_9Hz = 0x4,
+	ACCEL_lpf_011_5Hz = 0x5,
+	ACCEL_lpf_005_7Hz = 0x6,
+	ACCEL_lpf_473Hz = 0x7,
 } 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_562_5Hz = 0,
+	ACCEL_samplerate_281_3Hz = 1,
+	ACCEL_samplerate_187_5Hz = 2,
+	ACCEL_samplerate_140_6Hz = 3,
+	ACCEL_samplerate_102_3Hz = 4,
+	ACCEL_samplerate_70_3Hz = 5,
+	ACCEL_samplerate_48_9Hz = 6,
+	ACCEL_samplerate_35_2Hz = 7,
+	ACCEL_samplerate_17_6Hz = 8,
+	ACCEL_samplerate_8_8Hz = 9,
+	ACCEL_samplerate_4_4Hz = 10,
+	ACCEL_samplerate_2_2Hz = 11,
+	ACCEL_samplerate_1_1Hz = 12,
+	ACCEL_samplerate_0_55Hz = 13,
+	ACCEL_samplerate_0_27Hz = 14,
 
 } 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_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;
 
 
@@ -107,16 +109,16 @@ typedef enum
 typedef enum
 {
 	/** output rate = 9000Hz */
-	GYRO_low_pass_OFF = 0,
+	GYRO_low_pass_OFF = 0xFE,
 	/** 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_lpf_196_6Hz = 0x0,
+	GYRO_lpf_151_8Hz = 0x1,
+	GYRO_lpf_119_5Hz = 0x2,
+	GYRO_lpf_051_2Hz = 0x3,
+	GYRO_lpf_023_9Hz = 0x4,
+	GYRO_lpf_011_6Hz = 0x5,
+	GYRO_lpf_005_7Hz = 0x6,
+	GYRO_lpf_361_4Hz = 0x7,
 } gyro_dlp_cfg;
 
 
@@ -163,7 +165,7 @@ typedef struct{
 
 typedef struct
 {
-	accel_dlp_cfg low_pass_filter = ACCEL_low_pass_OFF;
+	accel_dlp_cfg low_pass_filter = ACCEL_lpf_OFF;
 	accel_samplerate sample_rate = ACCEL_samplerate_562_5Hz;
 }Config_Accel_t;