• MikroKopter.de
• Wiki
• SHOP
• Datenschutz
• Impressum

FlightCtrlBL-CtrlMK3MagNaviCtrlProjects

Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

Compare Revisions

• This comparison shows the changes necessary to convert path

  → /branches/dongfang_FC_rewrite/heightControl.c @ 2072

  → /branches/dongfang_FC_rewrite/heightControl.c @ 2073

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 2072 → Rev 2073

/branches/dongfang_FC_rewrite/heightControl.c
96,7 → 96,7
HC_periodicTask();
int16_t throttle = PRTY[CONTROL_THROTTLE];
int32_t height = analog_getHeight();
int32_t heightError = /*ramped*/ targetHeight - height;
int32_t heightError = targetHeight - height;
int16_t dHeight = analog_getDHeight();
debugOut.analog[22] = height/10L;
114,27 → 114,37
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
}
int32_t heightErrorForIntegral = heightError;
int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << 10;
if (heightErrorForIntegral > heightErrorForIntegralLimit) {
heightErrorForIntegral = heightErrorForIntegralLimit;
} else if (heightErrorForIntegral < -heightErrorForIntegralLimit) {
heightErrorForIntegral =- heightErrorForIntegralLimit;
}
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
iHeight += heightError;
iHeight += heightErrorForIntegral;
#define IHEIGHT_SCALE 24
// dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);
if (dThrottleI > staticParams.heightControlMaxIntegral) {
dThrottleI = staticParams.heightControlMaxIntegral;
iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
} else if (dThrottleI < -staticParams.heightControlMaxIntegral) {
dThrottleI = -staticParams.heightControlMaxIntegral;
iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
if (dThrottleI > staticParams.heightControlMaxIntegralOut) {
dThrottleI = staticParams.heightControlMaxIntegralOut;
iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
} else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) {
dThrottleI = -staticParams.heightControlMaxIntegralOut;
iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
}
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
//debugOut.analog[24] = dThrottleP;
//debugOut.analog[25] = dThrottleI;
//debugOut.analog[26] = dThrottleD;
debugOut.analog[10] = dThrottleP;
debugOut.analog[11] = dThrottleI;
debugOut.analog[12] = dThrottleD;
debugOut.analog[13] = heightError/10;
//debugOut.analog[27] = dynamicParams.heightP;
//debugOut.analog[28] = dynamicParams.heightI;