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; }
} // 高度调节器工作完成
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;
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;
// +++++++++++++++++++++++++++++++++++++++++++++++ }