Rev 1298 | Rev 1312 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1298 | Rev 1309 | ||
---|---|---|---|
Line 150... | Line 150... | ||
150 | long GIER_GRAD_FAKTOR = 1291; |
150 | long GIER_GRAD_FAKTOR = 1291; |
151 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
151 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
152 | unsigned char RequiredMotors = 4; |
152 | unsigned char RequiredMotors = 4; |
153 | unsigned char Motor[MAX_MOTORS]; |
153 | unsigned char Motor[MAX_MOTORS]; |
154 | signed int tmp_motorwert[MAX_MOTORS]; |
154 | signed int tmp_motorwert[MAX_MOTORS]; |
- | 155 | #define LIMIT_MIN(value, min) {if(value < min) value = min;} |
|
- | 156 | #define LIMIT_MAX(value, max) {if(value > max) value = max;} |
|
- | 157 | #define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
|
Line 155... | Line 158... | ||
155 | 158 | ||
156 | int MotorSmoothing(int neu, int alt) |
159 | int MotorSmoothing(int neu, int alt) |
157 | { |
160 | { |
158 | int motor; |
161 | int motor; |
Line 532... | Line 535... | ||
532 | //############################################################################ |
535 | //############################################################################ |
533 | // |
536 | // |
534 | void MotorRegler(void) |
537 | void MotorRegler(void) |
535 | //############################################################################ |
538 | //############################################################################ |
536 | { |
539 | { |
537 | int pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int; |
540 | int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int; |
538 | int GierMischanteil,GasMischanteil; |
541 | int GierMischanteil,GasMischanteil; |
539 | static long SummeNick=0,SummeRoll=0; |
542 | static long SummeNick=0,SummeRoll=0; |
540 | static long sollGier = 0,tmp_long,tmp_long2; |
543 | static long sollGier = 0,tmp_long,tmp_long2; |
541 | static long IntegralFehlerNick = 0; |
544 | static long IntegralFehlerNick = 0; |
542 | static long IntegralFehlerRoll = 0; |
545 | static long IntegralFehlerRoll = 0; |
543 | static unsigned int RcLostTimer; |
546 | static unsigned int RcLostTimer; |
544 | static unsigned char delay_neutral = 0; |
547 | static unsigned char delay_neutral = 0; |
545 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
548 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
546 | static int hoehenregler = 0; |
- | |
547 | static char TimerWerteausgabe = 0; |
549 | static char TimerWerteausgabe = 0; |
548 | static char NeueKompassRichtungMerken = 0; |
550 | static char NeueKompassRichtungMerken = 0; |
549 | static long ausgleichNick, ausgleichRoll; |
551 | static long ausgleichNick, ausgleichRoll; |
550 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
552 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
551 | unsigned char i; |
553 | unsigned char i; |
Line 560... | Line 562... | ||
560 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
562 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
561 | // Empfang schlecht |
563 | // Empfang schlecht |
562 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
564 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
563 | if(SenderOkay < 100) |
565 | if(SenderOkay < 100) |
564 | { |
566 | { |
565 | if(!PcZugriff) |
- | |
566 | { |
- | |
567 | if(BeepMuster == 0xffff) |
- | |
568 | { |
- | |
569 | beeptime = 15000; |
- | |
570 | BeepMuster = 0x0c00; |
- | |
571 | } |
- | |
572 | } |
- | |
573 | if(RcLostTimer) RcLostTimer--; |
567 | if(RcLostTimer) RcLostTimer--; |
574 | else |
568 | else |
575 | { |
569 | { |
576 | MotorenEin = 0; |
570 | MotorenEin = 0; |
577 | MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
571 | MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
Line 1267... | Line 1261... | ||
1267 | // Höhenregelung |
1261 | // Höhenregelung |
1268 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1262 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1269 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1263 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1270 | if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1264 | if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1271 | GasMischanteil *= STICK_GAIN; |
1265 | GasMischanteil *= STICK_GAIN; |
- | 1266 | ||
1272 | 1267 | ||
- | 1268 | // if height control is activated |
|
1273 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
1269 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
1274 | { |
1270 | { |
- | 1271 | #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
|
- | 1272 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
|
- | 1273 | #define OPA_OFFSET_STEP 10 |
|
1275 | int tmp_int; |
1274 | int HCGas, HeightDeviation; |
- | 1275 | static int HeightTrimming = 0; // rate for change of height setpoint |
|
1276 | long tmp_long_h; |
1276 | static int FilterHCGas = 0; |
1277 | static int hoover = 400, maxhoover,minhoover,gas_old; |
1277 | static int HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
- | 1278 | static unsigned long HooverGasFilter = 0; |
|
1278 | static char delay = 100; |
1279 | static unsigned char delay = 100; |
- | 1280 | long tmp_long3; |
|
- | 1281 | const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255}; |
|
1279 | unsigned char angle_gas = 0; |
1282 | // get the current hooverpoint |
- | 1283 | DebugOut.Analog[25] = HooverGas; |
|
- | 1284 | DebugOut.Analog[18] = VarioMeter; |
|
- | 1285 | ||
- | 1286 | // if height control is activated by an rc channel |
|
1280 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1287 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1281 | { |
- | |
1282 | if(Parameter_MaxHoehe < 50) |
1288 | { // check if parameter is less than activation threshold |
- | 1289 | if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
|
1283 | { |
1290 | { //height control not active |
1284 | if(!delay--) |
1291 | if(!delay--) |
1285 | { |
1292 | { |
1286 | #define OPA_OFFSET_STEP 10 |
1293 | // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
1287 | if((MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) ) |
1294 | if( (MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) ) |
1288 | { |
1295 | { // increase offset |
1289 | ExpandBaro -= 1; |
1296 | ExpandBaro -= 1; |
1290 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; |
1297 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
1291 | beeptime = 300; |
1298 | beeptime = 300; |
1292 | delay = 250; |
1299 | delay = 250; |
1293 | } |
1300 | } |
- | 1301 | // measurement of air pressure close to lower limit and |
|
1294 | else if((MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP)) |
1302 | else if( (MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP) ) |
1295 | { |
1303 | { // decrease offset |
1296 | ExpandBaro += 1; |
1304 | ExpandBaro += 1; |
1297 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; |
1305 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
1298 | beeptime = 300; |
1306 | beeptime = 300; |
1299 | delay = 250; |
1307 | delay = 250; |
1300 | } |
1308 | } |
1301 | else |
1309 | else |
1302 | { |
1310 | { |
1303 | SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
1311 | SollHoehe = HoehenWert; // update SetPoint with current reading |
1304 | HoehenReglerAktiv = 0; |
1312 | HoehenReglerAktiv = 0; // disable height control |
1305 | delay = 1; |
1313 | delay = 1; |
- | 1314 | } |
|
1306 | } |
1315 | } |
- | 1316 | } |
|
- | 1317 | else |
|
- | 1318 | { //height control is activated |
|
- | 1319 | HoehenReglerAktiv = 1; // enable height control |
|
- | 1320 | delay = 200; |
|
- | 1321 | } |
|
1307 | } |
1322 | } |
1308 | } |
- | |
1309 | else |
1323 | else // no switchable height control |
1310 | { |
1324 | { |
- | 1325 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
|
1311 | HoehenReglerAktiv = 1; |
1326 | HoehenReglerAktiv = 1; |
1312 | delay = 200; |
- | |
1313 | } |
1327 | } |
1314 | } |
- | |
1315 | else |
- | |
1316 | { |
- | |
1317 | SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; |
- | |
1318 | HoehenReglerAktiv = 1; |
- | |
1319 | } |
- | |
Line 1320... | Line 1328... | ||
1320 | 1328 | ||
- | 1329 | if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
|
- | 1330 | { |
|
- | 1331 | #define HEIGHT_TRIM_UP 0x01 |
|
- | 1332 | #define HEIGHT_TRIM_DOWN 0x02 |
|
- | 1333 | static unsigned char HeightTrimmingFlag = 0x00; |
|
- | 1334 | ||
- | 1335 | #define HEIGHT_CONTROL_STICKTHRESHOLD 15 * STICK_GAIN |
|
- | 1336 | // Holger original version |
|
- | 1337 | // start of height control algorithm |
|
- | 1338 | // the height control is only an attenuation of the actual gas stick. |
|
- | 1339 | // I.e. it will work only if the gas stick is higher than the hover gas |
|
- | 1340 | // and the hover height will be allways larger than height setpoint. |
|
1321 | if(MikroKopterFlags & FLAG_NOTLANDUNG) SollHoehe = 0; |
1341 | if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
1322 | h = HoehenWert; |
1342 | { // old version |
- | 1343 | HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
|
1323 | if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
1344 | HeightTrimming = 0; |
1324 | { |
1345 | } |
- | 1346 | else |
|
1325 | int vario; |
1347 | { |
1326 | static char delay = 1; |
1348 | // alternative height control |
1327 | // + P-Part ++++++++++++++++++++++++++ |
1349 | // PD-Control with respect to hoover point |
1328 | h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
1350 | // the thrust loss out of horizontal attitude is compensated |
- | 1351 | // the setpoint will be fine adjusted with the gas stick position |
|
1329 | h = GasMischanteil - h; // vom Gas abziehen |
1352 | if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying |
- | 1353 | { // gas stick is above hoover point |
|
1330 | if(!delay--) |
1354 | if(GasMischanteil > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) ) |
- | 1355 | { |
|
- | 1356 | if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
|
1331 | { |
1357 | { |
1332 | delay = 2; |
1358 | HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
- | 1359 | SollHoehe = HoehenWert; // update setpoint to current heigth |
|
1333 | hoover = (15*hoover + h) / 16 + (GasMischanteil - gas_old); |
1360 | } |
- | 1361 | HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
|
1334 | gas_old = GasMischanteil; |
1362 | HeightTrimming += abs(GasMischanteil - (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD)) / 4; |
- | 1363 | } // gas stick is below hoover point |
|
- | 1364 | else if(GasMischanteil < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) ) |
|
- | 1365 | { |
|
- | 1366 | if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
|
- | 1367 | { |
|
- | 1368 | HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
|
- | 1369 | SollHoehe = HoehenWert; // update setpoint to current heigth |
|
- | 1370 | } |
|
- | 1371 | HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
|
- | 1372 | HeightTrimming -= abs(GasMischanteil - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD)) / 4; |
|
1335 | if(hoover < h) hoover++; else hoover--; |
1373 | } |
1336 | if(EE_Parameter.Hoehe_HoverBand) |
1374 | else // Gas Stick in Hoover Range |
- | 1375 | { |
|
- | 1376 | if(HeightTrimmingFlag & (HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN)) |
|
- | 1377 | { |
|
1337 | { |
1378 | HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN); |
1338 | unsigned int band; |
1379 | HeightTrimming = 0; |
- | 1380 | SollHoehe = HoehenWert; // update setpoint to current height |
|
- | 1381 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
|
- | 1382 | } |
|
1339 | band = hoover / EE_Parameter.Hoehe_HoverBand; |
1383 | } |
1340 | maxhoover = hoover + band; // max. 16% |
1384 | // Trim height set point |
1341 | minhoover = hoover - band; // max. 16% |
1385 | if(abs(HeightTrimming) > 512) |
- | 1386 | { |
|
- | 1387 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
|
- | 1388 | HeightTrimming = 0; |
|
1342 | } |
1389 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
- | 1390 | } |
|
- | 1391 | } //if MikroKopterFlags & MKFLAG_FLY |
|
- | 1392 | else SollHoehe = HoehenWert - 200; |
|
- | 1393 | HCGas = HooverGas; // take hoover gas (neutral point) |
|
- | 1394 | } |
|
- | 1395 | ||
1343 | else |
1396 | if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
- | 1397 | { |
|
- | 1398 | // ------------------------- P-Part ---------------------------- |
|
- | 1399 | HeightDeviation = (int)(HoehenWert - SollHoehe); // positive when to high |
|
1344 | { |
1400 | tmp_int = (HeightDeviation * (int)Parameter_Hoehe_P) / 16; // p-part |
- | 1401 | HCGas -= tmp_int; |
|
1345 | maxhoover = 1023; |
1402 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
- | 1403 | tmp_int = VarioMeter / 8; |
|
- | 1404 | if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
|
- | 1405 | if(tmp_int > 0) tmp_int = VarioMeter + (tmp_int * tmp_int) / 4; |
|
- | 1406 | else tmp_int = VarioMeter - (tmp_int * tmp_int) / 4; |
|
- | 1407 | tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter |
|
- | 1408 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
|
- | 1409 | HCGas -= tmp_int; |
|
- | 1410 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
|
- | 1411 | tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
|
- | 1412 | DebugOut.Analog[23] = -tmp_int; |
|
- | 1413 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
|
- | 1414 | HCGas -= tmp_int; |
|
- | 1415 | ||
- | 1416 | // limit deviation from hoover point within the target region |
|
1346 | minhoover = 0; |
1417 | if( (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
- | 1418 | { |
|
1347 | } |
1419 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
- | 1420 | } |
|
- | 1421 | ||
1348 | } |
1422 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1349 | if(maxhoover < 200) maxhoover = 200; |
1423 | tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
- | 1424 | DebugOut.Analog[24] = -tmp_int; |
|
- | 1425 | HCGas -= tmp_int; |
|
- | 1426 | // strech control output by inverse attitude projection |
|
- | 1427 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
|
1350 | DebugOut.Analog[25] = hoover; |
1428 | tmp_long3 = labs(IntegralNick) + labs(IntegralRoll); |
1351 | DebugOut.Analog[26] = maxhoover; |
1429 | tmp_long3 /= 1500;//1024 * 2; |
- | 1430 | if(tmp_long3 > 29) tmp_long3 = 29; |
|
1352 | DebugOut.Analog[28] = minhoover; |
1431 | HCGas = ((long) HCGas * GAS_TAB[tmp_long3]) / 128L; |
1353 | 1432 | ||
1354 | // + D1-Part - Vario ++++++++++++++++++++++++++ |
1433 | // update height control gas averaging |
1355 | vario = VarioMeter / 8; |
1434 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
1356 | if(vario > 8) vario = 8; |
1435 | // limit height control gas pd-control output |
1357 | if(vario > 0) vario = VarioMeter + (vario * vario) / 4; |
1436 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
1358 | else vario = VarioMeter - (vario * vario) / 4; |
1437 | // set GasMischanteil to HeightControlGasFilter |
1359 | vario = (Parameter_Luftdruck_D * (long)vario)/128L; // D-Anteil |
1438 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
1360 | DebugOut.Analog[24] = -vario; |
1439 | { // old version |
- | 1440 | if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas |
|
1361 | if(vario > 255) vario = 255; |
1441 | } |
- | 1442 | GasMischanteil = FilterHCGas; |
|
1362 | else if(vario < -127) vario = -127; |
1443 | } |
1363 | h -= vario; |
1444 | }// EOF height control active |
1364 | 1445 | ||
1365 | // + D2-Part - ACC ++++++++++++++++++++++++++ |
1446 | // Hoover gas estimation by averaging gas control output on small z-velocities |
- | 1447 | // this is done only if height contol option is selected in global config and aircraft is flying |
|
- | 1448 | if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
|
1366 | // tmp_int = (((Mess_Integral_Hoch / 128)/*+AdWertAccHoch*/) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (32); |
1449 | { |
- | 1450 | if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
|
- | 1451 | if(abs(VarioMeter) < 100) // only on small vertical speed |
|
1367 | tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1452 | { |
- | 1453 | tmp_long3 = (long)GasMischanteil; // take current thrust |
|
1368 | DebugOut.Analog[23] = -tmp_int; |
1454 | tmp_long3 = labs(IntegralNick) + labs(IntegralRoll); |
1369 | if(tmp_int > 255) tmp_int = 255; |
1455 | tmp_long3 /= 1500;//1024 * 2; |
1370 | else if(tmp_int < -127) tmp_int = -127; |
1456 | if(tmp_long3 > 29) tmp_long3 = 29; |
1371 | h -= tmp_int; |
1457 | tmp_long3 = ((long) GasMischanteil * 128L) / (long) GAS_TAB[tmp_long3]; |
- | 1458 | ||
- | 1459 | // average vertical projected thrust |
|
- | 1460 | if(modell_fliegt < 2000) // the first 4 seconds |
|
- | 1461 | { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
|
1372 | 1462 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
|
1373 | if(h > maxhoover) h = maxhoover; |
1463 | HooverGasFilter += 8L * tmp_long3; |
- | 1464 | } |
|
- | 1465 | else if(modell_fliegt < 4000) // the first 8 seconds |
|
- | 1466 | { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
|
1374 | else |
1467 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
1375 | if(h < minhoover) h = minhoover; |
1468 | HooverGasFilter += 4L * tmp_long3; |
- | 1469 | } |
|
- | 1470 | else if(modell_fliegt < 8000) // the first 16 seconds |
|
- | 1471 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
|
1376 | 1472 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
|
- | 1473 | HooverGasFilter += 2L * tmp_long3; |
|
- | 1474 | } |
|
- | 1475 | else //later |
|
- | 1476 | { |
|
- | 1477 | HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
|
- | 1478 | HooverGasFilter += tmp_long3; |
|
- | 1479 | } |
|
- | 1480 | HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
|
- | 1481 | if(EE_Parameter.Hoehe_HoverBand) |
|
- | 1482 | { |
|
1377 | // + D3-Part - GPS ++++++++++++++++++++++++++ |
1483 | int16_t band; |
1378 | vario = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; // D-Anteil |
1484 | band = HooverGas / EE_Parameter.Hoehe_HoverBand; // the higher the parameter the smaller the range |
1379 | DebugOut.Analog[24] = -vario; |
1485 | HooverGasMin = HooverGas - band; |
- | 1486 | HooverGasMax = HooverGas + band; |
|
1380 | // if(vario > 127) vario = 127; |
1487 | } |
- | 1488 | else |
|
- | 1489 | { // no limit |
|
- | 1490 | HooverGasMin = 0; |
|
- | 1491 | HooverGasMax = 1023; |
|
- | 1492 | } |
|
- | 1493 | } |
|
1381 | // else |
1494 | } |
- | 1495 | DebugOut.Analog[25] = HooverGas; |
|
1382 | // if(vario < -63) vario = -63; |
1496 | DebugOut.Analog[26] = HooverGasMax; |
Line -... | Line 1497... | ||
- | 1497 | DebugOut.Analog[28] = HooverGasMin; |
|
- | 1498 | ||
- | 1499 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
|
- | 1500 | ||
Line 1383... | Line -... | ||
1383 | h -= vario; |
- | |
1384 | - | ||
1385 | - | ||
1386 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
- | |
1387 | const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255}; |
- | |
1388 | tmp_long_h = labs(IntegralNick) + labs(IntegralRoll); |
- | |
1389 | tmp_long_h /= 1500;//1024 * 2; |
- | |
1390 | if(tmp_long_h > 29) tmp_long_h = 29; |
- | |
1391 | h = ((long) h * GAS_TAB[tmp_long_h]) / 128L; |
- | |
1392 | - | ||
1393 | // + Begrenzung ++++++++++++++++++++++++++ |
- | |
1394 | hoehenregler = (hoehenregler*3 + h) / 4; |
- | |
1395 | if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN |
- | |
1396 | { |
- | |
1397 | if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN; |
- | |
1398 | if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil; |
- | |
1399 | } |
- | |
1400 | if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
- | |
1401 | GasMischanteil = hoehenregler; |
- | |
1402 | } |
- | |
1403 | else |
- | |
1404 | { |
- | |
1405 | hoover = GasMischanteil; |
- | |
1406 | gas_old = GasMischanteil; |
1501 | // limit gas to parameter setting |
Line 1407... | Line 1502... | ||
1407 | } |
1502 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
1408 | } |
1503 | |
1409 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
1504 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |