MPU Calibration (#74)

* MPU calibration

When the bot is in a steady horizontal position the value recorded by the gyroscope and accelerometer of the MPU6050 should be :
gx = 0 gy = 0 gz = 0 and ax = 0 ay = 0 az = 16384

But, we get some error in the values at the steady horizontal position.
This PR aims to add a function to calculate optimum offsets for MPU6050.
Subtracting these offsets from accelerometer and gyroscope raw values will account for the zero error caused.

The approach followed is as follows:
1. Keep the bot in a horizontal stable state when the flashed code starts running.
2. Take an average of 1000 readings and set the offset to these readings.
3. Take the average of next 1000 readings, but deduct the offset obtained in step 2 from each raw reading.
4. Add this average to the offset.
5. Repeat this process until the average becomes close to zero.

* Add documentation and made requested changes.

* changed while loop to for loop

* Move the warning statement out of the for loop

i will never be equal to the MAX_CALIBRATION_ATTEMPTS. Move the if
condition out of the loop to fix this condition.

* fix the typo in G_RAW_VALUE and remove comments

Co-authored-by: Rishikesh Donadkar <rishikeshdonadkar@gmail.com>

Kconfig

@@ -134,4 +134,10 @@ config NUMBER_OF_SAMPLES
     help
         sets the number of ADC samples to average to use with LSA
 
+config MPU_CALIBRATION_AVG_COUNT
+    int "Number of readings to take while taking average"
+    default 100
+    help
+	sets the number of reading to take while taking average for MPU calibration
+
 endmenu

include/mpu6050.h

@@ -73,6 +73,8 @@
 #define RAD_TO_DEG 57.2957795
 #define BUFF_SIZE 6
 
+#define MPU_CALIBRATION_AVG_COUNT CONFIG_MPU_CALIBRATION_AVG_COUNT
+
 /**
  * @brief Initialise the ESP32 I2C Driver in Master Mode
  * 
@@ -168,4 +170,20 @@ void complementary_filter(int16_t *acce_raw_value, int16_t *gyro_raw_value, floa
  */
 esp_err_t read_mpu6050(float *euler_angle, float *mpu_offset);
 
+/**
+ * @brief Function to calculate the MPU offset for raw values.
+ * @return esp_err_t returns ESP_OK if successful, else ESP_FAIL
+ */
+esp_err_t calibrate_mpu6050();
+
+/**
+ * @brief Helper function for the function calibrate_mpu6050() to calculate the average of the raw values.
+ * @param acce_raw_value_avg Input array of accelerometer raw values (passed by reference) to be filled by the function.
+ * @param gyro_raw_value_avg Input array of gyroscope raw values (passed by reference) to be filled by the function.
+ * @param acce_offs Offset to be applied to the accelerometer raw values.
+ * @param gyro_offs Offset to be applied to the gyroscope raw values.
+ * @return esp_err_t returns ESP_OK if successful, else ESP_FAIL
+ */
+esp_err_t avg_sensors(int16_t *acce_raw_value_avg, int16_t *gyro_raw_value_avg, const int16_t *acce_offs, const int16_t *gyro_offs);
+
 #endif

src/mpu6050.c

@@ -28,6 +28,13 @@
 static const char *TAG_MPU = "mpu_6050";
 static i2c_dev_t mpu6050_dev_t;
 
+#define MIN_ACCE_ERROR 5
+#define MIN_GYRO_ERROR 5
+#define MAX_CALIBRATION_ATTEMPTS 20
+#define G_RAW_VALUE 16384  //accelerometer reads this value for 1 g acceleration.
+
+static int16_t acce_raw_value_offset[BUFF_SIZE / 2] = {0, 0, 0}, gyro_raw_value_offset[BUFF_SIZE / 2] = {0, 0, 0};
+
 // Initialise the I2C bus and install driver to specified pins
 esp_err_t i2c_master_init(void)
 {
@@ -192,6 +199,89 @@ void complementary_filter(int16_t *acce_raw_value, int16_t *gyro_raw_value, floa
     }
 }
 
+esp_err_t avg_sensors(int16_t *acce_raw_value_avg, int16_t *gyro_raw_value_avg, const int16_t *acce_offs, const int16_t *gyro_offs)
+{
+	int16_t curr_acce_raw_value[BUFF_SIZE / 2], curr_gyro_raw_value[BUFF_SIZE / 2];
+	long acce_raw_value_sum[BUFF_SIZE / 2] = {0, 0, 0}, gyro_raw_value_sum[BUFF_SIZE / 2] = {0, 0, 0};
+	int i, j, k;
+	for (i = 0; i < MPU_CALIBRATION_AVG_COUNT; i++)
+	{
+		if (read_mpu6050_raw(curr_acce_raw_value, curr_gyro_raw_value) != ESP_OK)
+		{
+			ESP_LOGE(TAG_MPU, "%s", "Failed to read MPU for calibration !");
+			return ESP_FAIL;
+		}
+
+		for (j = 0; j < BUFF_SIZE / 2; j++)
+		{
+			acce_raw_value_sum[j] += curr_acce_raw_value[j] - acce_offs[j];
+			gyro_raw_value_sum[j] += curr_gyro_raw_value[j] - gyro_offs[j];
+		}
+	}
+
+	for (k = 0; k < BUFF_SIZE / 2; k++)
+	{
+		acce_raw_value_avg[k] = acce_raw_value_sum[k] / MPU_CALIBRATION_AVG_COUNT;
+		gyro_raw_value_avg[k] = gyro_raw_value_sum[k] / MPU_CALIBRATION_AVG_COUNT;
+	}
+
+	return ESP_OK;
+}
+
+// Calculate mpu_offset for calibration.
+esp_err_t calibrate_mpu6050()
+{
+	int16_t acce_raw_value_avg[BUFF_SIZE / 2] = {0, 0, 0}, gyro_raw_value_avg[BUFF_SIZE / 2] = {0, 0, 0};
+	int8_t i, offset_ready;
+
+	if (avg_sensors(acce_raw_value_avg, gyro_raw_value_avg, acce_raw_value_offset, gyro_raw_value_offset) != ESP_OK)
+		return ESP_FAIL;
+
+	gyro_raw_value_offset[0] = gyro_raw_value_avg[0];
+	gyro_raw_value_offset[1] = gyro_raw_value_avg[1];
+	gyro_raw_value_offset[2] = gyro_raw_value_avg[2];
+
+	acce_raw_value_offset[0] = acce_raw_value_avg[0];
+	acce_raw_value_offset[1] = acce_raw_value_avg[1];
+	acce_raw_value_offset[2] = (G_RAW_VALUE - acce_raw_value_avg[2]);
+
+	for (i = 0, offset_ready = 0; i < MAX_CALIBRATION_ATTEMPTS && offset_ready != 6; i++)
+	{
+		if (avg_sensors(acce_raw_value_avg, gyro_raw_value_avg, acce_raw_value_offset, gyro_raw_value_offset) != ESP_OK)
+			return ESP_FAIL;
+
+		if(abs(gyro_raw_value_avg[0]) <= MIN_GYRO_ERROR) offset_ready++;
+		else gyro_raw_value_offset[0] += gyro_raw_value_avg[0];
+
+		if(abs(gyro_raw_value_avg[1]) <= MIN_GYRO_ERROR) offset_ready++;
+		else gyro_raw_value_offset[1] += gyro_raw_value_avg[1];
+
+		if(abs(gyro_raw_value_avg[2]) <= MIN_GYRO_ERROR) offset_ready++;
+		else gyro_raw_value_offset[2] += gyro_raw_value_avg[2];
+
+		if(abs(acce_raw_value_avg[0]) <= MIN_ACCE_ERROR) offset_ready++;
+		else acce_raw_value_offset[0] += acce_raw_value_avg[0];
+
+		if(abs(acce_raw_value_avg[1]) <= MIN_ACCE_ERROR) offset_ready++;
+		else acce_raw_value_offset[1] += acce_raw_value_avg[1];
+
+		if(abs(G_RAW_VALUE - acce_raw_value_avg[2]) <= MIN_ACCE_ERROR) offset_ready++;
+		else acce_raw_value_offset[2] += acce_raw_value_avg[2] - G_RAW_VALUE;
+
+		vTaskDelay(10 / portTICK_PERIOD_MS);
+	}
+
+	if (i == MAX_CALIBRATION_ATTEMPTS)
+	{
+		ESP_LOGW(TAG_MPU, "Maximum calibration attemps limit exceeded, quiting calibration.");
+		return ESP_FAIL;
+	}
+
+	ESP_LOGI("accelerometer offset values: ", "%d | %d | %d", acce_raw_value_offset[0], acce_raw_value_offset[1], acce_raw_value_offset[2]);
+	ESP_LOGI("gyroscope offset values: ", "%d | %d | %d", gyro_raw_value_offset[0], gyro_raw_value_offset[1], gyro_raw_value_offset[2]);
+	return ESP_OK;
+}
+
 // Calculate roll and pitch angles of the MPU after applying the complementary filter
 esp_err_t read_mpu6050(float *euler_angle, float *mpu_offset)
 {
@@ -210,3 +300,4 @@ esp_err_t read_mpu6050(float *euler_angle, float *mpu_offset)
 
     return err;
 }
+