forked from logzhan/RobotHardware-UESTC
59 lines
1.4 KiB
C++
59 lines
1.4 KiB
C++
#include "GY65.h"
|
|
#include "board.h"
|
|
#include "print.h"
|
|
#include "delay.h"
|
|
|
|
#define ADX_SCALE 0.00239420166015625 // 4mg/LSB 16/63356*9.80665
|
|
#define GRY_SCALE 0.001065264417860243 //rad/s 4000/65536*(PI/180)
|
|
|
|
#define WRITE_INTERVAL 20
|
|
|
|
bool GY65::Init()
|
|
{
|
|
delay_ms(500);
|
|
Board::get()->i2c_init();
|
|
|
|
if (!mpu6050.testConnection()) {
|
|
#if IMU_DEBUG_ENABLE
|
|
log("MPU6050 NOT FOUND!");
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
mpu6050.initialize();
|
|
delay_ms(WRITE_INTERVAL);
|
|
|
|
#if IMU_DEBUG_ENABLE
|
|
uint8_t buffer=0;
|
|
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, &buffer);
|
|
log("MPU6050_RA_PWR_MGMT_1=%d", buffer);
|
|
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, &buffer);
|
|
log("MPU6050_RA_GYRO_CONFIG=%d", buffer);
|
|
Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, &buffer);
|
|
log("MPU6050_RA_ACCEL_CONFIG=%d", buffer);
|
|
#endif
|
|
|
|
log("GY65 INIT SUCCESS!");
|
|
|
|
return true;
|
|
}
|
|
|
|
void GY65::GetData(float* imu_data)
|
|
{
|
|
mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
|
|
|
|
#if IMU_DEBUG_ENABLE
|
|
log("[%d %d %d] [%d %d %d]", ax, ay, az, gx, gy, gz);
|
|
#endif
|
|
|
|
imu_data[0] = ax*ADX_SCALE;
|
|
imu_data[1] = ay*ADX_SCALE;
|
|
imu_data[2] = az*ADX_SCALE;
|
|
imu_data[3] = gx*GRY_SCALE;
|
|
imu_data[4] = gy*GRY_SCALE;
|
|
imu_data[5] = gz*GRY_SCALE;
|
|
imu_data[6] = 0;
|
|
imu_data[7] = 0;
|
|
imu_data[8] = 0;
|
|
}
|