Rev 1590 | Rev 1598 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1590 | Rev 1591 | ||
---|---|---|---|
Line 156... | Line 156... | ||
156 | unsigned int modell_fliegt = 0; |
156 | unsigned int modell_fliegt = 0; |
157 | volatile unsigned char FCFlags = 0; |
157 | volatile unsigned char FCFlags = 0; |
158 | long GIER_GRAD_FAKTOR = 1291; |
158 | long GIER_GRAD_FAKTOR = 1291; |
159 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
159 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
160 | signed int tmp_motorwert[MAX_MOTORS]; |
160 | signed int tmp_motorwert[MAX_MOTORS]; |
161 | unsigned char LoadHandler = 0; |
161 | char VarioCharacter = ' '; |
- | 162 | ||
162 | #define LIMIT_MIN(value, min) {if(value <= min) value = min;} |
163 | #define LIMIT_MIN(value, min) {if(value <= min) value = min;} |
163 | #define LIMIT_MAX(value, max) {if(value >= max) value = max;} |
164 | #define LIMIT_MAX(value, max) {if(value >= max) value = max;} |
164 | #define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} |
165 | #define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} |
Line 165... | Line 166... | ||
165 | 166 | ||
Line 563... | Line 564... | ||
563 | static char TimerWerteausgabe = 0; |
564 | static char TimerWerteausgabe = 0; |
564 | static char NeueKompassRichtungMerken = 0; |
565 | static char NeueKompassRichtungMerken = 0; |
565 | static long ausgleichNick, ausgleichRoll; |
566 | static long ausgleichNick, ausgleichRoll; |
566 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
567 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
567 | unsigned char i; |
568 | unsigned char i; |
568 | if(--LoadHandler == 0) LoadHandler = 5; // verteilt die Prozessorlast |
- | |
569 | Mittelwert(); |
569 | Mittelwert(); |
570 | GRN_ON; |
570 | GRN_ON; |
571 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
571 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
572 | // Gaswert ermitteln |
572 | // Gaswert ermitteln |
573 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
573 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1331... | Line 1331... | ||
1331 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
1331 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
1332 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1332 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1333 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
1333 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
1334 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1334 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1335 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1335 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
- | 1336 | VarioCharacter = ' '; |
|
1336 | if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG)) |
1337 | if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG)) |
1337 | { |
1338 | { |
1338 | #define HEIGHT_TRIM_UP 0x01 |
1339 | #define HEIGHT_TRIM_UP 0x01 |
1339 | #define HEIGHT_TRIM_DOWN 0x02 |
1340 | #define HEIGHT_TRIM_DOWN 0x02 |
1340 | static unsigned char HeightTrimmingFlag = 0x00; |
1341 | static unsigned char HeightTrimmingFlag = 0x00; |
Line 1365... | Line 1366... | ||
1365 | HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
1366 | HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
1366 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1367 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1367 | } |
1368 | } |
1368 | HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
1369 | HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
1369 | HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
1370 | HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
- | 1371 | VarioCharacter = '+'; |
|
1370 | } // gas stick is below hoover point |
1372 | } // gas stick is below hoover point |
1371 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1373 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1372 | { |
1374 | { |
1373 | if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
1375 | if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
1374 | { |
1376 | { |
1375 | HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
1377 | HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
1376 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1378 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1377 | } |
1379 | } |
1378 | HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
1380 | HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
1379 | HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1381 | HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
- | 1382 | VarioCharacter = '-'; |
|
1380 | } |
1383 | } |
1381 | else // Gas Stick in Hover Range |
1384 | else // Gas Stick in Hover Range |
1382 | { |
1385 | { |
1383 | if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN)) |
1386 | if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN)) |
1384 | { |
1387 | { |
1385 | HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN); |
1388 | HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN); |
1386 | HeightTrimming = 0; |
1389 | HeightTrimming = 0; |
1387 | SollHoehe = HoehenWert; // update setpoint to current height |
1390 | SollHoehe = HoehenWert; // update setpoint to current height |
1388 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1391 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1389 | } |
1392 | } |
- | 1393 | VarioCharacter = '='; |
|
1390 | } |
1394 | } |
1391 | // Trim height set point |
1395 | // Trim height set point |
1392 | if(abs(HeightTrimming) > 512) |
1396 | if(abs(HeightTrimming) > 512) |
1393 | { |
1397 | { |
1394 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1398 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |