| 156,6 → 156,7 |
|---|
| unsigned char RequiredMotors = 4; |
| unsigned char Motor[MAX_MOTORS]; |
| signed int tmp_motorwert[MAX_MOTORS]; |
| unsigned char LoadHandler = 0; |
| #define LIMIT_MIN(value, min) {if(value < min) value = min;} |
| #define LIMIT_MAX(value, max) {if(value > max) value = max;} |
| #define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| 555,8 → 556,8 |
|---|
| static long ausgleichNick, ausgleichRoll; |
| int IntegralNickMalFaktor,IntegralRollMalFaktor; |
| unsigned char i; |
| if(--LoadHandler == 0) LoadHandler = 5; // verteilt die Prozessorlast |
| Mittelwert(); |
| |
| GRN_ON; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Gaswert ermitteln |
| 724,7 → 725,6 |
|---|
| else delay_ausschalten = 0; |
| } |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // neue Werte von der Funke |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 969,31 → 969,14 |
|---|
| Mess_IntegralNick2 -= IntegralFehlerNick; |
| Mess_IntegralRoll2 -= IntegralFehlerRoll; |
| |
| // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
| // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
| if(EE_Parameter.Driftkomp) |
| { |
| if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
| if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
| } |
| //DebugOut.Analog[22] = MittelIntegralRoll / 26; |
| //DebugOut.Analog[24] = GierGyroFehler; |
| GierGyroFehler = 0; |
| |
| |
| /*DebugOut.Analog[17] = IntegralAccNick / 26; |
| DebugOut.Analog[18] = IntegralAccRoll / 26; |
| DebugOut.Analog[19] = IntegralFehlerNick;// / 26; |
| DebugOut.Analog[20] = IntegralFehlerRoll;// / 26; |
| */ |
| //DebugOut.Analog[21] = MittelIntegralNick / 26; |
| //MittelIntegralRoll = MittelIntegralRoll; |
| //DebugOut.Analog[28] = ausgleichNick; |
| /* |
| DebugOut.Analog[29] = ausgleichRoll; |
| DebugOut.Analog[30] = LageKorrekturRoll * 10; |
| */ |
| |
| #define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2) |
| #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4 |
| #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16 |
| 1190,7 → 1173,7 |
|---|
| //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
| DebugOut.Analog[18] = VarioMeter; |
| DebugOut.Analog[19] = WinkelOut.CalcState; |
| DebugOut.Analog[20] = ServoValue; |
| DebugOut.Analog[20] = ServoNickValue; |
| // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
| // DebugOut.Analog[24] = MesswertNick/2; |
| // DebugOut.Analog[25] = MesswertRoll/2; |
| 1201,7 → 1184,6 |
|---|
| DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
| DebugOut.Analog[30] = GPS_Nick; |
| DebugOut.Analog[31] = GPS_Roll; |
| |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1237,17 → 1219,18 |
|---|
| if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
| GasMischanteil *= STICK_GAIN; |
| |
| DebugOut.Analog[16] = BaroExpandActive; |
| // if height control is activated |
| J4High; |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
| { |
| #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
| // #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| #define HOOVER_GAS_AVERAGE 1024L // 1024 * 10ms = 10s averaging |
| #define HC_GAS_AVERAGE 2 // 2 * 10ms= 20ms averaging |
| #define OPA_OFFSET_STEP 10 |
| int HCGas, HeightDeviation; |
| static int HeightTrimming = 0; // rate for change of height setpoint |
| static int FilterHCGas = 0; |
| static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023, LastHCGas = 0; |
| static unsigned long HooverGasFilter = 0; |
| static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| int CosAttitude; // for projection of hoover gas |
| 1254,26 → 1237,11 |
|---|
| long tmp_long3; |
| // 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}; |
| // get the current hooverpoint |
| if(LoadHandler == 1) |
| { |
| DebugOut.Analog[21] = HooverGas; |
| DebugOut.Analog[18] = VarioMeter; |
| |
| // if height control is activated by an rc channel |
| if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
| { // check if parameter is less than activation threshold |
| if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
| { //height control not active |
| if(!delay--) |
| { |
| HoehenReglerAktiv = 0; // disable height control |
| SollHoehe = HoehenWert; // update SetPoint with current reading |
| delay = 1; |
| } |
| } |
| else |
| { //height control is activated |
| HoehenReglerAktiv = 1; // enable height control |
| delay = 200; |
| } |
| // Expand the measurement |
| // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
| if(!BaroExpandActive) |
| 1285,8 → 1253,7 |
|---|
| ExpandBaro -= 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
| beeptime = 300; |
| delay = 250; |
| BaroExpandActive = 250; |
| BaroExpandActive = 50; |
| } |
| else |
| { |
| 1302,8 → 1269,7 |
|---|
| ExpandBaro += 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
| beeptime = 300; |
| delay = 250; |
| BaroExpandActive = 250; |
| BaroExpandActive = 50; |
| } |
| else |
| { |
| 1323,6 → 1289,24 |
|---|
| VarioMeter = 0; |
| BaroExpandActive--; |
| } |
| |
| // if height control is activated by an rc channel |
| if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
| { // check if parameter is less than activation threshold |
| if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
| { //height control not active |
| if(!delay--) |
| { |
| HoehenReglerAktiv = 0; // disable height control |
| SollHoehe = HoehenWert; // update SetPoint with current reading |
| delay = 1; |
| } |
| } |
| else |
| { //height control is activated |
| HoehenReglerAktiv = 1; // enable height control |
| delay = 40; |
| } |
| } |
| else // no switchable height control |
| { |
| 1331,15 → 1315,16 |
|---|
| } |
| |
| // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
| J4Low; |
| tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg |
| tmp_long *= tmp_long; |
| tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg |
| tmp_long2 *= tmp_long2; |
| |
| CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg |
| DebugOut.Analog[16] = CosAttitude; |
| LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
| CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
| |
| J4High; |
| if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
| { |
| #define HEIGHT_TRIM_UP 0x01 |
| 1396,9 → 1381,9 |
|---|
| } |
| } |
| // Trim height set point |
| if(abs(HeightTrimming) > 512) |
| if(abs(HeightTrimming) > 256) |
| { |
| SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
| SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(512 / 2); // move setpoint |
| HeightTrimming = 0; |
| if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
| //update hoover gas stick value when setpoint is shifted |
| 1411,7 → 1396,8 |
|---|
| else |
| { |
| SollHoehe = HoehenWert - 400; |
| StickGasHoover = 120; |
| if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
| else StickGasHoover = 120; |
| } |
| HCGas = HooverGas; // take hoover gas (neutral point) |
| } |
| 1434,7 → 1420,7 |
|---|
| tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
| LIMIT_MIN_MAX(tmp_int, -127, 255); |
| HCGas -= tmp_int; |
| |
| |
| // limit deviation from hoover point within the target region |
| if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
| { |
| 1476,6 → 1462,7 |
|---|
| else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
| if(StickGasHoover < 70) StickGasHoover = 70; |
| else if(StickGasHoover > 150) StickGasHoover = 150; |
| FilterHCGas = GasMischanteil; |
| } |
| |
| // Hoover gas estimation by averaging gas control output on small z-velocities |
| 1525,9 → 1512,15 |
|---|
| } |
| } |
| } |
| DebugOut.Analog[25] = HooverGas; |
| LastHCGas = GasMischanteil; |
| } |
| else |
| { |
| GasMischanteil = LastHCGas; |
| } |
| //DebugOut.Analog[26] = HooverGasMax; |
| }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| J4Low; |
| |
| // limit gas to parameter setting |
| LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |