Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 686 → Rev 687

/branches/V0.68d Code Redesign killagreg/fc.c
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;