65 lines
2.6 KiB
C
65 lines
2.6 KiB
C
#include "mpu9250.h"
|
|
#include "esp_timer.h"
|
|
|
|
i2c_device_config_t MPU9250_DEV_CFG = {
|
|
.dev_addr_length = I2C_ADDR_BIT_LEN_7,
|
|
.device_address = 0x68,
|
|
.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, 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);
|
|
if (ret != ESP_OK)
|
|
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
|
|
|
|
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, 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
|
|
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
|
|
|
|
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()
|
|
{
|
|
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
|
|
} |