Rev 1328 | Rev 1332 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1328 | Rev 1330 | ||
---|---|---|---|
Line 54... | Line 54... | ||
54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 55... | Line 55... | ||
55 | 55 | ||
56 | #include "main.h" |
56 | #include "main.h" |
57 | #include "eeprom.c" |
57 | #include "eeprom.c" |
- | 58 | #include "mymath.h" |
|
Line 58... | Line 59... | ||
58 | #include "mymath.h" |
59 | #include "isqrt.h" |
59 | 60 | ||
60 | unsigned char h,m,s,BaroExpandActive = 0; |
61 | unsigned char h,m,s,BaroExpandActive = 0; |
61 | volatile unsigned int I2CTimeout = 100; |
62 | volatile unsigned int I2CTimeout = 100; |
Line 540... | Line 541... | ||
540 | //############################################################################ |
541 | //############################################################################ |
541 | // |
542 | // |
542 | void MotorRegler(void) |
543 | void MotorRegler(void) |
543 | //############################################################################ |
544 | //############################################################################ |
544 | { |
545 | { |
545 | int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int; |
546 | int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2; |
546 | int GierMischanteil,GasMischanteil; |
547 | int GierMischanteil,GasMischanteil; |
547 | static long SummeNick=0,SummeRoll=0; |
548 | static long SummeNick=0,SummeRoll=0; |
548 | static long sollGier = 0,tmp_long,tmp_long2; |
549 | static long sollGier = 0,tmp_long,tmp_long2; |
549 | static long IntegralFehlerNick = 0; |
550 | static long IntegralFehlerNick = 0; |
550 | static long IntegralFehlerRoll = 0; |
551 | static long IntegralFehlerRoll = 0; |
Line 562... | Line 563... | ||
562 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
563 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
563 | // Gaswert ermitteln |
564 | // Gaswert ermitteln |
564 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
565 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
565 | GasMischanteil = StickGas; |
566 | GasMischanteil = StickGas; |
566 | if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10; |
567 | if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10; |
567 | 568 | ||
568 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
569 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
569 | // Empfang schlecht |
570 | // Empfang schlecht |
570 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
571 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
571 | if(SenderOkay < 100) |
572 | if(SenderOkay < 100) |
572 | { |
573 | { |
Line 648... | Line 649... | ||
648 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
649 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
649 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
650 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
650 | { |
651 | { |
651 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
652 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
652 | } |
653 | } |
653 | ServoActive = 0; |
654 | ServoActive = 0; |
654 | SetNeutral(); |
655 | SetNeutral(); |
655 | ServoActive = 1; |
656 | ServoActive = 1; |
656 | DDRD |=0x80; // enable J7 -> Servo signal |
657 | DDRD |=0x80; // enable J7 -> Servo signal |
657 | Piep(GetActiveParamSetNumber(),120); |
658 | Piep(GetActiveParamSetNumber(),120); |
658 | } |
659 | } |
Line 744... | Line 745... | ||
744 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
745 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
745 | if(StickGier > 2) StickGier -= 2; else |
746 | if(StickGier > 2) StickGier -= 2; else |
746 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
747 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
Line 747... | Line 748... | ||
747 | 748 | ||
748 | StickGas = (StickGas + PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120) / 2; |
749 | StickGas = (StickGas + PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120) / 2; |
749 | 750 | ||
750 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
751 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
751 | IntegralFaktor = Parameter_Gyro_I; |
752 | IntegralFaktor = Parameter_Gyro_I; |
752 | GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
753 | GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
Line 762... | Line 763... | ||
762 | StickGier += ExternControl.Gier; |
763 | StickGier += ExternControl.Gier; |
763 | ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
764 | ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
764 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
765 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
765 | } |
766 | } |
766 | if(StickGas < 0) StickGas = 0; |
767 | if(StickGas < 0) StickGas = 0; |
767 | 768 | ||
768 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
769 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
769 | if(GyroFaktor < 0) GyroFaktor = 0; |
770 | //if(GyroFaktor < 0) GyroFaktor = 0; |
770 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
771 | //if(IntegralFaktor < 0) IntegralFaktor = 0; |
Line 771... | Line 772... | ||
771 | 772 | ||
772 | if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
773 | if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
773 | { |
774 | { |
774 | MaxStickNick = abs(StickNick)/STICK_GAIN; |
775 | MaxStickNick = abs(StickNick)/STICK_GAIN; |
Line 1232... | Line 1233... | ||
1232 | static int FilterHCGas = 0; |
1233 | static int FilterHCGas = 0; |
1233 | static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023, LastHCGas = 0; |
1234 | static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023, LastHCGas = 0; |
1234 | static unsigned long HooverGasFilter = 0; |
1235 | static unsigned long HooverGasFilter = 0; |
1235 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1236 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1236 | int CosAttitude; // for projection of hoover gas |
1237 | int CosAttitude; // for projection of hoover gas |
1237 | long tmp_long3; |
- | |
1238 | // 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}; |
- | |
- | 1238 | ||
1239 | // get the current hooverpoint |
1239 | // get the current hooverpoint |
1240 | if(LoadHandler == 1) |
1240 | if(LoadHandler == 1) |
1241 | { |
1241 | { |
1242 | DebugOut.Analog[21] = HooverGas; |
1242 | DebugOut.Analog[21] = HooverGas; |
1243 | DebugOut.Analog[18] = VarioMeter; |
1243 | DebugOut.Analog[18] = VarioMeter; |
Line 1246... | Line 1246... | ||
1246 | // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
1246 | // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
1247 | if(!BaroExpandActive) |
1247 | if(!BaroExpandActive) |
1248 | { |
1248 | { |
1249 | if(MessLuftdruck > 920) |
1249 | if(MessLuftdruck > 920) |
1250 | { // increase offset |
1250 | { // increase offset |
1251 | if(OCR0A < (255 - OPA_OFFSET_STEP)) |
1251 | if(OCR0A < (255 - OPA_OFFSET_STEP)) |
1252 | { |
1252 | { |
1253 | ExpandBaro -= 1; |
1253 | ExpandBaro -= 1; |
1254 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
1254 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
1255 | beeptime = 300; |
1255 | beeptime = 300; |
1256 | BaroExpandActive = 50; |
1256 | BaroExpandActive = 50; |
1257 | } |
1257 | } |
1258 | else |
1258 | else |
1259 | { |
1259 | { |
1260 | BaroAtLowerLimit = 1; |
1260 | BaroAtLowerLimit = 1; |
1261 | } |
1261 | } |
1262 | } |
1262 | } |
1263 | // measurement of air pressure close to lower limit and |
1263 | // measurement of air pressure close to lower limit and |
1264 | else |
1264 | else |
1265 | if(MessLuftdruck < 100) |
1265 | if(MessLuftdruck < 100) |
1266 | { // decrease offset |
1266 | { // decrease offset |
1267 | if(OCR0A > OPA_OFFSET_STEP) |
1267 | if(OCR0A > OPA_OFFSET_STEP) |
1268 | { |
1268 | { |
1269 | ExpandBaro += 1; |
1269 | ExpandBaro += 1; |
1270 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
1270 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
1271 | beeptime = 300; |
1271 | beeptime = 300; |
1272 | BaroExpandActive = 50; |
1272 | BaroExpandActive = 50; |
1273 | } |
1273 | } |
1274 | else |
1274 | else |
1275 | { |
1275 | { |
1276 | BaroAtUpperLimit = 1; |
1276 | BaroAtUpperLimit = 1; |
1277 | } |
1277 | } |
1278 | } |
1278 | } |
1279 | else |
1279 | else |
1280 | { |
1280 | { |
1281 | BaroAtUpperLimit = 0; |
1281 | BaroAtUpperLimit = 0; |
1282 | BaroAtLowerLimit = 0; |
1282 | BaroAtLowerLimit = 0; |
1283 | } |
1283 | } |
1284 | } |
1284 | } |
Line 1313... | Line 1313... | ||
1313 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1313 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1314 | HoehenReglerAktiv = 1; |
1314 | HoehenReglerAktiv = 1; |
1315 | } |
1315 | } |
Line 1316... | Line 1316... | ||
1316 | 1316 | ||
1317 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1317 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1318 | J4Low; |
1318 | J4Low; |
1319 | tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg |
- | |
1320 | tmp_long *= tmp_long; |
1319 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
1321 | tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg |
- | |
1322 | tmp_long2 *= tmp_long2; |
1320 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1323 | CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg |
1321 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
1324 | DebugOut.Analog[16] = CosAttitude; |
1322 | DebugOut.Analog[16] = CosAttitude; |
1325 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1323 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1326 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1324 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1327 | J4High; |
1325 | J4High; |
1328 | if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1326 | if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1329 | { |
1327 | { |
1330 | #define HEIGHT_TRIM_UP 0x01 |
1328 | #define HEIGHT_TRIM_UP 0x01 |
1331 | #define HEIGHT_TRIM_DOWN 0x02 |
1329 | #define HEIGHT_TRIM_DOWN 0x02 |
Line 1332... | Line 1330... | ||
1332 | static unsigned char HeightTrimmingFlag = 0x00; |
1330 | static unsigned char HeightTrimmingFlag = 0x00; |
1333 | 1331 | ||
1334 | #define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
1332 | #define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
1335 | // Holger original version |
1333 | // Holger original version |
1336 | // start of height control algorithm |
1334 | // start of height control algorithm |
1337 | // the height control is only an attenuation of the actual gas stick. |
1335 | // the height control is only an attenuation of the actual gas stick. |
Line 1386... | Line 1384... | ||
1386 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(512 / 2); // move setpoint |
1384 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(512 / 2); // move setpoint |
1387 | HeightTrimming = 0; |
1385 | HeightTrimming = 0; |
1388 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
1386 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
1389 | //update hoover gas stick value when setpoint is shifted |
1387 | //update hoover gas stick value when setpoint is shifted |
1390 | // StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1388 | // StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1391 | // StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1389 | // StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1392 | // if(StickGasHoover < 70) StickGasHoover = 70; |
1390 | // if(StickGasHoover < 70) StickGasHoover = 70; |
1393 | // else if(StickGasHoover > 150) StickGasHoover = 150; |
1391 | // else if(StickGasHoover > 150) StickGasHoover = 150; |
1394 | } |
1392 | } |
1395 | } //if MikroKopterFlags & MKFLAG_FLY |
1393 | } //if MikroKopterFlags & MKFLAG_FLY |
1396 | else |
1394 | else |
1397 | { |
1395 | { |
1398 | SollHoehe = HoehenWert - 400; |
1396 | SollHoehe = HoehenWert - 400; |
1399 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1397 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1400 | else StickGasHoover = 120; |
1398 | else StickGasHoover = 120; |
1401 | } |
1399 | } |
1402 | HCGas = HooverGas; // take hoover gas (neutral point) |
1400 | HCGas = HooverGas; // take hoover gas (neutral point) |
Line 1418... | Line 1416... | ||
1418 | HCGas -= tmp_int; |
1416 | HCGas -= tmp_int; |
1419 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1417 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1420 | tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1418 | tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1421 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1419 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1422 | HCGas -= tmp_int; |
1420 | HCGas -= tmp_int; |
1423 | 1421 | ||
1424 | // limit deviation from hoover point within the target region |
1422 | // limit deviation from hoover point within the target region |
1425 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1423 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1426 | { |
1424 | { |
1427 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
1425 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
1428 | } |
1426 | } |
Line 1430... | Line 1428... | ||
1430 | if(BaroExpandActive) HCGas = HooverGas; |
1428 | if(BaroExpandActive) HCGas = HooverGas; |
1431 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1429 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1432 | tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
1430 | tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
1433 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1431 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1434 | HCGas -= tmp_int; |
1432 | HCGas -= tmp_int; |
1435 | 1433 | ||
1436 | // strech control output by inverse attitude projection 1/cos |
1434 | // strech control output by inverse attitude projection 1/cos |
1437 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1435 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1438 | tmp_long3 = (int32_t)HCGas; |
1436 | tmp_long2 = (int32_t)HCGas; |
1439 | tmp_long3 *= 8192L; |
1437 | tmp_long2 *= 8192L; |
1440 | tmp_long3 /= CosAttitude; |
1438 | tmp_long2 /= CosAttitude; |
1441 | HCGas = (int16_t)tmp_long3; |
1439 | HCGas = (int16_t)tmp_long2; |
1442 | // update height control gas averaging |
1440 | // update height control gas averaging |
1443 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
1441 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
1444 | // limit height control gas pd-control output |
1442 | // limit height control gas pd-control output |
1445 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
1443 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
1446 | // set GasMischanteil to HeightControlGasFilter |
1444 | // set GasMischanteil to HeightControlGasFilter |
Line 1458... | Line 1456... | ||
1458 | { |
1456 | { |
1459 | StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1457 | StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1460 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1458 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1461 | } |
1459 | } |
1462 | else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1460 | else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1463 | if(StickGasHoover < 70) StickGasHoover = 70; |
1461 | if(StickGasHoover < 70) StickGasHoover = 70; |
1464 | else if(StickGasHoover > 150) StickGasHoover = 150; |
1462 | else if(StickGasHoover > 150) StickGasHoover = 150; |
1465 | FilterHCGas = GasMischanteil; |
1463 | FilterHCGas = GasMischanteil; |
1466 | } |
1464 | } |
Line 1467... | Line 1465... | ||
1467 | 1465 | ||
1468 | // Hoover gas estimation by averaging gas control output on small z-velocities |
1466 | // Hoover gas estimation by averaging gas control output on small z-velocities |
1469 | // this is done only if height contol option is selected in global config and aircraft is flying |
1467 | // this is done only if height contol option is selected in global config and aircraft is flying |
1470 | if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1468 | if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1471 | { |
1469 | { |
1472 | if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1470 | if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1473 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1471 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1474 | { |
1472 | { |
1475 | tmp_long3 = (int32_t)GasMischanteil; // take current thrust |
1473 | tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
1476 | tmp_long3 *= CosAttitude; // apply attitude projection |
1474 | tmp_long2 *= CosAttitude; // apply attitude projection |
1477 | tmp_long3 /= 8192; |
1475 | tmp_long2 /= 8192; |
1478 | 1476 | ||
1479 | // average vertical projected thrust |
1477 | // average vertical projected thrust |
1480 | if(modell_fliegt < 2000) // the first 4 seconds |
1478 | if(modell_fliegt < 2000) // the first 4 seconds |
1481 | { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
1479 | { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
1482 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
1480 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
1483 | HooverGasFilter += 8L * tmp_long3; |
1481 | HooverGasFilter += 8L * tmp_long2; |
1484 | } |
1482 | } |
1485 | else if(modell_fliegt < 4000) // the first 8 seconds |
1483 | else if(modell_fliegt < 4000) // the first 8 seconds |
1486 | { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
1484 | { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
1487 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
1485 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
1488 | HooverGasFilter += 4L * tmp_long3; |
1486 | HooverGasFilter += 4L * tmp_long2; |
1489 | } |
1487 | } |
1490 | else if(modell_fliegt < 8000) // the first 16 seconds |
1488 | else if(modell_fliegt < 8000) // the first 16 seconds |
1491 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
1489 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
1492 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
1490 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
1493 | HooverGasFilter += 2L * tmp_long3; |
1491 | HooverGasFilter += 2L * tmp_long2; |
1494 | } |
1492 | } |
1495 | else //later |
1493 | else //later |
1496 | { |
1494 | { |
1497 | HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
1495 | HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
1498 | HooverGasFilter += tmp_long3; |
1496 | HooverGasFilter += tmp_long2; |
1499 | } |
1497 | } |
1500 | HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
1498 | HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
1501 | if(EE_Parameter.Hoehe_HoverBand) |
1499 | if(EE_Parameter.Hoehe_HoverBand) |
1502 | { |
1500 | { |
Line 1511... | Line 1509... | ||
1511 | HooverGasMax = 1023; |
1509 | HooverGasMax = 1023; |
1512 | } |
1510 | } |
1513 | } |
1511 | } |
1514 | } |
1512 | } |
1515 | LastHCGas = GasMischanteil; |
1513 | LastHCGas = GasMischanteil; |
1516 | } |
1514 | } |
1517 | else |
1515 | else |
1518 | { |
1516 | { |
1519 | GasMischanteil = LastHCGas; |
1517 | GasMischanteil = LastHCGas; |
1520 | } |
1518 | } |
1521 | //DebugOut.Analog[26] = HooverGasMax; |
1519 | //DebugOut.Analog[26] = HooverGasMax; |
1522 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
1520 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
1523 | J4Low; |
1521 | J4Low; |
Line 1524... | Line 1522... | ||
1524 | 1522 | ||
Line 1527... | Line 1525... | ||
1527 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
1525 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
Line 1528... | Line 1526... | ||
1528 | 1526 | ||
1529 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1527 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1530 | // all BL-Ctrl connected? |
1528 | // all BL-Ctrl connected? |
1531 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1529 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1532 | if(MissingMotor) |
1530 | if(MissingMotor) |
1533 | if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) |
1531 | if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) |
1534 | { |
1532 | { |
1535 | modell_fliegt = 1; |
1533 | modell_fliegt = 1; |
1536 | GasMischanteil = MIN_GAS; |
1534 | GasMischanteil = MIN_GAS; |