这个关于6050的应用例程似乎有问题，请“里手”的看看。 -

标题: 这个关于6050的应用例程似乎有问题，请“里手”的看看。 [打印本页]

作者: zdzdzddd 时间: 2017-11-11 09:15
标题: 这个关于6050的应用例程似乎有问题，请“里手”的看看。
STC网给出的四轴飞控开源例程，是俺正想要的，额为欣喜，“四轴飞行器开源资料”  。可是读过后觉得很有问题，没打算尝试。希望请里手人士看看先，知道可能用不，以免无端耽误俺的太多“瞌睡”。比如用到了6050芯片，可是压根就不见读取相关寄存器的环节，形同一顿乱读放至“tp”缓存，然后还煞有其事的对莫名奇妙“tp”缓存内容进行处理。不知是何革命原理：

void Read_MPU6050(u8 *buf)
{
u8 i;

I2C_Start();                //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(ACCEL_XOUT_H); //内部寄存器地址，
I2C_Start();                //起始信号
I2C_SendByte(SlaveAddress+1);  //发送设备地址+读信号
for(i=0; i<13; i++)
{
      buf = I2C_RecvByte();    //读出寄存器数据
      SDA = 0;                   //写应答信号
      SCL = 1;                   //拉高时钟线
      Delay2us();                //延时
      SCL = 0;                   //拉低时钟线
      Delay2us();                //延时
}
      buf = I2C_RecvByte(); //最后一个字节
      SDA = 1;                   //写非应答信号
      SCL = 1;                   //拉高时钟线
      Delay2us();                //延时
      SCL = 0;                   //拉低时钟线
      Delay2us();                //延时
I2C_Stop();                   //停止信号
}
。。。。。。。。。。。。

//**************************************
u8 I2C_RecvByte(void)
{
u8 i;
u8 dat = 0;
SDA = 1;                   //使能内部上拉,准备读取数据,
for (i=0; i<8; i++)       //8位计数器
{
      dat <<= 1;
      SCL = 1;             //拉高时钟线
      Delay2us();          //延时
      dat |= SDA;          //读数据
      SCL = 0;             //拉低时钟线
      Delay2us();          //延时
}
return dat;
}
。。。。。。。。。。。。
//********************************************************************************************
Read_MPU6050(tp); //680us

Angle_ax = ((float)(((int *)&tp)[0])) / 8192.0; //加速度处理结果单位是 +- g
Angle_ay = ((float)(((int *)&tp)[1])) / 8192.0; //转换关系 8192 LSB/g, 1g对应读数8192
Angle_az = ((float)(((int *)&tp)[2])) / 8192.0; //加速度量程 +-4g/S
Last_Angle_gx = Angle_gx;       //储存上一次角速度数据
Last_Angle_gy = Angle_gy;
Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5; //陀螺仪处理结果单位是 +-度
Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5; //陀螺仪量程 +-500度/S, 1度/秒对应读数 65.536
Angle_gz = ((float)(((int *)&tp)[6] - g_z)) / 65.5; //转换关系65.5 LSB/度

IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, Angle_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az);

//**********************************X轴指向************************************************
Ax  = AngleX - a_x - ((float)FRX - 128) / 4.0;       //角度控制量加载至角度

if(YM > 35) ERRORX_Out += Ax, ERRORX_Out += Ax, ERRORX_Out += Ax; //外环积分(油门小于某个值时不积分)
else       ERRORX_Out = 0; //油门小于定值时清除积分值
      if(ERRORX_Out >  1500) ERRORX_Out =  1500;
else if(ERRORX_Out < -1500) ERRORX_Out = -1500; //积分限幅

Out_PID_X = Ax*Out_XP + ERRORX_Out*Out_XI + (Ax-Last_Ax)*Out_XD; //外环PI
Last_Ax = Ax;

if(YM > 35) ERRORX_In += (Angle_gy - Out_PID_X); //内环积分(油门小于某个值时不积分)
else       ERRORX_In = 0; //油门小于定值时清除积分值
      if(ERRORX_In >  500) ERRORX_In =  500;
else if(ERRORX_In < -500) ERRORX_In = -500; //积分限幅

PID_x = (Angle_gy + Out_PID_X) * In_XP + ERRORX_In * In_XI + (Angle_gy - Last_Angle_gy) * In_XD; //内环PID
if(PID_x >  500) PID_x =  500; //输出量限幅
if(PID_x < -500) PID_x = -500;

//**************Y轴指向**************************************************
Ay  = AngleY - a_y + ((float)FRY - 128) / 4.0;       //角度控制量加载至角度

if(YM > 35) ERRORY_Out += Ay, ERRORY_Out += Ay, ERRORY_Out += Ay; //外环积分(油门小于某个值时不积分)
else       ERRORY_Out = 0; //油门小于定值时清除积分值
      if(ERRORY_Out >  1500) ERRORY_Out =  1500;
else if(ERRORY_Out < -1500) ERRORY_Out = -1500; //积分限幅

Out_PID_Y = Ay * Out_YP + ERRORY_Out * Out_YI + (Ay-Last_Ay)*Out_YD; //外环PID
Last_Ay = Ay;

if(YM > 35) ERRORY_In += (Angle_gx - Out_PID_Y);  //内环积分(油门小于某个值时不积分)
else       ERRORY_In = 0; //油门小于定值时清除积分值
      if(ERRORY_In >  500) ERRORY_In =  500;
else if(ERRORY_In < -500) ERRORY_In = -500; //积分限幅

PID_y = (Angle_gx + Out_PID_Y) * In_YP + ERRORY_In * In_YI + (Angle_gx - Last_Angle_gx) * In_YD; //内环PID

if(PID_y > 500) PID_y =  500; //输出量限幅
if(PID_y <-500) PID_y = -500;

//**************Z轴指向(Z轴随便啦，自旋控制没必要上串级PID)*****************************
Az = Angle_gz - ((float)FRZ - 128);

if(YM > 35) Z_integral += Az; //Z轴积分
else       Z_integral = 0;       //油门小于40积分清零
      if(Z_integral >  500.0f) Z_integral =  500.0f; //积分限幅
else if(Z_integral < -500.0f) Z_integral = -500.0f; //积分限幅

PID_z = Az * ZP + Z_integral * ZI + (Az - Last_Az) * ZD;
Last_Az = Az;
if(PID_z >  200) PID_z =  200; //输出量限幅
if(PID_z < -200) PID_z = -200;

speed0 = (int)(  PID_x + PID_y + PID_z); //M1改为逆时针
speed1 = (int)(  PID_x - PID_y - PID_z);
speed2 = (int)( -PID_x - PID_y + PID_z);
speed3 = (int)( -PID_x + PID_y - PID_z);

四轴飞控-STC8A8K64S4A12-LQFP44-PPM-V10.rar

3.86 MB, 下载次数: 5

作者: zdzdzddd 时间: 2017-11-11 09:19
问题1：读数据的Read_MPU6050(tp); 并没涉及0x3B~0x48的位置数据寄存器，那么填入tp[0]~tp[13]的是啥内容呢？
问题2；即便是能“特异的”读到数据，（依6050手册描述）那么tp中的14个数据也应为：去掉tp[0]和tp[13]数据帧头尾，从tp[1]~tp[12]顺序分6组16位数据，分别表达加速度x、y、z和角速度x、y、z。而处理程序中却只用到tp[0]~tp[2]和tp[4]~tp[6],嘛意思？
问题3：所谓的“四轴飞行器开源资料”是否可以无顾虑使用？确实是曾调试通过的资料？搞清存疑对咱这帮只有盲目折腾的门边人士太重要了。

作者: zdzdzddd 时间: 2017-11-11 09:20

作者: sunrei_he 时间: 2021-8-18 10:44
void Read_MPU6050(u8 *buf)这段子程序，个人认为不能直接使用，程序应该不完整！！！

欢迎光临 (http://www.51hei.com/bbs/)