68 lines
2.2 KiB
C
68 lines
2.2 KiB
C
|
// IMU.c — GY-521 (MPU-6050) driver over I2C1
|
||
|
|
// Assumes I2C.c driver on PB2 (SCL) / PB3 (SDA) at 400 kHz.
|
||
|
|
// AD0 tied low for slave address 0x68.
|
||
|
|
|
||
|
|
#include <stdint.h>
|
||
|
|
#include "IMU.h"
|
||
|
|
#include "../inc/I2C.h"
|
||
|
|
|
||
|
|
volatile int16_t IMU_AccelX;
|
||
|
|
volatile int16_t IMU_AccelY;
|
||
|
|
volatile int16_t IMU_AccelZ;
|
||
|
|
volatile int16_t IMU_Temp;
|
||
|
|
volatile int16_t IMU_GyroX;
|
||
|
|
volatile int16_t IMU_GyroY;
|
||
|
|
volatile int16_t IMU_GyroZ;
|
||
|
|
|
||
|
|
// Error codes: 0 = OK, 1 = WHO_AM_I send failed, 2 = WHO_AM_I recv failed,
|
||
|
|
// 3 = WHO_AM_I mismatch, 4 = wake write failed,
|
||
|
|
// 5 = DLPF config write failed
|
||
|
|
int IMU_Init(void){
|
||
|
|
uint8_t who;
|
||
|
|
|
||
|
|
I2C_Init();
|
||
|
|
|
||
|
|
// WHO_AM_I sanity check (readable even while chip is asleep)
|
||
|
|
if(I2C_Send1(I2C_ADDRESS, MPU_WHO_AM_I) == 0) return 1;
|
||
|
|
if(I2C_RecvN(I2C_ADDRESS, &who, 1) == 0) return 2;
|
||
|
|
if(who != I2C_ADDRESS) return 3;
|
||
|
|
|
||
|
|
// Wake chip: clear SLEEP bit in PWR_MGMT_1
|
||
|
|
if(I2C_Send2(I2C_ADDRESS, MPU_PWR_MGMT_1, 0x00) == 0) return 4;
|
||
|
|
|
||
|
|
// Enable digital low-pass filter (reduces gyro/accel noise)
|
||
|
|
if(I2C_Send2(I2C_ADDRESS, MPU_CONFIG, IMU_DLPF_CFG) == 0) return 5;
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int IMU_Read(void){
|
||
|
|
uint8_t buf[14];
|
||
|
|
int32_t sum_ax = 0, sum_ay = 0, sum_az = 0;
|
||
|
|
int32_t sum_tp = 0;
|
||
|
|
int32_t sum_gx = 0, sum_gy = 0, sum_gz = 0;
|
||
|
|
|
||
|
|
for(int i = 0; i < IMU_AVG_SAMPLES; i++){
|
||
|
|
if(I2C_Send1(I2C_ADDRESS, MPU_ACCEL_XOUT_H) == 0) return 1;
|
||
|
|
if(I2C_RecvN(I2C_ADDRESS, buf, 14) == 0) return 2;
|
||
|
|
|
||
|
|
sum_ax += (int16_t)((buf[0] << 8) | buf[1]);
|
||
|
|
sum_ay += (int16_t)((buf[2] << 8) | buf[3]);
|
||
|
|
sum_az += (int16_t)((buf[4] << 8) | buf[5]);
|
||
|
|
sum_tp += (int16_t)((buf[6] << 8) | buf[7]);
|
||
|
|
sum_gx += (int16_t)((buf[8] << 8) | buf[9]);
|
||
|
|
sum_gy += (int16_t)((buf[10] << 8) | buf[11]);
|
||
|
|
sum_gz += (int16_t)((buf[12] << 8) | buf[13]);
|
||
|
|
}
|
||
|
|
|
||
|
|
IMU_AccelX = (int16_t)(sum_ax / IMU_AVG_SAMPLES);
|
||
|
|
IMU_AccelY = (int16_t)(sum_ay / IMU_AVG_SAMPLES);
|
||
|
|
IMU_AccelZ = (int16_t)(sum_az / IMU_AVG_SAMPLES);
|
||
|
|
IMU_Temp = (int16_t)(sum_tp / IMU_AVG_SAMPLES);
|
||
|
|
IMU_GyroX = (int16_t)(sum_gx / IMU_AVG_SAMPLES);
|
||
|
|
IMU_GyroY = (int16_t)(sum_gy / IMU_AVG_SAMPLES);
|
||
|
|
IMU_GyroZ = (int16_t)(sum_gz / IMU_AVG_SAMPLES);
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|