| 66,84 → 66,97 |
|---|
| #include "twimaster.h" |
| #include "mm3.h" |
| |
| unsigned char h,m,s; |
| |
| volatile unsigned int I2CTimeout = 100; |
| volatile int ReadingPitch, ReadingRoll, ReadingYaw; |
| volatile int AdNeutralPitch = 0,AdNeutralRoll = 0,AdNeutralYaw = 0, StartNeutralRoll = 0, StartNeutralPitch = 0; |
| volatile int Mean_AccPitch, Mean_AccRoll, Mean_AccTop, NeutralAccX=0, NeutralAccY=0; |
| // gyro readings |
| volatile int16_t ReadingPitch, ReadingRoll, ReadingYaw; |
| // gyro neutral readings |
| volatile int16_t AdNeutralPitch = 0, AdNeutralRoll = 0, AdNeutralYaw = 0; |
| volatile int16_t StartNeutralRoll = 0, StartNeutralPitch = 0; |
| // mean accelerations |
| volatile int16_t Mean_AccPitch, Mean_AccRoll, Mean_AccTop; |
| |
| // neutral acceleration readings |
| volatile int16_t NeutralAccX=0, NeutralAccY=0; |
| volatile float NeutralAccZ = 0; |
| volatile long IntegralPitch = 0,IntegralPitch2 = 0; |
| volatile long IntegralRoll = 0,IntegralRoll2 = 0; |
| volatile long IntegralAccPitch = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
| volatile long Integral_Yaw = 0; |
| volatile long Reading_IntegralPitch = 0,Reading_IntegralPitch2 = 0; |
| volatile long Reading_IntegralRoll = 0,Reading_IntegralRoll2 = 0; |
| volatile long Reading_Integral_Yaw = 0,Reading_Integral_Yaw2 = 0; |
| volatile long MeanIntegralPitch, MeanIntegralRoll, MeanIntegralPitch2, MeanIntegralRoll2; |
| volatile long Reading_Integral_Top = 0; |
| volatile int CompassHeading = 0; |
| volatile int CompassCourse = 0; |
| volatile int CompassOffCourse = 0; |
| unsigned char MAX_GAS,MIN_GAS; |
| unsigned char EmergencyLanding = 0; |
| unsigned char HightControlActive = 0; |
| long TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L; |
| |
| // attitude gyro integrals |
| volatile int32_t IntegralPitch = 0,IntegralPitch2 = 0; |
| volatile int32_t IntegralRoll = 0,IntegralRoll2 = 0; |
| volatile int32_t IntegralYaw = 0; |
| volatile int32_t Reading_IntegralPitch = 0, Reading_IntegralPitch2 = 0; |
| volatile int32_t Reading_IntegralRoll = 0, Reading_IntegralRoll2 = 0; |
| volatile int32_t Reading_IntegralYaw = 0, Reading_IntegralYaw2 = 0; |
| volatile int32_t MeanIntegralPitch, MeanIntegralPitch2; |
| volatile int32_t MeanIntegralRoll, MeanIntegralRoll2; |
| |
| // attitude acceleration integrals |
| volatile int32_t IntegralAccPitch = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
| volatile int32_t Reading_Integral_Top = 0; |
| |
| // compass course |
| volatile int16_t CompassHeading = 0; |
| volatile int16_t CompassCourse = 0; |
| 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; |
| volatile int DiffPitch,DiffRoll; |
| int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
| volatile unsigned char Motor_Front, Motor_Rear, Motor_Right, Motor_Left, Count; |
| unsigned char MotorValue[5]; |
| |
| volatile int16_t DiffPitch, DiffRoll; |
| |
| int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
| |
| // setpoints for motors |
| volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; |
| |
| // stick values derived by rc channels readings |
| int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
| char MotorsOn = 0; |
| int ReadingHight = 0; |
| int SetPointHight = 0; |
| int AttitudeCorrectionRoll = 0, AttitudeCorrectionPitch = 0; |
| float Ki = FAKTOR_I; |
| unsigned char Looping_Pitch = 0, Looping_Roll = 0; |
| unsigned char Looping_Left = 0, Looping_Right = 0, Looping_Down = 0, Looping_Top = 0; |
| // stick values derived by uart inputs |
| int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHightValue = -20; |
| |
| unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
| unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250 |
| unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
| unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
| unsigned char Parameter_CompassYawEffect = 64; // Wert : 0-250 |
| unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
| unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
| unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
| unsigned char Parameter_I_Factor = 10; // Wert : 1-20 |
| unsigned char Parameter_UserParam1 = 0; |
| unsigned char Parameter_UserParam2 = 0; |
| unsigned char Parameter_UserParam3 = 0; |
| unsigned char Parameter_UserParam4 = 0; |
| unsigned char Parameter_UserParam5 = 0; |
| unsigned char Parameter_UserParam6 = 0; |
| unsigned char Parameter_UserParam7 = 0; |
| unsigned char Parameter_UserParam8 = 0; |
| unsigned char Parameter_ServoPitchControl = 100; |
| unsigned char Parameter_LoopGasLimit = 70; |
| unsigned char Parameter_AchsKopplung1 = 0; |
| unsigned char Parameter_AchsGegenKopplung1 = 0; |
| unsigned char Parameter_DynamicStability = 100; |
| |
| signed int ExternStickPitch = 0, ExternStickRoll = 0,ExternStickYaw = 0, ExternHightValue = -20; |
| int MaxStickPitch = 0, MaxStickRoll = 0; |
| |
| void Piep(unsigned char Anzahl) |
| |
| int16_t ReadingHight = 0; |
| int16_t SetPointHight = 0; |
| |
| int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionPitch = 0; |
| |
| float Ki = FACTOR_I; |
| |
| uint8_t Looping_Pitch = 0, Looping_Roll = 0; |
| uint8_t Looping_Left = 0, Looping_Right = 0, Looping_Down = 0, Looping_Top = 0; |
| |
| |
| fc_param_t FCParam = {48,251,16,58,64,150,150,2,10,0,0,0,0,0,0,0,0,100,70,0,0,100}; |
| |
| |
| /************************************************************************/ |
| /* Creates numbeeps beeps at the speaker */ |
| /************************************************************************/ |
| void Beep(uint8_t numbeeps) |
| { |
| while(Anzahl--) |
| { |
| if(MotorsOn) return; //auf keinen Fall im Flug! |
| BeepTime = 100; |
| Delay_ms(250); |
| } |
| while(numbeeps--) |
| { |
| if(MotorsOn) return; //auf keinen Fall im Flug! |
| BeepTime = 100; // 0.1 second |
| Delay_ms(250); // blocks 250 ms as pause to next beep, |
| // this will block the flight control loop, |
| // therefore do not use this funktion if motors are running |
| } |
| } |
| |
| //############################################################################ |
| // Nullwerte ermitteln |
| /************************************************************************/ |
| /* Neutral Readings */ |
| /************************************************************************/ |
| void SetNeutral(void) |
| //############################################################################ |
| { |
| NeutralAccX = 0; |
| NeutralAccY = 0; |
| 151,165 → 164,169 |
|---|
| AdNeutralPitch = 0; |
| AdNeutralRoll = 0; |
| AdNeutralYaw = 0; |
| Parameter_AchsKopplung1 = 0; |
| Parameter_AchsGegenKopplung1 = 0; |
| FCParam.AchsKopplung1 = 0; |
| FCParam.AchsGegenKopplung1 = 0; |
| CalibMean(); |
| Delay_ms_Mess(100); |
| CalibMean(); |
| if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung aktiviert? |
| { |
| if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
| } |
| |
| AdNeutralPitch= AdValueGyrPitch; |
| AdNeutralRoll= AdValueGyrRoll; |
| AdNeutralYaw= AdValueGyrYaw; |
| StartNeutralRoll = AdNeutralRoll; |
| StartNeutralPitch = AdNeutralPitch; |
| if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Hight Control activated? |
| { |
| if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
| } |
| AdNeutralPitch = AdValueGyrPitch; |
| AdNeutralRoll = AdValueGyrRoll; |
| AdNeutralYaw = AdValueGyrYaw; |
| StartNeutralRoll = AdNeutralRoll; |
| StartNeutralPitch = AdNeutralPitch; |
| if(GetParamByte(PID_ACC_PITCH) > 4) |
| { |
| NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY; |
| NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY; |
| NeutralAccZ = Current_AccZ; |
| NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY; |
| NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY; |
| NeutralAccZ = Current_AccZ; |
| } |
| else |
| { // why not GetParamWord()? |
| NeutralAccX = (int)GetParamByte(PID_ACC_PITCH) * 256 + (int)GetParamByte(PID_ACC_PITCH+1); |
| NeutralAccY = (int)GetParamByte(PID_ACC_ROLL) * 256 + (int)GetParamByte(PID_ACC_ROLL+1); |
| NeutralAccZ = (int)GetParamByte(PID_ACC_Z) * 256 + (int)GetParamByte(PID_ACC_Z+1); |
| { |
| NeutralAccX = (int16_t)GetParamWord(PID_ACC_PITCH); |
| NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
| NeutralAccZ = (int16_t)GetParamWord(PID_ACC_Z); |
| } |
| |
| Reading_IntegralPitch = 0; |
| Reading_IntegralPitch2 = 0; |
| Reading_IntegralRoll = 0; |
| Reading_IntegralRoll2 = 0; |
| Reading_Integral_Yaw = 0; |
| Reading_IntegralYaw = 0; |
| ReadingPitch = 0; |
| ReadingRoll = 0; |
| ReadingYaw = 0; |
| StartAirPressure = AirPressure; |
| HoeheD = 0; |
| HightD = 0; |
| Reading_Integral_Top = 0; |
| CompassCourse = CompassHeading; |
| GPS_Neutral(); |
| BeepTime = 50; |
| TurnOver180Pitch = (long) ParamSet.AngleTurnOverPitch * 2500L; |
| TurnOver180Roll = (long) ParamSet.AngleTurnOverRoll * 2500L; |
| TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
| TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
| ExternHightValue = 0; |
| } |
| |
| //############################################################################ |
| // Bearbeitet die Messwerte |
| /************************************************************************/ |
| /* Averaging Measurement Readings */ |
| /************************************************************************/ |
| void Mean(void) |
| //############################################################################ |
| { |
| static int32_t tmpl,tmpl2; |
| ReadingYaw = (int16_t) AdNeutralYaw - AdValueGyrYaw; |
| ReadingRoll = (int16_t) AdValueGyrRoll - AdNeutralRoll; |
| ReadingPitch = (int16_t) AdValueGyrPitch - AdNeutralPitch; |
| |
| //DebugOut.Analog[26] = ReadingPitch; |
| DebugOut.Analog[28] = ReadingRoll; |
| // Get offset corrected gyro readings |
| ReadingYaw = AdNeutralYaw - AdValueGyrYaw; |
| ReadingRoll = AdValueGyrRoll - AdNeutralRoll; |
| ReadingPitch = AdValueGyrPitch - AdNeutralPitch; |
| |
| // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
| Mean_AccPitch = ((long)Mean_AccPitch * 1 + ((ACC_AMPLIFY * (long)AdValueAccPitch))) / 2L; |
| Mean_AccRoll = ((long)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdValueAccRoll))) / 2L; |
| Mean_AccTop = ((long)Mean_AccTop * 1 + ((long)AdValueAccTop)) / 2L; |
| DebugOut.Analog[26] = ReadingPitch; |
| DebugOut.Analog[28] = ReadingRoll; |
| |
| // Acceleration Sensor |
| Mean_AccPitch = ((int32_t)Mean_AccPitch * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccPitch))) / 2L; |
| Mean_AccRoll = ((int32_t)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 2L; |
| Mean_AccTop = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L; |
| |
| IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch; |
| IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
| IntegralAccZ += Current_AccZ - NeutralAccZ; |
| // Yaw ++++++++++++++++++++++++++++++++++++++++++++++++ |
| Reading_Integral_Yaw += ReadingYaw; |
| Reading_Integral_Yaw2 += ReadingYaw; |
| // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
| if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
| { |
| tmpl = Reading_IntegralPitch / 4096L; |
| tmpl *= ReadingYaw; |
| tmpl *= Parameter_AchsKopplung1; //125 |
| tmpl /= 2048L; |
| tmpl2 = Reading_IntegralRoll / 4096L; |
| tmpl2 *= ReadingYaw; |
| tmpl2 *= Parameter_AchsKopplung1; |
| tmpl2 /= 2048L; |
| } |
| else tmpl = tmpl2 = 0; |
| // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
| ReadingRoll += tmpl; |
| ReadingRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109 |
| Reading_IntegralRoll2 += ReadingRoll; |
| Reading_IntegralRoll += ReadingRoll - AttitudeCorrectionRoll; |
| if(Reading_IntegralRoll > TurnOver180Roll) |
| { |
| Reading_IntegralRoll = -(TurnOver180Roll - 10000L); |
| Reading_IntegralRoll2 = Reading_IntegralRoll; |
| } |
| if(Reading_IntegralRoll <-TurnOver180Roll) |
| { |
| Reading_IntegralRoll = (TurnOver180Roll - 10000L); |
| Reading_IntegralRoll2 = Reading_IntegralRoll; |
| } |
| if(AdValueGyrRoll < 15) ReadingRoll = -1000; |
| if(AdValueGyrRoll < 7) ReadingRoll = -2000; |
| if(BoardRelease == 10) |
| { |
| if(AdValueGyrRoll > 1010) ReadingRoll = +1000; |
| if(AdValueGyrRoll > 1017) ReadingRoll = +2000; |
| } |
| else |
| { |
| if(AdValueGyrRoll > 2020) ReadingRoll = +1000; |
| if(AdValueGyrRoll > 2034) ReadingRoll = +2000; |
| } |
| // Pitch ++++++++++++++++++++++++++++++++++++++++++++++++ |
| ReadingPitch -= tmpl2; |
| ReadingPitch -= (tmpl*Parameter_AchsGegenKopplung1)/512L; |
| Reading_IntegralPitch2 += ReadingPitch; |
| Reading_IntegralPitch += ReadingPitch - AttitudeCorrectionPitch; |
| if(Reading_IntegralPitch > TurnOver180Pitch) |
| { |
| Reading_IntegralPitch = -(TurnOver180Pitch - 10000L); |
| Reading_IntegralPitch2 = Reading_IntegralPitch; |
| } |
| if(Reading_IntegralPitch <-TurnOver180Pitch) |
| { |
| Reading_IntegralPitch = (TurnOver180Pitch - 10000L); |
| Reading_IntegralPitch2 = Reading_IntegralPitch; |
| } |
| if(AdValueGyrPitch < 15) ReadingPitch = -1000; |
| if(AdValueGyrPitch < 7) ReadingPitch = -2000; |
| if(BoardRelease == 10) |
| { |
| if(AdValueGyrPitch > 1010) ReadingPitch = +1000; |
| if(AdValueGyrPitch > 1017) ReadingPitch = +2000; |
| } |
| else |
| { |
| if(AdValueGyrPitch > 2020) ReadingPitch = +1000; |
| if(AdValueGyrPitch > 2034) ReadingPitch = +2000; |
| } |
| //++++++++++++++++++++++++++++++++++++++++++++++++ |
| // ADC einschalten |
| IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
| IntegralAccZ += Current_AccZ - NeutralAccZ; |
| |
| // Yaw |
| Reading_IntegralYaw += ReadingYaw; |
| Reading_IntegralYaw2 += ReadingYaw; |
| |
| // Coupling fraction |
| if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
| { |
| tmpl = Reading_IntegralPitch / 4096L; |
| tmpl *= ReadingYaw; |
| tmpl *= FCParam.AchsKopplung1; //125 |
| tmpl /= 2048L; |
| tmpl2 = Reading_IntegralRoll / 4096L; |
| tmpl2 *= ReadingYaw; |
| tmpl2 *= FCParam.AchsKopplung1; |
| tmpl2 /= 2048L; |
| } |
| else tmpl = tmpl2 = 0; |
| // Roll |
| ReadingRoll += tmpl; |
| ReadingRoll += (tmpl2 * FCParam.AchsGegenKopplung1) / 512L; //109 |
| Reading_IntegralRoll2 += ReadingRoll; |
| Reading_IntegralRoll += ReadingRoll - AttitudeCorrectionRoll; |
| if(Reading_IntegralRoll > TurnOver180Roll) |
| { |
| Reading_IntegralRoll = -(TurnOver180Roll - 10000L); |
| Reading_IntegralRoll2 = Reading_IntegralRoll; |
| } |
| if(Reading_IntegralRoll < -TurnOver180Roll) |
| { |
| Reading_IntegralRoll = (TurnOver180Roll - 10000L); |
| Reading_IntegralRoll2 = Reading_IntegralRoll; |
| } |
| if(AdValueGyrRoll < 15) ReadingRoll = -1000; |
| if(AdValueGyrRoll < 7) ReadingRoll = -2000; |
| if(BoardRelease == 10) |
| { |
| if(AdValueGyrRoll > 1010) ReadingRoll = +1000; |
| if(AdValueGyrRoll > 1017) ReadingRoll = +2000; |
| } |
| else |
| { |
| if(AdValueGyrRoll > 2020) ReadingRoll = +1000; |
| if(AdValueGyrRoll > 2034) ReadingRoll = +2000; |
| } |
| // Pitch |
| ReadingPitch -= tmpl2; |
| ReadingPitch -= (tmpl*FCParam.AchsGegenKopplung1) / 512L; |
| Reading_IntegralPitch2 += ReadingPitch; |
| Reading_IntegralPitch += ReadingPitch - AttitudeCorrectionPitch; |
| if(Reading_IntegralPitch > TurnOver180Pitch) |
| { |
| Reading_IntegralPitch = -(TurnOver180Pitch - 10000L); |
| Reading_IntegralPitch2 = Reading_IntegralPitch; |
| } |
| if(Reading_IntegralPitch < -TurnOver180Pitch) |
| { |
| Reading_IntegralPitch = (TurnOver180Pitch - 10000L); |
| Reading_IntegralPitch2 = Reading_IntegralPitch; |
| } |
| if(AdValueGyrPitch < 15) ReadingPitch = -1000; |
| if(AdValueGyrPitch < 7) ReadingPitch = -2000; |
| if(BoardRelease == 10) |
| { |
| if(AdValueGyrPitch > 1010) ReadingPitch = +1000; |
| if(AdValueGyrPitch > 1017) ReadingPitch = +2000; |
| } |
| else |
| { |
| if(AdValueGyrPitch > 2020) ReadingPitch = +1000; |
| if(AdValueGyrPitch > 2034) ReadingPitch = +2000; |
| } |
| |
| // start ADC |
| ADC_Enable(); |
| //++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| Integral_Yaw = Reading_Integral_Yaw; |
| IntegralYaw = Reading_IntegralYaw; |
| IntegralPitch = Reading_IntegralPitch; |
| IntegralRoll = Reading_IntegralRoll; |
| IntegralPitch2 = Reading_IntegralPitch2; |
| IntegralRoll2 = Reading_IntegralRoll2; |
| |
| if(ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER && !Looping_Pitch && !Looping_Roll) |
| { |
| if(ReadingPitch > 200) ReadingPitch += 4 * (ReadingPitch - 200); |
| else if(ReadingPitch < -200) ReadingPitch += 4 * (ReadingPitch + 200); |
| if(ReadingRoll > 200) ReadingRoll += 4 * (ReadingRoll - 200); |
| else if(ReadingRoll < -200) ReadingRoll += 4 * (ReadingRoll + 200); |
| } |
| if(ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER && !Looping_Pitch && !Looping_Roll) |
| { |
| if(ReadingPitch > 200) ReadingPitch += 4 * (ReadingPitch - 200); |
| else if(ReadingPitch < -200) ReadingPitch += 4 * (ReadingPitch + 200); |
| if(ReadingRoll > 200) ReadingRoll += 4 * (ReadingRoll - 200); |
| else if(ReadingRoll < -200) ReadingRoll += 4 * (ReadingRoll + 200); |
| } |
| //update poti values by rc-signals (why not +127?) |
| if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
| if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
| if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
| if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
| //limit poti values |
| if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
| if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
| if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
| 316,41 → 333,45 |
|---|
| if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
| } |
| |
| //############################################################################ |
| // Messwerte beim Ermitteln der Nullage |
| /************************************************************************/ |
| /* Averaging Measurement Readings for Calibration */ |
| /************************************************************************/ |
| void CalibMean(void) |
| //############################################################################ |
| { |
| // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
| // stop ADC to avoid changing values during calculation |
| ADC_Disable(); |
| |
| ReadingPitch = AdValueGyrPitch; |
| ReadingRoll = AdValueGyrRoll; |
| ReadingYaw = AdValueGyrYaw; |
| Mean_AccPitch = ACC_AMPLIFY * (long)AdValueAccPitch; |
| Mean_AccRoll = ACC_AMPLIFY * (long)AdValueAccRoll; |
| Mean_AccTop = (long)AdValueAccTop; |
| // ADC einschalten |
| |
| Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch; |
| Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
| Mean_AccTop = (int32_t)AdValueAccTop; |
| // start ADC |
| ADC_Enable(); |
| //update poti values by rc-signals (why not +127?) |
| if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
| if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
| if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
| if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
| //limit poti values |
| if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
| if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
| if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
| if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
| |
| TurnOver180Pitch = (long) ParamSet.AngleTurnOverPitch * 2500L; |
| TurnOver180Roll = (long) ParamSet.AngleTurnOverRoll * 2500L; |
| TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
| TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
| } |
| |
| //############################################################################ |
| // Senden der Motorwerte per I2C-Bus |
| /************************************************************************/ |
| /* Transmit Motor Data via I2C */ |
| /************************************************************************/ |
| void SendMotorData(void) |
| //############################################################################ |
| { |
| if(MOTOR_OFF || !MotorsOn) |
| { |
| { |
| Motor_Rear = 0; |
| Motor_Front = 0; |
| Motor_Right = 0; |
| 359,7 → 380,7 |
|---|
| if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
| if(MotorTest[2]) Motor_Left = MotorTest[2]; |
| if(MotorTest[3]) Motor_Right = MotorTest[3]; |
| } |
| } |
| |
| DebugOut.Analog[12] = Motor_Front; |
| DebugOut.Analog[13] = Motor_Rear; |
| 374,38 → 395,36 |
|---|
| |
| |
| |
| //############################################################################ |
| // Trägt ggf. das Poti als Parameter ein |
| void ParameterZuordnung(void) |
| //############################################################################ |
| /************************************************************************/ |
| /* Maps the parameter to poti values */ |
| /************************************************************************/ |
| void ParameterMapping(void) |
| { |
| |
| #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;} |
| CHK_POTI(Parameter_MaxHoehe,ParamSet.MaxHight,0,255); |
| CHK_POTI(Parameter_Luftdruck_D,ParamSet.AirPressure_D,0,100); |
| CHK_POTI(Parameter_Hoehe_P,ParamSet.Hight_P,0,100); |
| CHK_POTI(Parameter_Hoehe_ACC_Wirkung,ParamSet.Hight_ACC_Effect,0,255); |
| CHK_POTI(Parameter_CompassYawEffect,ParamSet.CompassYawEffect,0,255); |
| CHK_POTI(Parameter_Gyro_P,ParamSet.Gyro_P,10,255); |
| CHK_POTI(Parameter_Gyro_I,ParamSet.Gyro_I,0,255); |
| CHK_POTI(Parameter_I_Factor,ParamSet.I_Factor,0,255); |
| CHK_POTI(Parameter_UserParam1,ParamSet.UserParam1,0,255); |
| CHK_POTI(Parameter_UserParam2,ParamSet.UserParam2,0,255); |
| CHK_POTI(Parameter_UserParam3,ParamSet.UserParam3,0,255); |
| CHK_POTI(Parameter_UserParam4,ParamSet.UserParam4,0,255); |
| CHK_POTI(Parameter_UserParam5,ParamSet.UserParam5,0,255); |
| CHK_POTI(Parameter_UserParam6,ParamSet.UserParam6,0,255); |
| CHK_POTI(Parameter_UserParam7,ParamSet.UserParam7,0,255); |
| CHK_POTI(Parameter_UserParam8,ParamSet.UserParam8,0,255); |
| CHK_POTI(Parameter_ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
| CHK_POTI(Parameter_LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
| CHK_POTI(Parameter_AchsKopplung1, ParamSet.AchsKopplung1,0,255); |
| CHK_POTI(Parameter_AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255); |
| CHK_POTI(Parameter_DynamicStability,ParamSet.DynamicStability,0,255); |
| CHK_POTI(FCParam.MaxHight,ParamSet.MaxHight,0,255); |
| CHK_POTI(FCParam.AirPressure_D,ParamSet.AirPressure_D,0,100); |
| CHK_POTI(FCParam.Hight_P,ParamSet.Hight_P,0,100); |
| CHK_POTI(FCParam.Hight_ACC_Effect,ParamSet.Hight_ACC_Effect,0,255); |
| CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255); |
| CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255); |
| CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255); |
| CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255); |
| CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1,0,255); |
| CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2,0,255); |
| CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3,0,255); |
| CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255); |
| CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255); |
| CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255); |
| CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255); |
| 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.DynamicStability,ParamSet.DynamicStability,0,255); |
| |
| Ki = (float) Parameter_I_Factor * 0.0001; |
| MAX_GAS = ParamSet.Gas_Max; |
| MIN_GAS = ParamSet.Gas_Min; |
| Ki = (float) FCParam.I_Factor * FACTOR_I; |
| } |
| |
| |
| 483,8 → 502,8 |
|---|
| { |
| SumPitch = 0; |
| SumRoll = 0; |
| Reading_Integral_Yaw = 0; |
| Reading_Integral_Yaw2 = 0; |
| Reading_IntegralYaw = 0; |
| Reading_IntegralYaw2 = 0; |
| } |
| if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0) |
| { |
| 515,7 → 534,7 |
|---|
| } |
| ParamSet_ReadFromEEProm(GetActiveParamSet()); |
| SetNeutral(); |
| Piep(GetActiveParamSet()); |
| Beep(GetActiveParamSet()); |
| } |
| } |
| else |
| 524,18 → 543,16 |
|---|
| if(++delay_neutral > 200) // nicht sofort |
| { |
| GRN_OFF; |
| SetParamByte(PID_ACC_PITCH,0xFF); // Werte löschen |
| SetParamWord(PID_ACC_PITCH,0xFFFF); // Werte löschen |
| MotorsOn = 0; |
| delay_neutral = 0; |
| modell_fliegt = 0; |
| SetNeutral(); |
| SetParamByte(PID_ACC_PITCH, NeutralAccX / 256); // ACC-NeutralWerte speichern |
| SetParamByte(PID_ACC_PITCH+1,NeutralAccX % 256); // ACC-NeutralWerte speichern |
| SetParamByte(PID_ACC_ROLL,NeutralAccY / 256); |
| SetParamByte(PID_ACC_ROLL+1,NeutralAccY % 256); |
| SetParamByte(PID_ACC_Z,(int)NeutralAccZ / 256); |
| SetParamByte(PID_ACC_Z+1,(int)NeutralAccZ % 256); |
| Piep(GetActiveParamSet()); |
| // 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; |
| 557,8 → 574,8 |
|---|
| modell_fliegt = 1; |
| MotorsOn = 1; |
| SetPointYaw = 0; |
| Reading_Integral_Yaw = 0; |
| Reading_Integral_Yaw2 = 0; |
| Reading_IntegralYaw = 0; |
| Reading_IntegralYaw2 = 0; |
| Reading_IntegralPitch = 0; |
| Reading_IntegralRoll = 0; |
| Reading_IntegralPitch2 = IntegralPitch; |
| 591,7 → 608,7 |
|---|
| if(!NewPpmData-- || EmergencyLanding) |
| { |
| int tmp_int; |
| ParameterZuordnung(); |
| 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; |
| 606,13 → 623,13 |
|---|
| MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); else MaxStickRoll--; |
| if(EmergencyLanding) {MaxStickPitch = 0; MaxStickRoll = 0;} |
| |
| GyroFactor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
| IntegralFactor = ((float) Parameter_Gyro_I) / 44000; |
| GyroFactor = ((float)FCParam.Gyro_P + 10.0) / 256.0; |
| IntegralFactor = ((float) FCParam.Gyro_I) / 44000; |
| |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //+ Digitale Steuerung per DubWise |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8) |
| #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; |
| 632,7 → 649,7 |
|---|
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //+ Analoge Steuerung per Seriell |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128) |
| if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
| { |
| StickPitch += (int) ExternControl.Pitch * (int) ParamSet.Stick_P; |
| StickRoll += (int) ExternControl.Roll * (int) ParamSet.Stick_P; |
| 924,9 → 941,9 |
|---|
| tmp_int = (long) ParamSet.Yaw_P * ((long)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
| tmp_int += (ParamSet.Yaw_P * StickYaw) / 4; |
| SetPointYaw = tmp_int; |
| Reading_Integral_Yaw -= tmp_int; |
| if(Reading_Integral_Yaw > 50000) Reading_Integral_Yaw = 50000; // begrenzen |
| if(Reading_Integral_Yaw <-50000) Reading_Integral_Yaw =-50000; |
| Reading_IntegralYaw -= tmp_int; |
| if(Reading_IntegralYaw > 50000) Reading_IntegralYaw = 50000; // begrenzen |
| if(Reading_IntegralYaw <-50000) Reading_IntegralYaw =-50000; |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Kompass |
| 951,11 → 968,11 |
|---|
| CompassCourse = CompassHeading; |
| StoreNewCompassCourse = 0; |
| } |
| w = (w * Parameter_CompassYawEffect) / 64; // auf die Wirkung normieren |
| w = Parameter_CompassYawEffect - w; // Wirkung ggf drosseln |
| w = (w * FCParam.CompassYawEffect) / 64; // auf die Wirkung normieren |
| w = FCParam.CompassYawEffect - w; // Wirkung ggf drosseln |
| if(w > 0) |
| { |
| Reading_Integral_Yaw -= (CompassOffCourse * w) / 32; // nach Kompass ausrichten |
| Reading_IntegralYaw -= (CompassOffCourse * w) / 32; // nach Kompass ausrichten |
| } |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 992,13 → 1009,13 |
|---|
| */ |
| // DebugOut.Analog[9] = ReadingPitch; |
| // DebugOut.Analog[9] = SetPointHight; |
| // DebugOut.Analog[10] = Reading_Integral_Yaw / 128; |
| // DebugOut.Analog[10] = Reading_IntegralYaw / 128; |
| // DebugOut.Analog[11] = CompassCourse; |
| // DebugOut.Analog[10] = Parameter_Gyro_I; |
| // DebugOut.Analog[10] = FCParam.Gyro_I; |
| // DebugOut.Analog[10] = ParamSet.Gyro_I; |
| // DebugOut.Analog[9] = CompassOffCourse; |
| // DebugOut.Analog[10] = GasMixingFraction; |
| // DebugOut.Analog[3] = HoeheD * 32; |
| // DebugOut.Analog[3] = HightD * 32; |
| // DebugOut.Analog[4] = hoehenregler; |
| } |
| |
| 1012,7 → 1029,7 |
|---|
| else ReadingPitch = IntegralPitch * IntegralFactor + ReadingPitch * GyroFactor; |
| if(Looping_Roll) ReadingRoll = ReadingRoll * GyroFactor; |
| else ReadingRoll = IntegralRoll * IntegralFactor + ReadingRoll * GyroFactor; |
| ReadingYaw = ReadingYaw * (2 * GyroFactor) + Integral_Yaw * IntegralFactor / 2; |
| ReadingYaw = ReadingYaw * (2 * GyroFactor) + IntegralYaw * IntegralFactor / 2; |
| |
| DebugOut.Analog[25] = IntegralRoll * IntegralFactor; |
| DebugOut.Analog[31] = StickRoll;// / (26*IntegralFactor); |
| 1038,9 → 1055,9 |
|---|
| int tmp_int; |
| if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) // Regler wird über Schalter gesteuert |
| { |
| if(Parameter_MaxHoehe < 50) |
| if(FCParam.MaxHight < 50) |
| { |
| SetPointHight = ReadingHight - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
| SetPointHight = ReadingHight - 20; // FCParam.MaxHight ist der PPM-Wert des Schalters |
| HightControlActive = 0; |
| } |
| else |
| 1048,7 → 1065,7 |
|---|
| } |
| else |
| { |
| SetPointHight = ((int) ExternHightValue + (int) Parameter_MaxHoehe) * (int)ParamSet.Hight_Gain - 20; |
| SetPointHight = ((int) ExternHightValue + (int) FCParam.MaxHight) * (int)ParamSet.Hight_Gain - 20; |
| HightControlActive = 1; |
| } |
| |
| 1055,10 → 1072,10 |
|---|
| if(EmergencyLanding) SetPointHight = 0; |
| h = ReadingHight; |
| if((h > SetPointHight) && HightControlActive) // zu hoch --> drosseln |
| { h = ((h - SetPointHight) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
| { h = ((h - SetPointHight) * (int) FCParam.Hight_P) / 16; // Differenz bestimmen --> P-Anteil |
| h = GasMixingFraction - h; // vom Gas abziehen |
| h -= (HoeheD * Parameter_Luftdruck_D)/8; // D-Anteil |
| tmp_int = ((Reading_Integral_Top / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32; |
| h -= (HightD * FCParam.AirPressure_D)/8; // D-Anteil |
| tmp_int = ((Reading_Integral_Top / 512) * (signed long) FCParam.Hight_ACC_Effect) / 32; |
| if(tmp_int > 50) tmp_int = 50; |
| else if(tmp_int < -50) tmp_int = -50; |
| h -= tmp_int; |
| 1072,7 → 1089,7 |
|---|
| GasMixingFraction = hoehenregler; |
| } |
| } |
| if(GasMixingFraction > MAX_GAS - 20) GasMixingFraction = MAX_GAS - 20; |
| if(GasMixingFraction > ParamSet.Gas_Max - 20) GasMixingFraction = ParamSet.Gas_Max - 20; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Mischer und PI-Regler |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1086,8 → 1103,8 |
|---|
| |
| if(YawMixingFraction > (GasMixingFraction / 2)) YawMixingFraction = GasMixingFraction / 2; |
| if(YawMixingFraction < -(GasMixingFraction / 2)) YawMixingFraction = -(GasMixingFraction / 2); |
| if(YawMixingFraction > ((MAX_GAS - GasMixingFraction))) YawMixingFraction = ((MAX_GAS - GasMixingFraction)); |
| if(YawMixingFraction < -((MAX_GAS - GasMixingFraction))) YawMixingFraction = -((MAX_GAS - GasMixingFraction)); |
| if(YawMixingFraction > ((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = ((ParamSet.Gas_Max - GasMixingFraction)); |
| if(YawMixingFraction < -((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = -((ParamSet.Gas_Max - GasMixingFraction)); |
| |
| if(GasMixingFraction < 20) YawMixingFraction = 0; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1100,20 → 1117,20 |
|---|
| if(SumPitch < -16000) SumPitch = -16000; |
| pd_ergebnis = DiffPitch + Ki * SumPitch; // PI-Regler für Pitch |
| // Motor Vorn |
| tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
| tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
| if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
| if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
| |
| MotorValue = GasMixingFraction + pd_ergebnis + YawMixingFraction; // Mischer |
| if ((MotorValue < 0)) MotorValue = 0; |
| else if(MotorValue > MAX_GAS) MotorValue = MAX_GAS; |
| if (MotorValue < MIN_GAS) MotorValue = MIN_GAS; |
| else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
| if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
| Motor_Front = MotorValue; |
| // Motor Heck |
| MotorValue = GasMixingFraction - pd_ergebnis + YawMixingFraction; |
| if ((MotorValue < 0)) MotorValue = 0; |
| else if(MotorValue > MAX_GAS) MotorValue = MAX_GAS; |
| if (MotorValue < MIN_GAS) MotorValue = MIN_GAS; |
| else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
| if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
| Motor_Rear = MotorValue; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Roll-Achse |
| 1124,7 → 1141,7 |
|---|
| if(SumRoll > 16000) SumRoll = 16000; |
| if(SumRoll < -16000) SumRoll = -16000; |
| pd_ergebnis = DiffRoll + Ki * SumRoll; // PI-Regler für Roll |
| tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
| tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
| if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
| if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
| // Motor Links |
| 1132,15 → 1149,15 |
|---|
| #define GRENZE Poti1 |
| |
| if ((MotorValue < 0)) MotorValue = 0; |
| else if(MotorValue > MAX_GAS) MotorValue = MAX_GAS; |
| if (MotorValue < MIN_GAS) MotorValue = MIN_GAS; |
| else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
| if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
| Motor_Left = MotorValue; |
| // Motor Rechts |
| MotorValue = GasMixingFraction - pd_ergebnis - YawMixingFraction; |
| |
| if ((MotorValue < 0)) MotorValue = 0; |
| else if(MotorValue > MAX_GAS) MotorValue = MAX_GAS; |
| if (MotorValue < MIN_GAS) MotorValue = MIN_GAS; |
| else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
| if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
| Motor_Right = MotorValue; |
| // +++++++++++++++++++++++++++++++++++++++++++++++ |
| } |