筆者目前在讀研究所學生，研究的方向正是飛行器的定位與導航。去年10月份開始設計的微型四旋翼，近日才完成整個系統的設計并且飛機可以較為穩定的懸停在空中。
下面就将筆者最近整理的制作過程梳理一遍，還希望與有興趣的網友共同讨論。
筆者将分為硬體設計與軟體設計兩大部分來分别闡述系統的構成：
硬體設計：
總體思路：
整個機架采用PCB闆，将四個電機固定在PCB闆的四個角，外接電池。
硬體子產品：
單片機、慣性測量子產品(IMU)、無線通訊子產品、電機驅動子產品、續流二極管、電源管理子產品(穩壓與充放電)、直流有刷電機、大電流放電電池、遙控器。
硬體選型：
| 子產品名稱 | 元件名稱 | 數量 | |
| 單片機 | STM32F103CBT6 | 1 | |
| 慣性測量子產品(IMU) | MPU6050(三軸加速度計+三軸陀螺儀) | 1 | |
| 無線通訊子產品 | NRF24L01 | 1 | |
| 電機驅動子產品 | AO3400 5.8A | 4 | |
| 續流二極管 | SS34 3A | 4 | |
| 電源管理子產品 | 穩壓 | TPS79333 3.3V | 1 |
| 充放電 | TP4057 USB相容5V充電 | 1 | |
| 直流有刷電機 | 空心杯有刷直流電機7*16mm | 4 | |
| 大電流放電電池 | 250mAh 20C | 1 | |
| 遙控器 | JOYPAD遊戲搖桿 | 1 |
硬體工作綜述：
單片機負責整個系統的協調工作；慣性測量子產品(IMU)負責測量四旋翼的姿态；無線通訊子產品負責四旋翼與遙控器的通訊；電機驅動子產品負責驅動電機；續流二極管負責對電機進行續流；電源管理子產品中的穩壓子產品負責整個系統的供電，電源管理子產品中的充放電子產品負責對電池充電；有刷電機負責提供四旋翼的飛行動力；大電流放電電池負責四旋翼的能量來源；遙控器負責對四旋翼進行遙控和控制。
硬體設計功能子產品圖：
實際效果圖與相關參數：
尺寸：對角電機軸距10x10cm
重量：33.2g(帶電池)
軟體設計：
總體思路：
慣性測量子產品(IMU)測量出目前飛機的三軸加速度與三軸角速度并傳送給單片機處理，由單片機進行基于四元數的姿态解算，求解出目前飛機的pitch、roll、yaw三個角度值，然後根據這三個角度經過PID控制運算，輸出四路PWM控制四個直流有刷電機的加減速進而達到飛機的平衡懸停。
其中，慣性測量子產品(IMU)的加速度計由于噪聲比較大，是以需要對其進行濾波處理；而遙控器則是對飛機進行實時的姿态控制；最後由于四旋翼制作的特殊性，在調試PID參數階段會頻繁的燒寫程式，鑒于此，筆者開發了基于NRF24L01的Bootloader技術，免除了燒寫Flash的實體連線限制，可實作遠端程式一鍵下載下傳。
姿态解算：
姿态解算屬于四旋翼制作的核心部分，如果姿态解算能夠實時的反應出飛機的狀态，那麼對于控制來講就相對來說比較容易了。而姿态結算所要做的事情就是兩個坐标系之間的正确轉化(地理坐标系與載體坐标系)，這種轉化有很多種表示方法，例如歐拉角法、方向餘弦矩陣法、四元數法、旋轉矢量法等。筆者采用的是應用廣泛的四元數法，而旋轉矢量法則是一種基于四元數法的改進四元數法。
四元數本是用于描述四維空間向量的一種方法，對于他的線性變換也就是在四維空間中的拉伸和旋轉，顯而易見，我們用四元數的向量乘法來表示三維空間中的旋轉是綽綽有餘的。
通過慣性測量子產品(IMU)傳送過來的目前飛機的三軸加速度和三軸角速度的值，這樣一個三維的向量，轉化為四維向量，然後在四維空間中做線性變換(也可以說在三維空間中旋轉)後輸出，利用四元數與歐拉角的關系(一定要注意旋轉順序)，将目前四元數轉換為歐拉角pitch、roll、yaw即得到目前飛機的姿态。
以下給出筆者姿态融合的代碼，該代碼網上都有，筆者在這裡做了些許注釋，友善了解。
/*********************************************************************************
函數名：void IMUupdata(float gx, float gy, float gz, float ax, float ay, float az)
說明：IMU單中繼資料融合，更新姿态四元數
入口：float gx 陀螺儀x分量
float gy 陀螺儀y分量
float gz 陀螺儀z分量
float ax 加速度計x分量
float ay 加速度計y分量
float az 加速度計z分量
出口：無
備注：核心思想:利用陀螺儀來計算高速動态下的姿态,利用加速度計來進行角度修正
**********************************************************************************/
void IMUupdata(float gx, float gy, float gz, float ax, float ay, float az)
{
float recipNorm; //平方根的倒數
float halfvx, halfvy, halfvz; //在目前載體坐标系中，重力分量在三個軸上的分量
float halfex, halfey, halfez; //目前加速度計測得的重力加速度在三個軸上的分量與目前姿态在三個軸上的重力分量的誤差,這裡采用差積的方式
float qa, qb, qc;
gx = gx * PI / 180; //轉換為弧度制
gy = gy * PI / 180;
gz = gz * PI / 180;
//如果加速度計處于自由落體狀态，可能會出現這種情況，不進行姿态解算，因為會産生分母無窮大的情況
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
//機關化加速度計,意義在于在變更了加速度計的量程之後不需要修改Kp參數,因為這裡歸一化了
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
//将目前姿态的重力在三個軸上的分量分離出來
//就是方向餘弦旋轉矩陣的第三列,注意是地理坐标系(n系)到載體坐标系(b系)的,不要弄反了.如果書上是b系到n系,轉置即可
//慣性測量器件測量的都是關于b系的值，為了友善，我們一般将b系轉換到n系進行導航參數求解。但是這裡并不需要這樣做，因為這裡是對陀螺儀進行補償
halfvx = g_q1 * g_q3 - g_q0 * g_q2;
halfvy = g_q0 * g_q1 + g_q2 * g_q3;
halfvz = g_q0 * g_q0 - 0.5f + g_q3 * g_q3;
//計算由目前姿态的重力在三個軸上的分量與加速度計測得的重力在三個軸上的分量的差,這裡采用三維空間的差積(向量積)方法求差
//計算公式由矩陣運算推導而來 公式參見http://en.wikipedia.org/wiki/Cross_product 中的Mnemonic部分
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
//積分求誤差,關于目前姿态分離出的重力分量與目前加速度計測得的重力分量的內插補點進行積分消除誤差
if(g_twoKi > 0.0f)
{
g_integralFBx += g_twoKi * halfex * CNTLCYCLE; //Ki積分
g_integralFBy += g_twoKi * halfey * CNTLCYCLE;
g_integralFBz += g_twoKi * halfez * CNTLCYCLE;
gx += g_integralFBx; //将積分誤差回報到陀螺儀上，修正陀螺儀的值
gy += g_integralFBy;
gz += g_integralFBz;
}
else //不進行積分運算，隻進行比例調節
{
g_integralFBx = 0.0f;
g_integralFBy = 0.0f;
g_integralFBz = 0.0f;
}
//直接應用比例調節，修正陀螺儀的值
gx += g_twoKp * halfex;
gy += g_twoKp * halfey;
gz += g_twoKp * halfez;
}
//以下為四元數微分方程.将陀螺儀和四元數結合起來，是姿态更新的核心算子
//計算方法由矩陣運算推導而來
// . 1
// q = - * q x Omega 式中左邊是四元數的倒數,右邊的x是四元數乘法,Omega是陀螺儀的值(即角速度)
// 2
// .
// [q0] [0 -wx -wy -wz] [q0]
// .
// [q1] [wx 0 wz -wy] [q1]
// . = *
// [q2] [wy -wz 0 wx ] [q2]
// .
// [q3] [wz wy -wx 0 ] [q3]
gx *= (0.5f * CNTLCYCLE);
gy *= (0.5f * CNTLCYCLE);
gz *= (0.5f * CNTLCYCLE);
qa = g_q0;
qb = g_q1;
qc = g_q2;
g_q0 += (-qb * gx - qc * gy - g_q3 * gz);
g_q1 += ( qa * gx + qc * gz - g_q3 * gy);
g_q2 += ( qa * gy - qb * gz + g_q3 * gx);
g_q3 += ( qa * gz + qb * gy - qc * gx);
//機關化四元數,意義在于機關化四元數在空間旋轉時是不會拉伸的,僅有旋轉角度.這類似與線性代數裡面的正交變換
recipNorm = invSqrt(g_q0 * g_q0 + g_q1 * g_q1 + g_q2 * g_q2 + g_q3 * g_q3);
g_q0 *= recipNorm;
g_q1 *= recipNorm;
g_q2 *= recipNorm;
g_q3 *= recipNorm;
//四元數到歐拉角轉換，轉換順序為Z-Y-X,參見<Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors>.pdf一文,P24
//注意此時的轉換順序是1-2-3，即X-Y-Z。但是由于畫圖友善，作者這裡做了一個轉換，即調換Z和X，是以順序沒變
g_Yaw = atan2(2 * g_q1 * g_q2 + 2 * g_q0 * g_q3, g_q1 * g_q1 + g_q0 * g_q0 - g_q3 * g_q3 - g_q2 * g_q2) * 180 / PI; //Yaw
g_Roll = asin(-2 * g_q1 * g_q3 + 2 * g_q0* g_q2) * 180 / PI; //Roll
g_Pitch = atan2(2 * g_q2 * g_q3 + 2 * g_q0 * g_q1, -2 * g_q1 * g_q1 - 2 * g_q2* g_q2 + 1) * 180 / PI; //Pitch
}
注意其中的快速開方函數來自維基百科，精度0.175%。并且注意輸入的陀螺儀必須是弧度制(這一點在進入函數的時候已經做了轉換)，否則姿态解算是錯誤的。
針對上述代碼我還想說明一個筆者發現的問題：有很多網友和許多外國的四軸代碼(CrazyFlie)在這個姿态解算的加速度計部分都做了零點校準處理。意思就是在開機的時候讀取一定次數的加速度計的值，然後平均一下，得到一個初始狀态的偏移量，最後在輸出的時候加速度計減掉這個值，然後再給到姿态解算代碼部分。而筆者在剛開始移植代碼的時候是沒有做這個零點校準處理的(當然，陀螺儀必須要做零點處理，因為陀螺儀的原理，必須要在靜止時輸出為0)，包括現在依舊沒有對加速度計做零點校準處理，仍然可以獲得較為實時的姿态。
那麼顯然對加速度計做不做零點校準處理都是可行的。為什麼呢？經過我的分析，首先在這段代碼中，我們對加速度計進行了歸一化處理，我們知道在數學當中，對數值進行機關化意味着長度不變而隻改變方向，對于加速度計來講，他的”長度”就是加速度的大小，他的”方向”就是加速度的方向。是以我們對加速度計做了機關化之後，其加速度的大小我們就無進而知，但是我們利用了他的方向來進行姿态解算。就這一點來講，無論我們做不做零點校準處理，進來的加速度的值始終都抛棄掉大小，并關注方向，與零點校準處理無關。另一方面，由于我們生活在重力場裡面，那麼加速度計在靜止狀态下測量的是重力加速度，會有一個g的輸出。而我們理想的加速度計應該是輸出0，而在有加速度的時候應該輸出相應的加速度，但是現實是我們生活在一個重力場裡面，是以必定有一個重力輸出。那麼零點校準處理的核心就是我們對于加速度計的了解問題，如果做了零點校準處理，那麼我們使用的加速度計就成為了”真正的”加速度計，當有重力的時候他輸出為0，有加速度的時候就輸出加速度；當我們沒有做零點校準處理的時候，那麼我們使用的加速度計就成了”重力”加速度計。但是細心的你其實可以發現那個并不是真正的加速度計，我将傳感器反過來放的話輸出就不是0了，而是z軸上的負值輸出。顯然這個零點标準處理做的不那麼标準。況且這種處理方式是非常粗糙的，因為加速度計的噪聲十分的大，資料波動非常厲害，我做了16深度的視窗滑動濾波再加19階的kaiser窗FIR低通濾波，其輸出仍然有1~4左右的波動。可見加速度計确實不好處理，除非用Kalman濾波。
鑒于以上兩點原因，本人就沒有做加速度計的零點校準處理。當需要測量飛機的加速度大小并實作定位時，那麼就需要做零點校準處理了。而當我們隻需要解算姿态，那麼加速度計就不需要做零點校準處理。
以上是筆者對于加速度計零點校準處理的愚見，如有錯誤，還望共同學習。
最後想說明一點的是關于陀螺儀的資料轉化，筆者在最開始編寫姿态解算代碼時，發現角度的變化與實時姿态差了好幾個數量級，展現出來的現象就是稍微移動一下飛機，姿态就呼呼的飛速變化。之前一直以為是姿态解算中的Kp和Ki的系數問題，後來才發現是陀螺儀的資料沒有轉化成标準機關(°/s)輸出，沒有參看pdf上的量程機關，是以沒有做資料轉化處理，在這裡提醒一下各位，不要犯筆者這種低級錯誤了。
PID控制：
PID控制屬于自動化領域，由于筆者的大學出生于自動化專業，是以對于自動控制原理有一點理論上的認識。P是比例，I是積分，D是微分，這是最基本的定義。對于一個系統，我們想要控制他，目前的理論是引入負回報，這個概念相當重要，是由維納提出來的。意思是，将輸出引入到輸入端，并且用輸入減去輸出，這就是著名的負回報系統。很顯然，我們要做的是輸出跟随輸入，使得系統可控。也就是說要求輸出和輸入的誤差為0，即輸出等于輸入。在實際的系統中，輸出與輸入肯定是存在誤差的，這種誤差就通過PID來控制使得滿足輸出與輸入誤差為0。當系統由于幹擾出現誤差時，此時的P參數就起到了“立竿見影”的作用，将目前系統誤差第一時間反應出來，也就是目前誤差多少，我就給你多少輸出值來補償你的誤差。這種調節方式的特點是快速而有勁，相應來說就是發散且不穩定的；而D參數則具有一種預見性，這種預見性可以提前預知系統的行為，比如距離設定值是越來越遠還是越來越近，前者D的作用越強，後者D的作用越弱。可以發現D參數與P參數具有一定的互補性質，P會引起發散，而D則是抑制發散，使系統非常敏感；最後I參數是積分，在連續系統中是時間的積分，在數字系統中是時間的累加。這種累加無疑會造成系統的不穩定，如果系統長時間處于不平衡的位置，那麼由于時間的累計，I的作用會變得越來越強，甚至超過了P的作用，那麼系統必定失控。但是他的作用有時候确實不可忽略的：消除靜差。
在這裡筆者尤其提醒大家一點，如果此時系統的輸出達到了我們給定的期望值，也就是說輸出與輸入誤差為0，即現在的PID控制器輸入0，是以輸出也是0，也就是說此時的執行機構是不會輸出的，讓裝置處于自由運動階段。而非我們認為的當你觀察到一個系統處于穩定運作并達到給定值的時候，他的執行機構是一直在輸出的，這是錯誤的。
淺談完PID，對于四旋翼的控制，筆者采用的就是經典控制論中的PID控制，利用的是期望姿态(pitch=0,roll=0,yaw=0)與目前姿态的誤差，通過PID的控制作用輸出四路不同的PWM驅動電機讓飛機調整自己的姿态滿足目前姿态與期望姿态的誤差為0的目标，這也是PID控制器的目标。
以下是筆者的PID控制代碼：
/************************************************************************************************
函數名：void Quadrotor_Control(const float Exp_Pitch, const float Exp_Roll, const float Exp_Yaw)
說明：四旋翼控制函數，用于PWM計算和輸出
入口：const float Exp_Pitch 期望傾仰角
const float Exp_Roll 期望橫滾角
const float Exp_Yaw 期望偏航角
出口：無
備注：目前控制環為姿态控制環
其中有 大角度放棄控制 和 懸停黃燈訓示
************************************************************************************************/
void Quadrotor_Control(const float Exp_Pitch, const float Exp_Roll, const float Exp_Yaw)
{
s16 outputPWM_Pitch, outputPWM_Roll, outputPWM_Yaw;
// --- 得到目前系統的誤差-->利用期望角度減去目前角度
g_Attitude_Error.g_Error_Pitch = Exp_Pitch - g_Pitch;
g_Attitude_Error.g_Error_Roll = Exp_Roll - g_Roll;
g_Attitude_Error.g_Error_Yaw = Exp_Yaw - g_Yaw;
// --- 傾角太大，放棄控制
if (fabs(g_Attitude_Error.g_Error_Pitch) >= 55 || fabs(g_Attitude_Error.g_Error_Roll) >= 55)
{
PWM2_LED = 0; //藍燈亮起
PWM_Set(0, 0, 0, 0); //停機
return ;
}
PWM2_LED = 1; //藍燈熄滅
// --- 穩定訓示燈,黃色.當角度值小于3°時，判定為基本穩定,黃燈亮起
if (fabs(g_Attitude_Error.g_Error_Pitch) <= 3 && fabs(g_Attitude_Error.g_Error_Roll) <= 3)
PWM4_LED = 0;
else
PWM4_LED = 1;
// --- 積分運算與積分誤差限幅
if (fabs(g_Attitude_Error.g_Error_Pitch) <= 20) //積分分離-->在姿态誤差角小于20°時引入積分
{ //Pitch
//累加誤差
g_Attitude_Error.g_ErrorI_Pitch += g_Attitude_Error.g_Error_Pitch;
//積分限幅
if (g_Attitude_Error.g_ErrorI_Pitch >= PITCH_I_MAX)
g_Attitude_Error.g_ErrorI_Pitch = PITCH_I_MAX;
else if (g_Attitude_Error.g_ErrorI_Pitch <= -PITCH_I_MAX)
g_Attitude_Error.g_ErrorI_Pitch = -PITCH_I_MAX;
}
if (fabs(g_Attitude_Error.g_Error_Roll) <= 20)
{ //Roll
//累加誤差
g_Attitude_Error.g_ErrorI_Roll += g_Attitude_Error.g_Error_Roll;
//積分限幅
if (g_Attitude_Error.g_ErrorI_Roll >= ROLL_I_MAX)
g_Attitude_Error.g_ErrorI_Roll = ROLL_I_MAX;
else if (g_Attitude_Error.g_ErrorI_Roll <= -ROLL_I_MAX)
g_Attitude_Error.g_ErrorI_Roll = -ROLL_I_MAX;
}
// --- PID運算-->這裡的微分D運算并非傳統意義上的利用前一次的誤差減去上一次的誤差得來
// --- 而是直接利用陀螺儀的值來替代微分項,這樣的處理非常好,因為巧妙利用了硬體設施,陀螺儀本身就是具有增量的效果
outputPWM_Pitch = (s16)(g_PID_Kp * g_Attitude_Error.g_Error_Pitch +
g_PID_Ki * g_Attitude_Error.g_ErrorI_Pitch - g_PID_Kd * g_MPU6050Data_Filter.gyro_x_c);
outputPWM_Roll = (s16)(g_PID_Kp * g_Attitude_Error.g_Error_Roll +
g_PID_Ki * g_Attitude_Error.g_ErrorI_Roll - g_PID_Kd * g_MPU6050Data_Filter.gyro_y_c);
outputPWM_Yaw = (s16)(g_PID_Yaw_Kp * g_Attitude_Error.g_Error_Yaw);
// --- 給出PWM控制量到四個電機-->X模式控制
//特别注意，這裡輸出反相了，因為誤差是反的
g_motor1_PWM = g_BasePWM + outputPWM_Pitch + outputPWM_Roll + outputPWM_Yaw;
g_motor2_PWM = g_BasePWM - outputPWM_Pitch + outputPWM_Roll - outputPWM_Yaw;
g_motor3_PWM = g_BasePWM - outputPWM_Pitch - outputPWM_Roll + outputPWM_Yaw;
g_motor4_PWM = g_BasePWM + outputPWM_Pitch - outputPWM_Roll - outputPWM_Yaw;
// --- PWM反向清零,因為沒有反轉
if (g_motor1_PWM < 0)
g_motor1_PWM = 0;
if (g_motor2_PWM < 0)
g_motor2_PWM = 0;
if (g_motor3_PWM < 0)
g_motor3_PWM = 0;
if (g_motor4_PWM < 0)
g_motor4_PWM = 0;
// --- PWM限幅
if (g_motor1_PWM >= g_MaxPWM)
g_motor1_PWM = g_MaxPWM;
if (g_motor2_PWM >= g_MaxPWM)
g_motor2_PWM = g_MaxPWM;
if (g_motor3_PWM >= g_MaxPWM)
g_motor3_PWM = g_MaxPWM;
if (g_motor4_PWM >= g_MaxPWM)
g_motor4_PWM = g_MaxPWM;
if (g_Fly_Enable) //允許起飛,給出PWM
PWM_Set(g_motor1_PWM, g_motor2_PWM, g_motor3_PWM, g_motor4_PWM);
else
PWM_Set(0, 0, 0, 0); //停機
}
在這段代碼中，首先得到期望值與目前值的誤差，然後經過積分分離與抗積分飽和處理後，計算PID輸出，關鍵點在于三軸PID輸出與四電機的融合處理，接着對運算結果進行反向清零和正向限幅處理。
我們知道四旋翼目前有兩種運作模式，一種成為+模式，一種成為x模式。前者表示四旋翼的運動方向與其中一對電機的軸線重合，後者則是将前一種方式旋轉45度的結果。相對而言，x模式穩定一些。但如果要完成翻跟頭等特技動作，可能需要用+模式。筆者觀看了網易公開課關于四旋翼的TED，他們的四軸運動方式全部是+模式。筆者在這裡就不細講+模式與x模式怎麼融合，這部分網上都有，其實也不難，想好符号和力矩關系，自己都可以寫出來。筆者就是這麼過來的。
而對于PID的參數整定來講，因為筆者制作的是小四軸，慣性小，很靈敏。是以P和D參數的耦合比大四軸嚴重很多，在調試的時候注意兩者的關系。先整定P，再整定D，然後反過來疊代P，再疊代D，直到找到一個最佳值。如果發現無論如何都找不到更好的效果時，考慮降低參數，因為可能在疊代的過程中已經超過了極值。
加速度計濾波：
在前面的姿态解算部分已經提到有必要對加速度計的值進行濾波。筆者為了達到濾波的最佳效果，當沒有考慮實時性時，采用了方才讨論的16深度的視窗滑動濾波再加19階的kaiser窗FIR低通濾波，效果确實理想很多，但是代價就是延遲較為嚴重；而在考慮實時性的要求之後，筆者去除了FIR低通濾波，隻用了8深度的視窗滑動濾波。雖然效果來講肯定沒有前述的要好，但是對于姿态解算的誤差來講，靜止時波動差不多在0.1~0.2°左右(有FIR濾波則穩定不動)。針對于本四旋翼的設計，0.1~0.2°的誤差顯得微不足道，是以就放棄了高階的FIR濾波。當然，這隻是在靜止狀态下的測試，如果打開電機，引入電機的高頻機械震動，那麼加速度計的值又會産生新的噪聲。筆者将四旋翼拿在手上測試他的角度變化，發現在大油門時大緻有4°左右的偏差，這個誤差還是較為嚴重的。鑒于此，筆者才做FIR濾波。但是在實際飛行過程中，當隻有8深度的視窗滑動濾波時，似乎可以平衡，沒有拿在手上測試的4°誤差。是以在這裡筆者就偷懶了，直接采用8深度的視窗滑動濾波，放棄了FIR低通濾波。具體的原因，如果有網友願意讨論可以聯系我。
以下是筆者的8深度視窗滑動濾波代碼，算法經過優化，減少了數組的移動和求和運算，利用了循環隊列的原理避免了求和運算：
/**************************************************
函數名：void IMU_Filter(void)
說明：IMU濾波，包括加速度計的滑動濾波和陀螺儀的标定
入口：無
出口：無
備注：采用視窗滑動濾波法，長度為ACC_FILTER_DELAY
用控制周期3ms*ACC_FILTER_DELAY得到滞後時間常數
屬于一階滞後的FIR濾波器，具體的之後環節
有待對加速度計采樣觀察後FFT檢視頻譜後給出
滞後的時間常數
**************************************************/
void IMU_Filter(void)
{
s32 resultx = 0;
static s32 s_resulttmpx[ACC_FILTER_DELAY] = {0};
static u8 s_bufferCounterx = 0;
static s32 s_totalx = 0;
s32 resulty = 0;
static s32 s_resulttmpy[ACC_FILTER_DELAY] = {0};
static u8 s_bufferCountery = 0;
static s32 s_totaly = 0;
s32 resultz = 0;
static s32 s_resulttmpz[ACC_FILTER_DELAY] = {0};
static u8 s_bufferCounterz = 0;
static s32 s_totalz = 0;
//加速度計濾波
s_totalx -= s_resulttmpx[s_bufferCounterx]; //從總和中删除頭部元素的值，履行頭部指針職責
s_resulttmpx[s_bufferCounterx] = g_MPU6050Data.accel_x; //将采樣值放到尾部指針處，履行尾部指針職責
s_totalx += g_MPU6050Data.accel_x; //更新總和
resultx = s_totalx / ACC_FILTER_DELAY; //計算平均值，并輸入到一個固定變量中
s_bufferCounterx++; //更新指針
if (s_bufferCounterx == ACC_FILTER_DELAY) //到達隊列邊界
s_bufferCounterx = 0;
g_MPU6050Data_Filter.accel_x_f = resultx;
s_totaly -= s_resulttmpy[s_bufferCountery];
s_resulttmpy[s_bufferCountery] = g_MPU6050Data.accel_y;
s_totaly += g_MPU6050Data.accel_y;
resulty = s_totaly / ACC_FILTER_DELAY;
s_bufferCountery++;
if (s_bufferCountery == ACC_FILTER_DELAY)
s_bufferCountery = 0;
g_MPU6050Data_Filter.accel_y_f = resulty;
s_totalz -= s_resulttmpz[s_bufferCounterz];
s_resulttmpz[s_bufferCounterz] = g_MPU6050Data.accel_z;
s_totalz += g_MPU6050Data.accel_z;
resultz = s_totalz / ACC_FILTER_DELAY;
s_bufferCounterz++;
if (s_bufferCounterz == ACC_FILTER_DELAY)
s_bufferCounterz = 0;
g_MPU6050Data_Filter.accel_z_f = resultz;
}
基于NRF24L01的Bootloader：
這一塊内容屬于獨立與四旋翼開發的部分，因為在最初設計之時，想到PID調試需要反複整定參數，就需要不斷的燒寫程式來變更參數，這樣就需要重複的插拔連線，顯得麻煩。是以筆者就在無線子產品NRF24L01的基礎之上，開發了Bootloader技術，使得下載下傳程式通過無線子產品下載下傳程式到Flash中，這樣極大的簡化了參數整定的過程。
筆者在這裡就不詳細介紹Bootloader的原理了，簡單點說就是在Flash中開辟兩個區域：A區域和B區域。其中A區域稱之為Bootloader，用以實作Flash的燒寫工作，相當于代替了J-LINK；B區域就是我們運作代碼的區域，也就是Bootloader将要操作的Flash區域，我們的代碼就在這裡運作。單片機在開機後首先運作A區域的Bootloader代碼，這段代碼等待NRF24L01接收二進制程式代碼，在接收的同時，就一邊将接收到的二進制程式代碼燒寫進B區域中。等全部接收完畢，同時也燒寫完畢。之後通過在彙編修改棧頂指針并跳轉到程式的APP代碼起始位置即可。
以下為Bootloader中的APP函數跳轉關鍵代碼：
/***************************************
函數名：void IAP_Load_App(u32 appxaddr)
說明：跳到APP程式
入口：u32 appxaddr: 應用程式的起始位址
出口：無
備注：無
****************************************/
void IAP_Load_App(u32 appxaddr)
{
if(((*(vu32*)appxaddr) & 0x2FFE0000) == 0x20000000) //檢查棧頂位址是否合法
{
Jump_To_App = (IAP_FunEntrance)*(vu32*)(appxaddr + 4); //使用者代碼區第二個字為程式開始位址(複位位址)-->詳見startup.s Line61
//(vu32*)(appxaddr + 4) --> 将FLASH的首位址強制轉換為vu32的指針
//*(vu32*)(appxaddr + 4) --> 解引用該位址上存放的APP跳轉位址,即main函數入口
//(IAP_FunEntrance)*(vu32*)(appxaddr + 4) --> 将main函數入口位址強制轉換為指向函數的指針給Jump_To_App
MSR_MSP(*(vu32*)appxaddr); //初始化APP堆棧指針(使用者代碼區的第一個字用于存放棧頂位址)
Jump_To_App(); //跳轉到APP,執行APP
}
}
尤其注意Jump_To_App和Jump_To_App()的用法，前提是Jump_To_App本身就是一個指向函數的指針。定義：
typedef void (*IAP_FunEntrance)(void); //定義一個指向函數的指針
IAP_FunEntrance Jump_To_App;
至此，整個設計到此結束，如果有不明白的地方，歡迎留言讨論，或者直接與本作者聯系QQ249442907，歡迎各大網友共同讨論學習。