calibration - divide to calibrate and store data

src/icm20948.cpp

@@ -326,6 +326,9 @@ Sensor::Sensor(SpiManager *spi, int8_t activeDevice){
 	_spi = spi;
 	_activeDevice = activeDevice;
 	_scaleFactor = 1.0f;
+	offset_x = 0;
+	offset_y = 0;
+	offset_z = 0;
 }
 
 /**
@@ -708,7 +711,6 @@ 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++)
 	{
@@ -726,17 +728,26 @@ void SensorGyro::Calibrate(void)
 	this->offset_x = gyro_bias[0];
 	this->offset_y = gyro_bias[1];
 	this->offset_z = gyro_bias[2];
+	this->SaveOffset();
+}
 
+void SensorGyro::SaveOffset(void)
+{
+	if((this->offset_x+this->offset_y+this->offset_z)==0)
+	{
+		return;
+	}
+	uint8_t gyro_offset[6] = {0};
 	// 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;
+	gyro_offset[0] = (-this->offset_x / 4  >> 8) & 0xFF;
+	gyro_offset[1] = (-this->offset_x / 4)       & 0xFF;
+	gyro_offset[2] = (-this->offset_y / 4  >> 8) & 0xFF;
+	gyro_offset[3] = (-this->offset_y / 4)       & 0xFF;
+	gyro_offset[4] = (-this->offset_z / 4  >> 8) & 0xFF;
+	gyro_offset[5] = (-this->offset_z / 4)       & 0xFF;
 
 	_spi->write_multiple_reg(_activeDevice, ub_2, B2_XG_OFFS_USRH, gyro_offset, 6);
 }
@@ -750,7 +761,6 @@ void SensorAccel::Calibrate()
 
 	int32_t accel_bias[3] = {0};
 	int32_t accel_bias_reg[3] = {0};
-	uint8_t accel_offset[6] = {0};
 
 	for(int q = 0; q < 3; q++)
 	{
@@ -786,14 +796,24 @@ void SensorAccel::Calibrate()
 	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;
+	this->SaveOffset();
+}
+
+void SensorAccel::SaveOffset(void)
+{
+	if((this->offset_x+this->offset_y+this->offset_z)==0)
+	{
+		return;
+	}
+	uint8_t accel_offset[6] = {0};
+	accel_offset[0] = (this->offset_x >> 8) & 0xFF;
+  	accel_offset[1] = (this->offset_x)      & 0xFE;
 
-	accel_offset[2] = (accel_bias_reg[1] >> 8) & 0xFF;
-  	accel_offset[3] = (accel_bias_reg[1])      & 0xFE;
+	accel_offset[2] = (this->offset_y >> 8) & 0xFF;
+  	accel_offset[3] = (this->offset_y)      & 0xFE;
 
-	accel_offset[4] = (accel_bias_reg[2] >> 8) & 0xFF;
-	accel_offset[5] = (accel_bias_reg[2])      & 0xFE;
+	accel_offset[4] = (this->offset_z >> 8) & 0xFF;
+	accel_offset[5] = (this->offset_z)      & 0xFE;
 
 
 	_spi->write_multiple_reg(_activeDevice, ub_1, B1_XA_OFFS_H, &accel_offset[0], 2);

src/icm20948.h

@@ -40,6 +40,7 @@ public:
 	void Calibrate(void);
 	uint8_t GetDataStatus(void);
 	bool _read_without_scale_factor(axises* data);
+	void SaveOffset(void);
 
 
 	virtual axises *GetData(void) = 0;
@@ -61,6 +62,7 @@ public:
 	uint8_t CheckLowPassInputValue(uint8_t);
 	void Calibrate(void);
 	bool _read_without_scale_factor(axises* data);
+	void SaveOffset(void);
 
 	uint8_t GetLowPassFilter(void);
 	uint8_t GetSampleRate(void);
@@ -80,6 +82,7 @@ public:
 	uint8_t SetSampleRate(gyro_samplerate divider);
 	uint8_t CheckLowPassInputValue(uint8_t);
 	void Calibrate(void);
+	void SaveOffset(void);
 
 	uint8_t GetLowPassFilter(void);
 	uint8_t GetSampleRate(void);