Init

main/hw/mpu9250.c

@@ -0,0 +1,46 @@
+#include "mpu9250.h"
+
+i2c_device_config_t MPU9250_DEV_CFG = {
+    .dev_addr_length = I2C_ADDR_BIT_LEN_7,
+    .device_address = 0x68,
+    .scl_speed_hz = 100000,
+};
+
+i2c_master_dev_handle_t MPU9250_DEV_HANDLE;
+
+esp_err_t mpu9250_read_sensor_data(i2c_master_dev_handle_t dev_handle, int16_t *accel, int16_t *gyro, int16_t *temp)
+{
+    uint8_t buffer[14]; // 6 (Accel) + 2 (Temp) + 6 (Gyro)
+    esp_err_t ret = i2c_read_register(dev_handle, 0x3B, buffer, 14);
+    if (ret != ESP_OK)
+        return ret;
+
+    // Convert raw data (Big-Endian)
+    accel[0] = (buffer[0] << 8) | buffer[1];  // Accel X
+    accel[1] = (buffer[2] << 8) | buffer[3];  // Accel Y
+    accel[2] = (buffer[4] << 8) | buffer[5];  // Accel Z
+    *temp = (buffer[6] << 8) | buffer[7];     // Temperature
+    gyro[0] = (buffer[8] << 8) | buffer[9];   // Gyro X
+    gyro[1] = (buffer[10] << 8) | buffer[11]; // Gyro Y
+    gyro[2] = (buffer[12] << 8) | buffer[13]; // Gyro Z
+
+    return ESP_OK;
+}
+
+void mpu9250_convert_data(int16_t *accel, int16_t *gyro, int16_t temp, float *accel_out, float *gyro_out, float *temp_out)
+{
+    accel_out[0] = accel[0] / 16384.0; // Accel X in g
+    accel_out[1] = accel[1] / 16384.0; // Accel Y in g
+    accel_out[2] = accel[2] / 16384.0; // Accel Z in g
+
+    gyro_out[0] = gyro[0] / 131.0; // Gyro X in deg/s
+    gyro_out[1] = gyro[1] / 131.0; // Gyro Y in deg/s
+    gyro_out[2] = gyro[2] / 131.0; // Gyro Z in deg/s
+
+    *temp_out = (temp / 333.87) + 21.0; // Temperature in °C
+}
+
+void mpu9250_init() {
+    ESP_ERROR_CHECK(i2c_master_bus_add_device(i2c0_bus_hdl, &MPU9250_DEV_CFG, &MPU9250_DEV_HANDLE));
+    i2c_write_register_16(MPU9250_DEV_HANDLE, 0x6B, 0x0001); // zapni uz tu hovadinu
+}