領航者飛控源碼下載（注釋非常詳細）

ID:150692 · 發表于 2017-7-28 14:45

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

/* 角速度誤差積分 */
      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  修復各種bug。

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

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

APB1 42m
APB2 84m

TIMx clk = APBx *2

中斷優先級：

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

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單次的值。

全部資料下載地址：

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