從開始做四軸到現(xiàn)在，已經(jīng)累計使用了三個月的時間，從開始的嘗試用四元數(shù)法進行姿態(tài)檢測，到接著使用的卡爾曼濾波算法，我們走過了很多彎路，我在從上周開始了對德國人四軸代碼的研究和移植，發(fā)現(xiàn)德國人的代碼的確有他的獨到之處，改變了很多我對模型的想法，因為本人是第一次嘗試著制作模型，因此感覺很多想法還是比較簡單。經(jīng)過了一周的時間，我將德國人的代碼翻譯并移植到了我目前的四軸上，并進行了調(diào)試，今天，專門請到了一個飛直升機的教練，對我們的四軸進行試飛，并與一個華科爾的四軸進行了現(xiàn)場比較，現(xiàn)在我們四軸的穩(wěn)定性已經(jīng)達到了商品四軸的程度。下面是我這一周時間內(nèi)對德國人代碼的一些理解：
德國人代碼中的姿態(tài)檢測算法：
首先，將陀螺儀和加速度及的測量值減常值誤差，得到角速度和加速度，并對角速度進行積分，然后對陀螺儀積分和加速度計的數(shù)值進行融合。融合分為兩部分，實時融合和長期融合,實時融合每一次算法周期都要執(zhí)行，而長期融合沒256個檢測周期執(zhí)行一次，(注意檢測周期小于控制周期的2ms)
實時融合：
1.將陀螺儀積分和加表濾波后的值做差;
2.按照情況對差值進行衰減，并作限幅處理;
3.將衰減值加入到角度中。
長期融合：
長期融合主要包括兩個部分，一是對角速度的漂移進行估計(估計值是要在每一個控制周期都耦合到角度中的)，二是對陀螺儀的常值誤差(也就是陀螺儀中立點)進行實時的修正。
1.將陀螺儀積分的積分和加速度積分做差(PS：為什么這里要使用加表積分和陀螺儀積分的積分，因為在256個檢測周期內(nèi)，有一些加速度計的值含有有害的加速度分量，如果只使用一個時刻的加表值對陀螺儀漂移進行估計，顯然估計值不會準確，使用多個周期的值進行疊加后做座平均處理，可以減小隨機的有害加速度對估計陀螺儀漂移過程中所鎖產(chǎn)生的影響)
2.將上面兩個值進行衰減，得到估計的陀螺儀漂移
3.對使考慮了陀螺儀漂移和不考慮陀螺儀漂移得到的角度做差，如果這兩個值較大，說明陀螺儀在前段時間內(nèi)測到的角速率不夠準確，需要對差值誤差(也就是陀螺儀中立點)進行修正，修正幅度和差值有關
長期融合十分關鍵，如果不能對陀螺儀漂移做修正，則系統(tǒng)運行一段時間后，速率環(huán)的穩(wěn)定性會降低。
下面比較一下德國四軸中姿態(tài)檢測部分和卡爾曼濾波之間的關系：
卡爾曼濾波是一種線性系統(tǒng)的最優(yōu)估計濾波方法。對于本系統(tǒng)而言，使用卡爾曼濾波的作用是通過對系統(tǒng)狀態(tài)量的估計，和通過加速度計測量值對系統(tǒng)狀態(tài)進行驗證，從而得到該系統(tǒng)的最優(yōu)狀態(tài)量，并實時更新系統(tǒng)的各參數(shù)(矩陣)，而最重要的一點，改濾波器能夠?qū)ν勇輧x的常值漂移進行估計，從而保證速率環(huán)的正常運行，并在加速度計敏感到各種有害加速度的時候，使姿態(tài)檢測更加準確。但是卡爾曼濾波器能否工作在最優(yōu)狀態(tài)很大程度上取決于系統(tǒng)模型的準確性，模型參數(shù)的標定和系統(tǒng)參數(shù)的選取。然而，僅僅通過上位機觀測而得到最優(yōu)工作參數(shù)是不顯示的，因為參數(shù)的修改會導致整個系統(tǒng)中很多地方發(fā)生改變，很難保證幾個值都恰好為最優(yōu)解，這需要扎實的理論知識，大量的測量數(shù)據(jù)和系統(tǒng)的仿真，通過我對卡爾曼濾波器的使用，發(fā)現(xiàn)要想兼顧鎖有的問題，還是有一定難度的。
而德國人的姿態(tài)檢測部分是在嘗試使用一種簡單方法去解決復雜問題，他既沒有使用傳統(tǒng)的四元數(shù)法進行姿態(tài)檢測，也么有使用卡爾曼濾波。他的計算量不比最簡單的卡爾曼濾程序波程序的計算量小，但與卡爾曼濾波相比，更加直觀，易于理解，參數(shù)調(diào)節(jié)也更加方便。我個人理解，這個方法是在嘗試著對卡爾曼濾波這一復雜相互耦合的多狀態(tài)變量的線性系統(tǒng)狀態(tài)估計過程進行了簡單的解耦，從而將姿態(tài)的最優(yōu)估計和陀螺儀漂移的最優(yōu)估計分隔開，這樣，就可以通過比較直觀的觀測手段對兩個部分的參數(shù)進行調(diào)整，但是，這種方法的理論性肯定不如使用四元數(shù)法和卡爾曼濾波，在一些特殊的情況下還可能出現(xiàn)問題，但是，由于卡爾曼濾波器設計的難度，使用這種方法還是比較現(xiàn)實的。
控制算法：
德國人的控制算法的核心是對角速度做PI計算，P的作用是使四軸能夠產(chǎn)生對于外界干擾的抵抗力矩，I的作用是讓四軸產(chǎn)生一個與角度成正比的抵抗力。
如果只有P的作用，將四軸拿在手上就會發(fā)現(xiàn)，四軸能夠抵抗外界的干擾力矩的作用，而且這個抵抗力非�？焖�，只要手妄圖改變四軸的轉(zhuǎn)速，四軸就會產(chǎn)生一個抵抗力矩，但是，如果用手將四軸扳過一個角度，則四軸無法自己回到水平的角度位置，這就需要I的調(diào)節(jié)作用。
對角速度做I(積分)預算實際得到的就是角度，德國人四軸里面用的也是角度值，如果四軸有一個傾斜角度，那么四軸就會自己進行調(diào)整，直到四軸的傾角為零，它所產(chǎn)生的抵抗力是與角度成正比的，但是，如果只有I的作用，會使四軸迅速產(chǎn)生振蕩，因此，必須將P和I結合起來一起使用，這時候基本上就會得到德國四軸的效果了。
在對角速度進行了PI調(diào)節(jié)之后，德國人將操縱桿的值融合到結果中去，并對得到的新的值有進行了一次PI計算，這個積分參數(shù)很小，使用這個積分的作用因為，四軸在有一個非常小的傾角的情況下產(chǎn)生的抵抗力矩很小，無法使四軸回到水平位置，這就會導致無論怎么手動調(diào)節(jié)微調(diào)，四軸都很難做到懸停，會不停得做水平漂移運動，這就必須不停的進行調(diào)整。
下面是我給德國四軸中飛控程序的一些注釋：
void Piep(unsigned char Anzahl)
{
while(Anzahl--)
{
if(MotorenEin) return; //auf keinen Fall im Flug!
beeptime = 100;
Delay_ms(250);
}
}
//函數(shù):SetNeutral設定傳感器發(fā)出參數(shù)的中立數(shù)值，因為有漂移所以要使其每次工作都要測量出來。
//############################################################################
// Nullwerte ermitteln
/*設置中立點*/
void SetNeutral(void)
//############################################################################
{
/*加速度計中立點*/
NeutralAccX = 0;
NeutralAccY = 0;
NeutralAccZ = 0;
/*陀螺儀中立點*/
AdNeutralNick = 0;
AdNeutralRoll = 0;
AdNeutralGier = 0;
Parameter_AchsKopplung1 = 0;
Parameter_AchsGegenKopplung1 = 0;。。。
/*這個地方我還沒有弄得太明白，檢測中立點的函數(shù)被調(diào)用了兩次，但是第一次的數(shù)據(jù)好像沒有保存，只用到了
第二次的數(shù)據(jù)*/
/*記錄中立點*/
CalibrierMittelwert();
Delay_ms_Mess(100);
/*記錄中立點*/
CalibrierMittelwert();
/*既然只使用了后一次的數(shù)據(jù)，為什么要進行兩次記錄中立點的函數(shù)*/
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();//如果氣壓表輸出在
950外750內(nèi)，則設定氣壓初始的偏差。
}
/*將量測值作為陀螺儀的中立點*/
AdNeutralNick= AdWertNick;
AdNeutralRoll= AdWertRoll;
AdNeutralGier= AdWertGier;
/*這兩個參數(shù)在飛控程序中沒有用到*/
StartNeutralRoll = AdNeutralRoll;
StartNeutralNick = AdNeutralNick;
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) //
{
/*由于在函數(shù)CalibrierMittelwert()中加速度計的輸出乘以了ACC_AMPLIFY，所以這里必須處以ACC_AMPLIFY，
在這段程序中，所有的對加速度計和陀螺儀的數(shù)值的衰減或者放大都是為了讓
陀螺儀積分和加速度計數(shù)值在同樣的角度偏差的情況下能基本匹配，如果不匹配，那么就談不上用加速度計來補
償陀螺儀積分了*/
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
NeutralAccZ = Aktuell_az;
}
else
{
/*如果發(fā)現(xiàn)變化不大，則仍然儲存上一次的*/
NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)
eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)
eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)
eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
}
/*將所有數(shù)據(jù)清零，這里帶2的變量都是加入了陀螺儀漂移補償值之后得到的角度*/
Mess_IntegralNick = 0;
Mess_IntegralNick2 = 0;
Mess_IntegralRoll = 0;
Mess_IntegralRoll2 = 0;
Mess_Integral_Gier = 0;
MesswertNick = 0;
MesswertRoll = 0;
MesswertGier = 0;
StartLuftdruck = Luftdruck;
HoeheD = 0;
Mess_Integral_Hoch = 0;
KompassStartwert = KompassValue;
GPS_Neutral();
beeptime = 50; //
/*從EEPROM中讀取陀螺儀積分到達90°時候的值，并儲存，當?shù)玫降淖藨B(tài)角度大于這個范圍時，說明超過了90°
，就要進行相應的處理*/
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
ExternHoehenValue = 0;
}
///////////////////////////////
//函數(shù)描述 ：求參數(shù)的平均數(shù)值
//////////////////////////////
//############################################################################
// Bearbeitet die Messwerte
void Mittelwert(void)
// 根據(jù)測量值 計算陀螺儀和加速度計數(shù)據(jù)
//############################################################################
{
static signed long tmpl,tmpl2;
/*將陀螺儀數(shù)據(jù)減去常值誤差，得到實際的叫速率的倍數(shù)*/
MesswertGier = (signed int) AdNeutralGier - AdWertGier;
MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
MesswertNick = (signed int) AdWertNick - AdNeutralNick;
//DebugOut.Analog[26] = MesswertNick;
DebugOut.Analog[28] = MesswertRoll;
//加速度傳感器輸出
/*加速度計數(shù)據(jù)濾波，ACC_AMPLIFY=12 得到的Mittelwert_AccNick是加速度計數(shù)值的12倍*/
/*AdWertAccNick為測量值*/
// Beschleuni×gssensor ++++++++++++++++++++++++++++++++++++++++++++++++
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) /
2L;//具有濾波功能的方法，用當前加速度和上次的加速度平均
Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
/*計算加速度計的積分，加速度計對運動十分敏感，采用加速度計積分，可以減少瞬間的運動加速度的影響*/
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
IntegralAccZ += Aktuell_az - NeutralAccZ;
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
/*偏航方向無法進行濾波，因此直接進行積分得到偏航角度*/
Mess_Integral_Gier += MesswertGier;
Mess_Integral_Gier2 += MesswertGier;
/*耦合項應該是另外兩個陀螺儀對這個軸上陀螺儀的影響，當四軸在穩(wěn)定姿態(tài)不為水平的時候，進行偏航運動時
候所進行的補償*/
/*假設目前的俯仰角是30°，而橫滾角是0°，這時候如保持俯仰和橫滾軸沒有任何運動，而將偏航軸轉(zhuǎn)動90°
，那么實際的俯仰角就會變?yōu)?°，橫滾角就會變?yōu)?0°
但是，按照目前的算法，由于俯仰和橫滾方向沒有運動，因此就不會有陀螺儀的積分，俯仰和橫滾角是不變的
，這就是采用陀螺儀直接積分測角度的不完善性，這時候
使用加速度計對姿態(tài)進行修正能夠起到作用，但是需要一段時間，使用下面的這段話，就是將偏航軸的運動耦
合在另外兩個軸上，使姿態(tài)角度能夠迅速收斂到真實值上*/
/*注：使用四元數(shù)法進行姿態(tài)結算可以避免出現(xiàn)這種問題，但這種方法需要有準確的陀螺儀和加表的數(shù)學模型，四元數(shù)法還需要進行大量的矩陣計算，
而且對四元數(shù)姿態(tài)進行加速度計的融合不太方便*/
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig &
CFG_ACHSENKOPPLUNG_AKTIV))//不在俯仰滾轉(zhuǎn)控制循環(huán)中
{
tmpl = Mess_IntegralNick / 4096L;
tmpl *= MesswertGier;
tmpl *= Parameter_AchsKopplung1; //125
tmpl /= 2048L;
tmpl2 = Mess_IntegralRoll / 4096L;
tmpl2 *= MesswertGier;
tmpl2 *= Parameter_AchsKopplung1;
tmpl2 /= 2048L;
}
else tmpl = tmpl2 = 0;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
MesswertRoll += tmpl;
MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L;
Mess_IntegralRoll2 += MesswertRoll;
Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll;
/*積分超過半圈的情況*/
if(Mess_IntegralRoll > Umschlag180Roll)
{
Mess_IntegralRoll = -(Umschlag180Roll - 10000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if(Mess_IntegralRoll <-Umschlag180Roll)//一樣
{
Mess_IntegralRoll = (Umschlag180Roll - 10000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if(AdWertRoll < 15) MesswertRoll = -1000;
if(AdWertRoll < 7) MesswertRoll = -2000;
if(PlatinenVersion == 10)
{
if(AdWertRoll > 1010) MesswertRoll = +1000;
if(AdWertRoll > 1017) MesswertRoll = +2000;
}
else
{
if(AdWertRoll > 2020) MesswertRoll = +1000;
if(AdWertRoll > 2034) MesswertRoll = +2000;
}
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
MesswertNick -= tmpl2;
MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
Mess_IntegralNick2 += MesswertNick;
/*LageKorrekturNick是通過加速度計積分和角速率積分的積分進行做差比較得到
的,*/
Mess_IntegralNick += MesswertNick - LageKorrekturNick;
if(Mess_IntegralNick > Umschlag180Nick)
{
Mess_IntegralNick = -(Umschlag180Nick - 10000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if(Mess_IntegralNick <-Umschlag180Nick)
{
Mess_IntegralNick = (Umschlag180Nick - 10000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if(AdWertNick < 15) MesswertNick = -1000;
if(AdWertNick < 7) MesswertNick = -2000;
if(PlatinenVersion == 10)
{
if(AdWertNick > 1010) MesswertNick = +1000;
if(AdWertNick > 1017) MesswertNick = +2000;
}
else
{
if(AdWertNick > 2020) MesswertNick = +1000;
if(AdWertNick > 2034) MesswertNick = +2000;
}
//++++++++++++++++++++++++++++++++++++++++++++++++
// ADC einschalten
ANALOG_ON; //重新開始模擬量的采集
//++++++++++++++++++++++++++++++++++++++++++++++++
/*上一步計算完了積分之后，現(xiàn)在將積分賦值，因此后面使用的就將是IntegralNick，IntegralNick2等數(shù)據(jù)了
*/
Integral_Gier = Mess_Integral_Gier;
IntegralNick = Mess_IntegralNick;
IntegralRoll = Mess_IntegralRoll;
IntegralNick2 = Mess_IntegralNick2;
IntegralRoll2 = Mess_IntegralRoll2;
/*這部分代碼不執(zhí)行，因為在EEPROM中CFG_DREHRATEN_BEGRENZER這一位為0*/
if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
{
if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);
else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);
else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
}
if(Poti1 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110) Poti1++;
else if(Poti1 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in
[EE_Parameter.Kanalbele×g[K_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in
[EE_Parameter.Kanalbele×g[K_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in
[EE_Parameter.Kanalbele×g[K_POTI4]] + 110 && Poti4) Poti4--;
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
}
//函數(shù)：校正平均值
//############################################################################
// Messwerte beim Ermitteln der Nullage
/*確定零位*/
/*記錄中立點*/
void CalibrierMittelwert(void)
//############################################################################
{
// ADC auschalten, damit die Werte sich nicht w鋒rend der Berechnung 鋘dern
ANALOG_OFF;
MesswertNick = AdWertNick;
MesswertRoll = AdWertRoll;
MesswertGier = AdWertGier;
/*要乘以 ACC_AMPLIFY 是為了進行數(shù)值的匹配*/
Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
Mittelwert_AccHoch = (long)AdWertAccHoch;
// ADC einschalten
ANALOG_ON; //開模擬量
if(Poti1 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110) Poti1++;
else if(Poti1 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI2]] + 110) Poti2++;
else if(Poti2 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI3]] + 110) Poti3++;
else if(Poti3 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI4]] + 110) Poti4++;
else if(Poti4 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI4]] + 110 && Poti4) Poti4--;
if(Poti1 < 0) Poti1 = 0;
else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0;
else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0;
else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0;
else if(Poti4 > 255) Poti4 = 255;
/*這兩個數(shù)據(jù)是在對陀螺儀積分區(qū)域進行的限制，如果超過這個范圍，說明就超出了+-90°的范圍，則需要相應
的改變*/
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
}
//發(fā)送電機數(shù)據(jù)
//############################################################################
// Senden der Motorwerte per I2C-Bus
void SendMotorData(void)
//############################################################################
{
if(MOTOR_OFF || !MotorenEin)//關機或未工作
{
Motor_Hinten = 0;
Motor_Vorne = 0;
Motor_Rechts = 0;
Motor_Links = 0;//都置零
if(MotorTest[0]) Motor_Vorne = MotorTest[0];
if(MotorTest[1]) Motor_Hinten = MotorTest[1];
if(MotorTest[2]) Motor_Links = MotorTest[2];
if(MotorTest[3]) Motor_Rechts = MotorTest[3];//如果是試驗就干。
}
DebugOut.Analog[12] = Motor_Vorne;
DebugOut.Analog[13] = Motor_Hinten;
DebugOut.Analog[14] = Motor_Links;
DebugOut.Analog[15] = Motor_Rechts;
//Start I2C Interrupt Mode
twi_state = 0;
motor = 0;
i2c_start();
}
//函數(shù)：參數(shù)分配
//############################################################################
// Tr鋑t ggf. das Poti als Parameter ein
void ParameterZuordnung(void)
//############################################################################
{
//
/*這個宏定義的作用是：將a中的值賦給b，并將b限制在max和min之間*/
#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b =
Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b =
min; else if(b >= max) b = max;}
CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255);
CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
Ki = (float) Parameter_I_Faktor * 0.0001;
MAX_GAS = EE_Parameter.Gas_Max;
MIN_GAS = EE_Parameter.Gas_Min;
}
/*飛控核心*/
//############################################################################
//
void MotorRegler(void)
//############################################################################
{
int motorwert,pd_ergebnis,h,tmp_int;//電機數(shù)值，PI算法的計算數(shù)值
int GierMischanteil,GasMischanteil;//偏航混合數(shù)值，油門混和數(shù)值
static long SummeNick=0,SummeRoll=0;//俯仰積分總和，滾轉(zhuǎn)積分總和
static long sollGier = 0,tmp_long,tmp_long2;//標準偏航值，
static long IntegralFehlerNick = 0;//俯仰誤差積分
static long IntegralFehlerRoll = 0;//滾轉(zhuǎn)誤差積分
static unsigned int RcLostTimer;
static unsigned char delay_neutral = 0;
static unsigned char delay_einschalten = 0,delay_ausschalten = 0;//延遲接通，延遲關閉
static unsigned int modell_fliegt = 0;//飛機飛行時間
static int hoehenregler = 0;//高度調(diào)節(jié)
static char TimerWerteausgabe = 0;//時間數(shù)值
static char NeueKompassRichtungMerken = 0;//羅盤方向調(diào)整中立值
static long ausgleichNick, ausgleichRoll;//俯仰均衡，滾轉(zhuǎn)均衡
/*根據(jù)測量值 計算陀螺儀和加速度計數(shù)據(jù)*/
Mittelwert();
GRN_ON;//打開端口
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln//判斷油門數(shù)值
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = StickGas;
if(GasMischanteil < 0) GasMischanteil = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang schlecht//無線電故障，不好
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay < 100)
{
if(!PcZugriff)
{
if(BeepMuster == 0xffff)
{
beeptime = 15000;
BeepMuster = 0x0c00;
}
}
if(RcLostTimer) RcLostTimer--;
else
{
MotorenEin = 0;
Notlandung = 0;
}
ROT_ON;
if(modell_fliegt > 2000)
{
GasMischanteil = EE_Parameter.NotGas;
Notlandung = 1;
PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] = 0;
PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] = 0;
PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] = 0;
}
else
MotorenEin = 0;
} // end of if(SenderOkay < 100)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang gut//
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
else if(SenderOkay > 140)
{
Notlandung = 0;
RcLostTimer = EE_Parameter.NotGasZeit * 50;
if(GasMischanteil > 40)
{
if(modell_fliegt < 0xffff)
modell_fliegt++;
}
if((modell_fliegt < 200) || (GasMischanteil < 40))
{
SummeNick = 0;
SummeRoll = 0;
Mess_Integral_Gier = 0;
Mess_Integral_Gier2 = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// auf Nullwerte kalibrieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((PPM_in[EE_Parameter.Kanalbele×g[K_GAS]] > 80) && MotorenEin == 0)
{
if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] > 75)
{
if(++delay_neutral > 200)
{
GRN_OFF;
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
if(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > 70 || abs(PPM_in
[EE_Parameter.Kanalbele×g[K_ROLL]]) > 70)
{
unsigned char setting=1;
if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > 70 && PPM_in
[EE_Parameter.Kanalbele×g[K_NICK]] < 70) setting = 1;
if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > 70 && PPM_in
[EE_Parameter.Kanalbele×g[K_NICK]] > 70) setting = 2;
if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] < 70 && PPM_in
[EE_Parameter.Kanalbele×g[K_NICK]] > 70) setting = 3;
if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] <-70 && PPM_in
[EE_Parameter.Kanalbele×g[K_NICK]] > 70) setting = 4;
if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] <-70 && PPM_in
[EE_Parameter.Kanalbele×g[K_NICK]] < 70) setting = 5;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);
}
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H鰄enregelung
aktiviert?
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750))
SucheLuftruckOffset();
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *)
&EE_Parameter.Kanalbele×g[0], STRUCT_PARAM_LAENGE);
SetNeutral();
Piep(GetActiveParamSetNumber());
}
}
else if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] < -75)
{
if(++delay_neutral > 200) // nicht sofort
{
GRN_OFF;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte l鰏chen
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
SetNeutral();//設立中性點。
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); //
ACC-NeutralWerte speichern
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); //
ACC-NeutralWerte speichern
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
Piep(GetActiveParamSetNumber());
}
}
else
delay_neutral = 0;
} // end if of if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] > 75)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbele×g[K_GAS]] < 35-120)
{
// Starten
if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] < -75)
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(++delay_einschalten > 200)
{
delay_einschalten = 200;
modell_fliegt = 1;
MotorenEin = 1;
sollGier = 0;
Mess_Integral_Gier = 0;
Mess_Integral_Gier2 = 0;
Mess_IntegralNick = 0;
Mess_IntegralRoll = 0;
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralRoll2 = IntegralRoll;
SummeNick = 0;
SummeRoll = 0;
}
}
else
delay_einschalten = 0;//沒事，就讓其延遲關閉為0
//Auf Neutralwerte setzen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Auschalten
/*切換*/
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] > 75)
{
if(++delay_ausschalten > 200) // nicht sofort
{
MotorenEin = 0;
delay_ausschalten = 200;
modell_fliegt = 0;
}
}
else delay_ausschalten = 0;
}
} // end if of else if(SenderOkay > 140)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!NewPpmData-- || Notlandung)
{
int tmp_int;
static int stick_nick,stick_roll;//俯仰桿，傾斜桿
ParameterZuordnung();
/*新老數(shù)據(jù)濾波混合，這里改變的應該是期望角位置，必須知道EE_Parameter.Stick_P的數(shù)值才可以得到濾波效
果*/
StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] *
EE_Parameter.Stick_P) / 4; //新數(shù)據(jù)和老數(shù)據(jù)混合起濾波作用
/*將期望角位置的微分加入操縱桿變量上，這里必須知道EE_Parameter.Kanalbele×g[K_ROLL]的求法，和
EE_Parameter.Stick_D得數(shù)值*/
StickNick += PPM_diff[EE_Parameter.Kanalbele×g[K_NICK]] * EE_Parameter.Stick_D;//增加上微分
量，用于提高反應的快速性。
StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] *
EE_Parameter.Stick_P) / 4;
StickRoll += PPM_diff[EE_Parameter.Kanalbele×g[K_ROLL]] * EE_Parameter.Stick_D;
StickGier = -PPM_in[EE_Parameter.Kanalbele×g[K_GIER]];
StickGas = PPM_in[EE_Parameter.Kanalbele×g[K_GAS]] + 120;
/*用此記錄歷史上的最大給桿量，如果給桿量很小，則Max數(shù)值會不斷減小，用于在后面給陀螺儀積分做補償時
，對加速度計數(shù)據(jù)和陀螺儀積分的差值做衰減*/
if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]]) > MaxStickNick)
MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]]); else MaxStickNick--;
if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]]) > MaxStickRoll)
MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]]); else MaxStickRoll--;
/*如果在降落過程中，則數(shù)據(jù)為0，也就是說降落的過程中不需要衰減，降落時候的保持位置全部為0，所以不需
要衰減*/
if(Notlandung)
{
MaxStickNick = 0; MaxStickRoll = 0;
}
/*可以認為是控制參數(shù)，前一個是陀螺儀的比例項(速率環(huán)參數(shù)) 后一個是陀螺儀積分即姿態(tài)角的比例(位置
環(huán)參數(shù))*/
GyroFaktor = ((float) Parameter_Gyro_P + 10.0) / 256.0;
IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Digitale Steuerung per DubWise
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8)//為了增加桿的輸入的豐富性，提供了擴展
的桿的描述，對最終桿的描述更加豐富。
if(DubWiseKeys[1])
beeptime = 10;
if(DubWiseKeys[1] & DUB_KEY_UP)
tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_DOWN)
tmp_int = -KEY_VALUE;
else
tmp_int = 0;
ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
if(DubWiseKeys[1] & DUB_KEY_LEFT)
tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_RIGHT)
tmp_int = -KEY_VALUE;
else
tmp_int = 0;
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
if(DubWiseKeys[0] & 8)
ExternStickGier = 50;
else if(DubWiseKeys[0] & 4)
ExternStickGier =-50;
else
ExternStickGier = 0;
if(DubWiseKeys[0] & 2)
ExternHoehenValue++;
if(DubWiseKeys[0] & 16)
ExternHoehenValue--;
StickNick += ExternStickNick / 8;
StickRoll += ExternStickRoll / 8;
StickGier += ExternStickGier;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)//同上，具有擴展功能的控制輸入
{
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
StickGier += ExternControl.Gier;
ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
}
/*陀螺儀積分比例為零，應該是Looping的情況?*/
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)
IntegralFaktor = 0;
if(GyroFaktor < 0) GyroFaktor = 0;
if(IntegralFaktor < 0) IntegralFaktor = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?//這里是在空中轉(zhuǎn)圈的情況
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1;
else
{
{
if((PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] < (EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese))) Looping_Links = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] < -EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
else
{
if(Looping_Rechts) // Hysterese
{
if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > -(EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
else
{
if(Looping_Oben) // Hysterese
{
if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] < (EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese))) Looping_Oben = 0;
}
}
if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] < -EE_Parameter.LoopThreshold) &&
EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
else
{
if(Looping_Unten) // Hysterese
{
if(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > -(EE_Parameter.LoopThreshold -
EE_Parameter.LoopHysterese)) Looping_Unten = 0;
}
}
/*不應該出現(xiàn)軸都是Looping的情況*/
if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0;
if(Looping_Oben || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0;
Looping_Rechts = 0;} else Looping_Nick = 0;
} // end if of if(!NewPpmData-- || Notlandung)
if(Looping_Roll) beeptime = 100;
if(Looping_Roll || Looping_Nick)
{
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Notlandung)
{
/*如果出現(xiàn)緊急降落，則將三個期望位置全部置零，即讓飛行器向最穩(wěn)定的方向調(diào)整，同時改變控制參
數(shù)，并且不讓飛行器處在空中打轉(zhuǎn)的狀態(tài)*/
StickGier = 0;
StickNick = 0;
StickRoll = 0;
GyroFaktor = 0.1;
IntegralFaktor = 0.005;
Looping_Roll = 0;
Looping_Nick = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen//加速度信號的積分校準
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define ABGLEICH_ANZAHL 256L
/*計算陀螺儀積分的積分，為了和加速度計的積分做比較，進行角速率的補償和陀螺儀中立點的修正*/
MittelIntegralNick += IntegralNick;
MittelIntegralRoll += IntegralRoll;
MittelIntegralNick2 += IntegralNick2;
MittelIntegralRoll2 += IntegralRoll2;
/*在空中打轉(zhuǎn)過程中，讓所有的積分項都為零，因為機動過程會產(chǎn)生很大的誤差，因此需要盡快結束其控制，然
后自動調(diào)平。 */
if(Looping_Nick || Looping_Roll)
{
IntegralAccNick = 0;
IntegralAccRoll = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
Mess_IntegralNick2 = Mess_IntegralNick;
Mess_IntegralRoll2 = Mess_IntegralRoll;
ZaehlMessungen = 0;
LageKorrekturNick = 0;
LageKorrekturRoll = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll)
{
long tmp_long, tmp_long2;
/*使用加速度計的值去補償陀螺儀的積分，這里必須知道EE_Parameter.GyroAccFaktor參數(shù)，才能夠知道補償了
多少*/
/*其中IntegralNick應該是陀螺儀積分
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
是濾波后的加速度，用當前加速度和上次的加速度平均 */
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)
Mittelwert_AccNick);//
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)
Mittelwert_AccRoll); //
tmp_long /= 16;
tmp_long2 /= 16;
/*如果歷史最大搖桿的量比較大，則說明在前段時間內(nèi)飛行器的姿態(tài)可能不為0，這就導致加速度計的輸出受到
有害加速度的影響，因此必須加速度計和陀螺儀積分差值的基礎上做一次衰減*/
if((MaxStickNick > 15) || (MaxStickRoll > 15))
{
tmp_long /= 3;
tmp_long2 /= 3;
}
/*當偏航軸的操縱桿輸入較大時候，則說明這時候偏航軸有一個角速度，為了消除有害加速度的影響，必須對這
兩個數(shù)值再做一次衰減*/
if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]]) > 25)
{
tmp_long /= 3;
tmp_long2 /= 3;
}
/*做一個限制，補償值必須在一定的范圍內(nèi)。將補償?shù)姆秶拗圃?-32*/
#define AUSGLEICH 32
if(tmp_long > AUSGLEICH)
tmp_long = AUSGLEICH;
if(tmp_long < -AUSGLEICH)
tmp_long =-AUSGLEICH;
if(tmp_long2 > AUSGLEICH)
tmp_long2 = AUSGLEICH;
if(tmp_long2 <-AUSGLEICH)
tmp_long2 =-AUSGLEICH;
/*將補償值考慮進去，這時候Mess_IntegralNick補償了，Mess_IntegralNick2沒有補償，因為在后面還要用到
*/
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
} // end if of if(!Looping_Nick && !Looping_Roll)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*當 >ABGLEICH_ANZAHL(256)時候 說明測量了256次航向*/
/*變量ZaehlMessungen是在AD檢測的函數(shù)中改變的，也就是說，下面這個if語句是每256個檢測周期計算一次，
而不是控制周期，檢測周期要高于控制周期*/
if(ZaehlMessungen >= ABGLEICH_ANZAHL)//關于時間積累的處理過程
{
static int cnt = 0;
static char last_n_p,last_n_n,last_r_p,last_r_n;
static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
if(!Looping_Nick && !Looping_Roll)
{
MittelIntegralNick /= ABGLEICH_ANZAHL;
MittelIntegralRoll /= ABGLEICH_ANZAHL;
/*計算加速度計積分的作用，在不運動時候，xy加速度計的積分應該是0，所以xy積
分而z不積分*/
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) /
ABGLEICH_ANZAHL;
IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) /
ABGLEICH_ANZAHL;
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
#define MAX_I 0//(Poti2/10)
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
/*不考慮補償?shù)耐勇輧x積分的積分-加速度計積分/平衡項*/
IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in
[EE_Parameter.Kanalbele×g[K_GIER]]) > 25))
{
/*這個參數(shù)在后面的程序中還要進行修正，修正后的值加入到陀螺儀的積分
中，可以認為這個參數(shù)是系統(tǒng)對于陀螺儀漂移的估計*/
LageKorrekturNick /= 2;
LageKorrekturRoll /= 2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln//陀螺漂移的確定
/*對陀螺儀漂移的估計過程*/
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*前面MittelIntegralNick已經(jīng)用過了，因此這里使用MittelIntegralNick2*/
MittelIntegralNick2 /= ABGLEICH_ANZAHL;
MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
/*有校正和沒有校正的陀螺儀積分做差，即陀螺儀的漂移*/
/*IntegralNick2是沒有校正的陀螺儀積分 IntegralNick是有校正的陀螺儀積分 這里的校正指的是使用加速度
計積分進行的校正*/
tmp_long = IntegralNick2 - IntegralNick;
tmp_long2 = IntegralRoll2 - IntegralRoll;
//DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
/*將差值加入到Mess_IntegralNick2和Mess_IntegralRoll2中 這時Mess_IntegralNick2和Mess_IntegralRoll2
被使用*/
IntegralFehlerNick = tmp_long;
IntegralFehlerRoll = tmp_long2;
/*下面兩個公式的作用就是讓Mess_IntegralNick2=Mess_IntegralNick，
Mess_IntegralRoll2=Mess_IntegralRoll為下一個計算周期做準備*/
Mess_IntegralNick2 -= IntegralFehlerNick;
Mess_IntegralRoll2 -= IntegralFehlerRoll;
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
DebugOut.Analog[21] = MittelIntegralNick / 26;
DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[28] = ausgleichNick;
DebugOut.Analog[29] = ausgleichRoll;
DebugOut.Analog[30] = LageKorrekturRoll * 10;
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
#define BEWE×GS_LIMIT 20000
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
/*以下部分就是對LageKorrekturNick的修正和對陀螺儀常值誤差的修正*/
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWE×GS_LIMIT)
{
if(IntegralFehlerNick > FEHLER_LIMIT2)
{
/*必須連續(xù)兩次的誤差都很大，才能進入下面的if語句*/
if(last_n_p)
{
/*連續(xù)兩次誤差較大時，對陀螺儀漂移進行補償*/
/*最后改變了LageKorrekturNick的值*/
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
ausgleichNick = IntegralFehlerNick / 8;
if(ausgleichNick > 5000)
ausgleichNick = 5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
}
else
last_n_p = 1;
}
else
last_n_p = 0;
if(IntegralFehlerNick < -FEHLER_LIMIT2)
{
if(last_n_n)
{
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
ausgleichNick = IntegralFehlerNick / 8;
if(ausgleichNick < -5000) ausgleichNick = -5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
}
else
last_n_n = 1;
}
else
last_n_n = 0;
}
else cnt = 0;
if(cnt > EE_Parameter.Driftkomp)
cnt = EE_Parameter.Driftkomp;
/*在飛行器飛行的過程中，如果發(fā)現(xiàn)陀螺儀的中立點發(fā)生變化，則仍然進行修正*/
/*誤差過大時候，改變陀螺儀的常值誤差，每次最多改變EE_Parameter.Driftkomp*/
if(IntegralFehlerNick > FEHLER_LIMIT)
AdNeutralNick += cnt;
if(IntegralFehlerNick < -FEHLER_LIMIT)
AdNeutralNick -= cnt;
DebugOut.Analog[27] = ausgleichRoll;
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
} // 整個的融合過程結束
else
{
LageKorrekturRoll = 0;
LageKorrekturNick = 0;
}
/*如果IntegralFaktor為零，也就是沒有使用陀螺儀積分對電機輸出進行修正，則不使用LageKorrekturRoll，
也就是不進行陀螺儀漂移的補償*/
/*在Heading_hold標志位被置位的情況下*/
if(!IntegralFaktor)
{
LageKorrekturRoll = 0;
LageKorrekturNick = 0;
} // z.B. bei HH
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
/*將上一次的值儲存下來*/
MittelIntegralNick_Alt = MittelIntegralNick;
MittelIntegralRoll_Alt = MittelIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
/*數(shù)據(jù)清零 加速度計積分每一次都進行清零*/
IntegralAccNick = 0;
IntegralAccRoll = 0;
IntegralAccZ = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
ZaehlMessungen = 0;
} //end if of if(ZaehlMessungen >= ABGLEICH_ANZAHL)
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren//偏航
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(StickGier) > 20) // war 35
{
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
NeueKompassRichtungMerken = 1;
}
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y
= ax + bx?
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
sollGier = tmp_int;
/*如果沒有這句話 那么偏航軸的期望角度將一直等于0度 那么如果需要調(diào)整偏航軸的角度 就必須一直
不斷的進行偏航的修正 加上這句話后 就不用一直修正了*/
Mess_Integral_Gier -= tmp_int;
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen約束和限制
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v;
static int SignalSchlecht = 0;
w = abs(IntegralNick /512);
v = abs(IntegralRoll /512);
if(v > w) w = v;
if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)
{
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
w = (w * Parameter_KompassWirkung) / 64;
w = Parameter_KompassWirkung - w;
if(w > 0)
{
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;
if(SignalSchlecht) SignalSchlecht--;
}
else SignalSchlecht = 500;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 24;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[2] = Mittelwert_AccNick;
DebugOut.Analog[3] = Mittelwert_AccRoll;
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[16] = Mittelwert_AccHoch;
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];
DebugOut.Analog[19] = motor_rx[3];
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
DebugOut.Analog[20] /= 14;
DebugOut.Analog[21] = motor_rx[4];
DebugOut.Analog[22] = motor_rx[5];
DebugOut.Analog[23] = motor_rx[6];
DebugOut.Analog[24] = motor_rx[7];
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
*/
// DebugOut.Analog[9] = MesswertNick;
// DebugOut.Analog[9] = SollHoehe;
// DebugOut.Analog[10] = Mess_Integral_Gier / 128;
// DebugOut.Analog[11] = KompassStartwert;
// DebugOut.Analog[10] = Parameter_Gyro_I;
// DebugOut.Analog[10] = EE_Parameter.Gyro_I;
// DebugOut.Analog[9] = KompassRichtung;
// DebugOut.Analog[10] = GasMischanteil;
// DebugOut.Analog[3] = HoeheD * 32;
// DebugOut.Analog[4] = hoehenregler;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen//角速度和角度變化的歸納部分
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//DebugOut.Analog[26] = MesswertNick;
//DebugOut.Analog[28] = MesswertRoll;
/*對角度做PD，也就是對角速率做了PI*/
if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
DebugOut.Analog[28] = MesswertRoll;
/*對控制器輸出進行幅度限制*/
#define MAX_SENSOR 2048
if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// H鰄enregelung
// Die H鰄enregelung schw鋍ht lediglich das Gas ab, erh鰄t es allerdings nicht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//OCR0B = 180 - (Poti1 + 120) / 4;
//DruckOffsetSetting = OCR0B;
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))
{
int tmp_int;
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)
{
if(Parameter_MaxHoehe < 50)
{
SollHoehe = HoehenWert - 20;
HoehenReglerAktiv = 0;
}
else
HoehenReglerAktiv = 1;
}
else
{
SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)
EE_Parameter.Hoehe_Verstaerkung - 20;
HoehenReglerAktiv = 1;
}
if(Notlandung)
SollHoehe = 0;
h = HoehenWert;
if((h > SollHoehe) && HoehenReglerAktiv)
{
h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16;
h = GasMischanteil - h;
h -= (HoeheD * Parameter_Luftdruck_D)/8;
tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung)
/ 32;
if(tmp_int > 50)
tmp_int = 50;
else if(tmp_int < -50) tmp_int = -50;
h -= tmp_int;
hoehenregler = (hoehenregler*15 + h) / 16;
if(hoehenregler < EE_Parameter.Hoehe_MinGas)
{
if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler =
EE_Parameter.Hoehe_MinGas;
if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler =
GasMischanteil;
}
if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil;
GasMischanteil = hoehenregler;
}
} // 高度調(diào)節(jié)器工作完成
if(GasMischanteil > MAX_GAS - 20)
GasMischanteil = MAX_GAS - 20;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mischer und PI-Regler 在PI控制器下的混合數(shù)值
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[7] = GasMischanteil;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil//偏航部分
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define MUL_G 1.0
GierMischanteil = MesswertGier - sollGier;
// GierMischanteil = 0;
/*對偏航數(shù)值進行限制，盡量避免最后計算出的四個電機的轉(zhuǎn)速小于0*/
if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS -
GasMischanteil));
if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS -
GasMischanteil));
/*油門本身如果太小了，就限制偏航為0*/
if(GasMischanteil < 20) GierMischanteil = 0;//
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Achse俯仰軸
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*PI調(diào)節(jié)器*/
/*這個控制算法實際上是位置環(huán)為PI控制，速率環(huán)為P控制*/
DiffNick = MesswertNick - (StickNick - GPS_Nick);
if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick);
else SummeNick += DiffNick;
if(SummeNick > 16000) SummeNick = 16000;
if(SummeNick < -16000) SummeNick = -16000;
pd_ergebnis = DiffNick + Ki * SummeNick; //PD控制結果為比例+積分控制
// Motor Vorn
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs
(GierMischanteil)/2)) / 64;
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; //如果控制器輸出太大，則要限制幅度
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
/*前后兩個電機的實際輸出*/
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
/*對電機數(shù)值進行限幅*/
if ((motorwert < 0))
motorwert = 0;
else if(motorwert > MAX_GAS)
motorwert = MAX_GAS;
if (motorwert < MIN_GAS)
motorwert = MIN_GAS;
Motor_Vorne = motorwert;
// Motor Heck
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
if ((motorwert < 0))
motorwert = 0;
else if(motorwert > MAX_GAS)
motorwert = MAX_GAS;
if (motorwert < MIN_GAS)
motorwert = MIN_GAS;
Motor_Hinten = motorwert;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Achse橫滾軸
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen
if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-
Anteil bei Winkelregelung
else SummeRoll += DiffRoll; // I-Anteil bei HH
if(SummeRoll > 16000) SummeRoll = 16000;
if(SummeRoll < -16000) SummeRoll = -16000;
pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler f黵 Roll
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs
(GierMischanteil)/2)) / 64;
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
// Motor Links
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
#define GRENZE Poti1
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Links = motorwert;
// Motor Rechts
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Rechts = motorwert;
// +++++++++++++++++++++++++++++++++++++++++++++++
} |
