/*#######################################################################################
Flight Control
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + 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.
// + 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,
// + 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
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + 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
// + 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,
// + 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
// + for non-commercial use (directly or indirectly)
// + 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
// + * 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
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// + 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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 6.10.2007
/*
Driftkompensation fuer Gyros verbessert
Linearsensor mit fixem Neutralwert
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
*/
#include "main.h"
unsigned char h
,m
,s
;
volatile unsigned char Timeout
= 0;
volatile int MesswertNick
,MesswertRoll
,MesswertGier
;
volatile int AdNeutralNick
= 0,AdNeutralRoll
= 0,AdNeutralGier
= 0;
volatile int Mittelwert_AccNick
, Mittelwert_AccRoll
,Mittelwert_AccHoch
, NeutralAccX
=0, NeutralAccY
=0;
volatile float NeutralAccZ
= 0;
unsigned char CosinusNickWinkel
= 0, CosinusRollWinkel
= 0;
volatile long IntegralNick
= 0,IntegralNick2
= 0;
volatile long IntegralRoll
= 0,IntegralRoll2
= 0;
volatile long Integral_Gier
= 0;
volatile long Mess_IntegralNick
= 0,Mess_IntegralNick2
= 0;
volatile long Mess_IntegralRoll
= 0,Mess_IntegralRoll2
= 0;
volatile long Mess_Integral_Gier
= 0,Mess_Integral_Gier2
= 0;
volatile long Mess_Integral_Hoch
= 0;
volatile int KompassValue
= 0;
volatile int KompassStartwert
= 0;
volatile int KompassRichtung
= 0;
unsigned char MAX_GAS
,MIN_GAS
;
unsigned char Notlandung
= 0;
unsigned char HoehenReglerAktiv
= 0;
float GyroFaktor
;
float IntegralFaktor
;
uint8_t gps_cmd
= GPS_CMD_STOP
;
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
;
unsigned char MotorWert
[5];
volatile unsigned char SenderOkay
= 0;
int StickNick
= 0,StickRoll
= 0,StickGier
= 0;
char MotorenEin
= 0;
int HoehenWert
= 0;
int SollHoehe
= 0;
int w
,v
;
float Kp
= FAKTOR_P
;
float Ki
= FAKTOR_I
;
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
= 50; // 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;
unsigned char Parameter_UserParam4
= 0;
unsigned char Parameter_ServoNickControl
= 100;
struct mk_param_struct EE_Parameter
;
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)
//############################################################################
{
unsigned int timer
;
NeutralAccX
= 0;
NeutralAccY
= 0;
NeutralAccZ
= 0;
AdNeutralNick
= 0;
AdNeutralRoll
= 0;
AdNeutralGier
= 0;
GPS_Neutral
();
CalibrierMittelwert
();
timer
= SetDelay
(5);
while (!CheckDelay
(timer
));
CalibrierMittelwert
();
if((EE_Parameter.
GlobalConfig & CFG_HOEHENREGELUNG
)) // Höhenregelung aktiviert?
{
if((MessLuftdruck
> 950) || (MessLuftdruck
< 750)) SucheLuftruckOffset
();
}
AdNeutralNick
= abs(MesswertNick
);
AdNeutralRoll
= abs(MesswertRoll
);
AdNeutralGier
= abs(MesswertGier
);
NeutralAccY
= abs(Mittelwert_AccRoll
) / ACC_AMPLIFY
;
NeutralAccX
= abs(Mittelwert_AccNick
) / ACC_AMPLIFY
;
NeutralAccZ
= Aktuell_az
;
Mess_IntegralNick
= 0;
Mess_IntegralNick2
= 0;
Mess_IntegralRoll
= 0;
Mess_IntegralRoll2
= 0;
Mess_Integral_Gier
= 0;
MesswertNick
= 0;
MesswertRoll
= 0;
MesswertGier
= 0;
StartLuftdruck
= Luftdruck
;
HoeheD
= 0;
Mess_Integral_Hoch
= 0;
KompassStartwert
= KompassValue
;
beeptime
= 50;
}
//############################################################################
// 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
);
if(MessanzahlRoll
) (MesswertRoll
= AccumulateRoll
/ MessanzahlRoll
);
if(MessanzahlGier
) (MesswertGier
= AccumulateGier
/ MessanzahlGier
);
if(messanzahl_AccNick
) Mittelwert_AccNick
= ((long)Mittelwert_AccNick
* 7 + ((ACC_AMPLIFY
* (long)accumulate_AccNick
) / messanzahl_AccNick
)) / 8L;
if(messanzahl_AccRoll
) Mittelwert_AccRoll
= ((long)Mittelwert_AccRoll
* 7 + ((ACC_AMPLIFY
* (long)accumulate_AccRoll
) / messanzahl_AccRoll
)) / 8L;
if(messanzahl_AccHoch
) Mittelwert_AccHoch
= ((long)Mittelwert_AccHoch
* 7 + ((long)accumulate_AccHoch
) / messanzahl_AccHoch
) / 8L;
AccumulateNick
= 0; MessanzahlNick
= 0;
AccumulateRoll
= 0; MessanzahlRoll
= 0;
AccumulateGier
= 0; MessanzahlGier
= 0;
accumulate_AccRoll
= 0;messanzahl_AccRoll
= 0;
accumulate_AccNick
= 0;messanzahl_AccNick
= 0;
accumulate_AccHoch
= 0;messanzahl_AccHoch
= 0;
Integral_Gier
= Mess_Integral_Gier
;
IntegralNick
= Mess_IntegralNick
;
IntegralRoll
= Mess_IntegralRoll
;
IntegralNick2
= Mess_IntegralNick2
;
IntegralRoll2
= Mess_IntegralRoll2
;
// ADC einschalten
ANALOG_ON
;
//------------------------------------------------------------------------------
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)
//############################################################################
{
// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
ANALOG_OFF
;
if(MessanzahlNick
) (MesswertNick
= AccumulateNick
/ MessanzahlNick
);
if(MessanzahlRoll
) (MesswertRoll
= AccumulateRoll
/ MessanzahlRoll
);
if(MessanzahlGier
) (MesswertGier
= AccumulateGier
/ MessanzahlGier
);
if(messanzahl_AccNick
) Mittelwert_AccNick
= ((ACC_AMPLIFY
* (long)accumulate_AccNick
) / messanzahl_AccNick
);
if(messanzahl_AccRoll
) Mittelwert_AccRoll
= (ACC_AMPLIFY
* (long)accumulate_AccRoll
) / messanzahl_AccRoll
;
if(messanzahl_AccHoch
) Mittelwert_AccHoch
= ((long)accumulate_AccHoch
) / messanzahl_AccHoch
;
AccumulateNick
= 0; MessanzahlNick
= 0;
AccumulateRoll
= 0; MessanzahlRoll
= 0;
AccumulateGier
= 0; MessanzahlGier
= 0;
accumulate_AccRoll
= 0;messanzahl_AccRoll
= 0;
accumulate_AccNick
= 0;messanzahl_AccNick
= 0;
accumulate_AccHoch
= 0;messanzahl_AccHoch
= 0;
// 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;
}
//############################################################################
// Senden der Motorwerte per I2C-Bus
void SendMotorData
(void)
//############################################################################
{
if(MOTOR_OFF
|| !MotorenEin
)
{
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];
}
DebugOut.
Analog[12] = Motor_Vorne
;
DebugOut.
Analog[13] = Motor_Hinten
;
DebugOut.
Analog[14] = Motor_Links
;
DebugOut.
Analog[15] = Motor_Rechts
;
//Start I2C Interrupt Mode
twi_state
= 0;
motor
= 0;
i2c_start
();
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Konstanten
// + 0-250 -> normale Werte
// + 251 -> Poti1
// + 252 -> Poti2
// + 253 -> Poti3
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void DefaultKonstanten1
(void)
{
EE_Parameter.
Kanalbelegung[K_NICK
] = 3;
EE_Parameter.
Kanalbelegung[K_ROLL
] = 4;
EE_Parameter.
Kanalbelegung[K_GAS
] = 1;
EE_Parameter.
Kanalbelegung[K_GIER
] = 2;
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.
Hoehe_MinGas = 30;
EE_Parameter.
MaxHoehe = 251; // Wert : 0-32 251 -> Poti1
EE_Parameter.
Hoehe_P = 10; // Wert : 0-32
EE_Parameter.
Luftdruck_D = 70; // Wert : 0-250
EE_Parameter.
Hoehe_ACC_Wirkung = 30; // Wert : 0-250
EE_Parameter.
Hoehe_Verstaerkung = 2; // Wert : 0-50
EE_Parameter.
Stick_P = 2; //2 // Wert : 1-6
EE_Parameter.
Stick_D = 4; //8 // Wert : 0-64
EE_Parameter.
Gier_P = 16; // Wert : 1-20
EE_Parameter.
Gas_Min = 15; // Wert : 0-32
EE_Parameter.
Gas_Max = 250; // Wert : 33-250
EE_Parameter.
GyroAccFaktor = 26; // Wert : 1-64
EE_Parameter.
KompassWirkung = 16; // Wert : 0-250
EE_Parameter.
Gyro_P = 120; //80 // Wert : 0-250
EE_Parameter.
Gyro_I = 150; // Wert : 0-250
EE_Parameter.
UnterspannungsWarnung = 90; // Wert : 0-250
EE_Parameter.
NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust
EE_Parameter.
NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
EE_Parameter.
UfoAusrichtung = 0; // X oder + Formation
EE_Parameter.
I_Faktor = 5;
EE_Parameter.
UserParam1 = 16; //zur freien Verwendung, derzeit P-Anteil GPS
EE_Parameter.
UserParam2 = 2; //zur freien Verwendung, derzeit I-Anteil GPS
EE_Parameter.
UserParam3 = 12; //zur freien Verwendung, derzeit D-Anteil GPS
EE_Parameter.
UserParam4 = 0; //zur freien Verwendung
EE_Parameter.
ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
EE_Parameter.
ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
EE_Parameter.
ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
EE_Parameter.
ServoNickMin = 50; // Wert : 0-250 // Anschlag
EE_Parameter.
ServoNickMax = 150; // Wert : 0-250 // Anschlag
EE_Parameter.
ServoNickRefresh = 5;
memcpy(EE_Parameter.
Name, "Normal\0", 12);
}
void DefaultKonstanten2
(void)
{
EE_Parameter.
Kanalbelegung[K_NICK
] = 3;
EE_Parameter.
Kanalbelegung[K_ROLL
] = 4;
EE_Parameter.
Kanalbelegung[K_GAS
] = 1;
EE_Parameter.
Kanalbelegung[K_GIER
] = 2;
EE_Parameter.
Kanalbelegung[K_POTI1
] = 5;
EE_Parameter.
Kanalbelegung[K_POTI2
] = 6;
EE_Parameter.
Kanalbelegung[K_POTI3
] = 7;
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
EE_Parameter.
Luftdruck_D = 50; // Wert : 0-250
EE_Parameter.
Hoehe_ACC_Wirkung = 50; // Wert : 0-250
EE_Parameter.
Hoehe_Verstaerkung = 2; // Wert : 0-50
EE_Parameter.
Stick_P = 2; //2 // Wert : 1-6
EE_Parameter.
Stick_D = 0; //8 // Wert : 0-64
EE_Parameter.
Gier_P = 16; // Wert : 1-20
EE_Parameter.
Gas_Min = 15; // Wert : 0-32
EE_Parameter.
Gas_Max = 250; // Wert : 33-250
EE_Parameter.
GyroAccFaktor = 26; // Wert : 1-64
EE_Parameter.
KompassWirkung = 16; // Wert : 0-250
EE_Parameter.
Gyro_P = 175; //80 // Wert : 0-250
EE_Parameter.
Gyro_I = 175; // Wert : 0-250
EE_Parameter.
UnterspannungsWarnung = 90; // Wert : 0-250
EE_Parameter.
NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust
EE_Parameter.
NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
EE_Parameter.
UfoAusrichtung = 0; // X oder + Formation
EE_Parameter.
I_Faktor = 5;
EE_Parameter.
UserParam1 = 12; //zur freien Verwendung
EE_Parameter.
UserParam2 = 2; //zur freien Verwendung
EE_Parameter.
UserParam3 = 16; //zur freien Verwendung
EE_Parameter.
UserParam4 = 0; //zur freien Verwendung
EE_Parameter.
ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
EE_Parameter.
ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
EE_Parameter.
ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
EE_Parameter.
ServoNickMin = 50; // Wert : 0-250 // Anschlag
EE_Parameter.
ServoNickMax = 150; // Wert : 0-250 // Anschlag
EE_Parameter.
ServoNickRefresh = 5;
memcpy(EE_Parameter.
Name, "Kamera\0", 12);
}
//############################################################################
// Trägt ggf. das Poti als Parameter ein
void ParameterZuordnung
(void)
//############################################################################
{
#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
CHK_POTI
(Parameter_MaxHoehe
,EE_Parameter.
MaxHoehe,0,255);
CHK_POTI
(Parameter_Luftdruck_D
,EE_Parameter.
Luftdruck_D,0,100);
CHK_POTI
(Parameter_Hoehe_P
,EE_Parameter.
Hoehe_P,0,100);
CHK_POTI
(Parameter_Hoehe_ACC_Wirkung
,EE_Parameter.
Hoehe_ACC_Wirkung,0,255);
CHK_POTI
(Parameter_KompassWirkung
,EE_Parameter.
KompassWirkung,0,255);
CHK_POTI
(Parameter_Gyro_P
,EE_Parameter.
Gyro_P,10,255);
CHK_POTI
(Parameter_Gyro_I
,EE_Parameter.
Gyro_I,0,255);
CHK_POTI
(Parameter_I_Faktor
,EE_Parameter.
I_Faktor,0,255);
CHK_POTI
(Parameter_UserParam1
,EE_Parameter.
UserParam1,0,255);
CHK_POTI
(Parameter_UserParam2
,EE_Parameter.
UserParam2,0,255);
CHK_POTI
(Parameter_UserParam3
,EE_Parameter.
UserParam3,0,255);
CHK_POTI
(Parameter_UserParam4
,EE_Parameter.
UserParam4,0,255);
CHK_POTI
(Parameter_ServoNickControl
,EE_Parameter.
ServoNickControl,0,255);
CHK_POTI
(Parameter_ServoNickControl
,EE_Parameter.
ServoNickControl,0,255);
CHK_POTI
(Parameter_ServoNickControl
,EE_Parameter.
ServoNickControl,0,255);
Ki
= (float) Parameter_I_Faktor
* 0.0001;
MAX_GAS
= EE_Parameter.
Gas_Max;
MIN_GAS
= EE_Parameter.
Gas_Min;
}
//############################################################################
//
void MotorRegler
(void)
//############################################################################
{
int motorwert
,pd_ergebnis
,h
,tmp_int
;
int GierMischanteil
,GasMischanteil
;
static long SummeNick
=0,SummeRoll
=0;
static long sollGier
= 0,tmp_long
,tmp_long2
;
static int IntegralFehlerNick
= 0;
static int IntegralFehlerRoll
= 0;
static unsigned int RcLostTimer
;
static unsigned char delay_neutral
= 0;
static unsigned char delay_einschalten
= 0,delay_ausschalten
= 0;
static unsigned int modell_fliegt
= 0;
static int hoehenregler
= 0;
static char TimerWerteausgabe
= 0;
static char NeueKompassRichtungMerken
= 0;
Mittelwert
();
//****** GPS Daten holen ***************
short int n
;
if (gps_alive_cnt
> 0) gps_alive_cnt
--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout)
n
= Get_Rel_Position
();
if (n
== 0)
{
ROT_ON
; //led blitzen lassen
}
//******PROVISORISCH***************
GRN_ON
;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil
= PPM_in
[EE_Parameter.
Kanalbelegung[K_GAS
]] + 120;
if(GasMischanteil
< 0) GasMischanteil
= 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Empfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay
< 100)
{
if(!PcZugriff
) beeptime
= 500;
if(RcLostTimer
) RcLostTimer
--;
else
{
MotorenEin
= 0;
Notlandung
= 0;
}
ROT_ON
;
if(modell_fliegt
> 2000) // wahrscheinlich in der Luft --> langsam absenken
{
GasMischanteil
= EE_Parameter.
NotGas;
Notlandung
= 1;
PPM_in
[EE_Parameter.
Kanalbelegung[K_NICK
]] = 0;
PPM_in
[EE_Parameter.
Kanalbelegung[K_ROLL
]] = 0;
PPM_in
[EE_Parameter.
Kanalbelegung[K_GIER
]] = 0;
/* Poti1 = 65;
Poti2 = 48;
Poti3 = 0;
*/ }
else MotorenEin
= 0;
}
else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Empfang gut
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay
> 140)
{
Notlandung
= 0;
RcLostTimer
= EE_Parameter.
NotGasZeit * 50;
if(GasMischanteil
> 40)
{
if(modell_fliegt
< 0xffff) modell_fliegt
++;
}
if((modell_fliegt
< 200) || (GasMischanteil
< 40))
{
SummeNick
= 0;
SummeRoll
= 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
;
if(++delay_neutral
> 200) // nicht sofort
{
GRN_OFF
;
SetNeutral
();
MotorenEin
= 0;
delay_neutral
= 0;
modell_fliegt
= 0;
if(PPM_in
[EE_Parameter.
Kanalbelegung[K_NICK
]] > 70 || abs(PPM_in
[EE_Parameter.
Kanalbelegung[K_ROLL
]]) > 70)
{
if(PPM_in
[EE_Parameter.
Kanalbelegung[K_ROLL
]] > 70 && PPM_in
[EE_Parameter.
Kanalbelegung[K_NICK
]] < 70) setting
= 1;
if(PPM_in
[EE_Parameter.
Kanalbelegung[K_ROLL
]] > 70 && PPM_in
[EE_Parameter.
Kanalbelegung[K_NICK
]] > 70) setting
= 2;
if(PPM_in
[EE_Parameter.
Kanalbelegung[K_ROLL
]] < 70 && PPM_in
[EE_Parameter.
Kanalbelegung[K_NICK
]] > 70) setting
= 3;
if(PPM_in
[EE_Parameter.
Kanalbelegung[K_ROLL
]] <-70 && PPM_in
[EE_Parameter.
Kanalbelegung[K_NICK
]] > 70) setting
= 4;
if(PPM_in
[EE_Parameter.
Kanalbelegung[K_ROLL
]] <-70 && PPM_in
[EE_Parameter.
Kanalbelegung[K_NICK
]] < 70) setting
= 5;
eeprom_write_byte
(&EEPromArray
[EEPROM_ADR_ACTIVE_SET
], setting
); // aktiven Datensatz merken
}
ReadParameterSet
(GetActiveParamSetNumber
(), (unsigned char *) &EE_Parameter.
Kanalbelegung[0], STRUCT_PARAM_LAENGE
);
Piep
(GetActiveParamSetNumber
());
if((EE_Parameter.
GlobalConfig & CFG_HOEHENREGELUNG
)) // Höhenregelung aktiviert?
{
if((MessLuftdruck
> 950) || (MessLuftdruck
< 750)) SucheLuftruckOffset
();
}
GPS_Save_Home
(); //Daten sind jetzt hoffentlich verfuegbar
if (gps_home_position.
status > 0 )
{
Delay_ms
(1000); //akustisch verkuenden dass GPS Home Daten da sind
beeptime
= 2000;
Delay_ms
(500);
}
}
}
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;
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;
}
}
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
)
{
ParameterZuordnung
();
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;
StickGier
= -PPM_in
[EE_Parameter.
Kanalbelegung[K_GIER
]];
GyroFaktor
= ((float)Parameter_Gyro_P
+ 10.0) / 256.0;
IntegralFaktor
= ((float) Parameter_Gyro_I
) / 44000;
if(EE_Parameter.
GlobalConfig & CFG_HEADING_HOLD
) IntegralFaktor
= 0;
if(GyroFaktor
< 0) GyroFaktor
= 0;
if(IntegralFaktor
< 0) IntegralFaktor
= 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Notlandung
)
{
StickGier
= 0;
StickNick
= 0;
StickRoll
= 0;
GyroFaktor
= 0.1;
IntegralFaktor
= 0.005;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift kompensieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define DRIFT_FAKTOR 3
if(ZaehlMessungen
>= 1000 / DRIFT_FAKTOR
)
{
IntegralFehlerNick
= IntegralNick2
- IntegralNick
;
IntegralFehlerRoll
= IntegralRoll2
- IntegralRoll
;
ZaehlMessungen
= 0;
if(IntegralFehlerNick
> 500/DRIFT_FAKTOR
) AdNeutralNick
++;
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--;
//if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++;
ANALOG_OFF
; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Mess_IntegralNick2
= IntegralNick
;
Mess_IntegralRoll2
= IntegralRoll
;
Mess_Integral_Gier2
= Integral_Gier
;
ANALOG_ON
; // ADC einschalten
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Integrale auf ACC-Signal abgleichen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
tmp_long
= (long)(IntegralNick
/ EE_Parameter.
GyroAccFaktor - (long)Mittelwert_AccNick
) / 16;
tmp_long2
= (long)(IntegralRoll
/ EE_Parameter.
GyroAccFaktor - (long)Mittelwert_AccRoll
) / 16;
#define AUSGLEICH 500
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
;
ANALOG_OFF
; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Mess_IntegralNick
-= tmp_long
;
Mess_IntegralRoll
-= tmp_long2
;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
sollGier
= StickGier
;
if(abs(StickGier
) > 30)
{
if(!(EE_Parameter.
GlobalConfig & CFG_KOMPASS_FIX
)) NeueKompassRichtungMerken
= 1;
}
tmp_int
= EE_Parameter.
Gier_P * (sollGier
* abs(sollGier
)) / 256; // expo
Mess_Integral_Gier
-= tmp_int
;
if(Mess_Integral_Gier
> 30000) Mess_Integral_Gier
= 30000; // begrenzen
if(Mess_Integral_Gier
<-30000) Mess_Integral_Gier
=-30000;
ANALOG_ON
; // ADC einschalten
// Salvo 6.10.2007
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
if ((EE_Parameter.
GlobalConfig & CFG_GPS_AKTIV
) && (abs(StickRoll
) < GPS_STICK_HOLDOFF
) && (abs(StickNick
) < GPS_STICK_HOLDOFF
) && (gps_alive_cnt
> 0))
{
if (Parameter_MaxHoehe
> 200)
{
if ( gps_cmd
== GPS_CMD_REQ_HOLD
) gps_cmd
= GPS_CMD_STOP
; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd
= GPS_CMD_REQ_HOME
;
n
= GPS_CRTL
(gps_cmd
);
}
else
{
if ( gps_cmd
== GPS_CMD_REQ_HOME
) gps_cmd
= GPS_CMD_STOP
; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd
= GPS_CMD_REQ_HOLD
;
n
= GPS_CRTL
(gps_cmd
);
}
}
else (n
= GPS_CRTL
(GPS_CMD_STOP
)); //GPS Lageregelung beenden
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue
&& (EE_Parameter.
GlobalConfig & CFG_KOMPASS_AKTIV
))
{
if(NeueKompassRichtungMerken
)
{
KompassStartwert
= KompassValue
;
NeueKompassRichtungMerken
= 0;
}
ANALOG_OFF
; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Mess_Integral_Gier
-= (KompassRichtung
* Parameter_KompassWirkung
) / 32; // nach Kompass ausrichten
ANALOG_ON
; // ADC einschalten
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.
Sekunden++;
if(!TimerWerteausgabe
--)
{
TimerWerteausgabe
= 49;
DebugOut.
Analog[0] = MM3.
NickGrad;
DebugOut.
Analog[1] = MM3.
RollGrad;
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[7] = GasMischanteil
;
DebugOut.
Analog[8] = KompassValue
;
DebugOut.
Analog[9] = KompassRichtung
;
DebugOut.
Analog[10] = MM3_heading
();
//DebugOut.Analog[9] = MM3.x_axis;
//DebugOut.Analog[10] = MM3.y_axis;
//DebugOut.Analog[11] = MM3.z_axis;
//DebugOut.Analog[13] = KompassRichtung;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
MesswertNick
= IntegralNick
* IntegralFaktor
+ MesswertNick
* GyroFaktor
;
MesswertRoll
= IntegralRoll
* IntegralFaktor
+ MesswertRoll
* GyroFaktor
;
MesswertGier
= MesswertGier
* (GyroFaktor
/2) + Integral_Gier
* IntegralFaktor
;
// Maximalwerte abfangen
#define MAX_SENSOR 2048
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
;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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
{
int tmp_int
;
if(EE_Parameter.
GlobalConfig & CFG_HOEHEN_SCHALTER
) // Regler wird über Schalter gesteuert
{
if(Parameter_MaxHoehe
< 50)
{
//Salvo 10.10.2007 Um Absacken beim Einschalten zu verhindern
// SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
SollHoehe
= HoehenWert
- 0; // Parameter_MaxHoehe ist der PPM-Wert des Schalters
// Salvo End
HoehenReglerAktiv
= 0;
}
else
HoehenReglerAktiv
= 1;
}
else
{
SollHoehe
= Parameter_MaxHoehe
* EE_Parameter.
Hoehe_Verstaerkung - 20;
HoehenReglerAktiv
= 1;
}
if(Notlandung
) SollHoehe
= 0;
h
= HoehenWert
;
if((h
> SollHoehe
) && HoehenReglerAktiv
) // zu hoch --> drosseln
{ h
= ((h
- SollHoehe
) * (int) Parameter_Hoehe_P
) / 16; // Differenz bestimmen --> P-Anteil
h
= GasMischanteil
- h
; // vom Gas abziehen
h
-= (HoeheD
* Parameter_Luftdruck_D
)/8; // D-Anteil
tmp_int
= ((Mess_Integral_Hoch
/ 512) * (signed long) Parameter_Hoehe_ACC_Wirkung
) / 32;
if(tmp_int
> 50) tmp_int
= 50;
else if(tmp_int
< -50) tmp_int
= -50;
h
-= tmp_int
;
hoehenregler
= (hoehenregler
*15 + h
) / 16;
if(hoehenregler
< EE_Parameter.
Hoehe_MinGas) // nicht unter MIN
{
if(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
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gier-Anteil
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GierMischanteil
= MesswertGier
- sollGier
; // Regler für Gier
if(GierMischanteil
> 100) GierMischanteil
= 100;
if(GierMischanteil
< -100) GierMischanteil
= -100;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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
> 16000) SummeNick
= 16000;
if(SummeNick
< -16000) SummeNick
= -16000;
pd_ergebnis
= DiffNick
+ Ki
* SummeNick
; // PI-Regler für Nick
// Motor Vorn
motorwert
= GasMischanteil
+ pd_ergebnis
+ GierMischanteil
; // Mischer
if ((motorwert
< 0) | (GasMischanteil
< 10)) 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
;
if ((motorwert
< 0) | (GasMischanteil
< 10)) motorwert
= 0;
else if(motorwert
> MAX_GAS
) motorwert
= MAX_GAS
;
if (motorwert
< MIN_GAS
) motorwert
= MIN_GAS
;
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;
if(SummeRoll
> 16000) SummeRoll
= 16000;
if(SummeRoll
< -16000) SummeRoll
= -16000;
pd_ergebnis
= DiffRoll
+ Ki
* SummeRoll
; // PI-Regler für Roll
// Motor Links
motorwert
= GasMischanteil
+ pd_ergebnis
- GierMischanteil
;
if ((motorwert
< 0) | (GasMischanteil
< 10)) 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
;
if ((motorwert
< 0) | (GasMischanteil
< 10)) motorwert
= 0;
else if(motorwert
> MAX_GAS
) motorwert
= MAX_GAS
;
if (motorwert
< MIN_GAS
) motorwert
= MIN_GAS
;
Motor_Rechts
= motorwert
;
// +++++++++++++++++++++++++++++++++++++++++++++++
}