| 100,14 → 100,12 |
|---|
| volatile int16_t CompassOffCourse = 0; |
| |
| // flags |
| uint8_t EmergencyLanding = 0; |
| uint8_t HightControlActive = 0; |
| uint8_t MotorsOn = 0; |
| |
| int32_t TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L; |
| |
| float GyroFactor; |
| float IntegralFactor; |
| float Gyro_P_Factor; |
| float Gyro_I_Factor; |
| |
| volatile int16_t DiffPitch, DiffRoll; |
| |
| 164,8 → 162,8 |
|---|
| AdNeutralPitch = 0; |
| AdNeutralRoll = 0; |
| AdNeutralYaw = 0; |
| FCParam.AchsKopplung1 = 0; |
| FCParam.AchsGegenKopplung1 = 0; |
| FCParam.Yaw_PosFeedback = 0; |
| FCParam.Yaw_NegFeedback = 0; |
| CalibMean(); |
| Delay_ms_Mess(100); |
| CalibMean(); |
| 242,17 → 240,17 |
|---|
| { |
| tmpl = Reading_IntegralPitch / 4096L; |
| tmpl *= ReadingYaw; |
| tmpl *= FCParam.AchsKopplung1; //125 |
| tmpl *= FCParam.Yaw_PosFeedback; //125 |
| tmpl /= 2048L; |
| tmpl2 = Reading_IntegralRoll / 4096L; |
| tmpl2 *= ReadingYaw; |
| tmpl2 *= FCParam.AchsKopplung1; |
| tmpl2 *= FCParam.Yaw_PosFeedback; |
| tmpl2 /= 2048L; |
| } |
| else tmpl = tmpl2 = 0; |
| // Roll |
| ReadingRoll += tmpl; |
| ReadingRoll += (tmpl2 * FCParam.AchsGegenKopplung1) / 512L; //109 |
| ReadingRoll += (tmpl2 * FCParam.Yaw_NegFeedback) / 512L; //109 |
| Reading_IntegralRoll2 += ReadingRoll; |
| Reading_IntegralRoll += ReadingRoll - AttitudeCorrectionRoll; |
| if(Reading_IntegralRoll > TurnOver180Roll) |
| 279,7 → 277,7 |
|---|
| } |
| // Pitch |
| ReadingPitch -= tmpl2; |
| ReadingPitch -= (tmpl*FCParam.AchsGegenKopplung1) / 512L; |
| ReadingPitch -= (tmpl*FCParam.Yaw_NegFeedback) / 512L; |
| Reading_IntegralPitch2 += ReadingPitch; |
| Reading_IntegralPitch += ReadingPitch - AttitudeCorrectionPitch; |
| if(Reading_IntegralPitch > TurnOver180Pitch) |
| 420,8 → 418,8 |
|---|
| CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255); |
| CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
| CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
| CHK_POTI(FCParam.AchsKopplung1, ParamSet.AchsKopplung1,0,255); |
| CHK_POTI(FCParam.AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255); |
| CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255); |
| CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255); |
| CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
| |
| Ki = (float) FCParam.I_Factor * FACTOR_I; |
| 428,354 → 426,393 |
|---|
| } |
| |
| |
| //############################################################################ |
| // |
| /************************************************************************/ |
| /* MotorControl */ |
| /************************************************************************/ |
| void MotorRegler(void) |
| //############################################################################ |
| { |
| int MotorValue, pd_ergebnis, h, tmp_int; |
| int YawMixingFraction, GasMixingFraction; |
| static long SumPitch = 0, SumRoll = 0; |
| static long SetPointYaw = 0, tmp_long, tmp_long2; |
| static long IntegralErrorPitch = 0; |
| static long IntegralErrorRoll = 0; |
| static unsigned int RcLostTimer; |
| static unsigned char delay_neutral = 0; |
| static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
| static unsigned int modell_fliegt = 0; |
| static int hoehenregler = 0; |
| static char TimerDebugOut = 0; |
| static char StoreNewCompassCourse = 0; |
| static long CorrectionPitch, CorrectionRoll; |
| int16_t MotorValue, pd_ergebnis, h, tmp_int; |
| int16_t YawMixingFraction, GasMixingFraction; |
| static int32_t SumPitch = 0, SumRoll = 0; |
| static int32_t SetPointYaw = 0, tmp_long, tmp_long2; |
| static int32_t IntegralErrorPitch = 0; |
| static int32_t IntegralErrorRoll = 0; |
| static uint16_t RcLostTimer; |
| static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
| static uint16_t Modell_Is_Flying = 0; |
| static uint8_t EmergencyLanding = 0; |
| static uint8_t HightControlActive = 0; |
| static int16_t hoehenregler = 0; |
| static int8_t TimerDebugOut = 0; |
| static int8_t StoreNewCompassCourse = 0; |
| static int32_t CorrectionPitch, CorrectionRoll; |
| |
| Mean(); |
| |
| GRN_ON; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Gaswert ermitteln |
| // determine GAS value |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| GasMixingFraction = StickGas; |
| if(GasMixingFraction < 0) GasMixingFraction = 0; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Emfang schlecht |
| // RC-signal is bad |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(SenderOkay < 100) |
| { |
| if(!PcZugriff) |
| { |
| if(BeepModulation == 0xFFFF) |
| { |
| BeepTime = 15000; // 1.5 seconds |
| BeepModulation = 0x0C00; |
| } |
| } |
| if(RcLostTimer) RcLostTimer--; |
| else |
| { |
| MotorsOn = 0; |
| EmergencyLanding = 0; |
| } |
| ROT_ON; |
| if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
| { |
| GasMixingFraction = ParamSet.EmergencyGas; |
| EmergencyLanding = 1; |
| PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
| PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
| PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
| } |
| else MotorsOn = 0; |
| } |
| else |
| // SenderOkay is incremented at good rc-level, i.e. if the ppm-signal deviation |
| // of a channel to previous frame is less than 1% the SenderOkay is incremented by 10. |
| // Typicaly within a frame of 8 channels (22.5ms) the SenderOkay is incremented by 8 * 10 = 80 |
| // The decremtation of 1 in the mainloop is done every 2 ms, i.e. within a time of one rc frame |
| // the main loop is running 11 times that decrements the SenderOkay by 11. |
| if(SenderOkay < 100) // the rc-frame signal is not reveived or noisy |
| { |
| if(!PcAccess) // if also no PC-Access via UART |
| { |
| if(BeepModulation == 0xFFFF) |
| { |
| BeepTime = 15000; // 1.5 seconds |
| BeepModulation = 0x0C00; |
| } |
| } |
| if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost |
| else // rc lost countdown finished |
| { |
| MotorsOn = 0; // stop all motors |
| EmergencyLanding = 0; // emergency landing is over |
| } |
| ROT_ON; // set red led |
| if(Modell_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken |
| { |
| GasMixingFraction = ParamSet.EmergencyGas; // set emergency gas |
| EmergencyLanding = 1; // enable emergency landing |
| // set neutral rc inputs |
| PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
| PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
| PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
| PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
| PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
| } |
| else MotorsOn = 0; // switch of all motors |
| } |
| else |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Emfang gut |
| // RC-signal is good |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(SenderOkay > 140) |
| { |
| EmergencyLanding = 0; |
| RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
| if(GasMixingFraction > 40) |
| { |
| if(modell_fliegt < 0xffff) modell_fliegt++; |
| } |
| if((modell_fliegt < 200) || (GasMixingFraction < 40)) |
| { |
| SumPitch = 0; |
| SumRoll = 0; |
| Reading_IntegralYaw = 0; |
| Reading_IntegralYaw2 = 0; |
| } |
| if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0) |
| { |
| if(SenderOkay > 140) |
| { |
| EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay |
| // reset emergency timer |
| RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
| if(GasMixingFraction > 40) |
| { |
| if(Modell_Is_Flying < 0xFFFF) Modell_Is_Flying++; |
| } |
| if((Modell_Is_Flying < 200) || (GasMixingFraction < 40)) |
| { |
| SumPitch = 0; |
| SumRoll = 0; |
| Reading_IntegralYaw = 0; |
| Reading_IntegralYaw2 = 0; |
| } |
| // if motors are off and the thrust stick is in the upper position |
| if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0) |
| { |
| // and if the yaw stick is in the leftmost position |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // auf Nullwerte kalibrieren |
| // calibrate the neutral readings of all attitude sensors |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) // Neutralwerte |
| { |
| if(++delay_neutral > 200) // nicht sofort |
| { |
| GRN_OFF; |
| MotorsOn = 0; |
| delay_neutral = 0; |
| modell_fliegt = 0; |
| if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
| { |
| unsigned char setting=1; |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 1; |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 2; |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 3; |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 4; |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 5; |
| SetActiveParamSet(setting); // aktiven Datensatz merken |
| } |
| if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung aktiviert? |
| { |
| if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
| } |
| ParamSet_ReadFromEEProm(GetActiveParamSet()); |
| SetNeutral(); |
| Beep(GetActiveParamSet()); |
| } |
| } |
| else |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) // ACC Neutralwerte speichern |
| { |
| if(++delay_neutral > 200) // nicht sofort |
| { |
| GRN_OFF; |
| SetParamWord(PID_ACC_PITCH,0xFFFF); // Werte löschen |
| MotorsOn = 0; |
| delay_neutral = 0; |
| modell_fliegt = 0; |
| SetNeutral(); |
| // ACC-NeutralWerte speichern |
| SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX); |
| SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
| SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
| Beep(GetActiveParamSet()); |
| } |
| } |
| else delay_neutral = 0; |
| } |
| { |
| if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
| { |
| delay_neutral = 0; |
| GRN_OFF; |
| Modell_Is_Flying = 0; |
| // check roll/pitch stick position |
| // if pitch stick is topmost or roll stick is leftmost --> change parameter setting |
| // according to roll/pitch stick position |
| if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
| { |
| uint8_t setting = 1; // default |
| // _________ |
| // |2 3 4| |
| // | | |
| // |1 5| |
| // | | |
| // |_________| |
| // |
| // roll stick leftmost and pitch stick centered --> setting 1 |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 1; |
| // roll stick leftmost and pitch stick topmost --> setting 2 |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 2; |
| // roll stick centered an pitch stick topmost --> setting 3 |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 3; |
| // roll stick rightmost and pitch stick topmost --> setting 4 |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 4; |
| // roll stick rightmost and pitch stick centered --> setting 5 |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 5; |
| // update active parameter set in eeprom |
| SetActiveParamSet(setting); |
| } |
| ParamSet_ReadFromEEProm(GetActiveParamSet()); |
| SetNeutral(); |
| Beep(GetActiveParamSet()); |
| } |
| } |
| // and if the yaw stick is in the rightmost position |
| // save the ACC neutral setting to eeprom |
| else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
| { |
| if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
| { |
| delay_neutral = 0; |
| GRN_OFF; |
| SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid |
| Modell_Is_Flying = 0; |
| SetNeutral(); |
| // Save ACC neutral settings to eeprom |
| SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX); |
| SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
| SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
| Beep(GetActiveParamSet()); |
| } |
| } |
| else delay_neutral = 0; |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Gas ist unten |
| // thrust stick is down |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 35-120) |
| { |
| // Starten |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
| { |
| if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85) |
| { |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Einschalten |
| // and yaw stick is rightmost --> start motors |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(++delay_einschalten > 200) |
| { |
| delay_einschalten = 200; |
| modell_fliegt = 1; |
| MotorsOn = 1; |
| SetPointYaw = 0; |
| Reading_IntegralYaw = 0; |
| Reading_IntegralYaw2 = 0; |
| Reading_IntegralPitch = 0; |
| Reading_IntegralRoll = 0; |
| Reading_IntegralPitch2 = IntegralPitch; |
| Reading_IntegralRoll2 = IntegralRoll; |
| SumPitch = 0; |
| SumRoll = 0; |
| } |
| } |
| else delay_einschalten = 0; |
| //Auf Neutralwerte setzen |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
| { |
| if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
| { |
| delay_startmotors = 200; // do not repeat if once executed |
| Modell_Is_Flying = 1; |
| MotorsOn = 1; |
| SetPointYaw = 0; |
| Reading_IntegralYaw = 0; |
| Reading_IntegralYaw2 = 0; |
| Reading_IntegralPitch = 0; |
| Reading_IntegralRoll = 0; |
| Reading_IntegralPitch2 = IntegralPitch; |
| Reading_IntegralRoll2 = IntegralRoll; |
| SumPitch = 0; |
| SumRoll = 0; |
| } |
| } |
| else delay_startmotors = 0; // reset delay timer if sticks are not in this position |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Auschalten |
| // and yaw stick is leftmost --> stop motors |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
| { |
| if(++delay_ausschalten > 200) // nicht sofort |
| { |
| MotorsOn = 0; |
| delay_ausschalten = 200; |
| modell_fliegt = 0; |
| } |
| } |
| else delay_ausschalten = 0; |
| } |
| } |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
| { |
| if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
| { |
| delay_stopmotors = 200; // do not repeat if once executed |
| Modell_Is_Flying = 0; |
| MotorsOn = 0; |
| |
| } |
| } |
| else delay_stopmotors = 0; // reset delay timer if sticks are not in this position |
| } |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // neue Werte von der Funke |
| // new values from RC |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(!NewPpmData-- || EmergencyLanding) |
| { |
| int tmp_int; |
| ParameterMapping(); |
| StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
| StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
| StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
| StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
| if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC |
| { |
| int tmp_int; |
| ParameterMapping(); // remapping params (online poti replacement) |
| |
| StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
| StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120; |
| // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
| StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
| StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
| StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
| StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
| |
| if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch) |
| MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]); else MaxStickPitch--; |
| if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll) |
| MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); else MaxStickRoll--; |
| if(EmergencyLanding) {MaxStickPitch = 0; MaxStickRoll = 0;} |
| // direct mapping of yaw and gas |
| StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
| StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers |
| |
| GyroFactor = ((float)FCParam.Gyro_P + 10.0) / 256.0; |
| IntegralFactor = ((float) FCParam.Gyro_I) / 44000; |
| // update max stick positions for pitch and roll |
| if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch) |
| MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]); |
| else MaxStickPitch--; |
| if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll) |
| MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); |
| else MaxStickRoll--; |
| |
| // update gyro control loop factors |
| |
| Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / 256.0; |
| Gyro_I_Factor = ((float) FCParam.Gyro_I) / 44000; |
| |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //+ Digitale Steuerung per DubWise |
| // Digital Control via DubWise |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #define KEY_VALUE (FCParam.UserParam1 * 4) //(Poti3 * 8) |
| if(DubWiseKeys[1]) BeepTime = 10; |
| if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else |
| if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0; |
| ExternStickPitch= (ExternStickPitch* 7 + tmp_int) / 8; |
| if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; else |
| if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0; |
| ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; |
| |
| if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else |
| if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0; |
| if(DubWiseKeys[0] & 2) ExternHightValue++; |
| if(DubWiseKeys[0] & 16) ExternHightValue--; |
| #define KEY_VALUE (FCParam.UserParam1 * 4) // step width |
| if(DubWiseKeys[1]) BeepTime = 10; |
| if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; |
| else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; |
| else tmp_int = 0; |
| ExternStickPitch = (ExternStickPitch * 7 + tmp_int) / 8; |
| if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; |
| else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; |
| else tmp_int = 0; |
| ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; |
| |
| StickPitch += ExternStickPitch/ 8; |
| StickRoll += ExternStickRoll / 8; |
| StickYaw += ExternStickYaw; |
| if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else |
| if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0; |
| if(DubWiseKeys[0] & 2) ExternHightValue++; |
| if(DubWiseKeys[0] & 16) ExternHightValue--; |
| |
| StickPitch += ExternStickPitch / 8; |
| StickRoll += ExternStickRoll / 8; |
| StickYaw += ExternStickYaw; |
| |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //+ Analoge Steuerung per Seriell |
| //+ Analoge Control via serial communication |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
| { |
| StickPitch += (int) ExternControl.Pitch * (int) ParamSet.Stick_P; |
| StickRoll += (int) ExternControl.Roll * (int) ParamSet.Stick_P; |
| StickYaw += ExternControl.Yaw; |
| ExternHightValue = (int) ExternControl.Hight * (int)ParamSet.Hight_Gain; |
| if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
| } |
| |
| if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) IntegralFactor = 0; |
| if(GyroFactor < 0) GyroFactor = 0; |
| if(IntegralFactor < 0) IntegralFactor = 0; |
| if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
| { |
| StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P; |
| StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P; |
| StickYaw += ExternControl.Yaw; |
| ExternHightValue = (int16_t) ExternControl.Hight * (int16_t)ParamSet.Hight_Gain; |
| if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
| } |
| // disable I part of gyro control feedback |
| if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0; |
| // avoid negative scaling factors |
| if(Gyro_P_Factor < 0) Gyro_P_Factor = 0; |
| if(Gyro_I_Factor < 0) Gyro_I_Factor = 0; |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Looping? |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Left = 1; |
| else |
| { |
| { |
| if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0; |
| } |
| } |
| if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1; |
| else |
| { |
| if(Looping_Right) // Hysterese |
| { |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0; |
| } |
| } |
| |
| if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1; |
| else |
| { |
| if(Looping_Top) // Hysterese |
| { |
| if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0; |
| } |
| } |
| if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1; |
| else |
| { |
| if(Looping_Down) // Hysterese |
| { |
| if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0; |
| } |
| } |
| if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Left = 1; |
| else |
| { |
| { |
| if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0; |
| } |
| } |
| if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1; |
| else |
| { |
| if(Looping_Right) // Hysterese |
| { |
| if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0; |
| } |
| } |
| |
| if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
| if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0; |
| } // Ende neue Funken-Werte |
| if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1; |
| else |
| { |
| if(Looping_Top) // Hysterese |
| { |
| if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0; |
| } |
| } |
| if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1; |
| else |
| { |
| if(Looping_Down) // Hysterese |
| { |
| if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0; |
| } |
| } |
| |
| if(Looping_Roll) BeepTime = 100; |
| if(Looping_Roll || Looping_Pitch) |
| { |
| if(GasMixingFraction > ParamSet.LoopGasLimit) GasMixingFraction = ParamSet.LoopGasLimit; |
| } |
| if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
| if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0; |
| } // End of new RC values |
| |
| |
| if(Looping_Roll) BeepTime = 100; |
| if(Looping_Roll || Looping_Pitch) |
| { |
| if(GasMixingFraction > ParamSet.LoopGasLimit) GasMixingFraction = ParamSet.LoopGasLimit; |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Bei Empfangsausfall im Flug |
| // in case of emergency landing |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(EmergencyLanding) |
| { |
| StickYaw = 0; |
| StickPitch = 0; |
| StickRoll = 0; |
| GyroFactor = 0.5;//Originalwert von Holger 0.1; |
| IntegralFactor = 0.003; //Originalwert von Holger 0.005; |
| Looping_Roll = 0; |
| Looping_Pitch = 0; |
| } |
| |
| if(EmergencyLanding) |
| { |
| StickYaw = 0; |
| StickPitch = 0; |
| StickRoll = 0; |
| Gyro_P_Factor = 0.5; |
| Gyro_I_Factor = 0.003; |
| Looping_Roll = 0; |
| Looping_Pitch = 0; |
| MaxStickPitch = 0; |
| MaxStickRoll = 0; |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Integrale auf ACC-Signal abgleichen |
| // Trim Gyro-Integrals to ACC-Signals |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #define ABGLEICH_ANZAHL 256L |
| |
| MeanIntegralPitch += IntegralPitch; // Für die Mittelwertbildung aufsummieren |
| MeanIntegralRoll += IntegralRoll; |
| MeanIntegralPitch2 += IntegralPitch2; |
| MeanIntegralRoll2 += IntegralRoll2; |
| #define ABGLEICH_ANZAHL 256L |
| |
| if(Looping_Pitch || Looping_Roll) |
| { |
| IntegralAccPitch = 0; |
| IntegralAccRoll = 0; |
| MeanIntegralPitch = 0; |
| MeanIntegralRoll = 0; |
| MeanIntegralPitch2 = 0; |
| MeanIntegralRoll2 = 0; |
| Reading_IntegralPitch2 = Reading_IntegralPitch; |
| Reading_IntegralRoll2 = Reading_IntegralRoll; |
| ZaehlMessungen = 0; |
| AttitudeCorrectionPitch = 0; |
| AttitudeCorrectionRoll = 0; |
| } |
| MeanIntegralPitch += IntegralPitch; // sum for averaging |
| MeanIntegralRoll += IntegralRoll; |
| MeanIntegralPitch2 += IntegralPitch2; |
| MeanIntegralRoll2 += IntegralRoll2; |
| |
| if(Looping_Pitch || Looping_Roll) // if looping in any direction |
| { |
| IntegralAccPitch = 0; |
| IntegralAccRoll = 0; |
| MeanIntegralPitch = 0; |
| MeanIntegralRoll = 0; |
| MeanIntegralPitch2 = 0; |
| MeanIntegralRoll2 = 0; |
| Reading_IntegralPitch2 = Reading_IntegralPitch; |
| Reading_IntegralRoll2 = Reading_IntegralRoll; |
| ZaehlMessungen = 0; |
| AttitudeCorrectionPitch = 0; |
| AttitudeCorrectionRoll = 0; |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(!Looping_Pitch && !Looping_Roll) |
| { |
| long tmp_long, tmp_long2; |
| tmp_long = (long)(IntegralPitch / ParamSet.GyroAccFaktor - (long)Mean_AccPitch); |
| tmp_long2 = (long)(IntegralRoll / ParamSet.GyroAccFaktor - (long)Mean_AccRoll); |
| tmp_long /= 16; |
| tmp_long2 /= 16; |
| if((MaxStickPitch > 15) || (MaxStickRoll > 15)) |
| { |
| tmp_long /= 3; |
| tmp_long2 /= 3; |
| } |
| if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) |
| { |
| tmp_long /= 3; |
| tmp_long2 /= 3; |
| } |
| if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction |
| { |
| int32_t tmp_long, tmp_long2; |
| tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch); |
| tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll); |
| tmp_long /= 16; |
| tmp_long2 /= 16; |
| if((MaxStickPitch > 15) || (MaxStickRoll > 15)) |
| { |
| tmp_long /= 3; |
| tmp_long2 /= 3; |
| } |
| if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) |
| { |
| tmp_long /= 3; |
| tmp_long2 /= 3; |
| } |
| |
| #define AUSGLEICH 32 |
| if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
| if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
| if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
| if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
| #define AUSGLEICH 32 |
| if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
| if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
| if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
| if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
| |
| Reading_IntegralPitch -= tmp_long; |
| Reading_IntegralRoll -= tmp_long2; |
| } |
| Reading_IntegralPitch -= tmp_long; |
| Reading_IntegralRoll -= tmp_long2; |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
| 915,7 → 952,7 |
|---|
| AttitudeCorrectionPitch = 0; |
| } |
| |
| if(!IntegralFactor) { AttitudeCorrectionRoll = 0; AttitudeCorrectionPitch = 0;} // z.B. bei HH |
| if(!Gyro_I_Factor) { AttitudeCorrectionRoll = 0; AttitudeCorrectionPitch = 0;} // z.B. bei HH |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| MeanIntegralPitch_old = MeanIntegralPitch; |
| MeanIntegralRoll_old = MeanIntegralRoll; |
| 929,7 → 966,7 |
|---|
| MeanIntegralRoll2 = 0; |
| ZaehlMessungen = 0; |
| } |
| //DebugOut.Analog[31] = StickRoll / (26*IntegralFactor); |
| //DebugOut.Analog[31] = StickRoll / (26*Gyro_I_Factor); |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Gieren |
| 1025,14 → 1062,14 |
|---|
| //DebugOut.Analog[26] = ReadingPitch; |
| //DebugOut.Analog[28] = ReadingRoll; |
| |
| if(Looping_Pitch) ReadingPitch = ReadingPitch * GyroFactor; |
| else ReadingPitch = IntegralPitch * IntegralFactor + ReadingPitch * GyroFactor; |
| if(Looping_Roll) ReadingRoll = ReadingRoll * GyroFactor; |
| else ReadingRoll = IntegralRoll * IntegralFactor + ReadingRoll * GyroFactor; |
| ReadingYaw = ReadingYaw * (2 * GyroFactor) + IntegralYaw * IntegralFactor / 2; |
| if(Looping_Pitch) ReadingPitch = ReadingPitch * Gyro_P_Factor; |
| else ReadingPitch = IntegralPitch * Gyro_I_Factor + ReadingPitch * Gyro_P_Factor; |
| if(Looping_Roll) ReadingRoll = ReadingRoll * Gyro_P_Factor; |
| else ReadingRoll = IntegralRoll * Gyro_I_Factor + ReadingRoll * Gyro_P_Factor; |
| ReadingYaw = ReadingYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2; |
| |
| DebugOut.Analog[25] = IntegralRoll * IntegralFactor; |
| DebugOut.Analog[31] = StickRoll;// / (26*IntegralFactor); |
| DebugOut.Analog[25] = IntegralRoll * Gyro_I_Factor; |
| DebugOut.Analog[31] = StickRoll;// / (26*Gyro_I_Factor); |
| DebugOut.Analog[28] = ReadingRoll; |
| |
| // Maximalwerte abfangen |
| 1111,7 → 1148,7 |
|---|
| // Pitch-Achse |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| DiffPitch = ReadingPitch - (StickPitch - GPS_Pitch); // Differenz bestimmen |
| if(IntegralFactor) SumPitch += IntegralPitch * IntegralFactor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung |
| if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung |
| else SumPitch += DiffPitch; // I-Anteil bei HH |
| if(SumPitch > 16000) SumPitch = 16000; |
| if(SumPitch < -16000) SumPitch = -16000; |
| 1136,7 → 1173,7 |
|---|
| // Roll-Achse |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| DiffRoll = ReadingRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
| if(IntegralFactor) SumRoll += IntegralRoll * IntegralFactor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
| if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
| else SumRoll += DiffRoll; // I-Anteil bei HH |
| if(SumRoll > 16000) SumRoll = 16000; |
| if(SumRoll < -16000) SumRoll = -16000; |