bcd 初始化库系统卷失败_MPU6050单轴双浆姿态平衡PID调戏之MPU6050初始化篇-CSDN博客

MPU6050单轴双浆姿态平衡PID调戏之MPU6050姿态解算

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

需要的设备

硬件介绍

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

编程平台

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

MPU6050之初始化

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

WriteByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); //复位

WriteByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);//唤醒

WriteByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);//设置时钟

//DLPF_CFG，设置低通滤波，

WriteByte(MPU6050_ADDRESS, CONFIG, DLPF_CFG);

// Set sample rate = gyroscope output rate 1000/(1 + SMPLRT_DIV)

// 0x04 use 200hz ,0x07 use 125hz

//SMPLRT_DIV 0x19

WriteByte(MPU6050_ADDRESS, SMPLRT_DIV, SMPLRT_DIV_RATE);

WriteByte(MPU6050_ADDRESS, GYRO_CONFIG, SetGscale << 3); //

WriteByte(MPU6050_ADDRESS, ACCEL_CONFIG, SetAscale << 3);

主要的初始化代码，主要是经常发现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[i] = 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