fixes

main/hw/mpu9250.c

@@ -1,4 +1,5 @@
 #include "mpu9250.h"
+#include "esp_timer.h"
 
 i2c_device_config_t MPU9250_DEV_CFG = {
     .dev_addr_length = I2C_ADDR_BIT_LEN_7,
@@ -6,9 +7,11 @@ i2c_device_config_t MPU9250_DEV_CFG = {
     .scl_speed_hz = 100000,
 };
 
+#define MAG_SCALE (4912.0f / 32760.0f)
+
 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)
+esp_err_t mpu9250_read_sensor_data(i2c_master_dev_handle_t dev_handle, int16_t *accel, int16_t *gyro, int16_t *temp, int16_t *magnet)
 {
     uint8_t buffer[14]; // 6 (Accel) + 2 (Temp) + 6 (Gyro)
     esp_err_t ret = i2c_read_register(dev_handle, 0x3B, buffer, 14);
@@ -16,18 +19,28 @@ esp_err_t mpu9250_read_sensor_data(i2c_master_dev_handle_t dev_handle, int16_t *
         return ret;
 
     // Convert raw data (Big-Endian)
-    accel[0] = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF);  // Accel X
-    accel[1] = (((uint16_t)(buffer[2] & 0xFF)) << 8) | (buffer[3] & 0xFF);  // Accel Y
-    accel[2] = (((uint16_t)(buffer[4] & 0xFF)) << 8) | (buffer[5] & 0xFF);  // Accel Z
-    *temp = (((uint16_t)(buffer[6] & 0xFF)) << 8) | (buffer[7] & 0xFF);     // Temperature
-    gyro[0] = (((uint16_t)(buffer[8] & 0xFF)) << 8) | (buffer[9] & 0xFF);;   // Gyro X
-    gyro[1] = (((uint16_t)(buffer[10] & 0xFF)) << 8) | (buffer[11] & 0xFF);; // Gyro Y
-    gyro[2] = (((uint16_t)(buffer[12] & 0xFF)) << 8) | (buffer[13] & 0xFF);; // Gyro Z
+    accel[0] = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF); // Accel X
+    accel[1] = (((uint16_t)(buffer[2] & 0xFF)) << 8) | (buffer[3] & 0xFF); // Accel Y
+    accel[2] = (((uint16_t)(buffer[4] & 0xFF)) << 8) | (buffer[5] & 0xFF); // Accel Z
+    *temp = (((uint16_t)(buffer[6] & 0xFF)) << 8) | (buffer[7] & 0xFF);    // Temperature
+    gyro[0] = (((uint16_t)(buffer[8] & 0xFF)) << 8) | (buffer[9] & 0xFF);
+    ; // Gyro X
+    gyro[1] = (((uint16_t)(buffer[10] & 0xFF)) << 8) | (buffer[11] & 0xFF);
+    ; // Gyro Y
+    gyro[2] = (((uint16_t)(buffer[12] & 0xFF)) << 8) | (buffer[13] & 0xFF);
+    ; // Gyro Z
+
+    ret = i2c_read_register(dev_handle, 0x03, buffer, 6);
+    if (ret != ESP_OK)
+        return ret;
+    magnet[0] = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF); // Magnet X
+    magnet[1] = (((uint16_t)(buffer[2] & 0xFF)) << 8) | (buffer[3] & 0xFF); // Magnet Y
+    magnet[2] = (((uint16_t)(buffer[4] & 0xFF)) << 8) | (buffer[5] & 0xFF); // Magnet 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)
+void mpu9250_convert_data(int16_t *accel, int16_t *gyro, int16_t temp, int16_t *magnet, float *accel_out, float *gyro_out, float *temp_out, float *magnet_out)
 {
     accel_out[0] = accel[0] / 16384.0; // Accel X in g
     accel_out[1] = accel[1] / 16384.0; // Accel Y in g
@@ -37,10 +50,16 @@ void mpu9250_convert_data(int16_t *accel, int16_t *gyro, int16_t temp, float *ac
     gyro_out[1] = gyro[1] / 131.0; // Gyro Y in deg/s
     gyro_out[2] = gyro[2] / 131.0; // Gyro Z in deg/s
 
+    magnet_out[0] = magnet[0] * MAG_SCALE; // Gyro X in deg/s
+    magnet_out[1] = magnet[1] * MAG_SCALE; // Gyro Y in deg/s
+    magnet_out[2] = magnet[2] * MAG_SCALE; // Gyro Z in deg/s
+
     *temp_out = (temp / 333.87) + 21.0; // Temperature in °C
 }
 
-void mpu9250_init() {
+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
+    i2c_write_register_16(MPU9250_DEV_HANDLE, 0x0A, 0x0012); // zapni uz tu hovadinu
 }