|
|
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);
|
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
