|
|
STC網(wǎng)給出的四軸飛控開源例程，是俺正想要的，額為欣喜，“四軸飛行器開源資料” �？墒亲x過后覺得很有問題，沒打算嘗試。希望請(qǐng)里手人士看看先，知道可能用不，以免無端耽誤俺的太多“瞌睡”。比如用到了6050芯片，可是壓根就不見讀取相關(guān)寄存器的環(huán)節(jié)，形同一頓亂讀放至“tp”緩存，然后還煞有其事的對(duì)莫名奇妙“tp”緩存內(nèi)容進(jìn)行處理。不知是何革命原理：
void Read_MPU6050(u8 *buf)
{
u8 i;
I2C_Start(); //起始信號(hào)
I2C_SendByte(SlaveAddress); //發(fā)送設(shè)備地址+寫信號(hào)
I2C_SendByte(ACCEL_XOUT_H); //內(nèi)部寄存器地址，
I2C_Start(); //起始信號(hào)
I2C_SendByte(SlaveAddress+1); //發(fā)送設(shè)備地址+讀信號(hào)
for(i=0; i<13; i++)
{
buf = I2C_RecvByte(); //讀出寄存器數(shù)據(jù)
SDA = 0; //寫應(yīng)答信號(hào)
SCL = 1; //拉高時(shí)鐘線
Delay2us(); //延時(shí)
SCL = 0; //拉低時(shí)鐘線
Delay2us(); //延時(shí)
}
buf = I2C_RecvByte(); //最后一個(gè)字節(jié)
SDA = 1; //寫非應(yīng)答信號(hào)
SCL = 1; //拉高時(shí)鐘線
Delay2us(); //延時(shí)
SCL = 0; //拉低時(shí)鐘線
Delay2us(); //延時(shí)
I2C_Stop(); //停止信號(hào)
}
。。。。。。。。。。。。
//**************************************
u8 I2C_RecvByte(void)
{
u8 i;
u8 dat = 0;
SDA = 1; //使能內(nèi)部上拉,準(zhǔn)備讀取數(shù)據(jù),
for (i=0; i<8; i++) //8位計(jì)數(shù)器
{
dat <<= 1;
SCL = 1; //拉高時(shí)鐘線
Delay2us(); //延時(shí)
dat |= SDA; //讀數(shù)據(jù)
SCL = 0; //拉低時(shí)鐘線
Delay2us(); //延時(shí)
}
return dat;
}
。。。。。。。。。。。。
//********************************************************************************************
Read_MPU6050(tp); //680us
Angle_ax = ((float)(((int *)&tp)[0])) / 8192.0; //加速度處理 結(jié)果單位是 +- g
Angle_ay = ((float)(((int *)&tp)[1])) / 8192.0; //轉(zhuǎn)換關(guān)系 8192 LSB/g, 1g對(duì)應(yīng)讀數(shù)8192
Angle_az = ((float)(((int *)&tp)[2])) / 8192.0; //加速度量程 +-4g/S
Last_Angle_gx = Angle_gx; //儲(chǔ)存上一次角速度數(shù)據(jù)
Last_Angle_gy = Angle_gy;
Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5; //陀螺儀處理 結(jié)果單位是 +-度
Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5; //陀螺儀量程 +-500度/S, 1度/秒 對(duì)應(yīng)讀數(shù) 65.536
Angle_gz = ((float)(((int *)&tp)[6] - g_z)) / 65.5; //轉(zhuǎn)換關(guān)系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; //外環(huán)積分(油門小于某個(gè)值時(shí)不積分)
else ERRORX_Out = 0; //油門小于定值時(shí)清除積分值
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; //外環(huán)PI
Last_Ax = Ax;
if(YM > 35) ERRORX_In += (Angle_gy - Out_PID_X); //內(nèi)環(huán)積分(油門小于某個(gè)值時(shí)不積分)
else ERRORX_In = 0; //油門小于定值時(shí)清除積分值
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; //內(nèi)環(huán)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; //外環(huán)積分(油門小于某個(gè)值時(shí)不積分)
else ERRORY_Out = 0; //油門小于定值時(shí)清除積分值
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; //外環(huán)PID
Last_Ay = Ay;
if(YM > 35) ERRORY_In += (Angle_gx - Out_PID_Y); //內(nèi)環(huán)積分(油門小于某個(gè)值時(shí)不積分)
else ERRORY_In = 0; //油門小于定值時(shí)清除積分值
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; //內(nèi)環(huán)PID
if(PID_y > 500) PID_y = 500; //輸出量限幅
if(PID_y <-500) PID_y = -500;
//**************Z軸指向(Z軸隨便啦，自旋控制沒必要上串級(jí)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改為逆時(shí)針
speed1 = (int)( PID_x - PID_y - PID_z);
speed2 = (int)( -PID_x - PID_y + PID_z);
speed3 = (int)( -PID_x + PID_y - PID_z);
|
|
