forked from logzhan/RobotHardware-UESTC
80 lines
2.7 KiB
C++
80 lines
2.7 KiB
C++
#include "MPU6050.h"
|
|
#include "board.h"
|
|
#include "delay.h"
|
|
#include "print.h"
|
|
|
|
MPU6050::MPU6050(uint8_t address):devAddr(address)
|
|
{
|
|
}
|
|
|
|
void MPU6050::initialize()
|
|
{
|
|
Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x80); // 设备复位
|
|
delay_ms(5);
|
|
Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x01); //设置设备时钟源
|
|
delay_ms(5);
|
|
Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG , MPU6050_DLPF_BW_42); //低通滤波 acc: 44hz, gyro: 42hz
|
|
delay_ms(5);
|
|
Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x00);
|
|
delay_ms(5);
|
|
Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS_2000<<3);
|
|
delay_ms(5);
|
|
Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FS_8<<3);
|
|
delay_ms(5);
|
|
|
|
Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_USER_CTRL,0X00);
|
|
delay_ms(5);
|
|
Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x02);
|
|
delay_ms(5);
|
|
}
|
|
|
|
bool MPU6050::testConnection()
|
|
{
|
|
uint8_t id = getDeviceID();
|
|
return (id == 0x39) || (id == 0x34);
|
|
}
|
|
|
|
void MPU6050::setRate(uint8_t rate)
|
|
{
|
|
Board::get()->i2c_write_byte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
|
|
}
|
|
|
|
void MPU6050::setExternalFrameSync(uint8_t sync)
|
|
{
|
|
Board::get()->i2c_write_bits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
|
|
}
|
|
|
|
void MPU6050::setFullScaleGyroRange(uint8_t range)
|
|
{
|
|
Board::get()->i2c_write_bits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
|
|
}
|
|
|
|
void MPU6050::setFullScaleAccelRange(uint8_t range)
|
|
{
|
|
Board::get()->i2c_write_bits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
|
|
}
|
|
|
|
void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
|
|
{
|
|
Board::get()->i2c_read_buf(devAddr, MPU6050_RA_ACCEL_XOUT_H, buffer, 14);
|
|
*ax = (((int16_t)buffer[0]) << 8) | buffer[1];
|
|
*ay = (((int16_t)buffer[2]) << 8) | buffer[3];
|
|
*az = (((int16_t)buffer[4]) << 8) | buffer[5];
|
|
*gx = (((int16_t)buffer[8]) << 8) | buffer[9];
|
|
*gy = (((int16_t)buffer[10]) << 8) | buffer[11];
|
|
*gz = (((int16_t)buffer[12]) << 8) | buffer[13];
|
|
}
|
|
|
|
void MPU6050::reset()
|
|
{
|
|
Board::get()->i2c_write_bits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1, 1);
|
|
}
|
|
|
|
uint8_t MPU6050::getDeviceID()
|
|
{
|
|
buffer[0]=0xff;
|
|
Board::get()->i2c_read_bits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
|
|
log("MPU6050 id: 0x%02x", buffer[0]);
|
|
return buffer[0];
|
|
}
|