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;
}
} // 高度调节器工作完成
if(GasMischanteil > MAX_GAS - 20)
GasMischanteil = MAX_GAS - 20;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mischer und PI-Regler 在PI控制器下的混合数值
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[7] = GasMischanteil;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil//偏航部分
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define MUL_G 1.0
GierMischanteil = MesswertGier - sollGier;
// GierMischanteil = 0;
/*对偏航数值进行限制，尽量避免最后计算出的四个电机的转速小于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调节器*/
/*这个控制算法实际上是位置环为PI控制，速率环为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;
/*对电机数值进行限幅*/
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;
上一页 [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] 下一页
本文关键字：飞行器 电脑-单片机-自动控制，电子学习 - 基础知识 - 电脑-单片机-自动控制
