51,8 → 51,9 |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdlib.h> |
#include <avr/io.h> |
|
|
#include "main.h" |
#include "eeprom.h" |
#include "timer0.h" |
126,7 → 127,7 |
unsigned char Parameter_AchsKopplung1 = 0; |
unsigned char Parameter_AchsGegenKopplung1 = 0; |
unsigned char Parameter_DynamicStability = 100; |
struct mk_param_struct EE_Parameter; |
|
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
int MaxStickNick = 0,MaxStickRoll = 0; |
|
156,7 → 157,7 |
CalibrierMittelwert(); |
Delay_ms_Mess(100); |
CalibrierMittelwert(); |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
if((ParamSet.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
166,7 → 167,7 |
AdNeutralGier= AdValueGyrGier; |
StartNeutralRoll = AdNeutralRoll; |
StartNeutralNick = AdNeutralNick; |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) |
if(GetParamByte(PID_ACC_NICK) > 4) |
{ |
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
174,9 → 175,9 |
} |
else |
{ |
NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]); |
NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]); |
NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]); |
NeutralAccX = (int)GetParamByte(PID_ACC_NICK) * 256 + (int)GetParamByte(PID_ACC_NICK+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); |
} |
|
Mess_IntegralNick = 0; |
193,8 → 194,8 |
KompassStartwert = KompassValue; |
GPS_Neutral(); |
BeepTime = 50; |
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
Umschlag180Nick = (long) ParamSet.WinkelUmschlagNick * 2500L; |
Umschlag180Roll = (long) ParamSet.WinkelUmschlagRoll * 2500L; |
ExternHoehenValue = 0; |
} |
|
222,7 → 223,7 |
Mess_Integral_Gier += MesswertGier; |
Mess_Integral_Gier2 += MesswertGier; |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
{ |
tmpl = Mess_IntegralNick / 4096L; |
tmpl *= MesswertGier; |
299,7 → 300,7 |
IntegralNick2 = Mess_IntegralNick2; |
IntegralRoll2 = Mess_IntegralRoll2; |
|
if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
if(ParamSet.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
{ |
if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
306,10 → 307,10 |
if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
} |
if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
if(Poti1 < PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
if(Poti4 < PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
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; |
331,17 → 332,17 |
Mittelwert_AccHoch = (long)AdValueAccTop; |
// ADC einschalten |
ADC_Enable(); |
if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
if(Poti1 < PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
if(Poti4 < PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
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; |
|
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
Umschlag180Nick = (long) ParamSet.WinkelUmschlagNick * 2500L; |
Umschlag180Roll = (long) ParamSet.WinkelUmschlagNick * 2500L; |
} |
|
//############################################################################ |
381,31 → 382,31 |
{ |
|
#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,EE_Parameter.MaxHoehe,0,255); |
CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100); |
CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255); |
CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255); |
CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255); |
CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255); |
CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255); |
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
CHK_POTI(Parameter_MaxHoehe,ParamSet.MaxHoehe,0,255); |
CHK_POTI(Parameter_Luftdruck_D,ParamSet.Luftdruck_D,0,100); |
CHK_POTI(Parameter_Hoehe_P,ParamSet.Hoehe_P,0,100); |
CHK_POTI(Parameter_Hoehe_ACC_Wirkung,ParamSet.Hoehe_ACC_Wirkung,0,255); |
CHK_POTI(Parameter_KompassWirkung,ParamSet.KompassWirkung,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_Faktor,ParamSet.I_Faktor,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_ServoNickControl,ParamSet.ServoNickControl,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); |
|
Ki = (float) Parameter_I_Faktor * 0.0001; |
MAX_GAS = EE_Parameter.Gas_Max; |
MIN_GAS = EE_Parameter.Gas_Min; |
MAX_GAS = ParamSet.Gas_Max; |
MIN_GAS = ParamSet.Gas_Min; |
} |
|
|
459,11 → 460,11 |
ROT_ON; |
if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
{ |
GasMischanteil = EE_Parameter.NotGas; |
GasMischanteil = ParamSet.NotGas; |
Notlandung = 1; |
PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
PPM_in[ParamSet.Kanalbelegung[K_NICK]] = 0; |
PPM_in[ParamSet.Kanalbelegung[K_ROLL]] = 0; |
PPM_in[ParamSet.Kanalbelegung[K_GIER]] = 0; |
} |
else MotorenEin = 0; |
} |
474,7 → 475,7 |
if(SenderOkay > 140) |
{ |
Notlandung = 0; |
RcLostTimer = EE_Parameter.NotGasZeit * 50; |
RcLostTimer = ParamSet.NotGasZeit * 50; |
if(GasMischanteil > 40) |
{ |
if(modell_fliegt < 0xffff) modell_fliegt++; |
486,12 → 487,12 |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier2 = 0; |
} |
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
if((PPM_in[ParamSet.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// auf Nullwerte kalibrieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
{ |
if(++delay_neutral > 200) // nicht sofort |
{ |
499,43 → 500,43 |
MotorenEin = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) |
if(PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[ParamSet.Kanalbelegung[K_ROLL]]) > 70) |
{ |
unsigned char setting=1; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > 70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] < 70) setting = 1; |
if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > 70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70) setting = 2; |
if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] < 70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70) setting = 3; |
if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] <-70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] > 70) setting = 4; |
if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] <-70 && PPM_in[ParamSet.Kanalbelegung[K_NICK]] < 70) setting = 5; |
SetActiveParamSet(setting); // aktiven Datensatz merken |
} |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
if((ParamSet.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(); |
Piep(GetActiveParamSetNumber()); |
Piep(GetActiveParamSet()); |
} |
} |
else |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
{ |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen |
SetParamByte(PID_ACC_NICK,0xFF); // Werte löschen |
MotorenEin = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
SetNeutral(); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256); |
Piep(GetActiveParamSetNumber()); |
SetParamByte(PID_ACC_NICK, NeutralAccX / 256); // ACC-NeutralWerte speichern |
SetParamByte(PID_ACC_NICK+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()); |
} |
} |
else delay_neutral = 0; |
543,10 → 544,10 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gas ist unten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120) |
if(PPM_in[ParamSet.Kanalbelegung[K_GAS]] < 35-120) |
{ |
// Starten |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] < -75) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Einschalten |
572,7 → 573,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Auschalten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
if(PPM_in[ParamSet.Kanalbelegung[K_GIER]] > 75) |
{ |
if(++delay_ausschalten > 200) // nicht sofort |
{ |
593,18 → 594,18 |
int tmp_int; |
static int stick_nick,stick_roll; |
ParameterZuordnung(); |
StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
StickNick = (StickNick * 3 + PPM_in[ParamSet.Kanalbelegung[K_NICK]] * ParamSet.Stick_P) / 4; |
StickNick += PPM_diff[ParamSet.Kanalbelegung[K_NICK]] * ParamSet.Stick_D; |
StickRoll = (StickRoll * 3 + PPM_in[ParamSet.Kanalbelegung[K_ROLL]] * ParamSet.Stick_P) / 4; |
StickRoll += PPM_diff[ParamSet.Kanalbelegung[K_ROLL]] * ParamSet.Stick_D; |
|
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
StickGier = -PPM_in[ParamSet.Kanalbelegung[K_GIER]]; |
StickGas = PPM_in[ParamSet.Kanalbelegung[K_GAS]] + 120; |
|
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick) |
MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--; |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll) |
MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; |
if(abs(PPM_in[ParamSet.Kanalbelegung[K_NICK]]) > MaxStickNick) |
MaxStickNick = abs(PPM_in[ParamSet.Kanalbelegung[K_NICK]]); else MaxStickNick--; |
if(abs(PPM_in[ParamSet.Kanalbelegung[K_ROLL]]) > MaxStickRoll) |
MaxStickRoll = abs(PPM_in[ParamSet.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; |
if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
|
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
635,49 → 636,49 |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128) |
{ |
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; |
StickNick += (int) ExternControl.Nick * (int) ParamSet.Stick_P; |
StickRoll += (int) ExternControl.Roll * (int) ParamSet.Stick_P; |
StickGier += ExternControl.Gier; |
ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
ExternHoehenValue = (int) ExternControl.Hight * (int)ParamSet.Hoehe_Verstaerkung; |
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
} |
|
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
if(GyroFaktor < 0) GyroFaktor = 0; |
if(IntegralFaktor < 0) IntegralFaktor = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
if((PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
else |
{ |
{ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0; |
if((PPM_in[ParamSet.Kanalbelegung[K_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysterese))) Looping_Links = 0; |
} |
} |
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; |
if((PPM_in[ParamSet.Kanalbelegung[K_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; |
else |
{ |
if(Looping_Rechts) // Hysterese |
{ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0; |
if(PPM_in[ParamSet.Kanalbelegung[K_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysterese)) Looping_Rechts = 0; |
} |
} |
|
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1; |
if((PPM_in[ParamSet.Kanalbelegung[K_NICK]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1; |
else |
{ |
if(Looping_Oben) // Hysterese |
{ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0; |
if((PPM_in[ParamSet.Kanalbelegung[K_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysterese))) Looping_Oben = 0; |
} |
} |
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1; |
if((PPM_in[ParamSet.Kanalbelegung[K_NICK]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1; |
else |
{ |
if(Looping_Unten) // Hysterese |
{ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0; |
if(PPM_in[ParamSet.Kanalbelegung[K_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysterese)) Looping_Unten = 0; |
} |
} |
|
688,7 → 689,7 |
if(Looping_Roll) BeepTime = 100; |
if(Looping_Roll || Looping_Nick) |
{ |
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
if(GasMischanteil > ParamSet.LoopGasLimit) GasMischanteil = ParamSet.LoopGasLimit; |
} |
|
|
736,8 → 737,8 |
if(!Looping_Nick && !Looping_Roll) |
{ |
long tmp_long, tmp_long2; |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
tmp_long = (long)(IntegralNick / ParamSet.GyroAccFaktor - (long)Mittelwert_AccNick); |
tmp_long2 = (long)(IntegralRoll / ParamSet.GyroAccFaktor - (long)Mittelwert_AccRoll); |
tmp_long /= 16; |
tmp_long2 /= 16; |
if((MaxStickNick > 15) || (MaxStickRoll > 15)) |
745,7 → 746,7 |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
if(abs(PPM_in[ParamSet.Kanalbelegung[K_GIER]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
771,21 → 772,21 |
{ |
MittelIntegralNick /= ABGLEICH_ANZAHL; |
MittelIntegralRoll /= ABGLEICH_ANZAHL; |
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
IntegralAccNick = (ParamSet.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; |
#define MAX_I 0//(Poti2/10) |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); |
ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
ausgleichNick = IntegralFehlerNick / ParamSet.GyroAccAbgleich; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); |
ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
ausgleichRoll = IntegralFehlerRoll / ParamSet.GyroAccAbgleich; |
|
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
|
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[ParamSet.Kanalbelegung[K_GIER]]) > 25)) |
{ |
LageKorrekturNick /= 2; |
LageKorrekturNick /= 2; |
849,7 → 850,7 |
else last_n_n = 1; |
} else last_n_n = 0; |
} else cnt = 0; |
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
if(cnt > ParamSet.Driftkomp) cnt = ParamSet.Driftkomp; |
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
|
886,7 → 887,7 |
cnt = 0; |
} |
|
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
if(cnt > ParamSet.Driftkomp) cnt = ParamSet.Driftkomp; |
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
DebugOut.Analog[27] = ausgleichRoll; |
920,10 → 921,10 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(StickGier) > 20) // war 35 |
{ |
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
if(!(ParamSet.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
} |
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
tmp_int = (long) ParamSet.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
tmp_int += (ParamSet.Gier_P * StickGier) / 4; |
sollGier = tmp_int; |
Mess_Integral_Gier -= tmp_int; |
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
932,7 → 933,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
if(KompassValue && (ParamSet.GlobalConfig & CFG_KOMPASS_AKTIV)) |
{ |
int w,v; |
static int SignalSchlecht = 0; |
961,8 → 962,8 |
if(!TimerWerteausgabe--) |
{ |
TimerWerteausgabe = 24; |
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[0] = IntegralNick / ParamSet.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFaktor; |
DebugOut.Analog[2] = Mittelwert_AccNick; |
DebugOut.Analog[3] = Mittelwert_AccRoll; |
DebugOut.Analog[4] = MesswertGier; |
990,7 → 991,7 |
// DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
// DebugOut.Analog[11] = KompassStartwert; |
// DebugOut.Analog[10] = Parameter_Gyro_I; |
// DebugOut.Analog[10] = EE_Parameter.Gyro_I; |
// DebugOut.Analog[10] = ParamSet.Gyro_I; |
// DebugOut.Analog[9] = KompassRichtung; |
// DebugOut.Analog[10] = GasMischanteil; |
// DebugOut.Analog[3] = HoeheD * 32; |
1028,10 → 1029,10 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//OCR0B = 180 - (Poti1 + 120) / 4; |
//DruckOffsetSetting = OCR0B; |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
if((ParamSet.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
{ |
int tmp_int; |
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
if(ParamSet.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
{ |
if(Parameter_MaxHoehe < 50) |
{ |
1043,7 → 1044,7 |
} |
else |
{ |
SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; |
SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)ParamSet.Hoehe_Verstaerkung - 20; |
HoehenReglerAktiv = 1; |
} |
|
1058,10 → 1059,10 |
else if(tmp_int < -50) tmp_int = -50; |
h -= tmp_int; |
hoehenregler = (hoehenregler*15 + h) / 16; |
if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN |
if(hoehenregler < ParamSet.Hoehe_MinGas) // nicht unter MIN |
{ |
if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas; |
if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil; |
if(GasMischanteil >= ParamSet.Hoehe_MinGas) hoehenregler = ParamSet.Hoehe_MinGas; |
if(GasMischanteil < ParamSet.Hoehe_MinGas) hoehenregler = GasMischanteil; |
} |
if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
GasMischanteil = hoehenregler; |