MPU6050单轴双浆姿态平衡PID调戏之MPU6050初始化

zzns · 发表于 2019-3-3 12:13:22

本帖最后由 zzns 于 2019-3-3 12:13 编辑

之前的MPU6050初始化出问题，终于解决，用了WringPi代替I2C库之后，解决。

这个其实是为了直观的观察PID的调节过程，因为本人对于数学并不擅长，又说PID是比较依赖经验，看着老外搭建了一个实验的设备，我也画了葫芦做了一个。以下是需要的设备介绍。

需要的设备硬件介绍

树莓派3b
MPU6050
PCA9685
2个有浆无刷电机（浆要两个不一样的，转的时候要抵消）

编程平台

树莓派运行官方Linux系统，编辑器用交叉编译工具（arm-linux-g++）。

MPU6050之初始化

这里就不介绍6050的各种功能了。树莓派与MPU6050通过IIC接口连接，供电也是用树莓派的3.3v接口。其实早在2个月前已经完成了功能，但是由于MPU6050的接线问题，导致初始化通不过，而且写入的数据和读出的数据对不上，一度以为是IIC库，或者是模块坏了，有时候灵光，有时候不灵光。MPU6050接线已经要稳可靠。期间还烧了一块MPU9250。

  void MPU6050lib::initMPU6050()
  {
uint8_t c = 0;
delay(1000);
for(;;)
{
   printf("INIT MPU6050\n");
   WriteByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); //复位
   delay(100);
   c =  ReadByte(MPU6050_ADDRESS, PWR_MGMT_1);
   if(c != 0x40)
      continue;
   delay(100);
   WriteByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);//唤醒
   delay(100);
   WriteByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);//设置时钟
   delay(100);
   c =  ReadByte(MPU6050_ADDRESS, PWR_MGMT_1);
   if(c != 0x01)
      continue;
   delay(100);
   //DLPF_CFG，设置低通滤波，我一度以为是这里出问题。后来想到是不是杜邦线接线不稳。
   WriteByte(MPU6050_ADDRESS, CONFIG, DLPF_CFG);
  #if defined MPU_DEBUG
   c =  ReadByte(MPU6050_ADDRESS, CONFIG);
   printf("CONFIG =%x\n", c);
  #endif
   if(c != DLPF_CFG)
      continue;
   // Set sample rate = gyroscope output rate 1000/(1 + SMPLRT_DIV)
   // 0x04 use 200hz ,0x07 use 125hz
   //SMPLRT_DIV    0x19
   delay(100);
   WriteByte(MPU6050_ADDRESS, SMPLRT_DIV, SMPLRT_DIV_RATE);
   delay(100);
  #if defined MPU_DEBUG
   c =  ReadByte(MPU6050_ADDRESS, SMPLRT_DIV);
   printf("SMPLRT_DIV0x13 =%x\n", c);
  #endif
   if(c != SMPLRT_DIV_RATE)
      continue;
   delay(100);

   uint8_t c =  ReadByte(MPU6050_ADDRESS, GYRO_CONFIG);
   WriteByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); //
   WriteByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0x18); //
   delay(100);
   //SetGscale = 0x03,这里设置2000的量程。
   WriteByte(MPU6050_ADDRESS, GYRO_CONFIG, SetGscale << 3); //
  #if defined MPU_DEBUG
c =  ReadByte(MPU6050_ADDRESS, GYRO_CONFIG);
printf("GYRO_CONFIG is 0x18 =%x\n", c);
  #endif
   if(c != (3<<3))
      continue;
   delay(100);

   c =  ReadByte(MPU6050_ADDRESS, ACCEL_CONFIG);
   WriteByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00);
   delay(100);
   WriteByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x18);
   WriteByte(MPU6050_ADDRESS, ACCEL_CONFIG, SetAscale << 3);
   delay(100);
  #if defined MPU_DEBUG
c =  ReadByte(MPU6050_ADDRESS, ACCEL_CONFIG);
printf("ACCEL_CONFIG0=%x\n", c);
  #endif
   delay(100);
   WriteByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x22);
   delay(100);
   WriteByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); //关闭所有中断
   delay(100);
   WriteByte(MPU6050_ADDRESS, USER_CTRL,0X00);//I2C主模式关闭
   delay(100);
   WriteByte(MPU6050_ADDRESS, FIFO_EN,0X00);//关闭FIFO
   delay(100);
   WriteByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00);
   delay(100);
   if(c == 0x00)
      break;
}
  }

这么复杂的初始化，主要是经常发现6050初始化常常失败，一开始以为是接线不牢固什么的，直到前两天才怀疑是I2C库文件的问题，后来改了WiringPi库。神奇的有效果了，再也没有出现问题，6050的原始数据也非常稳定。

MPU6050读写函数实现  //wiringPiI2CWriteReg8，改成WiringPi后的代码实现，参数address可以省略，这里没改。
  uint8_t MPU6050lib::WriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
  {

   if(wiringPiI2CWriteReg8(fd,subAddress,data) < 0)
      return -1;
   return 0;
  }
  
  uint8_t MPU6050lib::ReadByte(uint8_t address, uint8_t subAddress)
  {

   uint8_t data;
   data = wiringPiI2CReadReg8(fd, subAddress);

return data;
  }
  
  uint8_t MPU6050lib::ReadBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest)
  {

for(uint8_t i = 0; i < count; i++)
{
   dest = ReadByte(address, subAddress + i);
}
return 0;
  }
如果要移植到各自平台，改上面3个函数，基本差不多可以用了。
  void MPU6050lib::readAccelData(int16_t *destination)
  {
uint8_t rawData[6];  // X,Y,Z,数据都放在这里
ReadBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
destination[0] = (int16_t)((rawData[0] << 8) | rawData[1]) ;
destination[1] = (int16_t)((rawData[2] << 8) | rawData[3]) ;
destination[2] = (int16_t)((rawData[4] << 8) | rawData[5]) ;
  }
  
  void MPU6050lib::readGyroData(int16_t *destination)
  {
uint8_t rawData[6];  // X,Y,Z,数据都放在这里
ReadBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
destination[0] = (int16_t)((rawData[0] << 8) | rawData[1]) ;
destination[1] = (int16_t)((rawData[2] << 8) | rawData[3]) ;
destination[2] = (int16_t)((rawData[4] << 8) | rawData[5]) ;
  }做姿态解算前的准备
接下来在sensor.c的ms_update()调用函数进入循环即可。
  int ms_update() {
mpu.readAccelData(accelCount);
accelCount[0] -= ACCEL_OFFSET_X; //先平放输出300次，求平均值
accelCount[1] -= ACCEL_OFFSET_Y;
accelCount[2] += ACCEL_OFFSET_Z;//平放数据理想情况是16384，这里加上3776 = 16384
ax = (float)(accelCount[0] )*aRes; //aRes = 1/16384 ,这里转换数据。
ay = (float)(accelCount[1] )*aRes;
az = (float)(accelCount[2] )*aRes;
   //delay_ms(5);
mpu.readGyroData(gyroCount);
gyroCount[0] -= GYRO_OFFSET_X;
gyroCount[1] -= GYRO_OFFSET_Y;
gyroCount[2] -= GYRO_OFFSET_Z;
gx = (float)(gyroCount[0] )*gRes ; //同上面的情况。
gy = (float)(gyroCount[1] )*gRes ;
gz = (float)(gyroCount[2] )*gRes ;
Mahony_update_6X(gx, gy, gz, ax, ay, az); //滤波
Mahony_computeAngles();
   //printf("%f, %f\n", getRoll(), getPitch());
delay_ms(15); //
return 0;
  }
  打印ROLL角的数据