| 152,6 → 152,9 |
|---|
| unsigned char RequiredMotors = 4; |
| unsigned char Motor[MAX_MOTORS]; |
| signed int tmp_motorwert[MAX_MOTORS]; |
| #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;} |
| |
| int MotorSmoothing(int neu, int alt) |
| { |
| 534,7 → 537,7 |
|---|
| void MotorRegler(void) |
| //############################################################################ |
| { |
| int pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int; |
| int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int; |
| int GierMischanteil,GasMischanteil; |
| static long SummeNick=0,SummeRoll=0; |
| static long sollGier = 0,tmp_long,tmp_long2; |
| 543,7 → 546,6 |
|---|
| static unsigned int RcLostTimer; |
| static unsigned char delay_neutral = 0; |
| static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
| static int hoehenregler = 0; |
| static char TimerWerteausgabe = 0; |
| static char NeueKompassRichtungMerken = 0; |
| static long ausgleichNick, ausgleichRoll; |
| 562,14 → 564,6 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(SenderOkay < 100) |
| { |
| if(!PcZugriff) |
| { |
| if(BeepMuster == 0xffff) |
| { |
| beeptime = 15000; |
| BeepMuster = 0x0c00; |
| } |
| } |
| if(RcLostTimer) RcLostTimer--; |
| else |
| { |
| 1269,143 → 1263,244 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
| GasMischanteil *= STICK_GAIN; |
| |
| |
| |
| // if height control is activated |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
| { |
| int tmp_int; |
| long tmp_long_h; |
| static int hoover = 400, maxhoover,minhoover,gas_old; |
| static char delay = 100; |
| unsigned char angle_gas = 0; |
| if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
| { |
| if(Parameter_MaxHoehe < 50) |
| { |
| if(!delay--) |
| { |
| #define OPA_OFFSET_STEP 10 |
| if((MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) ) |
| { |
| ExpandBaro -= 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; |
| beeptime = 300; |
| delay = 250; |
| } |
| else if((MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP)) |
| { |
| ExpandBaro += 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; |
| beeptime = 300; |
| delay = 250; |
| } |
| else |
| { |
| SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
| HoehenReglerAktiv = 0; |
| delay = 1; |
| } |
| { |
| #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms 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 HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| static unsigned long HooverGasFilter = 0; |
| static unsigned char delay = 100; |
| 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 |
| DebugOut.Analog[25] = 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--) |
| { |
| // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
| if( (MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) ) |
| { // increase offset |
| ExpandBaro -= 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
| beeptime = 300; |
| delay = 250; |
| } |
| // measurement of air pressure close to lower limit and |
| else if( (MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP) ) |
| { // decrease offset |
| ExpandBaro += 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
| beeptime = 300; |
| delay = 250; |
| } |
| else |
| { |
| SollHoehe = HoehenWert; // update SetPoint with current reading |
| HoehenReglerAktiv = 0; // disable height control |
| delay = 1; |
| } |
| } |
| } |
| else |
| { //height control is activated |
| HoehenReglerAktiv = 1; // enable height control |
| delay = 200; |
| } |
| } |
| } |
| else |
| { |
| HoehenReglerAktiv = 1; |
| delay = 200; |
| else // no switchable height control |
| { |
| SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
| HoehenReglerAktiv = 1; |
| } |
| } |
| else |
| { |
| SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; |
| HoehenReglerAktiv = 1; |
| } |
| |
| if(MikroKopterFlags & FLAG_NOTLANDUNG) SollHoehe = 0; |
| h = HoehenWert; |
| if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
| { |
| int vario; |
| static char delay = 1; |
| // + P-Part ++++++++++++++++++++++++++ |
| h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
| h = GasMischanteil - h; // vom Gas abziehen |
| if(!delay--) |
| { |
| delay = 2; |
| hoover = (15*hoover + h) / 16 + (GasMischanteil - gas_old); |
| gas_old = GasMischanteil; |
| if(hoover < h) hoover++; else hoover--; |
| if(EE_Parameter.Hoehe_HoverBand) |
| if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
| { |
| #define HEIGHT_TRIM_UP 0x01 |
| #define HEIGHT_TRIM_DOWN 0x02 |
| static unsigned char HeightTrimmingFlag = 0x00; |
| |
| #define HEIGHT_CONTROL_STICKTHRESHOLD 15 * STICK_GAIN |
| // Holger original version |
| // start of height control algorithm |
| // the height control is only an attenuation of the actual gas stick. |
| // I.e. it will work only if the gas stick is higher than the hover gas |
| // and the hover height will be allways larger than height setpoint. |
| if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
| { // old version |
| HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
| HeightTrimming = 0; |
| } |
| else |
| { |
| // alternative height control |
| // PD-Control with respect to hoover point |
| // the thrust loss out of horizontal attitude is compensated |
| // the setpoint will be fine adjusted with the gas stick position |
| if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying |
| { // gas stick is above hoover point |
| if(GasMischanteil > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) ) |
| { |
| if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
| { |
| HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
| SollHoehe = HoehenWert; // update setpoint to current heigth |
| } |
| HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
| HeightTrimming += abs(GasMischanteil - (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD)) / 4; |
| } // gas stick is below hoover point |
| else if(GasMischanteil < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) ) |
| { |
| if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
| { |
| HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
| SollHoehe = HoehenWert; // update setpoint to current heigth |
| } |
| HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
| HeightTrimming -= abs(GasMischanteil - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD)) / 4; |
| } |
| else // Gas Stick in Hoover Range |
| { |
| if(HeightTrimmingFlag & (HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN)) |
| { |
| HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN); |
| HeightTrimming = 0; |
| SollHoehe = HoehenWert; // update setpoint to current height |
| if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
| } |
| } |
| // Trim height set point |
| if(abs(HeightTrimming) > 512) |
| { |
| SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
| HeightTrimming = 0; |
| if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
| } |
| } //if MikroKopterFlags & MKFLAG_FLY |
| else SollHoehe = HoehenWert - 200; |
| HCGas = HooverGas; // take hoover gas (neutral point) |
| } |
| |
| if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
| { |
| unsigned int band; |
| band = hoover / EE_Parameter.Hoehe_HoverBand; |
| maxhoover = hoover + band; // max. 16% |
| minhoover = hoover - band; // max. 16% |
| } |
| else |
| { |
| maxhoover = 1023; |
| minhoover = 0; |
| } |
| } |
| if(maxhoover < 200) maxhoover = 200; |
| DebugOut.Analog[25] = hoover; |
| DebugOut.Analog[26] = maxhoover; |
| DebugOut.Analog[28] = minhoover; |
| // ------------------------- P-Part ---------------------------- |
| HeightDeviation = (int)(HoehenWert - SollHoehe); // positive when to high |
| tmp_int = (HeightDeviation * (int)Parameter_Hoehe_P) / 16; // p-part |
| HCGas -= tmp_int; |
| // ------------------------- D-Part 1: Vario Meter ---------------------------- |
| tmp_int = VarioMeter / 8; |
| if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
| if(tmp_int > 0) tmp_int = VarioMeter + (tmp_int * tmp_int) / 4; |
| else tmp_int = VarioMeter - (tmp_int * tmp_int) / 4; |
| tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter |
| LIMIT_MIN_MAX(tmp_int, -127, 255); |
| HCGas -= tmp_int; |
| // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
| tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
| DebugOut.Analog[23] = -tmp_int; |
| LIMIT_MIN_MAX(tmp_int, -127, 255); |
| HCGas -= tmp_int; |
| |
| // + D1-Part - Vario ++++++++++++++++++++++++++ |
| vario = VarioMeter / 8; |
| if(vario > 8) vario = 8; |
| if(vario > 0) vario = VarioMeter + (vario * vario) / 4; |
| else vario = VarioMeter - (vario * vario) / 4; |
| vario = (Parameter_Luftdruck_D * (long)vario)/128L; // D-Anteil |
| DebugOut.Analog[24] = -vario; |
| if(vario > 255) vario = 255; |
| else if(vario < -127) vario = -127; |
| h -= vario; |
| |
| // + D2-Part - ACC ++++++++++++++++++++++++++ |
| // tmp_int = (((Mess_Integral_Hoch / 128)/*+AdWertAccHoch*/) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (32); |
| tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
| DebugOut.Analog[23] = -tmp_int; |
| if(tmp_int > 255) tmp_int = 255; |
| else if(tmp_int < -127) tmp_int = -127; |
| h -= tmp_int; |
| // limit deviation from hoover point within the target region |
| if( (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
| { |
| LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
| } |
| |
| if(h > maxhoover) h = maxhoover; |
| else |
| if(h < minhoover) h = minhoover; |
| // ------------------------ D-Part 3: GpsZ ---------------------------------- |
| tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
| DebugOut.Analog[24] = -tmp_int; |
| HCGas -= tmp_int; |
| // strech control output by inverse attitude projection |
| // + 1/cos(angle) ++++++++++++++++++++++++++ |
| tmp_long3 = labs(IntegralNick) + labs(IntegralRoll); |
| tmp_long3 /= 1500;//1024 * 2; |
| if(tmp_long3 > 29) tmp_long3 = 29; |
| HCGas = ((long) HCGas * GAS_TAB[tmp_long3]) / 128L; |
| |
| // + D3-Part - GPS ++++++++++++++++++++++++++ |
| vario = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; // D-Anteil |
| DebugOut.Analog[24] = -vario; |
| // if(vario > 127) vario = 127; |
| // else |
| // if(vario < -63) vario = -63; |
| h -= vario; |
| // update height control gas averaging |
| FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
| // limit height control gas pd-control output |
| LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
| // set GasMischanteil to HeightControlGasFilter |
| if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
| { // old version |
| if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas |
| } |
| GasMischanteil = FilterHCGas; |
| } |
| }// EOF height control active |
| |
| // Hoover gas estimation by averaging gas control output on small z-velocities |
| // this is done only if height contol option is selected in global config and aircraft is flying |
| if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
| { |
| if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
| if(abs(VarioMeter) < 100) // only on small vertical speed |
| { |
| tmp_long3 = (long)GasMischanteil; // take current thrust |
| tmp_long3 = labs(IntegralNick) + labs(IntegralRoll); |
| tmp_long3 /= 1500;//1024 * 2; |
| if(tmp_long3 > 29) tmp_long3 = 29; |
| tmp_long3 = ((long) GasMischanteil * 128L) / (long) GAS_TAB[tmp_long3]; |
| |
| // + 1/cos(angle) ++++++++++++++++++++++++++ |
| 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}; |
| tmp_long_h = labs(IntegralNick) + labs(IntegralRoll); |
| tmp_long_h /= 1500;//1024 * 2; |
| if(tmp_long_h > 29) tmp_long_h = 29; |
| h = ((long) h * GAS_TAB[tmp_long_h]) / 128L; |
| // average vertical projected thrust |
| if(modell_fliegt < 2000) // the first 4 seconds |
| { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
| HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
| HooverGasFilter += 8L * tmp_long3; |
| } |
| else if(modell_fliegt < 4000) // the first 8 seconds |
| { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
| HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
| HooverGasFilter += 4L * tmp_long3; |
| } |
| else if(modell_fliegt < 8000) // the first 16 seconds |
| { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
| HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
| HooverGasFilter += 2L * tmp_long3; |
| } |
| else //later |
| { |
| HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
| HooverGasFilter += tmp_long3; |
| } |
| HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
| if(EE_Parameter.Hoehe_HoverBand) |
| { |
| int16_t band; |
| band = HooverGas / EE_Parameter.Hoehe_HoverBand; // the higher the parameter the smaller the range |
| HooverGasMin = HooverGas - band; |
| HooverGasMax = HooverGas + band; |
| } |
| else |
| { // no limit |
| HooverGasMin = 0; |
| HooverGasMax = 1023; |
| } |
| } |
| } |
| DebugOut.Analog[25] = HooverGas; |
| DebugOut.Analog[26] = HooverGasMax; |
| DebugOut.Analog[28] = HooverGasMin; |
| |
| // + Begrenzung ++++++++++++++++++++++++++ |
| hoehenregler = (hoehenregler*3 + h) / 4; |
| if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN |
| { |
| if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN; |
| if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil; |
| } |
| if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
| GasMischanteil = hoehenregler; |
| } |
| else |
| { |
| hoover = GasMischanteil; |
| gas_old = GasMischanteil; |
| } |
| } |
| }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| |
| // limit gas to parameter setting |
| LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
| |
| if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |