53 lines
1.7 KiB
C
53 lines
1.7 KiB
C
#include <stdint.h>
|
|
#include <avr/io.h>
|
|
#include <util/delay.h>
|
|
#include "MPU6050.h"
|
|
#include "i2c.h"
|
|
#include "uart.h"
|
|
|
|
// Function to initialize MPU-6050
|
|
void MPU6050_Init() {
|
|
DEBUG("MPU6050_init: Initializing...");
|
|
// Wake up MPU-6050 by writing to PWR_MGMT_1 register
|
|
I2C_start(MPU6050_ADDR << 1 | TW_WRITE);
|
|
|
|
// These two seem to be causing problems
|
|
DEBUG("MPU6050_init: Sending PWR_MGMT_1 register address");
|
|
I2C_write(MPU6050_REG_PWR_MGMT_1);
|
|
|
|
DEBUG("Writing 0x00 to PWR_MGMT_1 register");
|
|
I2C_write(0x00); // Clear sleep mode bit
|
|
|
|
DEBUG("MPU6050_init: Stopping...");
|
|
I2C_stop();
|
|
DEBUG("MPU6050_init: Initialized!");
|
|
}
|
|
|
|
// 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);
|
|
DEBUG("MPU6050_Read_Accel: Sending ACCEL_XOUT_H register address");
|
|
I2C_write(MPU6050_REG_ACCEL_XOUT_H);
|
|
|
|
DEBUG("MPU6050_Read_Accel: Reading accelerometer data");
|
|
I2C_start(MPU6050_ADDR << 1 | TW_READ);
|
|
for (int i = 0; i < 6; i++) {
|
|
DEBUG("MPU6050_Read_Accel: Reading byte %d", i);
|
|
buffer[i] = I2C_read(i < 5); // Read 6 bytes with ACK for all except last byte
|
|
}
|
|
DEBUG("MPU6050_Read_Accel: Stopping...");
|
|
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) {
|
|
// TODO: Implement this function
|
|
}
|