6,14 → 6,14 |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
24,21 → 24,21 |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
50,7 → 50,7 |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
#include "main.h" |
75,19 → 75,12 |
unsigned char MAX_GAS,MIN_GAS; |
unsigned char Notlandung = 0; |
unsigned char HoehenReglerAktiv = 0; |
//-------HF------- |
unsigned char LED_Switch = 0; |
unsigned char LED_flash = 0; |
unsigned char LED_1 = 0; |
unsigned char LED_2 = 0; |
unsigned char LED_2on = 0; |
//-------HF------- |
|
float GyroFaktor; |
float IntegralFaktor; |
|
volatile int DiffNick,DiffRoll; |
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
unsigned char MotorWert[5]; |
volatile unsigned char SenderOkay = 0; |
133,42 → 126,42 |
//############################################################################ |
{ |
unsigned int timer; |
NeutralAccX = 0; |
NeutralAccX = 0; |
NeutralAccY = 0; |
NeutralAccZ = 0; |
AdNeutralNick = 0; |
AdNeutralRoll = 0; |
AdNeutralNick = 0; |
AdNeutralRoll = 0; |
AdNeutralGier = 0; |
CalibrierMittelwert(); |
timer = SetDelay(5); |
CalibrierMittelwert(); |
timer = SetDelay(5); |
while (!CheckDelay(timer)); |
CalibrierMittelwert(); |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
{ |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
|
if(PlatinenVersion == 10) |
if(PlatinenVersion == 10) |
{ |
AdNeutralNick= abs(MesswertNick); |
AdNeutralRoll= abs(MesswertRoll); |
AdNeutralNick= abs(MesswertNick); |
AdNeutralRoll= abs(MesswertRoll); |
AdNeutralGier= abs(MesswertGier); |
} |
else |
} |
else |
{ |
AdNeutralNick= abs(MesswertNick) / 2; |
AdNeutralRoll= abs(MesswertRoll) / 2; |
AdNeutralNick= abs(MesswertNick) / 2; |
AdNeutralRoll= abs(MesswertRoll) / 2; |
AdNeutralGier= abs(MesswertGier) / 2; |
} |
} |
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
NeutralAccZ = Aktuell_az; |
|
Mess_IntegralNick = 0; |
|
Mess_IntegralNick = 0; |
Mess_IntegralNick2 = 0; |
Mess_IntegralRoll = 0; |
Mess_IntegralRoll = 0; |
Mess_IntegralRoll2 = 0; |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier = 0; |
MesswertNick = 0; |
MesswertRoll = 0; |
MesswertGier = 0; |
177,7 → 170,7 |
Mess_Integral_Hoch = 0; |
KompassStartwert = KompassValue; |
GPS_Neutral(); |
beeptime = 50; |
beeptime = 50; |
} |
|
//############################################################################ |
184,7 → 177,7 |
// Bildet den Mittelwert aus den Messwerten |
void Mittelwert(void) |
//############################################################################ |
{ |
{ |
// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
ANALOG_OFF; |
if(MessanzahlNick) (MesswertNick = AccumulateNick / MessanzahlNick); |
203,19 → 196,19 |
// Integral_Gier2 = Mess_Integral_Gier2; |
IntegralNick = Mess_IntegralNick; |
IntegralRoll = Mess_IntegralRoll; |
IntegralNick2 = Mess_IntegralNick2; |
IntegralNick2 = Mess_IntegralNick2; |
IntegralRoll2 = Mess_IntegralRoll2; |
// ADC einschalten |
ANALOG_ON; |
ANALOG_ON; |
|
/* |
//------------------------------------------------------------------------------ |
if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
else |
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); |
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--; |
232,7 → 225,7 |
// Messwerte beim Ermitteln der Nullage |
void CalibrierMittelwert(void) |
//############################################################################ |
{ |
{ |
// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
ANALOG_OFF; |
if(MessanzahlNick) (MesswertNick = AccumulateNick / MessanzahlNick); |
248,7 → 241,7 |
accumulate_AccNick = 0;messanzahl_AccNick = 0; |
accumulate_AccHoch = 0;messanzahl_AccHoch = 0; |
// ADC einschalten |
ANALOG_ON; |
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--; |
263,7 → 256,7 |
// Senden der Motorwerte per I2C-Bus |
void SendMotorData(void) |
//############################################################################ |
{ |
{ |
if(MOTOR_OFF || !MotorenEin) |
{ |
Motor_Hinten = 0; |
279,16 → 272,16 |
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; |
|
//Start I2C Interrupt Mode |
twi_state = 0; |
motor = 0; |
i2c_start(); |
i2c_start(); |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Konstanten |
// + Konstanten |
// + 0-250 -> normale Werte |
// + 251 -> Poti1 |
// + 252 -> Poti2 |
296,15 → 289,15 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
void DefaultKonstanten1(void) |
{ |
EE_Parameter.Kanalbelegung[K_NICK] = 3; |
EE_Parameter.Kanalbelegung[K_NICK] = 1; |
EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
EE_Parameter.Kanalbelegung[K_GAS] = 1; |
EE_Parameter.Kanalbelegung[K_GAS] = 3; |
EE_Parameter.Kanalbelegung[K_GIER] = 4; |
EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01; |
EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01; |
EE_Parameter.Hoehe_MinGas = 30; |
EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
338,20 → 331,20 |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
memcpy(EE_Parameter.Name, "Sport\0", 12); |
memcpy(EE_Parameter.Name, "Sport\0", 12); |
} |
|
void DefaultKonstanten2(void) |
{ |
EE_Parameter.Kanalbelegung[K_NICK] = 3; |
EE_Parameter.Kanalbelegung[K_NICK] = 1; |
EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
EE_Parameter.Kanalbelegung[K_GAS] = 1; |
EE_Parameter.Kanalbelegung[K_GAS] = 3; |
EE_Parameter.Kanalbelegung[K_GIER] = 4; |
EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
EE_Parameter.Hoehe_MinGas = 30; |
EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
384,14 → 377,14 |
EE_Parameter.ServoNickRefresh = 5; |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
memcpy(EE_Parameter.Name, "Kamera\0", 12); |
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
memcpy(EE_Parameter.Name, "Kamera\0", 12); |
} |
|
|
//############################################################################ |
// Trägt ggf. das Poti als Parameter ein |
void ParameterZuordnung(void) |
void ParameterZuordnung(void) |
//############################################################################ |
{ |
|
403,11 → 396,11 |
CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
CHK_POTI(Parameter_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_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
437,34 → 430,34 |
static int hoehenregler = 0; |
static char TimerWerteausgabe = 0; |
static char NeueKompassRichtungMerken = 0; |
Mittelwert(); |
Mittelwert(); |
|
GRN_ON; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gaswert ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
if(GasMischanteil < 0) GasMischanteil = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang schlecht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay < 100) |
{ |
if(!PcZugriff) |
if(!PcZugriff) |
{ |
if(BeepMuster == 0xffff) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 15000; |
BeepMuster = 0x0c00; |
} |
} |
} |
if(RcLostTimer) RcLostTimer--; |
else |
if(RcLostTimer) RcLostTimer--; |
else |
{ |
MotorenEin = 0; |
Notlandung = 0; |
} |
} |
ROT_ON; |
if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
{ |
476,10 → 469,10 |
} |
else MotorenEin = 0; |
} |
else |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
else |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang gut |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay > 140) |
{ |
Notlandung = 0; |
492,14 → 485,14 |
{ |
SummeNick = 0; |
SummeRoll = 0; |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier2 = 0; |
} |
if((GasMischanteil > 200) && MotorenEin == 0) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// auf Nullwerte kalibrieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
{ |
unsigned char setting; |
521,25 → 514,25 |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
} |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
SetNeutral(); |
Piep(GetActiveParamSetNumber()); |
} |
} |
} |
else delay_neutral = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gas ist unten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(GasMischanteil < 35) |
{ |
// Starten |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Einschalten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(++delay_einschalten > 200) |
{ |
delay_einschalten = 200; |
546,7 → 539,7 |
modell_fliegt = 1; |
MotorenEin = 1; |
sollGier = 0; |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier2 = 0; |
Mess_IntegralNick = 0; |
Mess_IntegralRoll = 0; |
554,13 → 547,13 |
Mess_IntegralRoll2 = IntegralRoll; |
SummeNick = 0; |
SummeRoll = 0; |
} |
} |
} |
} |
else delay_einschalten = 0; |
//Auf Neutralwerte setzen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Auschalten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
{ |
if(++delay_ausschalten > 200) // nicht sofort |
568,19 → 561,19 |
MotorenEin = 0; |
delay_ausschalten = 200; |
modell_fliegt = 0; |
} |
} |
} |
else delay_ausschalten = 0; |
} |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// neue Werte von der Funke |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!NewPpmData-- || Notlandung) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!NewPpmData-- || Notlandung) |
{ |
ParameterZuordnung(); |
StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P; |
StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P; |
StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P; |
StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
591,73 → 584,59 |
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
if(GyroFaktor < 0) GyroFaktor = 0; |
if(IntegralFaktor < 0) IntegralFaktor = 0; |
|
//------HF------- LEDs Schalten - LED1 Normal, LED2 Blitzlicht |
LED_Switch = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]]; |
if (((LED_Switch > 32) & (LED_Switch < 96)) | (LED_Switch > 160)) LED_2++; else if (LED_2 > 0) LED_2--; |
if ((LED_Switch > 96)) LED_1++; else if (LED_1 > 0) LED_1--; |
if (LED_1 >= 10) {LED_1 = 10; LED1_ON;} |
if (LED_1 <= 1) LED1_OFF; |
if (LED_2 >= 10) {LED_2 = 10; LED_2on = 1;} |
if (LED_2 <= 1) LED_2on = 0; |
if (LED_2on == 1) LED_flash++; |
if (LED_flash > 77) LED2_ON; else LED2_OFF; |
if (LED_flash > 80) LED_flash = 0; |
//------HF------- End LEDs Schalten |
|
// greift in den Stick ein, um ungewolltes überschlagen zu verhindern |
if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
{ |
if(IntegralNick > 60000) |
{ |
StickNick -= 8 * EE_Parameter.Stick_P; |
if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
} |
if(IntegralNick > 60000) |
{ |
StickNick -= 8 * EE_Parameter.Stick_P; |
if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
} |
else |
if(IntegralNick < -60000) |
if(IntegralNick < -60000) |
{ |
StickNick += 8 * EE_Parameter.Stick_P; |
if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
} |
if(IntegralRoll > 60000) |
{ |
StickRoll -= 8 * EE_Parameter.Stick_P; |
if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
} |
if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
} |
if(IntegralRoll > 60000) |
{ |
StickRoll -= 8 * EE_Parameter.Stick_P; |
if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
} |
else |
if(IntegralRoll < -60000) |
if(IntegralRoll < -60000) |
{ |
StickRoll += 8 * EE_Parameter.Stick_P; |
if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
} |
if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
} |
} |
|
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) || |
((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
{ |
{ |
Looping_Roll = 1; |
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
} |
else Looping_Roll = 0; |
else Looping_Roll = 0; |
|
if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) || |
((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN)) |
{ |
{ |
Looping_Nick = 1; |
Looping_Roll = 0; |
Looping_Roll = 0; |
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
} |
else Looping_Nick = 0; |
else Looping_Nick = 0; |
|
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Bei Empfangsausfall im Flug |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Bei Empfangsausfall im Flug |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(Notlandung) |
{ |
StickGier = 0; |
667,10 → 646,10 |
IntegralFaktor = 0.005; |
Looping_Roll = 0; |
Looping_Nick = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gyro-Drift kompensieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define DRIFT_FAKTOR 3 |
if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
{ |
681,24 → 660,24 |
if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
// if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; // macht nur mit Referenz (Kompass Sinn) |
// if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; // macht nur mit Referenz (Kompass Sinn) |
// if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; // macht nur mit Referenz (Kompass Sinn) |
// if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; // macht nur mit Referenz (Kompass Sinn) |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
Mess_IntegralNick2 = IntegralNick; |
Mess_IntegralNick2 = IntegralNick; |
Mess_IntegralRoll2 = IntegralRoll; |
Mess_Integral_Gier2 = Integral_Gier; |
ANALOG_ON; // ADC einschalten |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Integrale auf ACC-Signal abgleichen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(IntegralFaktor && !Looping_Nick && !Looping_Roll) |
{ |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8; |
if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8; |
else tmp_long /= 16; |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8; |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8; |
else tmp_long2 /= 16; |
|
#define AUSGLEICH 500 |
707,7 → 686,7 |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
} |
else |
else |
{ |
tmp_long = 0; |
tmp_long2 = 0; |
715,26 → 694,26 |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
Mess_IntegralNick -= tmp_long; |
Mess_IntegralRoll -= tmp_long2; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(StickGier) > 35) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(StickGier) > 35) |
{ |
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
} |
tmp_int = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo y = ax + bx² |
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
sollGier = tmp_int; |
Mess_Integral_Gier -= tmp_int; |
Mess_Integral_Gier -= tmp_int; |
if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000; // begrenzen |
if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000; |
|
|
ANALOG_ON; // ADC einschalten |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
{ |
int w,v; |
static int SignalSchlecht = 0; |
741,8 → 720,8 |
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
if(v > w) w = v; // grösste Neigung ermitteln |
if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) |
{ |
if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) |
{ |
KompassStartwert = KompassValue; |
NeueKompassRichtungMerken = 0; |
} |
754,14 → 733,14 |
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
ANALOG_ON; // ADC einschalten |
if(SignalSchlecht) SignalSchlecht--; |
} |
} |
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!TimerWerteausgabe--) |
{ |
TimerWerteausgabe = 49; |
790,17 → 769,17 |
// 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] = 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 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor; |
else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; |
if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
817,10 → 796,10 |
if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR; |
if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Höhenregelung |
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//OCR0B = 180 - (Poti1 + 120) / 4; |
//DruckOffsetSetting = OCR0B; |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
828,15 → 807,15 |
int tmp_int; |
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
{ |
if(Parameter_MaxHoehe < 50) |
if(Parameter_MaxHoehe < 50) |
{ |
SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
HoehenReglerAktiv = 0; |
} |
else |
else |
HoehenReglerAktiv = 1; |
} |
else |
else |
{ |
SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20; |
HoehenReglerAktiv = 1; |
852,24 → 831,24 |
if(tmp_int > 50) tmp_int = 50; |
else if(tmp_int < -50) tmp_int = -50; |
h -= tmp_int; |
hoehenregler = (hoehenregler*15 + h) / 16; |
hoehenregler = (hoehenregler*15 + h) / 16; |
if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN |
{ |
if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas; |
if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil; |
} |
} |
if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
GasMischanteil = hoehenregler; |
} |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mischer und PI-Regler |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[7] = GasMischanteil; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gier-Anteil |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define MUL_G 1.4 |
GierMischanteil = MesswertGier - sollGier; // Regler für Gier |
//GierMischanteil = 0; |
877,35 → 856,35 |
if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil); |
|
if(GasMischanteil < 10) GierMischanteil = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Nick-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick)); // Differenz bestimmen |
SummeNick += DiffNick; // I-Anteil |
if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1; |
if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1; |
if(SummeNick > 16000) SummeNick = 16000; |
if(SummeNick < -16000) SummeNick = -16000; |
pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick |
pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick |
// Motor Vorn |
#define MUL 2 |
if((pd_ergebnis > MUL * (GasMischanteil))) pd_ergebnis = MUL * (GasMischanteil); |
if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil); |
if((pd_ergebnis > MUL * (GasMischanteil))) pd_ergebnis = MUL * (GasMischanteil); |
if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil); |
|
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer |
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 < MIN_GAS) motorwert = MIN_GAS; |
Motor_Vorne = motorwert; |
// Motor Heck |
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
Motor_Hinten = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Motor_Hinten = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = Kp * (MesswertRoll - (StickRoll - GPS_Roll)); // Differenz bestimmen |
SummeRoll += DiffRoll; // I-Anteil |
if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1; |
912,19 → 891,19 |
if(SummeRoll > 16000) SummeRoll = 16000; |
if(SummeRoll < -16000) SummeRoll = -16000; |
pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll |
if((pd_ergebnis > MUL * (GasMischanteil))) pd_ergebnis = MUL * (GasMischanteil); |
if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil); |
if((pd_ergebnis > MUL * (GasMischanteil))) pd_ergebnis = MUL * (GasMischanteil); |
if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil); |
// Motor Links |
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
Motor_Links = motorwert; |
Motor_Links = motorwert; |
// Motor Rechts |
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil; |
if ((motorwert < 0)) motorwert = 0; |
else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
Motor_Rechts = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
} |