publish Calibrate methods

src/icm20948.cpp

@@ -60,9 +60,9 @@ Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
 
 	while(!this->icm20948_who_am_i());
 
-	this->accSensor->data.type = ICM20948_ID;
-	this->gyroSensor->data.type = ICM20948_ID + 1;
-	this->magSensor->data.type = ICM20948_ID + 2;
+	this->accSensor->data.type = ICM20948_ACCEL + _activeDevice;
+	this->gyroSensor->data.type = ICM20948_GYRO + _activeDevice;
+	this->magSensor->data.type = ICM20948_MAG + _activeDevice;
 
 	this->icm20948_device_reset();
 	this->Wakeup();
@@ -95,8 +95,8 @@ Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
 
 void Icm20948::Calibrate(void)
 {
-	this->icm20948_gyro_calibration();
-	this->icm20948_accel_calibration();
+	this->accSensor->Calibrate();
+	this->gyroSensor->Calibrate();
 }
 
 bool Icm20948::icm20948_who_am_i()
@@ -204,97 +204,6 @@ void Icm20948::icm20948_odr_align_enable()
 	_spi->write_single_reg(_activeDevice, ub_2, B2_ODR_ALIGN_EN, 0x01);
 }
 
-
-void Icm20948::icm20948_gyro_calibration(void)
-{
-	axises temp;
-	int32_t gyro_bias[3] = {0};
-	uint8_t gyro_offset[6] = {0};
-
-	for(int i = 0; i < 100; i++)
-	{
-		this->gyroSensor->Read(&temp);
-		gyro_bias[0] += temp.x;
-		gyro_bias[1] += temp.y;
-		gyro_bias[2] += temp.z;
-	}
-
-	gyro_bias[0] /= 100;
-	gyro_bias[1] /= 100;
-	gyro_bias[2] /= 100;
-
-	// Construct the gyro biases for push to the hardware gyro bias registers,
-	// which are reset to zero upon device startup.
-	// Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format.
-	// Biases are additive, so change sign on calculated average gyro biases
-	gyro_offset[0] = (-gyro_bias[0] / 4  >> 8) & 0xFF;
-	gyro_offset[1] = (-gyro_bias[0] / 4)       & 0xFF;
-	gyro_offset[2] = (-gyro_bias[1] / 4  >> 8) & 0xFF;
-	gyro_offset[3] = (-gyro_bias[1] / 4)       & 0xFF;
-	gyro_offset[4] = (-gyro_bias[2] / 4  >> 8) & 0xFF;
-	gyro_offset[5] = (-gyro_bias[2] / 4)       & 0xFF;
-
-	_spi->write_multiple_reg(_activeDevice, ub_2, B2_XG_OFFS_USRH, gyro_offset, 6);
-}
-
-void Icm20948::icm20948_accel_calibration()
-{
-	axises temp;
-	uint8_t* temp2;
-	uint8_t* temp3;
-	uint8_t* temp4;
-
-	int32_t accel_bias[3] = {0};
-	int32_t accel_bias_reg[3] = {0};
-	uint8_t accel_offset[6] = {0};
-
-	for(int i = 0; i < 100; i++)
-	{
-		this->accSensor->Read(&temp);
-		accel_bias[0] += temp.x;
-		accel_bias[1] += temp.y;
-		accel_bias[2] += temp.z;
-	}
-
-	accel_bias[0] /= 100;
-	accel_bias[1] /= 100;
-	accel_bias[2] /= 100;
-
-	uint8_t mask_bit[3] = {0, 0, 0};
-
-	temp2 = _spi->read_multiple_reg(_activeDevice, ub_1, B1_XA_OFFS_H, 2);
-	accel_bias_reg[0] = (int32_t)(temp2[0] << 8 | temp2[1]);
-	mask_bit[0] = temp2[1] & 0x01;
-
-	temp3 = _spi->read_multiple_reg(_activeDevice, ub_1, B1_YA_OFFS_H, 2);
-	accel_bias_reg[1] = (int32_t)(temp3[0] << 8 | temp3[1]);
-	mask_bit[1] = temp3[1] & 0x01;
-
-	temp4 = _spi->read_multiple_reg(_activeDevice, ub_1, B1_ZA_OFFS_H, 2);
-	accel_bias_reg[2] = (int32_t)(temp4[0] << 8 | temp4[1]);
-	mask_bit[2] = temp4[1] & 0x01;
-
-	accel_bias_reg[0] -= (accel_bias[0] / 8);
-	accel_bias_reg[1] -= (accel_bias[1] / 8);
-	accel_bias_reg[2] -= (accel_bias[2] / 8);
-
-	accel_offset[0] = (accel_bias_reg[0] >> 8) & 0xFF;
-  	accel_offset[1] = (accel_bias_reg[0])      & 0xFE;
-	accel_offset[1] = accel_offset[1] | mask_bit[0];
-
-	accel_offset[2] = (accel_bias_reg[1] >> 8) & 0xFF;
-  	accel_offset[3] = (accel_bias_reg[1])      & 0xFE;
-	accel_offset[3] = accel_offset[3] | mask_bit[1];
-
-	accel_offset[4] = (accel_bias_reg[2] >> 8) & 0xFF;
-	accel_offset[5] = (accel_bias_reg[2])      & 0xFE;
-	accel_offset[5] = accel_offset[5] | mask_bit[2];
-
-	_spi->write_multiple_reg(_activeDevice, ub_1, B1_XA_OFFS_H, &accel_offset[0], 2);
-	_spi->write_multiple_reg(_activeDevice, ub_1, B1_YA_OFFS_H, &accel_offset[2], 2);
-	_spi->write_multiple_reg(_activeDevice, ub_1, B1_ZA_OFFS_H, &accel_offset[4], 2);
-}
-
 void Icm20948::SetInterruptSource(interrupt_source_enum int_source)
 {
 	_spi->write_single_reg(_activeDevice, ub_0, B0_INT_PIN_CFG, (INT_PIN_CFG_2CLEAR));  // ( INT_PIN_CFG_LATCH | ...
@@ -384,6 +293,10 @@ void Icm20948::ak09916_operation_mode_setting(AK09916_operation_mode mode)
 }
 
 
+uint8_t Icm20948::GetIndex(void)
+{
+	return this->_activeDevice + 1;
+}
 
 /* ----------------- SENSORS --------------------
  * 1) Sensor Accel
@@ -743,3 +656,94 @@ uint8_t SensorGyro::GetRange(void){
 	uint8_t new_val = _spi->read_single_reg(_activeDevice, ub_2, B2_GYRO_CONFIG_1);
 	return (new_val >> 1) & 0x03;
 }
+
+void SensorGyro::Calibrate(void)
+{
+	axises temp;
+	int32_t gyro_bias[3] = {0};
+	uint8_t gyro_offset[6] = {0};
+
+	for(int i = 0; i < 100; i++)
+	{
+//		this->gyroSensor->Read(&temp);
+		this->Read(&temp);
+		gyro_bias[0] += temp.x;
+		gyro_bias[1] += temp.y;
+		gyro_bias[2] += temp.z;
+	}
+
+	gyro_bias[0] /= 100;
+	gyro_bias[1] /= 100;
+	gyro_bias[2] /= 100;
+
+	// Construct the gyro biases for push to the hardware gyro bias registers,
+	// which are reset to zero upon device startup.
+	// Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format.
+	// Biases are additive, so change sign on calculated average gyro biases
+	gyro_offset[0] = (-gyro_bias[0] / 4  >> 8) & 0xFF;
+	gyro_offset[1] = (-gyro_bias[0] / 4)       & 0xFF;
+	gyro_offset[2] = (-gyro_bias[1] / 4  >> 8) & 0xFF;
+	gyro_offset[3] = (-gyro_bias[1] / 4)       & 0xFF;
+	gyro_offset[4] = (-gyro_bias[2] / 4  >> 8) & 0xFF;
+	gyro_offset[5] = (-gyro_bias[2] / 4)       & 0xFF;
+
+	_spi->write_multiple_reg(_activeDevice, ub_2, B2_XG_OFFS_USRH, gyro_offset, 6);
+}
+
+void SensorAccel::Calibrate()
+{
+	axises temp;
+	uint8_t* temp2;
+	uint8_t* temp3;
+	uint8_t* temp4;
+
+	int32_t accel_bias[3] = {0};
+	int32_t accel_bias_reg[3] = {0};
+	uint8_t accel_offset[6] = {0};
+
+	for(int i = 0; i < 100; i++)
+	{
+		this->Read(&temp);
+		accel_bias[0] += temp.x;
+		accel_bias[1] += temp.y;
+		accel_bias[2] += temp.z;
+	}
+
+	accel_bias[0] /= 100;
+	accel_bias[1] /= 100;
+	accel_bias[2] /= 100;
+
+	uint8_t mask_bit[3] = {0, 0, 0};
+
+	temp2 = _spi->read_multiple_reg(_activeDevice, ub_1, B1_XA_OFFS_H, 2);
+	accel_bias_reg[0] = (int32_t)(temp2[0] << 8 | temp2[1]);
+	mask_bit[0] = temp2[1] & 0x01;
+
+	temp3 = _spi->read_multiple_reg(_activeDevice, ub_1, B1_YA_OFFS_H, 2);
+	accel_bias_reg[1] = (int32_t)(temp3[0] << 8 | temp3[1]);
+	mask_bit[1] = temp3[1] & 0x01;
+
+	temp4 = _spi->read_multiple_reg(_activeDevice, ub_1, B1_ZA_OFFS_H, 2);
+	accel_bias_reg[2] = (int32_t)(temp4[0] << 8 | temp4[1]);
+	mask_bit[2] = temp4[1] & 0x01;
+
+	accel_bias_reg[0] -= (accel_bias[0] / 8);
+	accel_bias_reg[1] -= (accel_bias[1] / 8);
+	accel_bias_reg[2] -= (accel_bias[2] / 8);
+
+	accel_offset[0] = (accel_bias_reg[0] >> 8) & 0xFF;
+  	accel_offset[1] = (accel_bias_reg[0])      & 0xFE;
+	accel_offset[1] = accel_offset[1] | mask_bit[0];
+
+	accel_offset[2] = (accel_bias_reg[1] >> 8) & 0xFF;
+  	accel_offset[3] = (accel_bias_reg[1])      & 0xFE;
+	accel_offset[3] = accel_offset[3] | mask_bit[1];
+
+	accel_offset[4] = (accel_bias_reg[2] >> 8) & 0xFF;
+	accel_offset[5] = (accel_bias_reg[2])      & 0xFE;
+	accel_offset[5] = accel_offset[5] | mask_bit[2];
+
+	_spi->write_multiple_reg(_activeDevice, ub_1, B1_XA_OFFS_H, &accel_offset[0], 2);
+	_spi->write_multiple_reg(_activeDevice, ub_1, B1_YA_OFFS_H, &accel_offset[2], 2);
+	_spi->write_multiple_reg(_activeDevice, ub_1, B1_ZA_OFFS_H, &accel_offset[4], 2);
+}

src/icm20948.h

@@ -34,6 +34,7 @@ public:
 	uint8_t GetSampleRate(void);
 	uint8_t GetRange(void);
 	uint8_t CheckLowPassInputValue(uint8_t);
+	void Calibrate(void);
 
 
 	virtual axises *GetData(void) = 0;
@@ -53,6 +54,7 @@ public:
 	void SetLowPassFilter(accel_dlp_cfg config);
 	uint8_t SetSampleRate(accel_samplerate divider);
 	uint8_t CheckLowPassInputValue(uint8_t);
+	void Calibrate(void);
 
 	uint8_t GetLowPassFilter(void);
 	uint8_t GetSampleRate(void);
@@ -71,6 +73,7 @@ public:
 	void SetLowPassFilter(gyro_dlp_cfg config);
 	uint8_t SetSampleRate(gyro_samplerate divider);
 	uint8_t CheckLowPassInputValue(uint8_t);
+	void Calibrate(void);
 
 	uint8_t GetLowPassFilter(void);
 	uint8_t GetSampleRate(void);
@@ -105,8 +108,6 @@ private:
 	void icm20948_i2c_master_clk_frq(uint8_t config);
 	void icm20948_clock_source(uint8_t source);
 	void icm20948_odr_align_enable();
-	void icm20948_gyro_calibration(void);
-	void icm20948_accel_calibration();
     void icm20948_device_reset();
 
     void ak09916_init(Config_Mag_t *config);
@@ -128,6 +129,7 @@ public:
 	uint8_t IsSleep(void);
 	bool IsReady(void);
 	uint8_t GetDataStatus(void);
+	uint8_t GetIndex(void);
 
 	void Stop(void);
 	void Start(void);

src/icm_datatypes.h

@@ -16,6 +16,11 @@
 #include "stm32l4xx_hal.h"
 #endif
 
+#define ICM20948_ACCEL					0x10
+#define ICM20948_GYRO					0x20
+#define ICM20948_MAG					0x30
+#define ICM20948_TEMP					0x40
+
 typedef struct
 {
 	uint8_t type;