AVR-Playground/MPU6050.c

#include <stdint.h>
#include <avr/io.h>
#include <util/delay.h>
#include "MPU6050.h"
#include "i2c.h"

// Function to initialize MPU-6050
void MPU6050_Init() {
    // Wake up MPU-6050 by writing to PWR_MGMT_1 register
    I2C_start(MPU6050_ADDR << 1 | TW_WRITE);
    I2C_write(MPU6050_REG_PWR_MGMT_1);
    I2C_write(0x00); // Clear sleep mode bit
    I2C_stop();
}

// Function to read accelerometer data from MPU-6050
void MPU6050_Read_Accel(int16_t* accel_data) {
    uint8_t buffer[6]; // Buffer to store accelerometer data
    // Read accelerometer data starting from ACCEL_XOUT_H register
    I2C_start(MPU6050_ADDR << 1 | TW_WRITE);
    I2C_write(MPU6050_REG_ACCEL_XOUT_H);
    I2C_start(MPU6050_ADDR << 1 | TW_READ);
    for (int i = 0; i < 6; i++) {
        buffer[i] = I2C_read(i < 5); // Read 6 bytes with ACK for all except last byte
    }
    I2C_stop();

    // Combine high and low bytes for each axis
    accel_data[0] = (buffer[0] << 8) | buffer[1]; // X-axis
    accel_data[1] = (buffer[2] << 8) | buffer[3]; // Y-axis
    accel_data[2] = (buffer[4] << 8) | buffer[5]; // Z-axis
}

// Function to read gyroscope data from MPU-6050
void MPU6050_Read_Gyro(int16_t* gyro_data) {
    uint8_t buffer[6]; // Buffer to store gyroscope data
    // Read gyroscope data starting from GYRO_XOUT_H register
    I2C_start(MPU6050_ADDR << 1 | TW_WRITE);
    I2C_write(MPU6050_REG_GYRO_XOUT_H);
    I2C_start(MPU6050_ADDR << 1 | TW_READ);
    for (int i = 0; i < 6; i++) {
        buffer[i] = I2C_read(i < 5); // Read 6 bytes with ACK for all except last byte
    }
    I2C_stop();

    // Combine high and low bytes for each axis
    gyro_data[0] = (buffer[0] << 8) | buffer[1]; // X-axis
    gyro_data[1] = (buffer[2] << 8) | buffer[3]; // Y-axis
    gyro_data[2] = (buffer[4] << 8) | buffer[5]; // Z-axis
}