| 67,6 → 67,7 |
|---|
| unsigned int NeutralAccX=0, NeutralAccY=0; |
| int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
| int NeutralAccZ = 0; |
| signed char NeutralAccZfine = 0; |
| unsigned char ControlHeading = 0;// in 2° |
| long IntegralNick = 0,IntegralNick2 = 0; |
| long IntegralRoll = 0,IntegralRoll2 = 0; |
| 211,13 → 212,11 |
|---|
| DebugOut.Analog[30] = GPS_Nick; |
| DebugOut.Analog[31] = GPS_Roll; |
| if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
| |
| DebugOut.Analog[16] = Variance; |
| DebugOut.Analog[17] = VerticalVelocity; |
| DebugOut.Analog[18] = HoehenWertF; |
| DebugOut.Analog[25] = Parameter_Hoehe_P; |
| DebugOut.Analog[26] = Parameter_Luftdruck_D; |
| |
| //DebugOut.Analog[16] = Variance; |
| //DebugOut.Analog[17] = VarioMeter; |
| //DebugOut.Analog[18] = HoehenWertF; |
| //DebugOut.Analog[25] = Parameter_Hoehe_P; |
| //DebugOut.Analog[26] = Parameter_Luftdruck_D; |
| } |
| |
| |
| 272,16 → 271,17 |
|---|
| |
| //############################################################################ |
| // Nullwerte ermitteln |
| void SetNeutral(unsigned char AccAdjustment) |
| unsigned char SetNeutral(unsigned char AccAdjustment) // retuns: "sucess" |
| //############################################################################ |
| { |
| unsigned char i; |
| unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0; |
| unsigned char i, sucess = 1; |
| unsigned int gier_neutral = 0, nick_neutral = 0, roll_neutral = 0, acc_z_neutral = 0; |
| VersionInfo.HardwareError[0] = 0; |
| // HEF4017Reset_ON; |
| NeutralAccX = 0; |
| NeutralAccY = 0; |
| NeutralAccZ = 0; |
| NeutralAccZfine = 0; |
| |
| AdNeutralNick = 0; |
| AdNeutralRoll = 0; |
| 308,10 → 308,12 |
|---|
| gier_neutral += AdWertGier; |
| nick_neutral += AdWertNick; |
| roll_neutral += AdWertRoll; |
| acc_z_neutral += Aktuell_az; |
| } |
| AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
| AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
| AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
| AdNeutralNick = (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
| AdNeutralRoll = (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
| AdNeutralGier = (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
| NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
| |
| StartNeutralRoll = AdNeutralRoll; |
| StartNeutralNick = AdNeutralNick; |
| 320,8 → 322,6 |
|---|
| { |
| NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
| NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
| NeutralAccZ = Aktuell_az; |
| |
| // Save ACC neutral settings to eeprom |
| SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX); |
| SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
| 332,17 → 332,15 |
|---|
| // restore from eeprom |
| NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
| NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
| NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
| // strange settings? |
| if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048) || ((unsigned int) NeutralAccZ > 1024)) |
| if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048)/* || ((unsigned int) NeutralAccZ > 1024)*/) |
| { |
| printf("\n\rACC not calibrated!\r\n"); |
| NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
| NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
| NeutralAccZ = Aktuell_az; |
| sucess = 0; |
| } |
| } |
| |
| MesswertNick = 0; |
| MesswertRoll = 0; |
| MesswertGier = 0; |
| 356,7 → 354,6 |
|---|
| Mess_Integral_Gier = 0; |
| StartLuftdruck = Luftdruck; |
| VarioMeter = 0; |
| VerticalVelocitySum = 0; |
| SummenHoehe = 0; Mess_Integral_Hoch = 0; |
| KompassSollWert = KompassValue; |
| KompassSignalSchlecht = 100; |
| 387,6 → 384,20 |
|---|
| NickServoValue = ((128 - 60) * 4 * 16); // neutral position = lower 1/4 |
| } |
| |
| #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| signed int tilt1, tilt2; |
| tilt1 = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
| tilt2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
| tilt1 = (int16_t)ihypot(tilt1,tilt2); // tilt angle over all |
| CosAttitude = c_cos_8192(tilt1); |
| NeutralAccZ = (long)((long) (NeutralAccZ - 512) * 8192 + 4096) / CosAttitude + 512; |
| if(tilt1 > 20) sucess = 0; // calibration must be within 20° Tilt angle |
| if(ACC_AltitudeControl) if((NeutralAccZ < 682 - 25) || (NeutralAccZ > 682 + 25)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;}; |
| |
| #else |
| NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
| #endif |
| |
| if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; }; |
| if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; }; |
| if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; }; |
| 393,10 → 404,13 |
|---|
| if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; }; |
| if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
| if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
| if(VersionInfo.HardwareError[0]) sucess = 0; |
| carefree_old = 70; |
| #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| LIBFC_HoTT_Clear(); |
| ACC_AltitudeFusion(2); // initalisation |
| #endif |
| return(sucess); |
| } |
| |
| |
| 863,13 → 877,13 |
|---|
| if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
| } |
| // ServoActive = 0; |
| SetNeutral(0); |
| CalibrationDone = 1; |
| CalibrationDone = SetNeutral(0); |
| ServoActive = 1; |
| DDRD |=0x80; // enable J7 -> Servo signal |
| #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| if(abs(Aktuell_az - NeutralAccZ) > 5 && ACC_AltitudeControl) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
| if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
| else |
| if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION; |
| else SpeakHoTT = SPEAK_CALIBRATE; |
| #endif |
| Piep(GetActiveParamSet(),120); |
| 884,10 → 898,12 |
|---|
| MotorenEin = 0; |
| delay_neutral = 0; |
| modell_fliegt = 0; |
| SetNeutral(1); |
| CalibrationDone = 1; |
| CalibrationDone = SetNeutral(1); |
| #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| SpeakHoTT = SPEAK_CALIBRATE; |
| if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
| else |
| if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION; |
| else SpeakHoTT = SPEAK_CALIBRATE; |
| #endif |
| Piep(GetActiveParamSet(),120); |
| } |
| 1530,18 → 1546,15 |
|---|
| else // delay, because of expanding the Baro-Range |
| { |
| // now clear the D-values |
| VarioMeter = 0; |
| #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| if(ACC_AltitudeControl) SummenHoehe = HoehenWert * SA_FILTER; |
| if(ACC_AltitudeControl) ACC_AltitudeFusion(1); // init |
| else SummenHoehe = HoehenWert * SM_FILTER; |
| #else |
| SummenHoehe = HoehenWert * SM_FILTER; |
| #endif |
| |
| VerticalVelocitySum = 0; |
| VarioMeter = 0; |
| BaroExpandActive--; |
| } |
| |
| // if height control is activated by an rc channel |
| if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
| { // check if parameter is less than activation threshold |
| 1722,8 → 1735,8 |
|---|
| } |
| if(HoehenWertF > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)) |
| { |
| if(!ACC_AltitudeControl) |
| { |
| if(!ACC_AltitudeControl) |
| { |
| // from this point the Heigth Control Algorithm is identical for both versions |
| if(BaroExpandActive) // baro range expanding active |
| { |
| 1806,9 → 1819,9 |
|---|
| GasMischanteil = FilterHCGas; |
| } |
| else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
| } |
| else |
| { |
| } |
| else // ACC-Altitude control |
| { |
| // from this point the Heigth Control Algorithm is identical for both versions |
| if(BaroExpandActive) // baro range expanding active |
| { |
| 1825,7 → 1838,7 |
|---|
| LIMIT_MIN_MAX(tmp_long, -511 * STICK_GAIN, 512 * STICK_GAIN); // more than full range makes sense |
| GasReduction = tmp_long; |
| // ------------------------ D-Part: ACC-Z Integral ------------------------ |
| tmp_long = VerticalVelocity + AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung/256; |
| tmp_long = VarioMeter + (AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung)/256; |
| // ------------------------- D-Part: Vario Meter ---------------------------- |
| if(WaypointTrimming) { |
| Variance = AltitudeSetpointTrimming * 8; |
| 1837,7 → 1850,6 |
|---|
| LIMIT_MIN_MAX(tmp_long,-511 * STICK_GAIN, 512 * STICK_GAIN); |
| GasReduction += tmp_long; |
| } // EOF no baro range expanding |
| DebugOut.Analog[19] = -GasReduction; |
| HCGas -= GasReduction; |
| LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limits gas around hover point |
| // strech control output by inverse attitude projection 1/cos |
| 1856,8 → 1868,8 |
|---|
| LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
| GasMischanteil = FilterHCGas; |
| } |
| else GasMischanteil = FilterHCGas;// + (GasMischanteil - HoverGas) / 4; // Qopter: geändert |
| } |
| else GasMischanteil = FilterHCGas; |
| } // end of ACC-Altitude control |
| } |
| }// EOF height control active |
| else // HC not active |
| 1879,8 → 1891,8 |
|---|
| // this is done only if height contol option is selected in global config and aircraft is flying |
| if((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
| { |
| //if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
| if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert |
| //if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
| if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert |
| if(StartTrigger == 1) StartTrigger = 2; |
| tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
| tmp_long2 *= CosAttitude; // apply attitude projection |
| 1897,8 → 1909,7 |
|---|
| HoverGasFilter += 4L * tmp_long2; |
| } |
| else //later |
| //if(abs(VerticalVelocity) < 50 && abs(HoehenWertF - SollHoehe) < 256) // Qopter: geändert, Anpassung nötig? |
| if(abs(VarioMeter) < 100 && abs(HoehenWert - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending) |
| if(abs(VarioMeter) < 100 && abs(HoehenWertF - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending) |
| { |
| HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
| HoverGasFilter += tmp_long2; |