#include "MPU_accel.h" #include #include void MPU_accel::I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr, uint8_t number_Bytes, uint8_t *read_Buffer){ myWire->beginTransmission(driver_Addr); myWire->write(start_Addr); myWire->endTransmission(false); uint8_t i = 0; myWire->requestFrom(driver_Addr,number_Bytes); //! Put read results in the Rx buffer while (myWire->available()) { read_Buffer[i++] = myWire->read(); } } void MPU_accel::I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr, uint8_t number_Bytes, uint8_t *write_Buffer){ myWire->beginTransmission(driver_Addr); myWire->write(start_Addr); myWire->write(*write_Buffer); myWire->endTransmission(); } int MPU_accel::Init(void){ unsigned char tempdata[1]; unsigned char regdata; I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_WHOAMI, 1, tempdata); const uint8_t whoami = tempdata[0]; if (whoami == 0x19) { model = 6886; // MPU6886 } else if (whoami == 0x71) { model = 9250; // MPU9250 } else { return -1; } delay(1); regdata = 0x00; I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data); delay(10); regdata = (0x01<<7); I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data); delay(10); regdata = (0x01<<0); I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data); delay(10); regdata = 0x10; // AFS_8G I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG, 1, ®data); delay(1); regdata = 0x18; // GFS_2000DPS I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, ®data); delay(1); regdata = 0x01; I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_CONFIG, 1, ®data); delay(1); regdata = 0x05; I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_SMPLRT_DIV, 1,®data); delay(1); regdata = 0x00; I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, ®data); delay(1); regdata = 0x00; I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG2, 1, ®data); delay(1); regdata = 0x00; I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_USER_CTRL, 1, ®data); delay(1); regdata = 0x00; I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_EN, 1, ®data); delay(1); regdata = 0x22; I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_PIN_CFG, 1, ®data); delay(1); regdata = 0x01; I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, ®data); delay(100); getGres(); getAres(); return 0; } void MPU_accel::getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az){ uint8_t buf[6]; I2C_Read_NBytes(MPU6886_ADDRESS,MPU6886_ACCEL_XOUT_H,6,buf); *ax=((int16_t)buf[0]<<8)|buf[1]; *ay=((int16_t)buf[2]<<8)|buf[3]; *az=((int16_t)buf[4]<<8)|buf[5]; } void MPU_accel::getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz){ uint8_t buf[6]; I2C_Read_NBytes(MPU6886_ADDRESS,MPU6886_GYRO_XOUT_H,6,buf); *gx=((uint16_t)buf[0]<<8)|buf[1]; *gy=((uint16_t)buf[2]<<8)|buf[3]; *gz=((uint16_t)buf[4]<<8)|buf[5]; } void MPU_accel::getTempAdc(int16_t *t){ uint8_t buf[2]; I2C_Read_NBytes(MPU6886_ADDRESS,MPU6886_TEMP_OUT_H,2,buf); *t=((uint16_t)buf[0]<<8)|buf[1]; } // //!俯仰，航向，横滚：pitch，yaw，roll，指三维空间中飞行器的旋转状态。 // void MPU_accel::getAhrsData(float *pitch,float *roll,float *yaw){ // float accX = 0; // float accY = 0; // float accZ = 0; // float gyroX = 0; // float gyroY = 0; // float gyroZ = 0; // getGyroData(&gyroX,&gyroY,&gyroZ); // getAccelData(&accX,&accY,&accZ); // MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ,pitch,roll,yaw); // } void MPU_accel::getGres(){ switch (Gyscale) { // Possible gyro scales (and their register bit settings) are: case GFS_250DPS: gRes = 250.0f/32768.0f; gyRange = 250; break; case GFS_500DPS: gRes = 500.0f/32768.0f; gyRange = 500; break; case GFS_1000DPS: gRes = 1000.0f/32768.0f; gyRange = 1000; break; case GFS_2000DPS: gRes = 2000.0f/32768.0f; gyRange = 2000; break; } } void MPU_accel::getAres(){ switch (Acscale) { // Possible accelerometer scales (and their register bit settings) are: // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: case AFS_2G: aRes = 2.0/32768.0; acRange = 2000; break; case AFS_4G: aRes = 4.0/32768.0; acRange = 4000; break; case AFS_8G: aRes = 8.0/32768.0; acRange = 8000; break; case AFS_16G: aRes = 16.0/32768.0; acRange = 16000; break; } } void MPU_accel::SetGyroFsr(Gscale scale) { //return IIC_Write_Byte(MPU_GYRO_CFG_REG,scale<<3);//设置陀螺仪满量程范围 unsigned char regdata; regdata = (scale<<3); I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, ®data); delay(10); Gyscale = scale; getGres(); } void MPU_accel::SetAccelFsr(Ascale scale) { unsigned char regdata; regdata = (scale<<3); I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG, 1, ®data); delay(10); Acscale = scale; getAres(); } // x/y/z are in 1/1000 if g // avoiding costly float calculations void MPU_accel::getAccelDataInt(int16_t* ax, int16_t* ay, int16_t* az) { int16_t accX = 0; int16_t accY = 0; int16_t accZ = 0; getAccelAdc(&accX, &accY, &accZ); if (ax != nullptr) { *ax = ((int32_t)accX * acRange) / 0x7FFFL; } if (ay != nullptr) { *ay = ((int32_t)accY * acRange) / 0x7FFFL; } if (az != nullptr) { *az = ((int32_t)accZ * acRange) / 0x7FFFL; } } void MPU_accel::getAccelData(float* ax, float* ay, float* az){ int16_t accX = 0; int16_t accY = 0; int16_t accZ = 0; getAccelAdc(&accX,&accY,&accZ); *ax = (float)accX * aRes; *ay = (float)accY * aRes; *az = (float)accZ * aRes; } // x/y/z are in dps - degrees per second // avoiding costly float calculations void MPU_accel::getGyroDataInt(int16_t* ax, int16_t* ay, int16_t* az) { int16_t gyX = 0; int16_t gyY = 0; int16_t gyZ = 0; getGyroAdc(&gyX, &gyY, &gyZ); if (ax != nullptr) { *ax = ((int32_t)gyX * gyRange) / 0x7FFFL; } if (ay != nullptr) { *ay = ((int32_t)gyY * gyRange) / 0x7FFFL; } if (az != nullptr) { *az = ((int32_t)gyZ * gyRange) / 0x7FFFL; } } void MPU_accel::getGyroData(float* gx, float* gy, float* gz){ int16_t gyroX = 0; int16_t gyroY = 0; int16_t gyroZ = 0; getGyroAdc(&gyroX,&gyroY,&gyroZ); *gx = (float)gyroX * gRes; *gy = (float)gyroY * gRes; *gz = (float)gyroZ * gRes; } void MPU_accel::getTempData(float *t){ int16_t temp = 0; getTempAdc(&temp); *t = (float)temp / 326.8 + 25.0; }