領(lǐng)航者飛控源碼下載（注釋非常詳細(xì)）

ID:150692 · 發(fā)表于 2017-7-28 14:45

當(dāng)大家看到這里就會有疑問，積分分離PID的這一段程序怎么來的，其實我也推導(dǎo)不出這段代碼是怎么推導(dǎo)出來的，希望有大俠們給個指點，回復(fù)一下。

/* 角速度誤差積分 */
      ctrl_1.err_i.x += ctrl_1.PID[PIDROLL].ki  *(ctrl_1.err.x - ctrl_1.damp.x) *T;
      ctrl_1.err_i.y += ctrl_1.PID[PIDPITCH].ki *(ctrl_1.err.y - ctrl_1.damp.y) *T;
      ctrl_1.err_i.z += ctrl_1.PID[PIDYAW].ki       *(ctrl_1.err.z - ctrl_1.damp.z) *T;

事件記錄，beta0.4開始。
2015.5.18
beta 0.4  修復(fù)各種bug。

2015.5.19
beta 0.5  完善搖桿識別。
beta 0.6  pwm_in優(yōu)化
beta 0.7  修復(fù)bug，優(yōu)化rc，添加mapping
beta 0.9  起飛成功
beta 1.0  修復(fù)控制周期bug
beta 1.1  添加部分注釋
beta 1.2  優(yōu)化整定參數(shù)，修改bug
beta 1.3  完成flash存儲，上位機(jī)HID通信
beta 1.35 增加磁力計糾正航向
beta 1.36 增加磁力計校準(zhǔn)時燈管提示
beta 1.37 增加mpu6050，ak8975，ms5611自檢，未通過的LED狀態(tài)分別是：間隔連續(xù)閃爍1次；間隔連續(xù)閃爍2次；間隔連續(xù)閃爍3次
beta 1.37.1 增加滑動窗口平均濾波函數(shù)，增加ks103超聲波讀取，該版?zhèn)浞荨?br /> beta 1.37.2 添加user數(shù)據(jù)，上位機(jī)顯示波形,串口超聲波改發(fā)送中斷。
beta 1.37.3 添加部分注釋，修改部分變量名，增加ano_private.lib。
beta 1.38 添加超聲波控制代碼（注意！暫未測試），修改磁力計糾正算法（重要更新），消除磁力計糾正YAW時對姿態(tài)的干擾。刪除ano_pribate.lib
beta 1.39 修復(fù)上一版數(shù)據(jù)傳輸一個小bug，添加環(huán)形緩存。
beta 1.39.1 超聲波直接控制高度，還有問題。
beta 1.40 超聲波定高ok，暫沒優(yōu)化參數(shù)，沒加上位機(jī)調(diào)參。可改慣導(dǎo)豎直方向速率控制。
beta 1.40.1

#define CTRL_HEIGHT 1    //0失能，1使能
#define HEIGHT_MODE 1    //0,無，1慣性控制高度，2氣壓計控制(空)，3超聲波控制高度
beta 1.41 修復(fù)磁力計糾正的一個bug。
beta 1.42 us100超聲波定高穩(wěn)定
beta 1.43
1.模式0（呼吸白，解鎖綠長），普通，油門直接輸出。模式1（呼吸淺綠，解鎖淺綠長），垂直速率控制（下一步改為氣壓定高）。模式2（呼吸紫，解鎖紫長），超聲波定高。
2.飛行時，只能向模式0切換，其他切換無效。
3.只有插上超聲波，模式3才有效。（這個未優(yōu)化，比如臨時拔下超聲波，也能進(jìn)入該模式。）
beta 1.44
修復(fù)定高控制中積分的bug，優(yōu)化超聲波定高，優(yōu)化高度設(shè)定值調(diào)節(jié)速度。增加超聲波最大高度，可達(dá)到到1.5米比較穩(wěn)定（高度越低越穩(wěn)定）。
beta 1.46
氣壓計定高，測試版（無高度靜差修正）。
beta 1.47
氣壓計定高，測試版（無高度靜差修正）。改進(jìn)磁力計糾正算法，解決磁力計糾正時對姿態(tài)的干擾，修正磁力計糾正中180度的一個錯誤。
beta 1.48
修復(fù)氣壓計速率的一個bug，并調(diào)整可控制的速率。
beta 1.48.1
注釋掉while(1);
beta 1.49 電賽版本，完善通信協(xié)議中串口接收通道數(shù)據(jù)部分。
beta 1.49.2 修改氣壓計定高速率環(huán)積分部分，優(yōu)化定高效果。
beta 1.49.3 濾波算法測試。
beta 1.50 修改磁力計糾正方法
beta 1.50.2 增加羅盤傾斜糾正，修復(fù)數(shù)據(jù)傳輸一個bug。
beta 1.51 修改氣壓計融合，定高控制等幾處小bug
beta 1.52 修改超聲波定高一個小bug
beta 1.6 細(xì)節(jié)優(yōu)化&新參數(shù)
beta 1.6.2 修正羅盤原始數(shù)據(jù)的一個錯誤。
beta 1.6.2_fix 上版漏掉一句代碼，導(dǎo)致解鎖后yaw混亂，I'm sorry。。。
beta 1.6.3 新融合策略，解決加速度零點漂移（水平姿態(tài)）。
beta 1.6.4 修復(fù)連接usb后，復(fù)位單片機(jī)偶爾卡死的問題；修復(fù)上版氣壓速度融合的小錯誤；修復(fù)LED的GPID初始化結(jié)構(gòu)體未給初值造成可能出錯的問題。
beta 1.6.5 小更新，增加加速度計判斷，解決6050不同批次量程問題。

APB1 42m
APB2 84m

TIMx clk = APBx *2

中斷優(yōu)先級：

第0組：所有4位用于指定響應(yīng)優(yōu)先級；
第1組：最高1位用于指定搶占式優(yōu)先級，最低3位用于指定響應(yīng)優(yōu)先級；
第2組：最高2位用于指定搶占式優(yōu)先級，最低2位用于指定響應(yīng)優(yōu)先級；
第3組：最高3位用于指定搶占式優(yōu)先級，最低1位用于指定響應(yīng)優(yōu)先級；
第4組：所有4位用于指定搶占式優(yōu)先級。
|先搶占，再響應(yīng)|
1）如果指定的搶占式優(yōu)先級別或響應(yīng)優(yōu)先級別超出了選定的優(yōu)先級分組所限定的范圍，將可能得到意想不到的結(jié)果；
2）搶占式優(yōu)先級別相同的中斷源之間沒有嵌套關(guān)系；
3）如果某個中斷源被指定為某個搶占式優(yōu)先級別，又沒有其它中斷源處于同一個搶占式優(yōu)先級別，則可以為這個中斷源指定任意有效的響應(yīng)優(yōu)先級別。

wz_acc_mms2 = (wz_acc/4096.0f) *10000 + hc_acc_i;

hc_acc_i += 0.4f *T *( (wz_speed - wz_speed_old)/T - wz_acc_mms2 );

wz_acc_mms2 = (wz_acc/4096.0f) *10000 + (hc_acc_lpf - wz_acc_mms2);

hc_acc_lpf += 0.4f *T *( (wz_speed - wz_speed_old)/T - hc_acc_lpf );

把hc_acc_i代成(hc_acc_lpf - wz_acc_mms2)，然后后邊等效，就有了。
注意wz_speed里邊包含hc_acc_i的積分，兩次wz_speed做差后，又得到hc_acc_i單次的值。

全部資料下載地址：

領(lǐng)航者飛控源碼.rar (2.06 MB, 下載次數(shù): 182)