• MikroKopter.de
• Wiki
• SHOP
• Datenschutz
• Impressum

FlightCtrlBL-CtrlMK3MagNaviCtrlProjects

Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

Compare Revisions

• This comparison shows the changes necessary to convert path

  → /branches/thjac/ @ 1132

  → /branches/thjac/ @ 1133

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 1132 → Rev 1133

/branches/thjac/V1_10/pitch_neutral.c
File deleted

/branches/thjac/V1_10/pitch_neutral.d
File deleted

/branches/thjac/V1_10/pitch_md.c
File deleted

/branches/thjac/V1_10/pitch_neutral.h
File deleted

/branches/thjac/V1_10/pitch_md.d
File deleted

/branches/thjac/V1_10/pitch_md.h
File deleted

/branches/thjac/V1_10/altcon.c
File deleted

/branches/thjac/V1_10/altcon.h
File deleted

/branches/thjac/V1_10/altcon.lst
File deleted

/branches/thjac/V1_10/pitch_neutral.lst
File deleted

/branches/thjac/V1_10/pitch_md.lst
File deleted

/branches/thjac/V1_10/fc.c
55,63 → 55,62
#include "main.h"
#include "parameter.h"
#include "pitch.h"
#include "altcon.h"
#include "eeprom.c"
unsigned char h, m, s;
unsigned char h,m,s;
volatile unsigned int I2CTimeout = 100;
volatile int MesswertNick, MesswertRoll, MesswertGier, MesswertGierBias;
volatile int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias;
int AdNeutralGierBias;
int AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralGier = 0, StartNeutralRoll = 0, StartNeutralNick = 0;
int Mittelwert_AccNick, Mittelwert_AccRoll, Mittelwert_AccHoch, NeutralAccX = 0, NeutralAccY = 0;
int NaviAccNick, NaviAccRoll, NaviCntAcc = 0;
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
volatile float NeutralAccZ = 0;
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
long IntegralNick = 0, IntegralNick2 = 0;
long IntegralRoll = 0, IntegralRoll2 = 0;
long IntegralAccNick = 0, IntegralAccRoll = 0, IntegralAccZ = 0;
long IntegralNick = 0,IntegralNick2 = 0;
long IntegralRoll = 0,IntegralRoll2 = 0;
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
long Integral_Gier = 0;
long Mess_IntegralNick = 0, Mess_IntegralNick2 = 0;
long Mess_IntegralRoll = 0, Mess_IntegralRoll2 = 0;
long Mess_Integral_Gier = 0, Mess_Integral_Gier2 = 0;
long MittelIntegralNick, MittelIntegralRoll, MittelIntegralNick2, MittelIntegralRoll2;
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
volatile long Mess_Integral_Hoch = 0;
volatile int KompassValue = 0;
volatile int KompassStartwert = 0;
volatile int KompassRichtung = 0;
unsigned int KompassSignalSchlecht = 500;
unsigned char MAX_GAS, MIN_GAS;
volatile int KompassValue = 0;
volatile int KompassStartwert = 0;
volatile int KompassRichtung = 0;
unsigned int KompassSignalSchlecht = 500;
unsigned char MAX_GAS,MIN_GAS;
unsigned char Notlandung = 0;
unsigned char HoehenReglerAktiv = 0;
unsigned char TrichterFlug = 0;
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
long ErsatzKompass;
int ErsatzKompassInGrad; // Kompasswert in Grad
int GierGyroFehler = 0;
long ErsatzKompass;
int ErsatzKompassInGrad; // Kompasswert in Grad
int GierGyroFehler = 0;
float GyroFaktor;
float IntegralFaktor;
volatile int DiffNick, DiffRoll;
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
volatile unsigned char Motor_Vorne, Motor_Hinten, Motor_Rechts, Motor_Links, Count;
volatile int DiffNick,DiffRoll;
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
volatile unsigned char SenderOkay = 0;
int StickNick = 0, StickRoll = 0, StickGier = 0, StickGas = 0;
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
char MotorenEin = 0;
int HoehenWert = 0;
int SollHoehe = 0;
int LageKorrekturRoll = 0, LageKorrekturNick = 0;
float Ki = FAKTOR_I;
unsigned char Looping_Nick = 0, Looping_Roll = 0;
int LageKorrekturRoll = 0,LageKorrekturNick = 0;
float Ki = FAKTOR_I;
unsigned char Looping_Nick = 0,Looping_Roll = 0;
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
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_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_KompassWirkung = 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_Faktor = 10; // Wert : 1-20
unsigned char Parameter_KompassWirkung = 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_Faktor = 10; // Wert : 1-20
unsigned char Parameter_UserParam1 = 0;
unsigned char Parameter_UserParam2 = 0;
unsigned char Parameter_UserParam3 = 0;
125,13 → 124,13
unsigned char Parameter_AchsKopplung1 = 0;
unsigned char Parameter_AchsGegenKopplung1 = 0;
unsigned char Parameter_DynamicStability = 100;
unsigned char Parameter_J16Bitmask; // for the J16 Output
unsigned char Parameter_J16Timing; // for the J16 Output
unsigned char Parameter_J16Brightness; // for the J16 Output
unsigned char Parameter_J17Bitmask; // for the J17 Output
unsigned char Parameter_J17Timing; // for the J17 Output
unsigned char Parameter_J17Brightness; // for the J17 Output
unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard
unsigned char Parameter_J16Bitmask; // for the J16 Output
unsigned char Parameter_J16Timing; // for the J16 Output
unsigned char Parameter_J16Brightness; // for the J16 Output
unsigned char Parameter_J17Bitmask; // for the J17 Output
unsigned char Parameter_J17Timing; // for the J17 Output
unsigned char Parameter_J17Brightness; // for the J17 Output
unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard
unsigned char Parameter_NaviGpsGain;
unsigned char Parameter_NaviGpsP;
unsigned char Parameter_NaviGpsI;
142,31 → 141,32
unsigned char Parameter_NaviSpeedCompensation;
unsigned char Parameter_ExternalControl;
struct mk_param_struct EE_Parameter;
signed int ExternStickNick = 0, ExternStickRoll = 0, ExternStickGier = 0, ExternHoehenValue = -20;
int MaxStickNick = 0, MaxStickRoll = 0;
unsigned int modell_fliegt = 0;
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
int MaxStickNick = 0,MaxStickRoll = 0;
unsigned int modell_fliegt = 0;
unsigned char MikroKopterFlags = 0;
void Piep(unsigned char Anzahl) {
while (Anzahl--) {
if (MotorenEin) return; //auf keinen Fall im Flug!
beeptime = 100;
Delay_ms(250);
}
void Piep(unsigned char Anzahl)
{
while(Anzahl--)
{
if(MotorenEin) return; //auf keinen Fall im Flug!
beeptime = 100;
Delay_ms(250);
}
}
//############################################################################
// Nullwerte ermitteln
void SetNeutral(void)
//############################################################################
{
NeutralAccX = 0;
NeutralAccY = 0;
NeutralAccZ = 0;
NeutralAccX = 0;
NeutralAccY = 0;
NeutralAccZ = 0;
AdNeutralNick = 0;
AdNeutralRoll = 0;
AdNeutralGier = 0;
AdNeutralRoll = 0;
AdNeutralGier = 0;
AdNeutralGierBias = 0;
Parameter_AchsKopplung1 = 0;
Parameter_AchsGegenKopplung1 = 0;
173,29 → 173,32
ExpandBaro = 0;
CalibrierMittelwert();
Delay_ms_Mess(100);
CalibrierMittelwert();
if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
CalibrierMittelwert();
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
AdNeutralNick= AdWertNick;
AdNeutralRoll= AdWertRoll;
AdNeutralGier= AdWertGier;
AdNeutralGierBias = AdWertGier;
StartNeutralRoll = AdNeutralRoll;
StartNeutralNick = AdNeutralNick;
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
{
if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
NeutralAccZ = Aktuell_az;
}
AdNeutralNick = AdWertNick;
AdNeutralRoll = AdWertRoll;
AdNeutralGier = AdWertGier;
AdNeutralGierBias = AdWertGier;
StartNeutralRoll = AdNeutralRoll;
StartNeutralNick = AdNeutralNick;
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) {
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
NeutralAccZ = Aktuell_az;
} 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]);
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]);
}
Mess_IntegralNick = 0;
Mess_IntegralNick = 0;
Mess_IntegralNick2 = 0;
Mess_IntegralRoll = 0;
Mess_IntegralRoll2 = 0;
203,7 → 206,7
MesswertNick = 0;
MesswertRoll = 0;
MesswertGier = 0;
Delay_ms_Mess(100);
Delay_ms_Mess(100);
StartLuftdruck = Luftdruck;
HoeheD = 0;
Mess_Integral_Hoch = 0;
210,8 → 213,8
KompassStartwert = KompassValue;
GPS_Neutral();
beeptime = 50;
Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
ExternHoehenValue = 0;
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
GierGyroFehler = 0;
223,164 → 226,162
FromNaviCtrl_Value.Kalman_MaxFusion = 32;
}
void LesePotis(void) {
/* Warum 110? Knüppel geht von -125 bis 125!
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[EE_Parameter.Kanalbelegung[K_POTI1]] + 125) Poti1++;
else if (Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 125 && Poti1) Poti1--;
if (Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125) Poti2++;
else if (Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125 && Poti2) Poti2--;
if (Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125) Poti3++;
else if (Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125 && Poti3) Poti3--;
if (Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125) Poti4++;
else if (Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125 && 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;
}
//############################################################################
// Bearbeitet die Messwerte
void Mittelwert(void)
//############################################################################
{
static signed long tmpl, tmpl2;
static signed long tmpl,tmpl2;
MesswertGier = (signed int) AdNeutralGier - AdWertGier;
MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
MesswertNick = (signed int) AdWertNick - AdNeutralNick;
//DebugOut.Analog[26] = MesswertNick;
// DebugOut.Analog[28] = MesswertRoll;
//DebugOut.Analog[26] = MesswertNick;
// DebugOut.Analog[28] = MesswertRoll;
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
Mittelwert_AccNick = ((long) Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long) AdWertAccNick))) / 2L;
Mittelwert_AccRoll = ((long) Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long) AdWertAccRoll))) / 2L;
Mittelwert_AccHoch = ((long) Mittelwert_AccHoch * 1 + ((long) AdWertAccHoch)) / 2L;
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
NaviAccNick += AdWertAccNick;
NaviAccRoll += AdWertAccRoll;
NaviAccNick += AdWertAccNick;
NaviAccRoll += AdWertAccRoll;
NaviCntAcc++;
IntegralAccZ += Aktuell_az - NeutralAccZ;
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
ErsatzKompass += MesswertGier;
Mess_Integral_Gier += MesswertGier;
// Mess_Integral_Gier2 += MesswertGier;
if (ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag
if (ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
if (!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) {
tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L;
tmpl *= Parameter_AchsKopplung1; //125
tmpl /= 4096L;
tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;
tmpl2 *= Parameter_AchsKopplung1;
tmpl2 /= 4096L;
if (labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
} else tmpl = tmpl2 = 0;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
MesswertRoll += tmpl;
MesswertRoll += (tmpl2 * Parameter_AchsGegenKopplung1) / 512L; //109
Mess_IntegralRoll2 += MesswertRoll;
Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll;
if (Mess_IntegralRoll > Umschlag180Roll) {
Mess_IntegralRoll = -(Umschlag180Roll - 25000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if (Mess_IntegralRoll <-Umschlag180Roll) {
Mess_IntegralRoll = (Umschlag180Roll - 25000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if (AdWertRoll < 15) MesswertRoll = -1000;
if (AdWertRoll < 7) MesswertRoll = -2000;
if (PlatinenVersion == 10) {
if (AdWertRoll > 1010) MesswertRoll = +1000;
if (AdWertRoll > 1017) MesswertRoll = +2000;
} else {
if (AdWertRoll > 2020) MesswertRoll = +1000;
if (AdWertRoll > 2034) MesswertRoll = +2000;
}
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
MesswertNick -= tmpl2;
MesswertNick -= (tmpl * Parameter_AchsGegenKopplung1) / 512L;
Mess_IntegralNick2 += MesswertNick;
Mess_IntegralNick += MesswertNick - LageKorrekturNick;
IntegralAccZ += Aktuell_az - NeutralAccZ;
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
ErsatzKompass += MesswertGier;
Mess_Integral_Gier += MesswertGier;
// Mess_Integral_Gier2 += MesswertGier;
if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag
if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
{
tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L;
tmpl *= Parameter_AchsKopplung1; //125
tmpl /= 4096L;
tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;
tmpl2 *= Parameter_AchsKopplung1;
tmpl2 /= 4096L;
if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
}
else tmpl = tmpl2 = 0;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
MesswertRoll += tmpl;
MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
Mess_IntegralRoll2 += MesswertRoll;
Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll;
if(Mess_IntegralRoll > Umschlag180Roll)
{
Mess_IntegralRoll = -(Umschlag180Roll - 25000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if(Mess_IntegralRoll <-Umschlag180Roll)
{
Mess_IntegralRoll = (Umschlag180Roll - 25000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if(AdWertRoll < 15) MesswertRoll = -1000;
if(AdWertRoll < 7) MesswertRoll = -2000;
if(PlatinenVersion == 10)
{
if(AdWertRoll > 1010) MesswertRoll = +1000;
if(AdWertRoll > 1017) MesswertRoll = +2000;
}
else
{
if(AdWertRoll > 2020) MesswertRoll = +1000;
if(AdWertRoll > 2034) MesswertRoll = +2000;
}
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
MesswertNick -= tmpl2;
MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
Mess_IntegralNick2 += MesswertNick;
Mess_IntegralNick += MesswertNick - LageKorrekturNick;
if (Mess_IntegralNick > Umschlag180Nick) {
Mess_IntegralNick = -(Umschlag180Nick - 25000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if (Mess_IntegralNick <-Umschlag180Nick) {
Mess_IntegralNick = (Umschlag180Nick - 25000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if (AdWertNick < 15) MesswertNick = -1000;
if (AdWertNick < 7) MesswertNick = -2000;
if (PlatinenVersion == 10) {
if (AdWertNick > 1010) MesswertNick = +1000;
if (AdWertNick > 1017) MesswertNick = +2000;
} else {
if (AdWertNick > 2020) MesswertNick = +1000;
if (AdWertNick > 2034) MesswertNick = +2000;
}
//++++++++++++++++++++++++++++++++++++++++++++++++
// ADC einschalten
if(Mess_IntegralNick > Umschlag180Nick)
{
Mess_IntegralNick = -(Umschlag180Nick - 25000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if(Mess_IntegralNick <-Umschlag180Nick)
{
Mess_IntegralNick = (Umschlag180Nick - 25000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if(AdWertNick < 15) MesswertNick = -1000;
if(AdWertNick < 7) MesswertNick = -2000;
if(PlatinenVersion == 10)
{
if(AdWertNick > 1010) MesswertNick = +1000;
if(AdWertNick > 1017) MesswertNick = +2000;
}
else
{
if(AdWertNick > 2020) MesswertNick = +1000;
if(AdWertNick > 2034) MesswertNick = +2000;
}
//++++++++++++++++++++++++++++++++++++++++++++++++
// ADC einschalten
ANALOG_ON;
//++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++
Integral_Gier = Mess_Integral_Gier;
Integral_Gier = Mess_Integral_Gier;
IntegralNick = Mess_IntegralNick;
IntegralRoll = Mess_IntegralRoll;
IntegralNick2 = Mess_IntegralNick2;
IntegralRoll2 = Mess_IntegralRoll2;
if (EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) {
if (MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);
else if (MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
if (MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);
else if (MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
}
LesePotis();
if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
{
if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);
else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
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 < 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;
}
//############################################################################
// Messwerte beim Ermitteln der Nullage
void CalibrierMittelwert(void)
//############################################################################
{
if (PlatinenVersion == 13) SucheGyroOffset();
if(PlatinenVersion == 13) SucheGyroOffset();
// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
ANALOG_OFF;
MesswertNick = AdWertNick;
MesswertRoll = AdWertRoll;
MesswertGier = AdWertGier;
Mittelwert_AccNick = ACC_AMPLIFY * (long) AdWertAccNick;
Mittelwert_AccRoll = ACC_AMPLIFY * (long) AdWertAccRoll;
Mittelwert_AccHoch = (long) AdWertAccHoch;
// ADC einschalten
ANALOG_OFF;
MesswertNick = AdWertNick;
MesswertRoll = AdWertRoll;
MesswertGier = AdWertGier;
Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
Mittelwert_AccHoch = (long)AdWertAccHoch;
// ADC einschalten
ANALOG_ON;
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 < 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;
LesePotis();
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
}
//############################################################################
// Senden der Motorwerte per I2C-Bus
void SendMotorData(void)
//############################################################################
{
387,19 → 388,20
DebugOut.Analog[12] = Motor_Vorne;
DebugOut.Analog[13] = Motor_Hinten;
DebugOut.Analog[14] = Motor_Links;
DebugOut.Analog[15] = Motor_Rechts;
DebugOut.Analog[15] = Motor_Rechts;
if (!(MotorenEin && PARAM_ENGINE_ENABLED)) {
if(!( MotorenEin && PARAM_ENGINE_ENABLED ) )
{
Motor_Hinten = 0;
Motor_Vorne = 0;
Motor_Rechts = 0;
Motor_Links = 0;
if (MotorTest[0]) Motor_Vorne = MotorTest[0];
if (MotorTest[1]) Motor_Hinten = MotorTest[1];
if (MotorTest[2]) Motor_Links = MotorTest[2];
if (MotorTest[3]) Motor_Rechts = MotorTest[3];
if(MotorTest[0]) Motor_Vorne = MotorTest[0];
if(MotorTest[1]) Motor_Hinten = MotorTest[1];
if(MotorTest[2]) Motor_Links = MotorTest[2];
if(MotorTest[3]) Motor_Rechts = MotorTest[3];
MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
} else MikroKopterFlags |= FLAG_MOTOR_RUN;
} else MikroKopterFlags |= FLAG_MOTOR_RUN;
//Start I2C Interrupt Mode
twi_state = 0;
411,54 → 413,53
//############################################################################
// Trägt ggf. das Poti als Parameter ein
void ParameterZuordnung(void)
//############################################################################
{
#define CHK_POTI_MM(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;}
#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; }
CHK_POTI(Parameter_MaxHoehe, EE_Parameter.MaxHoehe, 0, 255);
CHK_POTI_MM(Parameter_Luftdruck_D, EE_Parameter.Luftdruck_D, 0, 100);
CHK_POTI_MM(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_MM(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);
#define CHK_POTI_MM(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;}
#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; }
CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
CHK_POTI_MM(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_MM(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_MM(Parameter_J16Timing, EE_Parameter.J16Timing, 1, 255);
CHK_POTI_MM(Parameter_J16Brightness, PARAM_LED_BRIGHTNESS_J16, 0, 250);
CHK_POTI_MM(Parameter_J17Timing, EE_Parameter.J17Timing, 1, 255);
CHK_POTI_MM(Parameter_J17Brightness, PARAM_LED_BRIGHTNESS_J17, 0, 250);
CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
CHK_POTI_MM(Parameter_J16Brightness,PARAM_LED_BRIGHTNESS_J16,0,250);
CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
CHK_POTI_MM(Parameter_J17Brightness,PARAM_LED_BRIGHTNESS_J17,0,250);
// CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
//CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
// CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
// CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
// CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
// CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
// CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255);
// CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);
// CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);
// CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
//CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
// CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
// CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
// CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
// CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
// CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255);
// CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);
// CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);
CHK_POTI(Parameter_ExternalControl, EE_Parameter.ExternalControl, 0, 255);
CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
Ki = (float) Parameter_I_Faktor * 0.0001;
MAX_GAS = EE_Parameter.Gas_Max;
MIN_GAS = EE_Parameter.Gas_Min;
Ki = (float) Parameter_I_Faktor * 0.0001;
MAX_GAS = EE_Parameter.Gas_Max;
MIN_GAS = EE_Parameter.Gas_Min;
}
465,48 → 466,51
//############################################################################
//
void MotorRegler(void)
//############################################################################
{
int motorwert, pd_ergebnis, tmp_int;
int GierMischanteil, GasMischanteil;
static long SummeNick = 0, SummeRoll = 0;
static long sollGier = 0, tmp_long, tmp_long2;
static long IntegralFehlerNick = 0;
static long IntegralFehlerRoll = 0;
static unsigned int RcLostTimer;
static unsigned char delay_neutral = 0;
static unsigned char delay_einschalten = 0, delay_ausschalten = 0;
static char TimerWerteausgabe = 0;
static char NeueKompassRichtungMerken = 0;
static long ausgleichNick, ausgleichRoll;
int motorwert,pd_ergebnis,tmp_int;
int GierMischanteil,GasMischanteil;
static long SummeNick=0,SummeRoll=0;
static long sollGier = 0,tmp_long,tmp_long2;
static long IntegralFehlerNick = 0;
static long IntegralFehlerRoll = 0;
static unsigned int RcLostTimer;
static unsigned char delay_neutral = 0;
static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
static char TimerWerteausgabe = 0;
static char NeueKompassRichtungMerken = 0;
static long ausgleichNick, ausgleichRoll;
Mittelwert();
Mittelwert();
GRN_ON;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = StickGas;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Empfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (SenderOkay < 100) {
if (!PcZugriff) {
if (BeepMuster == 0xffff) {
beeptime = 15000;
BeepMuster = 0x0c00;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = StickGas;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Empfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay < 100)
{
if(!PcZugriff)
{
if(BeepMuster == 0xffff)
{
beeptime = 15000;
BeepMuster = 0x0c00;
}
}
if (RcLostTimer) RcLostTimer--;
else {
MotorenEin = 0;
Notlandung = 0;
}
}
if(RcLostTimer) RcLostTimer--;
else
{
MotorenEin = 0;
Notlandung = 0;
}
ROT_ON;
if (modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken
{
if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken
{
GasMischanteil = EE_Parameter.NotGas;
Notlandung = 1;
PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
514,665 → 518,721
PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
} else MotorenEin = 0;
} else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang gut
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (SenderOkay > 140) {
Notlandung = 0;
RcLostTimer = EE_Parameter.NotGasZeit * 50;
if (GasMischanteil > 40 && MotorenEin) {
if (modell_fliegt < 0xffff) modell_fliegt++;
}
else MotorenEin = 0;
}
if ((modell_fliegt < 256)) {
SummeNick = 0;
SummeRoll = 0;
if (modell_fliegt == 250) {
NeueKompassRichtungMerken = 1;
sollGier = 0;
Mess_Integral_Gier = 0;
// Mess_Integral_Gier2 = 0;
}
} else MikroKopterFlags |= FLAG_FLY;
if ((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) {
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// auf Nullwerte kalibrieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang gut
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay > 140)
{
if (++delay_neutral > 200) // nicht sofort
Notlandung = 0;
RcLostTimer = EE_Parameter.NotGasZeit * 50;
if(GasMischanteil > 40 && MotorenEin)
{
GRN_OFF;
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) {
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;
SetActiveParamSetNumber(setting); // aktiven Datensatz merken
if(modell_fliegt < 0xffff) modell_fliegt++;
}
if((modell_fliegt < 256))
{
SummeNick = 0;
SummeRoll = 0;
if(modell_fliegt == 250)
{
NeueKompassRichtungMerken = 1;
sollGier = 0;
Mess_Integral_Gier = 0;
// Mess_Integral_Gier2 = 0;
}
} else MikroKopterFlags |= FLAG_FLY;
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// auf Nullwerte kalibrieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
{
if(++delay_neutral > 200) // nicht sofort
{
GRN_OFF;
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)
{
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;
SetActiveParamSetNumber(setting); // aktiven Datensatz merken
}
// else
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
{
WinkelOut.CalcState = 1;
beeptime = 1000;
}
else
{
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
SetNeutral();
Piep(GetActiveParamSetNumber());
}
}
}
// else
if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) {
WinkelOut.CalcState = 1;
beeptime = 1000;
} else {
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) & EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
else
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern
{
if(++delay_neutral > 200) // nicht sofort
{
if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
GRN_OFF;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_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());
}
}
else delay_neutral = 0;
}
} else
if (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern
{
if (++delay_neutral > 200) // nicht sofort
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
{
GRN_OFF;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_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());
}
} else delay_neutral = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35 - 125) {
// Starten
if (!MotorenEin && PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) {
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (++delay_einschalten > 200) {
delay_einschalten = 200;
modell_fliegt = 1;
MotorenEin = 1;
sollGier = 0;
Mess_Integral_Gier = 0;
Mess_Integral_Gier2 = 0;
Mess_IntegralNick = 0;
Mess_IntegralRoll = 0;
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralRoll2 = IntegralRoll;
SummeNick = 0;
SummeRoll = 0;
MikroKopterFlags |= FLAG_START;
// Beim Einschalten automatisch kalibrieren
if (PARAM_CAL_ON_START) {
if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) {
if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) {
SucheLuftruckOffset();
}
// Starten
if( !MotorenEin && PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75 )
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(++delay_einschalten > 200)
{
delay_einschalten = 200;
modell_fliegt = 1;
MotorenEin = 1;
sollGier = 0;
Mess_Integral_Gier = 0;
Mess_Integral_Gier2 = 0;
Mess_IntegralNick = 0;
Mess_IntegralRoll = 0;
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralRoll2 = IntegralRoll;
SummeNick = 0;
SummeRoll = 0;
MikroKopterFlags |= FLAG_START;
// Beim Einschalten automatisch kalibrieren
if( PARAM_CAL_ON_START ) {
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) {
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) {
SucheLuftruckOffset();
}
}
SetNeutral();
}
}
SetNeutral();
}
else delay_einschalten = 0;
//Auf Neutralwerte setzen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Auschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
{
if(++delay_ausschalten > 200) // nicht sofort
{
MotorenEin = 0;
delay_ausschalten = 200;
modell_fliegt = 0;
}
}
else delay_ausschalten = 0;
}
} else delay_einschalten = 0;
//Auf Neutralwerte setzen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Auschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) {
if (++delay_ausschalten > 200) // nicht sofort
{
MotorenEin = 0;
delay_ausschalten = 200;
modell_fliegt = 0;
}
} else delay_ausschalten = 0;
}
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (!NewPpmData-- || Notlandung) {
static int chanNickPrev = 0;
static int chanRollPrev = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!NewPpmData-- || Notlandung) {
static int chanNickPrev = 0;
static int chanRollPrev = 0;
static int stick_nick,stick_roll;
ParameterZuordnung();
static int stick_nick, stick_roll;
ParameterZuordnung();
#define MAX_CHAN_VAL 125L
#define COS45 7071L // cos( -45 ) * 10000
long chanNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
long chanRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
long chanNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
long chanRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
int chanNickDiff;
int chanRollDiff;
int chanNickDiff;
int chanRollDiff;
/* Über Parameter läßt sich zwischen "+" und "X" - Formations
* umschalten (sh. parameter.h)
*/
if (PARAM_X_FORMATION) {
/* Über Parameter läßt sich zwischen "+" und "X" - Formations
* umschalten (sh. parameter.h)
*/
if( PARAM_X_FORMATION ) {
chanRoll = -chanRoll;
// Stick-Koordinatensystem um -45° (rechts) drehen
chanNick *= COS45;
chanRoll *= COS45;
int chanNickTemp = ( chanNick - chanRoll ) / 10000L;
int chanRollTemp = ( chanRoll + chanNick ) / 10000L;
chanRoll = -chanRoll;
chanNick = chanNickTemp;
chanRoll = -chanRollTemp;
// Stick-Koordinatensystem um -45° (rechts) drehen
chanNick *= COS45;
chanRoll *= COS45;
if (chanNick > MAX_CHAN_VAL)
chanNick = MAX_CHAN_VAL;
if (chanNick < -MAX_CHAN_VAL)
chanNick = -MAX_CHAN_VAL;
if (chanRoll > MAX_CHAN_VAL)
chanRoll = MAX_CHAN_VAL;
if (chanRoll < -MAX_CHAN_VAL)
chanRoll = -MAX_CHAN_VAL;
}
int chanNickTemp = (chanNick - chanRoll) / 10000L;
int chanRollTemp = (chanRoll + chanNick) / 10000L;
chanNickDiff = ( ( chanNick - chanNickPrev ) / 3) * 3;
chanRollDiff = ( ( chanRoll - chanRollPrev ) / 3) * 3;
chanNickPrev = chanNick;
chanRollPrev = chanRoll;
stick_nick = (stick_nick * 3 + ( (int) chanNick ) * EE_Parameter.Stick_P) / 4;
stick_nick += chanNickDiff * EE_Parameter.Stick_D;
StickNick = stick_nick - GPS_Nick;
chanNick = chanNickTemp;
chanRoll = -chanRollTemp;
stick_roll = (stick_roll * 3 + ( (int) chanRoll ) * EE_Parameter.Stick_P) / 4;
stick_roll += chanRollDiff * EE_Parameter.Stick_D;
StickRoll = stick_roll - GPS_Roll;
if (chanNick > MAX_CHAN_VAL)
chanNick = MAX_CHAN_VAL;
if (chanNick < -MAX_CHAN_VAL)
chanNick = -MAX_CHAN_VAL;
if (chanRoll > MAX_CHAN_VAL)
chanRoll = MAX_CHAN_VAL;
if (chanRoll < -MAX_CHAN_VAL)
chanRoll = -MAX_CHAN_VAL;
}
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
chanNickDiff = ((chanNick - chanNickPrev) / 3) * 3;
chanRollDiff = ((chanRoll - chanRollPrev) / 3) * 3;
// Gaswert übernehmen
if( pitchNeutral() ) {
StickGas = pitch();
} else {
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
}
chanNickPrev = chanNick;
chanRollPrev = chanRoll;
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / (256 / STICK_GAIN );
IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN );
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
{
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
StickGier += ExternControl.Gier;
ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
}
if(StickGas < 0) StickGas = 0;
stick_nick = (stick_nick * 3 + ((int) chanNick) * EE_Parameter.Stick_P) / 4;
stick_nick += chanNickDiff * EE_Parameter.Stick_D;
StickNick = stick_nick - GPS_Nick;
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
if(GyroFaktor < 0) GyroFaktor = 0;
if(IntegralFaktor < 0) IntegralFaktor = 0;
stick_roll = (stick_roll * 3 + ((int) chanRoll) * EE_Parameter.Stick_P) / 4;
stick_roll += chanRollDiff * EE_Parameter.Stick_D;
StickRoll = stick_roll - GPS_Roll;
if(abs(StickNick/STICK_GAIN) > MaxStickNick)
{
MaxStickNick = abs(StickNick)/STICK_GAIN;
if(MaxStickNick > 100) MaxStickNick = 100;
}
else MaxStickNick--;
if(abs(StickRoll/STICK_GAIN) > MaxStickRoll)
{
MaxStickRoll = abs(StickRoll)/STICK_GAIN;
if(MaxStickRoll > 100) MaxStickRoll = 100;
}
else MaxStickRoll--;
if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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;
}
}
// Gaswert übernehmen
StickGas = pitch_value();
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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;
}
}
GyroFaktor = ((float) Parameter_Gyro_P + 10.0) / (256 / STICK_GAIN);
IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0;
if(Looping_Oben || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
} // Ende neue Funken-Werte
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) {
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
StickGier += ExternControl.Gier;
ExternHoehenValue = (int) ExternControl.Hight * (int) EE_Parameter.Hoehe_Verstaerkung;
if (ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
}
if (StickGas < 0) StickGas = 0;
if(Looping_Roll || Looping_Nick)
{
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
}
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
if (GyroFaktor < 0) GyroFaktor = 0;
if (IntegralFaktor < 0) IntegralFaktor = 0;
if (abs(StickNick / STICK_GAIN) > MaxStickNick) {
MaxStickNick = abs(StickNick) / STICK_GAIN;
if (MaxStickNick > 100) MaxStickNick = 100;
} else MaxStickNick--;
if (abs(StickRoll / STICK_GAIN) > MaxStickRoll) {
MaxStickRoll = abs(StickRoll) / STICK_GAIN;
if (MaxStickRoll > 100) MaxStickRoll = 100;
} else MaxStickRoll--;
if (Notlandung) {
MaxStickNick = 0;
MaxStickRoll = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & 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 (Looping_Links || Looping_Rechts) Looping_Roll = 1;
else Looping_Roll = 0;
if (Looping_Oben || Looping_Unten) {
Looping_Nick = 1;
Looping_Roll = 0;
Looping_Links = 0;
Looping_Rechts = 0;
} else Looping_Nick = 0;
} // Ende neue Funken-Werte
if (Looping_Roll || Looping_Nick) {
if (GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Notlandung)
{
StickGier = 0;
StickNick = 0;
StickRoll = 0;
GyroFaktor = (float) 100 / (256.0 / STICK_GAIN);
IntegralFaktor = (float) 120 / (44000 / STICK_GAIN);
Looping_Roll = 0;
Looping_Nick = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (Notlandung) {
StickGier = 0;
StickNick = 0;
StickRoll = 0;
GyroFaktor = (float) 100 / (256.0 / STICK_GAIN);
IntegralFaktor = (float) 120 / (44000 / STICK_GAIN);
Looping_Roll = 0;
Looping_Nick = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define ABGLEICH_ANZAHL 256L
MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren
MittelIntegralRoll += IntegralRoll;
MittelIntegralNick2 += IntegralNick2;
MittelIntegralRoll2 += IntegralRoll2;
MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren
MittelIntegralRoll += IntegralRoll;
MittelIntegralNick2 += IntegralNick2;
MittelIntegralRoll2 += IntegralRoll2;
if (Looping_Nick || Looping_Roll) {
IntegralAccNick = 0;
IntegralAccRoll = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
Mess_IntegralNick2 = Mess_IntegralNick;
Mess_IntegralRoll2 = Mess_IntegralRoll;
ZaehlMessungen = 0;
LageKorrekturNick = 0;
LageKorrekturRoll = 0;
}
if(Looping_Nick || Looping_Roll)
{
IntegralAccNick = 0;
IntegralAccRoll = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
Mess_IntegralNick2 = Mess_IntegralNick;
Mess_IntegralRoll2 = Mess_IntegralRoll;
ZaehlMessungen = 0;
LageKorrekturNick = 0;
LageKorrekturRoll = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (!Looping_Nick && !Looping_Roll) {
long tmp_long, tmp_long2;
if (FromNaviCtrl_Value.Kalman_K != -1) {
tmp_long = (long) (IntegralNick / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccNick);
tmp_long2 = (long) (IntegralRoll / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccRoll);
tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
if ((MaxStickNick > 64) || (MaxStickRoll > 64)) {
tmp_long /= 2;
tmp_long2 /= 2;
}
if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) {
tmp_long /= 3;
tmp_long2 /= 3;
}
if (tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
if (tmp_long < (long) - FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) - FromNaviCtrl_Value.Kalman_MaxFusion;
if (tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
if (tmp_long2 < (long) - FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) - FromNaviCtrl_Value.Kalman_MaxFusion;
} else {
tmp_long = (long) (IntegralNick / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccNick);
tmp_long2 = (long) (IntegralRoll / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccRoll);
tmp_long /= 16;
tmp_long2 /= 16;
if ((MaxStickNick > 64) || (MaxStickRoll > 64)) {
tmp_long /= 3;
tmp_long2 /= 3;
}
if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 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;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll)
{
long tmp_long, tmp_long2;
if(FromNaviCtrl_Value.Kalman_K != -1)
{
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
if((MaxStickNick > 64) || (MaxStickRoll > 64))
{
tmp_long /= 2;
tmp_long2 /= 2;
}
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
{
tmp_long /= 3;
tmp_long2 /= 3;
}
if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
}
else
{
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
tmp_long /= 16;
tmp_long2 /= 16;
if((MaxStickNick > 64) || (MaxStickRoll > 64))
{
tmp_long /= 3;
tmp_long2 /= 3;
}
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 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;
}
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (ZaehlMessungen >= ABGLEICH_ANZAHL) {
static int cnt = 0;
static char last_n_p, last_n_n, last_r_p, last_r_n;
static long MittelIntegralNick_Alt, MittelIntegralRoll_Alt;
if (!Looping_Nick && !Looping_Roll && !TrichterFlug) {
MittelIntegralNick /= ABGLEICH_ANZAHL;
MittelIntegralRoll /= ABGLEICH_ANZAHL;
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ZaehlMessungen >= ABGLEICH_ANZAHL)
{
static int cnt = 0;
static char last_n_p,last_n_n,last_r_p,last_r_n;
static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
if(!Looping_Nick && !Looping_Roll && !TrichterFlug)
{
MittelIntegralNick /= ABGLEICH_ANZAHL;
MittelIntegralRoll /= ABGLEICH_ANZAHL;
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
IntegralAccRoll = (EE_Parameter.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;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
IntegralFehlerRoll = (long) (MittelIntegralRoll - (long) IntegralAccRoll);
ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
if (((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1)) {
LageKorrekturNick /= 2;
LageKorrekturRoll /= 2;
}
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
{
LageKorrekturNick /= 2;
LageKorrekturRoll /= 2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
MittelIntegralNick2 /= ABGLEICH_ANZAHL;
MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
tmp_long = IntegralNick2 - IntegralNick;
tmp_long2 = IntegralRoll2 - IntegralRoll;
//DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
MittelIntegralNick2 /= ABGLEICH_ANZAHL;
MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
tmp_long = IntegralNick2 - IntegralNick;
tmp_long2 = IntegralRoll2 - IntegralRoll;
//DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
IntegralFehlerNick = tmp_long;
IntegralFehlerRoll = tmp_long2;
Mess_IntegralNick2 -= IntegralFehlerNick;
Mess_IntegralRoll2 -= IntegralFehlerRoll;
IntegralFehlerNick = tmp_long;
IntegralFehlerRoll = tmp_long2;
Mess_IntegralNick2 -= IntegralFehlerNick;
Mess_IntegralRoll2 -= IntegralFehlerRoll;
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
if (GierGyroFehler > ABGLEICH_ANZAHL / 2) {
AdNeutralGier++;
AdNeutralGierBias++;
}
if (GierGyroFehler <-ABGLEICH_ANZAHL / 2) {
AdNeutralGier--;
AdNeutralGierBias--;
}
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[24] = GierGyroFehler;
GierGyroFehler = 0;
DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[24] = GierGyroFehler;
GierGyroFehler = 0;
/*DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
*/
//DebugOut.Analog[21] = MittelIntegralNick / 26;
//MittelIntegralRoll = MittelIntegralRoll;
//DebugOut.Analog[28] = ausgleichNick;
/*
DebugOut.Analog[29] = ausgleichRoll;
DebugOut.Analog[30] = LageKorrekturRoll * 10;
*/
/*DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
*/
//DebugOut.Analog[21] = MittelIntegralNick / 26;
//MittelIntegralRoll = MittelIntegralRoll;
//DebugOut.Analog[28] = ausgleichNick;
/*
DebugOut.Analog[29] = ausgleichRoll;
DebugOut.Analog[30] = LageKorrekturRoll * 10;
*/
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4)
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
#define BEWEGUNGS_LIMIT 20000
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1; // + labs(IntegralFehlerNick) / 4096;
if (labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3 * 16)) {
if (IntegralFehlerNick > FEHLER_LIMIT2) {
if (last_n_p) {
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
ausgleichNick = IntegralFehlerNick / 8;
if (ausgleichNick > 5000) ausgleichNick = 5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
} else last_n_p = 1;
} else last_n_p = 0;
if (IntegralFehlerNick < -FEHLER_LIMIT2) {
if (last_n_n) {
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
ausgleichNick = IntegralFehlerNick / 8;
if (ausgleichNick < -5000) ausgleichNick = -5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
} else last_n_n = 1;
} else last_n_n = 0;
} else {
cnt = 0;
KompassSignalSchlecht = 1000;
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
{
if(IntegralFehlerNick > FEHLER_LIMIT2)
{
if(last_n_p)
{
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
ausgleichNick = IntegralFehlerNick / 8;
if(ausgleichNick > 5000) ausgleichNick = 5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
}
else last_n_p = 1;
} else last_n_p = 0;
if(IntegralFehlerNick < -FEHLER_LIMIT2)
{
if(last_n_n)
{
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
ausgleichNick = IntegralFehlerNick / 8;
if(ausgleichNick < -5000) ausgleichNick = -5000;
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
}
if (cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if (cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
if (IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
if (IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
else last_n_n = 1;
} else last_n_n = 0;
}
else
{
cnt = 0;
KompassSignalSchlecht = 1000;
}
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1; // + labs(IntegralFehlerNick) / 4096;
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
ausgleichRoll = 0;
if (labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3 * 16)) {
if (IntegralFehlerRoll > FEHLER_LIMIT2) {
if (last_r_p) {
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
ausgleichRoll = IntegralFehlerRoll / 8;
if (ausgleichRoll > 5000) ausgleichRoll = 5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
} else last_r_p = 1;
} else last_r_p = 0;
if (IntegralFehlerRoll < -FEHLER_LIMIT2) {
if (last_r_n) {
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
ausgleichRoll = IntegralFehlerRoll / 8;
if (ausgleichRoll < -5000) ausgleichRoll = -5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
} else last_r_n = 1;
} else last_r_n = 0;
} else {
cnt = 0;
KompassSignalSchlecht = 1000;
}
if (cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if (cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
if (IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
if (IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
} else {
LageKorrekturRoll = 0;
LageKorrekturNick = 0;
TrichterFlug = 0;
ausgleichRoll = 0;
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
{
if(IntegralFehlerRoll > FEHLER_LIMIT2)
{
if(last_r_p)
{
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
ausgleichRoll = IntegralFehlerRoll / 8;
if(ausgleichRoll > 5000) ausgleichRoll = 5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
}
else last_r_p = 1;
} else last_r_p = 0;
if(IntegralFehlerRoll < -FEHLER_LIMIT2)
{
if(last_r_n)
{
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
ausgleichRoll = IntegralFehlerRoll / 8;
if(ausgleichRoll < -5000) ausgleichRoll = -5000;
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
}
else last_r_n = 1;
} else last_r_n = 0;
} else
{
cnt = 0;
KompassSignalSchlecht = 1000;
}
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
}
else
{
LageKorrekturRoll = 0;
LageKorrekturNick = 0;
TrichterFlug = 0;
}
if (!IntegralFaktor) {
LageKorrekturRoll = 0;
LageKorrekturNick = 0;
} // z.B. bei HH
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
MittelIntegralNick_Alt = MittelIntegralNick;
MittelIntegralRoll_Alt = MittelIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
IntegralAccNick = 0;
IntegralAccRoll = 0;
IntegralAccZ = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
ZaehlMessungen = 0;
}
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
MittelIntegralNick_Alt = MittelIntegralNick;
MittelIntegralRoll_Alt = MittelIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
IntegralAccNick = 0;
IntegralAccRoll = 0;
IntegralAccZ = 0;
MittelIntegralNick = 0;
MittelIntegralRoll = 0;
MittelIntegralNick2 = 0;
MittelIntegralRoll2 = 0;
ZaehlMessungen = 0;
}
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
if (abs(StickGier) > 15) // war 35
{
KompassSignalSchlecht = 1000;
if (!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) {
NeueKompassRichtungMerken = 1;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
if(abs(StickGier) > 15) // war 35
{
KompassSignalSchlecht = 1000;
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
{
NeueKompassRichtungMerken = 1;
};
}
tmp_int = (long) EE_Parameter.Gier_P * ((long) StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
}
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx²
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
sollGier = tmp_int;
Mess_Integral_Gier -= tmp_int;
if (Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
if (Mess_Integral_Gier <-50000) Mess_Integral_Gier = -50000;
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
if (KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) {
int w, v, r, fehler, korrektur;
w = abs(IntegralNick / 512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll / 512);
if (v > w) w = v; // grösste Neigung ermitteln
korrektur = w / 8 + 1;
fehler = ((540 + KompassValue - (ErsatzKompass / GIER_GRAD_FAKTOR)) % 360) - 180;
if (NeueKompassRichtungMerken) {
fehler = 0;
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v,r,fehler,korrektur;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
korrektur = w / 8 + 1;
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
if(NeueKompassRichtungMerken)
{
fehler = 0;
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
}
if(!KompassSignalSchlecht && w < 25)
{
GierGyroFehler += fehler;
if(NeueKompassRichtungMerken)
{
beeptime = 200;
// KompassStartwert = KompassValue;
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
NeueKompassRichtungMerken = 0;
}
}
if (!KompassSignalSchlecht && w < 25) {
GierGyroFehler += fehler;
if (NeueKompassRichtungMerken) {
beeptime = 200;
// KompassStartwert = KompassValue;
KompassStartwert = (ErsatzKompass / GIER_GRAD_FAKTOR);
NeueKompassRichtungMerken = 0;
}
ErsatzKompass += (fehler * 8) / korrektur;
w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w >= 0)
{
if(!KompassSignalSchlecht)
{
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
// r = KompassRichtung;
v = (r * w) / v; // nach Kompass ausrichten
w = 3 * Parameter_KompassWirkung;
if(v > w) v = w; // Begrenzen
else
if(v < -w) v = -w;
Mess_Integral_Gier += v;
}
if(KompassSignalSchlecht) KompassSignalSchlecht--;
}
ErsatzKompass += (fehler * 8) / korrektur;
w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if (w >= 0) {
if (!KompassSignalSchlecht) {
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
r = ((540 + (ErsatzKompass / GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
// r = KompassRichtung;
v = (r * w) / v; // nach Kompass ausrichten
w = 3 * Parameter_KompassWirkung;
if (v > w) v = w; // Begrenzen
else
if (v < -w) v = -w;
Mess_Integral_Gier += v;
}
if (KompassSignalSchlecht) KompassSignalSchlecht--;
} else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (!TimerWerteausgabe--) {
TimerWerteausgabe = 24;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 24;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[2] = Mittelwert_AccNick;
DebugOut.Analog[3] = Mittelwert_AccRoll;
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
DebugOut.Analog[10] = SenderOkay;
//DebugOut.Analog[16] = Mittelwert_AccHoch;
DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
DebugOut.Analog[18] = (int) FromNaviCtrl_Value.OsdBar;
DebugOut.Analog[19] = WinkelOut.CalcState;
DebugOut.Analog[20] = ServoValue;
// DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
// DebugOut.Analog[30] = GPS_Nick;
// DebugOut.Analog[31] = GPS_Roll;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[2] = Mittelwert_AccNick;
DebugOut.Analog[3] = Mittelwert_AccRoll;
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
DebugOut.Analog[10] = SenderOkay;
//DebugOut.Analog[16] = Mittelwert_AccHoch;
DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
DebugOut.Analog[19] = WinkelOut.CalcState;
DebugOut.Analog[20] = ServoValue;
// DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
// DebugOut.Analog[30] = GPS_Nick;
// DebugOut.Analog[31] = GPS_Roll;
// DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
// DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];
DebugOut.Analog[19] = motor_rx[3];
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
DebugOut.Analog[20] /= 14;
DebugOut.Analog[21] = motor_rx[4];
DebugOut.Analog[22] = motor_rx[5];
DebugOut.Analog[23] = motor_rx[6];
DebugOut.Analog[24] = motor_rx[7];
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
*/
// DebugOut.Analog[9] = MesswertNick;
// DebugOut.Analog[9] = SollHoehe;
// 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[9] = KompassRichtung;
// DebugOut.Analog[10] = GasMischanteil;
// DebugOut.Analog[3] = HoeheD * 32;
// DebugOut.Analog[4] = hoehenregler;
}
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];
DebugOut.Analog[19] = motor_rx[3];
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
DebugOut.Analog[20] /= 14;
DebugOut.Analog[21] = motor_rx[4];
DebugOut.Analog[22] = motor_rx[5];
DebugOut.Analog[23] = motor_rx[6];
DebugOut.Analog[24] = motor_rx[7];
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
*/
// DebugOut.Analog[9] = MesswertNick;
// DebugOut.Analog[9] = SollHoehe;
// 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[9] = KompassRichtung;
// DebugOut.Analog[10] = GasMischanteil;
// DebugOut.Analog[3] = HoeheD * 32;
// DebugOut.Analog[4] = hoehenregler;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
if (Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
DebugOut.Analog[21] = MesswertNick;
DebugOut.Analog[22] = MesswertRoll;
// Maximalwerte abfangen
#define MAX_SENSOR (4096*STICK_GAIN)
if (MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
if (MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
if (MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
if (MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
if (MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
if (MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
#define MAX_SENSOR (4096*STICK_GAIN)
if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas-Mischanteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas-Mischanteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Zur besseren Auflösung hochskalieren
GasMischanteil *= STICK_GAIN;
// Zur besseren Auflösung hochskalieren
GasMischanteil *= STICK_GAIN;
// Fehlerwert der Höhenregelung einmischen
GasMischanteil -= altcon_error();
GasMischanteil -= altitudeController();
// Mindestens auf Minimalgas stellen
if( GasMischanteil < MIN_GAS )
1182,96 → 1242,88
if( GasMischanteil > ( MAX_GAS - 20 ) * STICK_GAIN )
GasMischanteil = ( MAX_GAS - 20 ) * STICK_GAIN;
// Mindestens auf Minimalgas stellen
if (GasMischanteil < MIN_GAS)
GasMischanteil = MIN_GAS;
DebugOut.Analog[7] = GasMischanteil;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Begrenzung des Gasmischanteils auf MAX_GAS - 20 (Reserve für Motoren)
if (GasMischanteil > (MAX_GAS - 20) * STICK_GAIN)
GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
GierMischanteil = MesswertGier - ( sollGier * STICK_GAIN ); // Regler für Gier
DebugOut.Analog[7] = GasMischanteil;
#define MIN_GIERGAS ( 40 * STICK_GAIN ) // unter diesem Gaswert trotzdem Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Reduzierten Gieranteil berechnen
if( GasMischanteil < MIN_GIERGAS ) {
GierMischanteil = ( GierMischanteil * GasMischanteil ) / MIN_GIERGAS;
}
GierMischanteil = MesswertGier - (sollGier * STICK_GAIN); // Regler für Gier
// Gieranteil darf nicht größer als der halbe Gasanteil sein
if( GierMischanteil > ( GasMischanteil >> 1 ) )
GierMischanteil = GasMischanteil >> 1;
if( GierMischanteil < -( GasMischanteil >> 1 ) )
GierMischanteil = -( GasMischanteil >> 1 );
#define MIN_GIERGAS ( 40 * STICK_GAIN ) // unter diesem Gaswert trotzdem Gieren
// Reduzierten Gieranteil berechnen
if (GasMischanteil < MIN_GIERGAS) {
GierMischanteil = (GierMischanteil * GasMischanteil) / MIN_GIERGAS;
}
// Gieranteil darf nicht größer als der halbe Gasanteil sein
if (GierMischanteil > (GasMischanteil >> 1))
GierMischanteil = GasMischanteil >> 1;
if (GierMischanteil < -(GasMischanteil >> 1))
GierMischanteil = -(GasMischanteil >> 1);
tmp_int = MAX_GAS * STICK_GAIN;
// Gieranteil darf die Gasreserve nicht überschreiten
if( GierMischanteil > ( ( tmp_int - GasMischanteil ) ) )
GierMischanteil = ( ( tmp_int - GasMischanteil ) );
if( GierMischanteil < -( ( tmp_int - GasMischanteil ) ) )
GierMischanteil = -( ( tmp_int - GasMischanteil ) );
// Gieranteil darf die Gasreserve nicht überschreiten
if (GierMischanteil > ((tmp_int - GasMischanteil)))
GierMischanteil = ((tmp_int - GasMischanteil));
if (GierMischanteil < -((tmp_int - GasMischanteil)))
GierMischanteil = -((tmp_int - GasMischanteil));
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffNick = MesswertNick - StickNick; // Differenz bestimmen
if (IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
else SummeNick += DiffNick; // I-Anteil bei HH
if (SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L);
if (SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffNick = MesswertNick - StickNick; // Differenz bestimmen
if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
else SummeNick += DiffNick; // I-Anteil bei HH
if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L);
if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
// Motor Vorn
tmp_int = (long) ((long) Parameter_DynamicStability * (long) (GasMischanteil + abs(GierMischanteil) / 2)) / 64;
if (pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
if (pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer
motorwert /= STICK_GAIN;
if ((motorwert < 0)) motorwert = 0;
else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Vorne = motorwert;
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Vorne = motorwert;
// Motor Heck
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
motorwert /= STICK_GAIN;
if ((motorwert < 0)) motorwert = 0;
else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Hinten = motorwert;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
if (IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll; // I-Anteil bei Winkelregelung
else SummeRoll += DiffRoll; // I-Anteil bei HH
if (SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
if (SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll
tmp_int = (long) ((long) Parameter_DynamicStability * (long) (GasMischanteil + abs(GierMischanteil) / 2)) / 64;
if (pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
if (pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Hinten = motorwert;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Achse
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung
else SummeRoll += DiffRoll; // I-Anteil bei HH
if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
// Motor Links
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
motorwert /= STICK_GAIN;
if ((motorwert < 0)) motorwert = 0;
else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Links = motorwert;
// Motor Rechts
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
motorwert /= STICK_GAIN;
if ((motorwert < 0)) motorwert = 0;
else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
if ((motorwert < 0)) motorwert = 0;
else if(motorwert > MAX_GAS) motorwert = MAX_GAS;
if (motorwert < MIN_GAS) motorwert = MIN_GAS;
Motor_Rechts = motorwert;
// +++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++
}

/branches/thjac/V1_10/flight.pnproj
1,0 → 0,0
<Project name="Flight-Ctrl"><File path="uart.h"></File><File path="main.c"></File><File path="main.h"></File><File path="makefile"></File><File path="uart.c"></File><File path="printf_P.h"></File><File path="printf_P.c"></File><File path="timer0.c"></File><File path="timer0.h"></File><File path="old_macros.h"></File><File path="twimaster.c"></File><File path="version.txt"></File><File path="twimaster.h"></File><File path="rc.c"></File><File path="rc.h"></File><File path="fc.h"></File><File path="menu.h"></File><File path="menu.c"></File><File path="_Settings.h"></File><File path="analog.c"></File><File path="analog.h"></File><File path="License.txt"></File><File path="eeprom.c"></File><File path="spi.h"></File><File path="spi.c"></File><File path="led.h"></File><File path="led.c"></File><File path="fc.c"></File><File path="parameter.h"></File><File path="pitch.c"></File><File path="pitch.h"></File><File path="gps.c"></File><File path="gps.h"></File><File path="altcon.h"></File><File path="altcon.c"></File><File path="pitch_neutral.h"></File><File path="pitch_neutral.c"></File><File path="pitch_md.h"></File><File path="pitch_md.c"></File></Project>
<Project name="Flight-Ctrl"><File path="uart.h"></File><File path="main.c"></File><File path="main.h"></File><File path="makefile"></File><File path="uart.c"></File><File path="printf_P.h"></File><File path="printf_P.c"></File><File path="timer0.c"></File><File path="timer0.h"></File><File path="old_macros.h"></File><File path="twimaster.c"></File><File path="version.txt"></File><File path="twimaster.h"></File><File path="rc.c"></File><File path="rc.h"></File><File path="fc.h"></File><File path="menu.h"></File><File path="menu.c"></File><File path="_Settings.h"></File><File path="analog.c"></File><File path="analog.h"></File><File path="License.txt"></File><File path="eeprom.c"></File><File path="spi.h"></File><File path="spi.c"></File><File path="led.h"></File><File path="led.c"></File><File path="fc.c"></File><File path="parameter.h"></File><File path="pitch.c"></File><File path="pitch.h"></File><File path="gps.c"></File><File path="gps.h"></File></Project>

/branches/thjac/V1_10/led.c
2,143 → 2,126
#include "main.h"
#include "parameter.h"
unsigned char J16Blinkcount = 0, J16Mask = 1;
unsigned char J17Blinkcount = 0, J17Mask = 1;
unsigned char J16 = 0, J17 = 0;
unsigned char pwmJ16 = 0, pwmJ17 = 0;
uint16_t LED1_Timing = 0;
uint16_t LED2_Timing = 0;
static unsigned char pwmtable[32] ={0, 1, 2, 2, 2, 3, 3, 4, 5, 6, 7, 8, 10, 11,
13, 16, 19, 23, 27, 32, 38, 45, 54, 64, 76,
91, 108, 128, 152, 181, 215, 250};
unsigned char J16Blinkcount = 0, J16Mask = 1;
unsigned char J17Blinkcount = 0, J17Mask = 1;
unsigned char J16 = 0, J17 = 0;
unsigned char lightsEnabled = 0, lightsOn = 0;
unsigned char lightsEnabled = 0, lightsOn = 0;
extern char MotorenEin;
extern char MotorenEin;
void setJ16(char enabled) {
/* if( PARAM_LED_NEGATE )
enabled = !enabled;
if( enabled && forceEnabled )
J16_ON;
else
J16_OFF;*/
if ((enabled && lightsOn) ^ LED_NEGATE_J16)
J16_ON;
else
J16_OFF;
void setJ16( char enabled ) {
/* if( PARAM_LED_NEGATE )
enabled = !enabled;
if( enabled && forceEnabled )
J16_ON;
else
J16_OFF;*/
if((enabled && lightsOn) ^ LED_NEGATE_J16)
J16_ON;
else
J16_OFF;
}
void setJ17(char enabled) {
/* if( PARAM_LED_NEGATE )
enabled = !enabled;
if( enabled && forceEnabled )
J17_ON;
else
J17_OFF;*/
if ((enabled && lightsOn) ^ LED_NEGATE_J17)
J17_ON;
else
J17_OFF;
void setJ17( char enabled ) {
/* if( PARAM_LED_NEGATE )
enabled = !enabled;
if( enabled && forceEnabled )
J17_ON;
else
J17_OFF;*/
if((enabled && lightsOn) ^ LED_NEGATE_J17)
J17_ON;
else
J17_OFF;
}
// initializes the LED control outputs J16, J17
void LED_Init(void) {
// set PC2 & PC3 as output (control of J16 & J17)
DDRC |= (1 << DDC2) | (1 << DDC3);
void LED_Init( void ) {
// set PC2 & PC3 as output (control of J16 & J17)
DDRC |= (1<<DDC2)|(1<<DDC3);
lightsOn = lightsEnabled = 0;
setJ16(0);
setJ17(0);
J16Blinkcount = 0;
J16Mask = 128;
J17Blinkcount = 0;
J17Mask = 128;
setJ16( 0);
setJ17( 0);
J16Blinkcount = 0; J16Mask = 128;
J17Blinkcount = 0; J17Mask = 128;
}
void checkLightsEnabled(void) {
// Mit dem Gier-Stick rechts lassen sich bei stehenden Motoren die LED's ein- und mit links ausschalten
if (!MotorenEin) {
if (PARAM_LED_STICK_ENABLED) {
if (PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 125
&& PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] < 125 - 35) {
if (PPM_in[ EE_Parameter.Kanalbelegung[ K_GIER ] ] < -75)
if( !MotorenEin) {
if( PARAM_LED_STICK_ENABLED) {
if ( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35-120
&& PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] < 80 ) {
if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GIER ] ] < -75 )
lightsEnabled = 1;
if (PPM_in[ EE_Parameter.Kanalbelegung[ K_GIER ] ] > 75)
if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GIER ] ] > 75 )
lightsEnabled = 0;
}
}
} else
lightsEnabled = 1;
lightsEnabled = 1;
}
// Die LED's können mit den Motoren ein- ausgeschaltet werden
if (PARAM_LED_ENGINE_ENABLED)
lightsEnabled = MotorenEin;
// Die LED's können mit den Motoren ein- ausgeschaltet werden
if( PARAM_LED_ENGINE_ENABLED)
lightsEnabled = MotorenEin;
lightsOn = lightsEnabled;
}
// called in UpdateMotors() every 2ms
void LED_Update( void ) {
void LED_Update(void) {
static char delay = 0;
checkLightsEnabled();
if( !delay-- ) {
static char delay = 0;
delay = 9; // 20ms Intervall
checkLightsEnabled();
// Soll die Unterspannungswarnung zu einem schnelleren Blinken führen?
// Grenze für Unterspannungswarnung erreicht?
if( PARAM_LED_WARNING_SPEEDUP && UBat < EE_Parameter.UnterspannungsWarnung ) {
if( PARAM_LED_FORCE_WARNING_ENABLED ) // Erzwingt die Aktivierung der Ausgänge
lightsOn = 1;
delay /= PARAM_LED_WARNING_SPEEDUP+1;
}
// J16
if( EE_Parameter.J16Timing > 250 && Parameter_J16Timing > 230 )
J16 = EE_Parameter.J16Bitmask & 128;
else if( EE_Parameter.J16Timing > 250 && Parameter_J16Timing < 10 )
J16 = !( EE_Parameter.J16Bitmask & 128 );
else if( !J16Blinkcount-- ) {
J16Blinkcount = Parameter_J16Timing-1;
J16Mask = (J16Mask == 1 ? 0x80 : J16Mask >> 1);
J16 = EE_Parameter.J16Bitmask & J16Mask;
}
// J17
if( EE_Parameter.J17Timing > 250 && Parameter_J17Timing > 230)
J17 = EE_Parameter.J17Bitmask & 128 ;
else if( EE_Parameter.J17Timing > 250 && Parameter_J17Timing < 10)
J17 = !( EE_Parameter.J17Bitmask & 128 );
else if( !J17Blinkcount-- ) {
J17Blinkcount = Parameter_J17Timing-1;
J17Mask = (J17Mask == 1 ? 0x80 : J17Mask >> 1);
J17 = EE_Parameter.J17Bitmask & J17Mask;
}
}
if (!delay--) {
delay = 9; // 20ms Intervall
// Soll die Unterspannungswarnung zu einem schnelleren Blinken führen?
// Grenze für Unterspannungswarnung erreicht?
if (PARAM_LED_WARNING_SPEEDUP && UBat < EE_Parameter.UnterspannungsWarnung) {
if (PARAM_LED_FORCE_WARNING_ENABLED) // Erzwingt die Aktivierung der Ausgänge
lightsOn = 1;
delay /= PARAM_LED_WARNING_SPEEDUP + 1;
}
// J16
if (EE_Parameter.J16Timing > 250 && Parameter_J16Timing > 230)
J16 = EE_Parameter.J16Bitmask & 128;
else if (EE_Parameter.J16Timing > 250 && Parameter_J16Timing < 10)
J16 = !(EE_Parameter.J16Bitmask & 128);
else if (!J16Blinkcount--) {
J16Blinkcount = Parameter_J16Timing - 1;
J16Mask = (J16Mask == 1 ? 0x80 : J16Mask >> 1);
J16 = EE_Parameter.J16Bitmask & J16Mask;
if (EE_Parameter.J16Bitmask & J16Mask)
pwmJ16 = pwmtable[(Parameter_J16Brightness + 6) / 4];
else
pwmJ16 = 0;
}
// J17
if (EE_Parameter.J17Timing > 250 && Parameter_J17Timing > 230)
J17 = EE_Parameter.J17Bitmask & 128;
else if (EE_Parameter.J17Timing > 250 && Parameter_J17Timing < 10)
J17 = !(EE_Parameter.J17Bitmask & 128);
else if (!J17Blinkcount--) {
J17Blinkcount = Parameter_J17Timing - 1;
J17Mask = (J17Mask == 1 ? 0x80 : J17Mask >> 1);
J17 = EE_Parameter.J17Bitmask & J17Mask;
if (EE_Parameter.J17Bitmask & J17Mask)
pwmJ17 = pwmtable[(Parameter_J17Brightness + 6) / 4];
else
pwmJ17 = 0;
}
}
// delay: 0...9 - BRIGHTNESS/23: 0-Aus...10-Max - Bei Unterspannung volle Leuchtkraft
setJ16(J16 && (delay < Parameter_J16Brightness / 23 || UBat < EE_Parameter.UnterspannungsWarnung));
setJ17(J17 && (delay < Parameter_J17Brightness / 23 || UBat < EE_Parameter.UnterspannungsWarnung));
// delay: 0...9 - BRIGHTNESS/23: 0-Aus...10-Max - Bei Unterspannung volle Leuchtkraft
setJ16( J16 && (delay < Parameter_J16Brightness/23 || UBat < EE_Parameter.UnterspannungsWarnung ));
setJ17( J17 && (delay < Parameter_J17Brightness/23 || UBat < EE_Parameter.UnterspannungsWarnung ));
}

/branches/thjac/V1_10/main.c
51,231 → 51,254
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
unsigned char EEPromArray[E2END + 1] EEMEM;
unsigned char EEPromArray[E2END+1] EEMEM;
unsigned char PlatinenVersion = 10;
unsigned char SendVersionToNavi = 1;
// -- Parametersatz aus EEPROM lesen ---
// number [1..5]
void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) {
if ((number > 5) || (number < 1)) number = 3;
eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * (number - 1)], length);
LED_Init();
void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)
{
if((number > 5)||(number < 1)) number = 3;
eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * (number - 1)], length);
LED_Init();
}
// -- Parametersatz ins EEPROM schreiben ---
// number [1..5]
void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) {
if (number > 5) number = 5;
if (number < 1) return;
eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * (number - 1)], length);
SetActiveParamSetNumber(number);
LED_Init();
void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)
{
if(number > 5) number = 5;
if(number < 1) return;
eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * (number - 1)], length);
SetActiveParamSetNumber(number);
LED_Init();
}
unsigned char GetActiveParamSetNumber(void) {
unsigned char set;
set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]);
if ((set > 5) || (set < 1)) {
set = 3;
SetActiveParamSetNumber(set); // diesen Parametersatz als aktuell merken
}
return (set);
unsigned char GetActiveParamSetNumber(void)
{
unsigned char set;
set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]);
if((set > 5) || (set < 1))
{
set = 3;
SetActiveParamSetNumber(set); // diesen Parametersatz als aktuell merken
}
return(set);
}
void SetActiveParamSetNumber(unsigned char number) {
if (number > 5) number = 5;
if (number < 1) return;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken
void SetActiveParamSetNumber(unsigned char number)
{
if(number > 5) number = 5;
if(number < 1) return;
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken
}
void CalMk3Mag(void) {
static unsigned char stick = 1;
if (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -20) stick = 0;
if ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) && !stick) {
stick = 1;
WinkelOut.CalcState++;
if (WinkelOut.CalcState > 4) {
// WinkelOut.CalcState = 0; // in Uart.c
beeptime = 1000;
} else Piep(WinkelOut.CalcState);
void CalMk3Mag(void)
{
static unsigned char stick = 1;
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -20) stick = 0;
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) && !stick)
{
stick = 1;
WinkelOut.CalcState++;
if(WinkelOut.CalcState > 4)
{
// WinkelOut.CalcState = 0; // in Uart.c
beeptime = 1000;
}
DebugOut.Analog[19] = WinkelOut.CalcState;
else Piep(WinkelOut.CalcState);
}
DebugOut.Analog[19] = WinkelOut.CalcState;
}
//############################################################################
//Hauptprogramm
int main(void)
int main (void)
//############################################################################
{
unsigned int timer;
unsigned int timer;
//unsigned int timer2 = 0;
DDRB = 0x00;
//unsigned int timer2 = 0;
DDRB = 0x00;
PORTB = 0x00;
for (timer = 0; timer < 1000; timer++); // verzögern
if (PINB & 0x01) {
if (PINB & 0x02) PlatinenVersion = 13;
else PlatinenVersion = 11;
} else {
if (PINB & 0x02) PlatinenVersion = 20;
else PlatinenVersion = 10;
}
for(timer = 0; timer < 1000; timer++); // verzögern
if(PINB & 0x01)
{
if(PINB & 0x02) PlatinenVersion = 13;
else PlatinenVersion = 11;
}
else
{
if(PINB & 0x02) PlatinenVersion = 20;
else PlatinenVersion = 10;
}
DDRC = 0x81; // SCL
DDRC |= 0x40; // HEF4017 Reset
DDRC = 0x81; // SCL
DDRC |=0x40; // HEF4017 Reset
PORTC = 0xff; // Pullup SDA
DDRB = 0x1B; // LEDs und Druckoffset
DDRB = 0x1B; // LEDs und Druckoffset
PORTB = 0x01; // LED_Rot
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5
DDRD |= 0x80; // J7 -> Servo signal
PORTD = 0x77; // LED
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5
DDRD |=0x80; // J7 -> Servo signal
PORTD = 0x77; // LED
MCUSR &= ~(1 << WDRF);
WDTCSR |= (1 << WDCE) | (1 << WDE);
MCUSR &=~(1<<WDRF);
WDTCSR |= (1<<WDCE)|(1<<WDE);
WDTCSR = 0;
beeptime = 2000;
StickGier = 0;
PPM_in[K_GAS] = 0;
StickRoll = 0;
StickNick = 0;
StickGier = 0; PPM_in[K_GAS] = 0;StickRoll = 0; StickNick = 0;
ROT_OFF;
Timer_Init();
UART_Init();
UART_Init();
rc_sum_init();
ADC_Init();
i2c_init();
SPI_MasterInit();
ADC_Init();
i2c_init();
SPI_MasterInit();
sei();
sei();
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ", PlatinenVersion / 10, PlatinenVersion % 10, VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
printf("\n\r==============================");
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_MAJOR, VERSION_MINOR,VERSION_PATCH + 'a');
printf("\n\r==============================");
GRN_ON;
GRN_ON;
ReadParameterSet(3, (unsigned char *) & EE_Parameter.Kanalbelegung[0], 9); // read only the first bytes
ReadParameterSet(3, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 9); // read only the first bytes
// valid Stick-Settings?
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) == 255 || eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) < EE_DATENREVISION ||
EE_Parameter.Kanalbelegung[0] > 9 || EE_Parameter.Kanalbelegung[1] > 9 || EE_Parameter.Kanalbelegung[2] > 9 || EE_Parameter.Kanalbelegung[3] > 9) {
printf("\n\rInit. EEPROM: Generating Default-Parameter and Stick-Settings...");
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) == 255 || eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) < EE_DATENREVISION ||
EE_Parameter.Kanalbelegung[0] > 9 || EE_Parameter.Kanalbelegung[1] > 9 || EE_Parameter.Kanalbelegung[2] > 9 || EE_Parameter.Kanalbelegung[3] > 9)
{
printf("\n\rInit. EEPROM: Generating Default-Parameter and Stick-Settings...");
DefaultStickMapping();
} else if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) printf("\n\rInit. EEPROM: Generating Default-Parameter using old Stick Settings");
}
else if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) printf("\n\rInit. EEPROM: Generating Default-Parameter using old Stick Settings");
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) {
DefaultKonstanten1();
for (unsigned char i = 1; i < 6; i++) {
if (i == 2) DefaultKonstanten2(); // Kamera
if (i == 3) DefaultKonstanten3(); // Beginner
if (i > 3) DefaultKonstanten2(); // Kamera
WriteParameterSet(i, (unsigned char *) & EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
}
SetActiveParamSetNumber(3); // default-Setting
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION);
}
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION)
{
DefaultKonstanten1();
for (unsigned char i=1;i<6;i++)
{
if(i==2) DefaultKonstanten2(); // Kamera
if(i==3) DefaultKonstanten3(); // Beginner
if(i>3) DefaultKonstanten2(); // Kamera
WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
}
SetActiveParamSetNumber(3); // default-Setting
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION);
}
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) {
printf("\n\rACC nicht abgeglichen!");
}
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
{
printf("\n\rACC nicht abgeglichen!");
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) & EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber());
if (EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) {
printf("\n\rAbgleich Luftdrucksensor..");
timer = SetDelay(1000);
SucheLuftruckOffset();
while (!CheckDelay(timer));
printf("OK\n\r");
}
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
{
printf("\n\rAbgleich Luftdrucksensor..");
timer = SetDelay(1000);
SucheLuftruckOffset();
while (!CheckDelay(timer));
printf("OK\n\r");
}
SetNeutral();
SetNeutral();
ROT_OFF;
ROT_OFF;
beeptime = 2000;
ExternControl.Digital[0] = 0x55;
printf("\n\rSteuerung: ");
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
else printf("Neutral");
printf("\n\rSteuerung: ");
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
else printf("Neutral");
printf("\n\n\r");
printf("\n\n\r");
LcdClear();
I2CTimeout = 5000;
WinkelOut.Orientation = 1;
while (1) {
while (1)
{
if (UpdateMotor) // ReglerIntervall
{
UpdateMotor = 0;
//PORTD |= 0x08;
if (WinkelOut.CalcState) CalMk3Mag();
if(UpdateMotor) // ReglerIntervall
{
UpdateMotor=0;
//PORTD |= 0x08;
if(WinkelOut.CalcState) CalMk3Mag();
else MotorRegler();
//PORTD &= ~0x08;
//PORTD &= ~0x08;
SendMotorData();
ROT_OFF;
if (PcZugriff) PcZugriff--;
else {
ExternControl.Config = 0;
ExternStickNick = 0;
ExternStickRoll = 0;
ExternStickGier = 0;
}
if (SenderOkay) SenderOkay--;
if (!I2CTimeout) {
I2CTimeout = 5;
i2c_reset();
if ((BeepMuster == 0xffff) && MotorenEin) {
if(PcZugriff) PcZugriff--;
else
{
ExternControl.Config = 0;
ExternStickNick = 0;
ExternStickRoll = 0;
ExternStickGier = 0;
}
if(SenderOkay) SenderOkay--;
if(!I2CTimeout)
{
I2CTimeout = 5;
i2c_reset();
if((BeepMuster == 0xffff) && MotorenEin)
{
beeptime = 10000;
BeepMuster = 0x0080;
}
}
} else {
I2CTimeout--;
ROT_OFF;
}
if (SIO_DEBUG && (!UpdateMotor || !MotorenEin)) {
DatenUebertragung();
BearbeiteRxDaten();
} else BearbeiteRxDaten();
if (CheckDelay(timer)) {
if (UBat < EE_Parameter.UnterspannungsWarnung) {
if (BeepMuster == 0xffff) {
beeptime = 6000;
BeepMuster = 0x0300;
}
else
{
I2CTimeout--;
ROT_OFF;
}
/* if(SendVersionToNavi)
{
SPI_StartTransmitPacket(SPI_CMD_VERSION);//#
SendVersionToNavi = 0;
}
else SPI_StartTransmitPacket(SPI_CMD_VALUE);//#
*/
SPI_StartTransmitPacket(); //#
if(SIO_DEBUG && (!UpdateMotor || !MotorenEin))
{
DatenUebertragung();
BearbeiteRxDaten();
}
else BearbeiteRxDaten();
if(CheckDelay(timer))
{
if(UBat < EE_Parameter.UnterspannungsWarnung)
{
if(BeepMuster == 0xffff)
{
beeptime = 6000;
BeepMuster = 0x0300;
}
}
/* if(SendVersionToNavi)
{
SPI_StartTransmitPacket(SPI_CMD_VERSION);//#
SendVersionToNavi = 0;
}
else SPI_StartTransmitPacket(SPI_CMD_VALUE);//#
*/
SPI_StartTransmitPacket();//#
SendSPI = 4;
timer = SetDelay(20);
SendSPI = 4;
timer = SetDelay(20);
}
//if(UpdateMotor) DebugOut.Analog[26]++;
LED_Update();
}
if (!SendSPI) {
SPI_TransmitByte();
}
//if(UpdateMotor) DebugOut.Analog[26]++;
LED_Update();
}
if(!SendSPI) { SPI_TransmitByte(); }
}
return (1);
return (1);
}

/branches/thjac/V1_10/makefile
5,7 → 5,7
#-------------------------------------------------------------------
VERSION_MAJOR = 1
VERSION_MINOR = 10
VERSION_PATCH = 4
VERSION_PATCH = 3
VERSION_SERIAL_MAJOR = 10 # Serial Protocol
VERSION_SERIAL_MINOR = 0 # Serial Protocol
86,7 → 86,7
##########################################################################################################
# List C source files here. (C dependencies are automatically generated.)
SRC = main.c uart.c printf_P.c timer0.c analog.c menu.c
SRC += twimaster.c rc.c fc.c spi.c led.c pitch.c pitch_neutral.c pitch_md.c altcon.c
SRC += twimaster.c rc.c fc.c spi.c led.c pitch.c
SRC += gps.c
##########################################################################################################

/branches/thjac/V1_10/menu.c
5,7 → 5,7
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "altcon.h"
#include "pitch.h"
unsigned int TestInt = 0;
#define ARRAYGROESSE 10

/branches/thjac/V1_10/parameter.h
4,65 → 4,43
#ifndef PARAMETER_H
#define PARAMETER_H
#define PARAM_TIMER_2S 100 // 2 Sekunden
#define PITCH_STICK_THRESHOLD 5
#define PARAM_PITCH_MIN2 EE_Parameter.UserParam2 // Minimalgas
#define PITCH_ALT_THRESHOLD PITCH_MIN2 // Schwellwert für Höhenregelung
#define PITCH_ALT_THRESHOLD PITCH_MIN2 // Schwellwert für Höhenregelung
#define PITCH_NEUTRAL_TIMER 25
#define PITCH_MIN2_TIMER 100
#define PITCH_NEUTRAL_DIFF EE_Parameter.UserParam1
#define PARAM_ALT_P Parameter_Hoehe_P
#define PARAM_ALT_I EE_Parameter.UserParam3
#define PARAM_ALT_D Parameter_Luftdruck_D
#define PARAM_ALT_P Parameter_Hoehe_P
#define PARAM_ALT_I EE_Parameter.UserParam3
#define PARAM_ALT_D Parameter_Luftdruck_D
#define PARAM_ALT_ACC Parameter_Hoehe_ACC_Wirkung
#define PARAM_ALT_GAIN EE_Parameter.Hoehe_Verstaerkung
#define PARAM_ALT_MAX EE_Parameter.MaxHoehe
#define PARAM_ALT_INT_MAX 1000000L
#define PARAM_ALT_EXP_SMOOTHING_FACTOR EE_Parameter.UserParam4 // Faktor für exp. Glättung
#define PARAM_EXP_SMOOTHING_FACTOR EE_Parameter.UserParam4 // Faktor für exp. Glättung
/******************************************************************************************
* Allgemeine Pitch-Steuerungsparameter
*/
#define PARAM_PITCH_STICK_THRESHOLD 5
#define PARAM_PITCH_MIN2 EE_Parameter.UserParam2 // Minimalgas
#define PITCH_ALT_THRESHOLD PITCH_MIN2 // Schwellwert für Höhenregelung
/******************************************************************************************
* Parameter für Neutral-Pitch-Steuerung
*/
#define PARAM_PITCH_NEUTRAL_DIFF EE_Parameter.UserParam1 // Stick-Loslass-Erkennung
/******************************************************************************************
* Parameter für MD-Pitch-Steuerung
*/
#define PARAM_PITCH_MD_HOVER EE_Parameter.UserParam1 // Standgaswert
#define PARAM_PITCH_MD_DELAY0 5 // Begrenzung der Pitch-Beschleunigung am Boden
#define PARAM_PITCH_MD_DELAY1 0 // Begrenzung der Pitch-Beschleunigung im Flug
/******************************************************************************************
* Parameter für LED-Ansteuerung
*/
/* Helligkeit J16
*/
#define PARAM_LED_BRIGHTNESS_J16 ( EE_Parameter.UserParam5 ) // 0-250, 25x=PotiX, Wert wird durch 23 geteilt
#define PARAM_LED_BRIGHTNESS_J16 ( EE_Parameter.UserParam5 ) // 0-250, 25x=PotiX, Wert wird durch 25 geteilt
/* Helligkeit J17
*/
#define PARAM_LED_BRIGHTNESS_J17 ( EE_Parameter.UserParam6 ) // 0-250, 25x=PotiX, Wert wird durch 23 geteilt
#define PARAM_LED_BRIGHTNESS_J17 ( EE_Parameter.UserParam6 ) // 0-250, 25x=PotiX, Wert wird durch 25 geteilt
/* Wenn die Unterspannungswarnung aktiv wird, kann mit diesem Parameter eingestellt
* werden, daß sich die Blinkfrequenz der LED's verdoppelt.
*/
#define PARAM_LED_WARNING_SPEEDUP ( EE_Parameter.UserParam7 & 0x03 ) // 0=deaktiviert >0=Blinkenbeschleunigung
/* Erhöht die Blinkfrequenz für die Unterspannungswarnung.
*/
#define PARAM_LED_WARNING_FAST_ENABLED ( EE_Parameter.UserParam7 & 0x02 ) // 0=Schnelles Blinken, 1=Ganz Schnelles Blinken
/* Erzwingt die Aktivierung der LED-Ausgänge im Fall einer
* Unterspannungswarnung.
*/
84,11 → 62,10
*/
#define PARAM_LED_ENGINE_ENABLED ( EE_Parameter.UserParam7 & 0x20 ) // 0=deaktiviert 1=an Motoren gekoppelt
/* Die Ausgänge J16/J17 lassen sich wahlweise bei stehenden Motoren üner den Gierstick schalten.
/* Die Ausgänge J16/J17 lassen sich mit dem Gear-Stick ein- und ausschalten.
*/
#define PARAM_LED_STICK_ENABLED ( EE_Parameter.UserParam7 & 0x40 ) // 0=deaktiviert 1=aktiviert, wird durch Motorkopplung übersteuert
/* Dieser Parameter legt fest, ob in '+'-Formation (normal) oder 'X'-Formation
* geflogen werden soll. Zur Umschaltung reicht das Setzen des Parameters. Ein
* Umbau der FC oder Einstellungen am Sender sind nicht notwendig.
100,14 → 77,13
*/
#define PARAM_CAL_ON_START ( EE_Parameter.UserParam8 & 0x02 ) // 0=deaktiviert 1=aktiviert
/* Setzt den zu verwendenden Pitch-Modus
/* Wenn gesetzt, dann wird mit neutralisiertem Pitch-Stick und automatisch
* zugeschalteter Höhenregelung geflogen. Damit bei versehentlich falscher
* Konfiguration kein Schaden entsteht, muß zusätzlich beim Einschalten
* der FlightControl der Gas-Stick in Mittelstellung stehen.
*/
#define PARAM_PITCH_MODE (( EE_Parameter.UserParam8 & 0x0C ) >> 2 ) // Pitch-Mode 0-3
#define PARAM_PITCH_NEUTRAL ( EE_Parameter.UserParam8 & 0x04 ) // 0=normal 1=Neutral-Pitch
#define PARAM_PITCH_MODE_NORMAL 0x00
#define PARAM_PITCH_MODE_NEUTRAL 0x01
#define PARAM_PITCH_MODE_MD 0x02
/* Wenn gesetzt, wird nach ca. 2s andauernder Stick-Stellung auf Minimum
* ein Reset der Pitch-Regelung durchgeführt, so daß der MK ohne Aus- und
* Einschalten der Motoren wieder starten kann. Die Pitch-Regelung
114,16 → 90,12
* schaltet auf Leerlaufgas zurück und der Stick kann losgelassen werden,
* ohne das Gas gegeben wird.
*/
#define PARAM_PITCH_RESTART_ENABLED ( EE_Parameter.UserParam8 & 0x10 ) // 0=deaktiviert 1=aktiviert
#define PARAM_PITCH_RESTART_ENABLED ( EE_Parameter.UserParam8 & 0x08 ) // 0=deaktiviert 1=aktiviert
/* Die Motoren können über diesen Parameter aktiviert werden. Ein Wert von 0
* deaktiviert die Motoren und kann zum Testen verwendet werden.
*/
#define PARAM_ENGINE_ENABLED ( EE_Parameter.UserParam8 & 0x20 ) // 0=deaktiviert 1=aktiviert
#define PARAM_ENGINE_ENABLED ( EE_Parameter.UserParam8 & 0x10 ) // 0=deaktiviert 1=aktiviert
/* Skalierung des Gasinkrements (Default 15)
*/
#define PARAM_STICK_SCALE (( EE_Parameter.UserParam8 & 0xC0 ) >> 6)
#endif // PARAMETER_H

/branches/thjac/V1_10/pitch.c
1,74 → 1,375
/* pitch.c
*
* Copyright 2009 Thomas Jachmann
*
* Pitch-Steuerung
*/
#include "main.h"
#include "parameter.h"
#include "pitch_neutral.h"
#include "pitch_md.h"
#include "fc.h"
#include "pitch.h"
#define STATE_STARTUP_WAIT 0x00 // Init-Timeout beim Einschalten abwarten
#define STATE_STARTUP_INIT 0x01 // Initialisierung beim Einschalten
#define STATE_BEGIN 0x02 // Anfangszustand nach Einschalten der Motoren
#define STATE_INITIALIZING 0x03 // Initialisierungsphase
#define STATE_MANUAL 0x04 // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert
#define STATE_INACTIVE 0x05 // Manuelle Kontrolle
#define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung
#define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung
#define STATE_ACTIVE 0x08 // Höhenregelung ist aktiv
// Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus
int (* pitch_value_ptr)( void );
#define PARAM_INIT_TIMEOUT 25
// Prototyp
int pitch_mk_value( void );
char initTimer = PARAM_INIT_TIMEOUT;
int stickValue = 0; // Aktueller Stick-Wert
int lastStickValue = 0; // Vorheriger Stick-Wert
int zeroStickOffset = 0; // Offset für Stick-Kalibrierung
int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung
char state = STATE_STARTUP_WAIT; // Zustand
/* Wird verwendet, um das Umschalten auf automatische Höhenregelung
* nach Erreichen der Neutralstellung zu verzögern.
*/
int pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
// Variable zur Höhenregelung
int pressureOffset = 0;
int accZOffset = 0;
int lastError = 0;
int lastN = 0; // Zuletzt errechneter Fehlerwert
int averageN = 0;
long altIntegral = 0;
int temp; // Temporäre Werte; wird mehrfach verwendet
char pitchNeutralStartup = 1; // 1=Gas-Stick beim Einschalten in Mittelstellung
extern unsigned char Notlandung; // aus fc.c
/*
* Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion
* wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung
* aufgerufen.
* Berechnet den aktuellen Pitch-Wert für die Regelung
*
* Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert
* für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite
* keine Rolle.
*
* Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert
* und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun
* durch Bewegen des Sticks im oberen Bereich das Gas geregelt.
*
* Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im
* Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die
* automatische Höhenregelung sofort aktiviert.
*
* Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert
* und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun
* über den gesamten Stick-Bereich regeln.
*
* Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung
* wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein
* ungestörtes manuelles Steuern möglich.
*
* Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt.
* Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb
* eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für
* Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde.
*/
void pitch_init( void ) {
int pitch( void ) {
// FIXME Funktioniert noch nicht
switch( PARAM_PITCH_MODE ) {
case PARAM_PITCH_MODE_NEUTRAL:
pitch_value_ptr = pitch_neutral_value;
break;
int register pitchCount = 0;
// Sind die Motoren eingeschaltet?
if( MotorenEin ) {
case PARAM_PITCH_MODE_MD:
pitch_value_ptr = pitch_md_value;
break;
// Vorigen Stick-Wert merken
lastStickValue = stickValue;
default:
pitch_value_ptr = pitch_mk_value;
}
/* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung
* nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer
* Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */
temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - zeroStickOffset;
if( temp > 0 ) {
temp = temp + ( ( temp * temp ) / 150 );
} else {
temp = temp - ( ( temp * temp ) / 150 );
}
// Original-Stick-Wert holen und glätten
stickValue = ( temp + 2 * lastStickValue ) / 3;
/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset
* zuzüglich dem Stick-Wert. */
pitchCount = stickValue + pitchOffset;
switch( state ) {
case STATE_BEGIN:
// Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung
if( ( lastStickValue > PITCH_STICK_THRESHOLD ) &&
( lastStickValue - stickValue >= PITCH_NEUTRAL_DIFF ) ) {
pitchOffset = lastStickValue;
state = STATE_INITIALIZING;
pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
}
break;
case STATE_INITIALIZING:
// Während der Initialisierung das Gas konstant halten
pitchCount = pitchOffset;
pitchNeutralTimer--;
/* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht,
* wird die Aktion nicht als "schnelles Bewegen in Neutralstellung"
* gedeutet. */
if( !pitchNeutralTimer ) {
pitchOffset = 0;
state = STATE_BEGIN;
}
// Ist die Neutralstellung erreicht?
if( abs( stickValue ) <= PITCH_STICK_THRESHOLD ) {
// Ist die Höhenregelung aktiviert?
if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) {
state = STATE_ACTIVATING;
} else {
state = STATE_MANUAL;
}
}
break;
/* Wenn die Höhenregelung per Konfiguration deaktiviert ist, verbleibt
* die Funktion in diesem Zustand. */
case STATE_MANUAL:
// Min2-Gas einstellen für Lageregelung bei Minimalgas
if( pitchCount < PARAM_PITCH_MIN2 ) {
pitchCount = PARAM_PITCH_MIN2;
}
break;
/* Die Höhenregelung ist per Konfiguration aktiviert, jedoch befindet
* sich der Stick außerhalb des als Neutralstellung anerkannten
* Wertebereiches. Es wird manuell geregelt. */
case STATE_INACTIVE:
// Ist ein Restart zulässig?
if( PARAM_PITCH_RESTART_ENABLED ) {
/* Wenn der Gashebel ganz unten steht, Timer für Reduzierung des Minimalgaswertes
* starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */
if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) {
pitchNeutralTimer = PITCH_MIN2_TIMER;
} else {
pitchNeutralTimer--;
/* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */
if( !pitchNeutralTimer ) {
state = STATE_BEGIN;
pitchOffset = 0;
// Signalisieren
beeptime = 500;
}
}
}
// Min2-Gas einstellen für Lageregelung bei Minimalgas
if( pitchCount < PARAM_PITCH_MIN2 ) {
pitchCount = PARAM_PITCH_MIN2;
}
// Stick ist innerhalb der Neutralstellung
if( abs( stickValue ) < PITCH_STICK_THRESHOLD ) {
// Timer neu setzen
pitchNeutralTimer = PITCH_NEUTRAL_TIMER;
state = STATE_WAIT;
}
break;
/* Der Stick ist in den für die Neutralstellung gültigen Wertebereich
* gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung
* erfolgen, um die automatische Höhenregelung zu aktivieren. */
case STATE_WAIT:
/* Stick ist innerhalb der Neutralstellung und
Stick-Differenzial ist < 2 */
if( abs( stickValue ) < PITCH_STICK_THRESHOLD &&
lastStickValue == stickValue ) {
pitchNeutralTimer--;
if( !pitchNeutralTimer ) {
state = STATE_ACTIVATING;
}
// Aktivierungskriterium nicht erfüllt, zurück in INACTIVE
} else {
state = STATE_INACTIVE;
}
break;
/* Die automatische Höhenregelung wird jetzt aktiviert. Der aktuelle
* Luftdruck wird gespeichert und notwendige Werte für den Regler
* werden initialisiert. */
case STATE_ACTIVATING:
// Die Referenzhöhe soll zu Beginn der Neutralstellung genommen werden
pressureOffset = airPressure;
accZOffset = Mess_Integral_Hoch / 128;
lastError = 0;
lastN = 0;
averageN = 0;
altIntegral = 0L;
state = STATE_ACTIVE;
// Einschalten der Höhenregelung signalisieren
beeptime = 500;
break;
/* Die automatische Höhenregelung ist aktiv. */
case STATE_ACTIVE:
// Stick ist außerhalb der Neutralstellung
if( abs( stickValue ) > PITCH_STICK_THRESHOLD ) {
pitchOffset -= averageN / 4;
pitchCount = stickValue + pitchOffset;
lastN = 0;
state = STATE_INACTIVE;
// Abschaltung der Höhenregelung signalisieren
beeptime = 500;
}
break;
}
// Motoren sind aus
} else {
// Hier können weitere Initialisierungen folgen
}
switch( state ) {
case STATE_STARTUP_WAIT:
if( !initTimer-- ) {
state = STATE_STARTUP_INIT;
}
break;
int pitch_value( void ) {
switch( PARAM_PITCH_MODE ) {
case PARAM_PITCH_MODE_NEUTRAL:
return pitch_neutral_value();
case STATE_STARTUP_INIT:
/* Lädt den beim Einschalten der FC anliegenden Stickwert als
* Offset für die Kalibrierung des Gas-Sticks. */
zeroStickOffset = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
case PARAM_PITCH_MODE_MD:
return pitch_md_value();
if( zeroStickOffset < -75 )
pitchNeutralStartup = 0;
default:
return pitch_mk_value();
// Die Einschaltphase ist jetzt beendet
state = STATE_BEGIN;
break;
default:
/* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben,
* da der Kopter sonst sofort hochschießen würde.
*/
pitchCount = 0;
pitchOffset = 0;
stickValue = 0;
state = STATE_BEGIN;
}
}
if( pitchOffset < 0 )
pitchOffset = 0;
// Pitch-Wert darf nicht < 0 sein
if( pitchCount < 0 ) {
pitchCount = 0;
}
// pitchCount als Debug-Wert rausschreiben
DebugOut.Analog[26] = stickValue;
DebugOut.Analog[28] = pitchCount;
DebugOut.Analog[29] = pitchOffset;
return pitchCount;
}
/*
* Führt eine Pitch-Berechnung aus, die der Original-SW entspricht.
* Berechnet den Korrekturwert für die Höhenregelung
*/
int pitch_mk_value( void ) {
register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
// Warum 120? Gas= 0 ist -125
// register int pitchCount = stickValue + 120;
register int pitchCount = stickValue + 125;
int altitudeController( void ) {
int register n = 0;
int register error;
if( ( state == STATE_ACTIVE ) && !Notlandung ) {
// Fehlerwert für Regler ermitteln
error = airPressure - pressureOffset;
DebugOut.Analog[26] = stickValue;
DebugOut.Analog[28] = pitchCount;
// Proportionalanteil
n = ( PARAM_ALT_P * error ) / 4; // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4
return pitchCount;
// Integralanteil
altIntegral += ( PARAM_ALT_I * error ) / 4;
// Integral begrenzen
if( altIntegral > PARAM_ALT_INT_MAX )
altIntegral = PARAM_ALT_INT_MAX;
else if( altIntegral < -PARAM_ALT_INT_MAX )
altIntegral = -PARAM_ALT_INT_MAX;
n += altIntegral / 4000;
// Differenzialanteil
n += ( PARAM_ALT_D * ( error - lastError ) ) / 2;
// ACC-Z-Integral zur Dämpfung einbeziehen
temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32;
// Dämpfung limitieren
if( temp > ( 70 * STICK_GAIN ) )
temp = 70 * STICK_GAIN;
else if( temp < -( 70 * STICK_GAIN ) )
temp = -( 70 * STICK_GAIN );
n += temp;
// Verstärkung des Fehlerwertes zur Anpassung des Gewichtes
n = n * PARAM_ALT_GAIN / 10;
int altMax = PARAM_ALT_MAX * STICK_GAIN;
// Limitierung des Korrekturwertes
if( n > altMax )
n = altMax;
else if( n < -altMax )
n = -altMax;
lastN = n;
lastError = error;
/* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der
* Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */
averageN = averageN + PARAM_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100;
}
DebugOut.Analog[30] = altIntegral / 4000;
DebugOut.Analog[27] = n;
return n;
}

/branches/thjac/V1_10/pitch.h
1,18 → 1,15
/* pitch.h
*
* copyright 2009 Thoams Jachmann
*/
#ifndef _PITCH_H
#define _PITCH_H
extern int (* pitch_value_ptr)( void );
extern int pressureOffset;
extern char pitchNeutralStartup;
// TODO Soll den Stick-Wert der Mittelstellung liefern
#define pitch_stickoffset() 0
#define pitchNeutral() ( pitchNeutralStartup | PARAM_PITCH_NEUTRAL )
extern int pitch_value( void );
int pitch( void );
int altitudeController( void );
// #define pitch_value() ( pitch_value_ptr != 0 ? pitch_value_ptr() : 0 )
#endif // PITCH_H
#endif

/branches/thjac/V1_10/timer0.c
14,204 → 14,212
unsigned int ServoValue = 0;
enum {
STOP = 0,
CK = 1,
CK8 = 2,
CK64 = 3,
CK256 = 4,
CK1024 = 5,
T0_FALLING_EDGE = 6,
T0_RISING_EDGE = 7
STOP = 0,
CK = 1,
CK8 = 2,
CK64 = 3,
CK256 = 4,
CK1024 = 5,
T0_FALLING_EDGE = 6,
T0_RISING_EDGE = 7
};
SIGNAL(SIG_OVERFLOW0) // 8kHz
SIGNAL (SIG_OVERFLOW0) // 8kHz
{
static unsigned char cnt_1ms = 1, cnt = 0;
static unsigned char cnt_1ms = 1,cnt = 0;
unsigned char pieper_ein = 0;
// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
if (SendSPI)
SendSPI--;
if (!cnt--) {
// cnt = 9; // Wenn der Kommentar 8kHz oben stimmt, muß durch 8 geteilt werden, nicht durch 10!
cnt = 7;
// cnt_1ms++;
// cnt_1ms %= 2;
cnt_1ms = !cnt_1ms; // So ist das etwas einfacher
if (!cnt_1ms)
UpdateMotor = 1;
CountMilliseconds++;
}
// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
if(SendSPI) SendSPI--;
if(!cnt--)
{
cnt = 9;
cnt_1ms++;
cnt_1ms %= 2;
if(!cnt_1ms) UpdateMotor = 1;
CountMilliseconds++;
}
if (beeptime > 1) {
beeptime--;
if (beeptime & BeepMuster)
pieper_ein = 1;
else
pieper_ein = 0;
} else {
pieper_ein = 0;
BeepMuster = 0xffff;
}
if(beeptime > 1)
{
beeptime--;
if(beeptime & BeepMuster)
{
pieper_ein = 1;
}
else pieper_ein = 0;
}
else
{
pieper_ein = 0;
BeepMuster = 0xffff;
}
if (pieper_ein) {
if (PlatinenVersion == 10)
PORTD |= (1 << 2); // Speaker an PORTD.2
else
PORTC |= (1 << 7); // Speaker an PORTC.7
} else {
if (PlatinenVersion == 10)
PORTD &= ~(1 << 2);
else
PORTC &= ~(1 << 7);
}
if (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) {
if (PINC & 0x10)
cntKompass++;
else {
if ((cntKompass) && (cntKompass < 362)) {
cntKompass += cntKompass / 41;
if (cntKompass > 10) KompassValue = cntKompass - 10;
else KompassValue = 0;
}
// if(cntKompass < 10) cntKompass = 10;
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
cntKompass = 0;
if(pieper_ein)
{
if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
else PORTC |= (1<<7); // Speaker an PORTC.7
}
}
else
{
if(PlatinenVersion == 10) PORTD &= ~(1<<2);
else PORTC &= ~(1<<7);
}
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
{
if(PINC & 0x10)
{
cntKompass++;
}
else
{
if((cntKompass) && (cntKompass < 362))
{
cntKompass += cntKompass / 41;
if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
}
// if(cntKompass < 10) cntKompass = 10;
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
cntKompass = 0;
}
}
}
//----------------------------
void Timer_Init(void) {
void Timer_Init(void)
{
tim_main = SetDelay(10);
TCCR0B = CK8;
TCCR0A = (1 << COM0A1) | (1 << COM0B1) | 3; //fast PWM
OCR0A = 0;
TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
OCR0A = 0;
OCR0B = 120;
TCNT0 = (unsigned char) - TIMER_RELOAD_VALUE; // reload
TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload
//OCR1 = 0x00;
TCCR2A = (1 << COM2A1) | (1 << COM2A0) | 3;
// TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22); // clk/256
TCCR2B = (0 << CS20) | (0 << CS21) | (1 << CS22); // clk/64
TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
// TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22); // clk/256
TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22); // clk/64
TIMSK2 |= _BV(OCIE2A);
TIMSK2 |= _BV(OCIE2A);
TIMSK0 |= _BV(TOIE0);
OCR2A = 10;
TCNT2 = 0;
}
// -----------------------------------------------------------------------
unsigned int SetDelay(unsigned int t) {
// TIMSK0 &= ~_BV(TOIE0);
return (CountMilliseconds + t + 1);
// TIMSK0 |= _BV(TOIE0);
unsigned int SetDelay (unsigned int t)
{
// TIMSK0 &= ~_BV(TOIE0);
return(CountMilliseconds + t + 1);
// TIMSK0 |= _BV(TOIE0);
}
// -----------------------------------------------------------------------
char CheckDelay(unsigned int t) {
// TIMSK0 &= ~_BV(TOIE0);
return (((t - CountMilliseconds) & 0x8000) >> 9);
// TIMSK0 |= _BV(TOIE0);
char CheckDelay(unsigned int t)
{
// TIMSK0 &= ~_BV(TOIE0);
return(((t - CountMilliseconds) & 0x8000) >> 9);
// TIMSK0 |= _BV(TOIE0);
}
// -----------------------------------------------------------------------
void Delay_ms(unsigned int w) {
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt));
void Delay_ms(unsigned int w)
{
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt));
}
void Delay_ms_Mess(unsigned int w) {
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt))
ANALOG_ON;
void Delay_ms_Mess(unsigned int w)
{
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt)) ANALOG_ON;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Servo ansteuern
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SIGNAL(SIG_OVERFLOW2) {
if (ServoState > 0)
PORTD |= 0x80;
else
PORTD &= ~0x80;
TCCR2A = 3;
TIMSK2 &= ~_BV(TOIE2);
if (ServoState > 0)
PORTD |= 0x80;
else
PORTD &= ~0x80;
TCCR2A = 3;
TIMSK2 &= ~_BV( TOIE2 );
}
SIGNAL(SIG_OUTPUT_COMPARE2A) {
static unsigned char postPulse = 0x80;
static int filterServo = 100;
static unsigned char postPulse = 0x80;
static int filterServo = 100;
if( ServoState == 4 ) {
ServoValue = 0x0030; // Offset Part1
filterServo = ( filterServo * 3 + (int) Parameter_ServoNickControl * 2 ) >> DIV_4;
ServoValue += filterServo;
if (ServoState == 4) {
// Min und Max vorverlegt, damit sich diese auf ServoNickControl beziehen und ggf. noch Nick-kompensiert werden
if( ServoValue < ( (int) EE_Parameter.ServoNickMin * 3 ) )
ServoValue = (int) EE_Parameter.ServoNickMin * 3;
else if( ServoValue > ( (int) EE_Parameter.ServoNickMax * 3 ) )
ServoValue = (int) EE_Parameter.ServoNickMax * 3;
long integral;
ServoValue = 0x0030; // Offset Part1
filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2) >> DIV_4;
ServoValue += filterServo;
/* Über Parameter läßt sich zwischen "+" und "X" - Formations
* umschalten (sh. parameter.h)
*/
if( PARAM_X_FORMATION ) {
integral = IntegralNick - IntegralRoll;
} else {
integral = IntegralNick;
}
// Min und Max vorverlegt, damit sich diese auf ServoNickControl beziehen und ggf. noch Nick-kompensiert werden
if (ServoValue < ((int) EE_Parameter.ServoNickMin * 3))
ServoValue = (int) EE_Parameter.ServoNickMin * 3;
else if (ServoValue > ((int) EE_Parameter.ServoNickMax * 3))
ServoValue = (int) EE_Parameter.ServoNickMax * 3;
if( EE_Parameter.ServoNickCompInvert & 0x01 )
ServoValue += ( (long) ( (long) EE_Parameter.ServoNickComp * integral ) >> DIV_128 ) / ( 512L >> DIV_4 );
else
ServoValue -= ( (long) ( (long) EE_Parameter.ServoNickComp * integral ) >> DIV_128 ) / ( 512L >> DIV_4 );
DebugOut.Analog[20] = ServoValue;
if ( ( ServoValue % 255 ) < 45 ) {
ServoValue += 77;
postPulse = 0x60 - 77;
} else {
postPulse = 0x60;
}
OCR2A = 255 - ( ServoValue % 256 );
TCCR2A = (1 << COM2A1 ) | ( 1 << COM2A0 ) | 3;
long integral;
/* Über Parameter läßt sich zwischen "+" und "X" - Formations
* umschalten (sh. parameter.h)
*/
if (PARAM_X_FORMATION)
integral = IntegralNick - IntegralRoll;
else
integral = IntegralNick;
if (EE_Parameter.ServoNickCompInvert & 0x01)
ServoValue += ((long) ((long) EE_Parameter.ServoNickComp * integral) >> DIV_128) / (512L >> DIV_4);
else
ServoValue -= ((long) ((long) EE_Parameter.ServoNickComp * integral) >> DIV_128) / (512L >> DIV_4);
DebugOut.Analog[20] = ServoValue;
if ((ServoValue % 255) < 45) {
ServoValue += 77;
postPulse = 0x60 - 77;
} else
postPulse = 0x60;
OCR2A = 255 - (ServoValue % 256);
TCCR2A = (1 << COM2A1) | (1 << COM2A0) | 3;
} else if ((ServoState > 0) && (ServoState < 4)) {
if (ServoValue > 255) {
PORTD |= 0x80;
TCCR2A = 3;
ServoValue -= 255;
} else {
TCCR2A = (1 << COM2A1) | (0 << COM2A0) | 3;
OCR2A = postPulse; // Offset Part2
ServoState = 1;
}
} else if (ServoState == 0) {
ServoState = (int) EE_Parameter.ServoNickRefresh << MUL_4;
PORTD &= ~0x80;
TCCR2A = 3;
}
ServoState--;
} else if( ( ServoState > 0 ) && ( ServoState < 4 ) ) {
if( ServoValue > 255 ) {
PORTD |= 0x80;
TCCR2A = 3;
ServoValue -= 255;
} else {
TCCR2A = ( 1 << COM2A1 ) | ( 0 << COM2A0 ) | 3;
OCR2A = postPulse; // Offset Part2
ServoState = 1;
}
} else if( ServoState == 0 ) {
ServoState = (int) EE_Parameter.ServoNickRefresh << MUL_4;
PORTD &= ~0x80;
TCCR2A = 3;
}
ServoState--;
}

/branches/thjac/V1_10/version.txt
218,10 → 218,3
- Reihenfolge der User Parameters and Bitfields aufgeräumt - Settings checken!
- User Parameter Defaults werden bei Initialisierung auf 0 gesetzt
- Einige Warnings bereinigt
V1_10e K. Rheinwald - Developer Version only! Do not fly!
- Poti-Offset auf +-125 korrigiert
- Gas-Offset auf 125 korrigiert
- Einige Module gePrettyPrinted
- Gas Modus wird wirklich automatisch geprüft und nicht forciert.
- Merge with altcon.c added by T. Jachmann