四旋翼飛行器的飛控實(shí)現

　　I2C_SendByte((u8) REG_Address); 

　　I2C_WaitAck();

　　I2C_Start();

　　I2C_SendByte(SlaveAddress+1);

　　I2C_WaitAck();

　　REG_data= I2C_RadeByte();

　　I2C_NoAck();

　　I2C_Stop();

　　return REG_data;

　　}

　　2.mpu6050;

　　然后用寫(xiě)好的模擬i2c函數讀取mpu6050，根據mpu6050手冊的各寄存器地址，讀取到了重力加速計和陀螺儀的各分量;

　　傳感器采樣率設置為200Hz,這個(gè)值是因為我電調頻率為200Hz，也就是說(shuō)，我的程序循環(huán)一次0.005s,一般來(lái)說(shuō)，采樣率高點(diǎn)沒(méi)問(wèn)題，別比執行一次閉環(huán)控制的周期長(cháng)就行了;

　　陀螺儀量程±2000°/s，加速計量程±2g， 量程越大，取值越不精確;

　　這里注意，由于我們沒(méi)有采用磁力計，而陀螺儀存在零偏，所以最終在yaw方向上沒(méi)有絕對的參考系，不能建立絕對的地理坐標系，這樣最好的結果也僅僅是在yaw上存在緩慢漂移。

　　3.互補濾波;

　　融合時(shí)，陀螺儀的積分運算很大程度上決定了飛行器的瞬時(shí)運動(dòng)情況，而重力加速計通過(guò)長(cháng)時(shí)間的累積不斷矯正陀螺儀產(chǎn)生的誤差，最終得到準確的機體姿態(tài)。

　　這里我們采用Madgwick提供的UpdateIMU算法來(lái)得到姿態(tài)角所對應的四元數，之后只需要經(jīng)過(guò)簡(jiǎn)單運算便可轉換為實(shí)時(shí)歐拉角。感謝Madgwick大大為開(kāi)源做出的貢獻。

　　1 #define sampleFreq 512.0f // sample frequency in Hz

　　2 #define betaDef 0.1f // 2 * proportional gain

　　3

　　4

　　5 volatile float beta = betaDef;

　　6 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;

　　7

　　8 float invSqrt(float x);

　　9

　　10 void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {

　　11 float recipNorm;

　　12 float s0, s1, s2, s3;

　　13 float qDot1, qDot2, qDot3, qDot4;

　　14 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

　　15

　　16 // Rate of change of quaternion from gyroscope

　　17 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);

　　18 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);

　　19 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);

　　20 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

　　21

　　22 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)

　　23 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

　　24

　　25 // Normalise accelerometer measurement

　　26 recipNorm = invSqrt(ax * ax + ay * ay + az * az);

　　27 ax *= recipNorm;

　　28 ay *= recipNorm;

　　29 az *= recipNorm;

　　30

　　31 // Auxiliary variables to avoid repeated arithmetic

　　32 _2q0 = 2.0f * q0;

　　33 _2q1 = 2.0f * q1;

　　34 _2q2 = 2.0f * q2;

　　35 _2q3 = 2.0f * q3;

　　36 _4q0 = 4.0f * q0;

　　37 _4q1 = 4.0f * q1;

　　38 _4q2 = 4.0f * q2;

　　39 _8q1 = 8.0f * q1;

　　40 _8q2 = 8.0f * q2;

　　41 q0q0 = q0 * q0;

　　42 q1q1 = q1 * q1;

　　43 q2q2 = q2 * q2;

　　44 q3q3 = q3 * q3;

　　45

　　46 // Gradient decent algorithm corrective step

　　47 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;

　　48 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;

　　49 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;

　　50 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;

　　51 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude

　　52 s0 *= recipNorm;

　　53 s1 *= recipNorm;

　　54 s2 *= recipNorm;

　　55 s3 *= recipNorm;

　　56

　　57 // Apply feedback step

　　58 qDot1 -= beta * s0;

　　59 qDot2 -= beta * s1;

　　60 qDot3 -= beta * s2;

　　61 qDot4 -= beta * s3;

　　62 }

　　63

　　64 // Integrate rate of change of quaternion to yield quaternion

　　65 q0 += qDot1 * (1.0f / sampleFreq);

　　66 q1 += qDot2 * (1.0f / sampleFreq);

　　67 q2 += qDot3 * (1.0f / sampleFreq);

　　68 q3 += qDot4 * (1.0f / sampleFreq);

　　69

　　70 // Normalise quaternion

　　71 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

　　72 q0 *= recipNorm;

　　73 q1 *= recipNorm;

　　74 q2 *= recipNorm;

　　75 q3 *= recipNorm;

　　76 }

　　77

　　78

　　79 float invSqrt(float x) {

　　80 float halfx = 0.5f * x;

　　81 float y = x;

　　82 long i = *(long*)&y;

　　83 i = 0x5f3759df - (i>>1);

　　84 y = *(float*)&i;

　　85 y = y * (1.5f - (halfx * y * y));

　　86 return y;

　　87 }

　　四元數轉歐拉角的算法可參考 http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html

　　4.獲取期望姿態(tài);

　　也就是遙控部分了，讓用戶(hù)介入控制。

　　本著(zhù)拿來(lái)主義的原則，用上”圓點(diǎn)博士開(kāi)源項目”提供的安卓的開(kāi)源藍牙控制端。

　　圓點(diǎn)博士給出了數據包格式，同過(guò)HC-06藍牙模塊接連到STM32串口1，再無(wú)線(xiàn)連接到控制端，這樣我們就可以獲得控制端不斷發(fā)送的數據包了，并實(shí)時(shí)更新期望姿態(tài)角，這里只需要注意輸出的姿態(tài)角和實(shí)時(shí)姿態(tài)角方向一致以及數據包的校驗就行了。

　　5.PID控制算法;

　　由于簡(jiǎn)單的線(xiàn)性控制不可能滿(mǎn)足四軸飛行器這個(gè)靈敏的系統，引入PID控制器來(lái)更好的糾正系統。

　　簡(jiǎn)介：PID實(shí)指“比例proportional”、“積分integral”、“微分derivative”，這三項構成PID基本要素。每一項完成不同任務(wù)，對系統功能產(chǎn)生不同的影響。

　　以Pitch為例：

　　error為期望角減去實(shí)時(shí)角度得到的誤差;

　　iState為積分i參數對應累積過(guò)去時(shí)間里的誤差總和;

　　if語(yǔ)句限定iState范圍，繁殖修正過(guò)度;

　　微分d參數為當前姿態(tài)減去上次姿態(tài)，估算當前速度(瞬間速度);

　　總調整量為p,i,d三者之和;

　　這樣，P代表控制系統的響應速度，越大，響應越快。

　　I，用來(lái)累積過(guò)去時(shí)間內的誤差，修正P無(wú)法達到的期望姿態(tài)值(靜差);

　　D，加強對機體變化的快速響應，對P有抑制作用。

　　PID各參數的整定需要綜合考慮控制系統的各個(gè)方面，才能達到最佳效果。

　　輸出PWM信號：

　　PID計算完成之后，便可以通過(guò)STM32自帶的定時(shí)資源很容易的調制出四路pwm信號，采用的電調pwm格式為50Hz，高電平持續時(shí)間0.5ms-2.5ms;

　　我以1.0ms-2.0ms為每個(gè)電機的油門(mén)行程，這樣，1ms的寬度均勻的對應電調的從最低到最高轉速。

　　至此，一個(gè)用stm32和mpu6050搭建的飛控系統就算實(shí)現了。

　　剩下的調試PID參數了。