update calibration process

src/icm20948.cpp

@@ -65,7 +65,21 @@ Icm20948::Icm20948(SpiManager *spi, icm20948_Config *config){
 
 	this->Reset(1);
 	this->Wakeup();
-	this->Calibrate(config);
+	//this->Calibrate(config);
+
+	this->SetInterruptSource(config->int_source);
+
+	this->gyroSensor->SetLowPassFilter(config->gyro.low_pass_filter);
+	this->accSensor->SetLowPassFilter(config->accel.low_pass_filter);
+
+	this->gyroSensor->SetSampleRate(config->gyro.sample_rate);
+	this->accSensor->SetSampleRate(config->accel.sample_rate);
+
+	this->accSensor->SetRange(config->accel.full_scale);
+	this->gyroSensor->SetRange(config->gyro.full_scale);
+
+	this->ak09916_init(&config->mag);
+
 
 	_sensor_ready = true;
 
@@ -324,6 +338,11 @@ uint8_t Sensor::GetDataStatus(void)
 	return new_val & 0x01;
 }
 
+bool Sensor::_read_without_scale_factor(axises* data)
+{
+	return this->Read(data);
+}
+
 SensorGyro::SensorGyro(SpiManager *spi, int8_t activeDevice):Sensor(spi, activeDevice){
 
 }
@@ -378,6 +397,21 @@ bool SensorAccel::Read(axises* data)
 	return true;
 }
 
+/**
+ * Do not use this function to read final data. It is only for calibration of sensor.
+ */
+bool SensorAccel::_read_without_scale_factor(axises* data)
+{
+	uint8_t* temp = this->_spi->read_multiple_reg(this->_activeDevice, ub_0, B0_ACCEL_XOUT_H, 6);
+
+	data->x = (int16_t)(temp[0] << 8 | temp[1]);
+	data->y = (int16_t)(temp[2] << 8 | temp[3]);
+	data->z = (int16_t)(temp[4] << 8 | temp[5]);
+	// Add scale factor because calibraiton function offset gravity acceleration.
+
+	return true;
+}
+
 
 bool SensorMag::Read(axises* data)
 {
@@ -689,6 +723,10 @@ void SensorGyro::Calibrate(void)
 	gyro_bias[1] /= 100;
 	gyro_bias[2] /= 100;
 
+	this->offset_x = gyro_bias[0];
+	this->offset_y = gyro_bias[1];
+	this->offset_z = gyro_bias[2];
+
 	// 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.
@@ -714,48 +752,48 @@ void SensorAccel::Calibrate()
 	int32_t accel_bias_reg[3] = {0};
 	uint8_t accel_offset[6] = {0};
 
-	for(int i = 0; i < 100; i++)
+	for(int q = 0; q < 3; q++)
 	{
-		while(this->GetDataStatus() == 0);
-		this->Read(&temp);
-		accel_bias[0] += temp.x;
-		accel_bias[1] += temp.y;
-		accel_bias[2] += temp.z;
+		for(int i = 0; i < 100; i++)
+		{
+			while(this->GetDataStatus() == 0);
+			this->_read_without_scale_factor(&temp);
+			accel_bias[0] += temp.x;
+			accel_bias[1] += temp.y;
+			accel_bias[2] += temp.z;
+		}
+		HAL_Delay(25);
 	}
 
-	accel_bias[0] /= 100;
-	accel_bias[1] /= 100;
-	accel_bias[2] /= 100;
-
-	uint8_t mask_bit[3] = {0, 0, 0};
+	accel_bias[0] /= 300;
+	accel_bias[1] /= 300;
+	accel_bias[2] /= 300;
 
 	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_bias[2]/8)
 
+	this->offset_x = accel_bias_reg[0];
+	this->offset_y = accel_bias_reg[1];
+	this->offset_z = accel_bias_reg[2];
+
 	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);

src/icm20948.h

@@ -24,6 +24,9 @@ protected:
 public:
 	float _scaleFactor;
 	axises data;
+	int16_t offset_x;
+	int16_t offset_y;
+	int16_t offset_z;
 
 	Sensor(SpiManager *spi, int8_t _activeDevice);
 	void SetScaleFactor(float sf);
@@ -36,6 +39,7 @@ public:
 	uint8_t CheckLowPassInputValue(uint8_t);
 	void Calibrate(void);
 	uint8_t GetDataStatus(void);
+	bool _read_without_scale_factor(axises* data);
 
 
 	virtual axises *GetData(void) = 0;
@@ -56,6 +60,7 @@ public:
 	uint8_t SetSampleRate(accel_samplerate divider);
 	uint8_t CheckLowPassInputValue(uint8_t);
 	void Calibrate(void);
+	bool _read_without_scale_factor(axises* data);
 
 	uint8_t GetLowPassFilter(void);
 	uint8_t GetSampleRate(void);