46 lines
1.9 KiB
C
46 lines
1.9 KiB
C
#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] = (((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
|
|
|
|
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
|
|
} |