// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 10.2007 by Jochen Kromayer
// + es handelt sich um eine Beta-Software, die zu nichtkommerziellen Zwecken frei veröffentlich und weitergegeben werden darf. Für evtl.
// + Personen- und/der Sachschäden ist jegliche Haftung ausgeschlossen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
//Sinusfunktion von 0° bis 359°. Ergebnis wurde mit 1024 multipliziert, damit als Definition platzsparend int
//verwendet werden kann. D.h., es muss also in der späteren Berechnung noch durch 1024 dividiert werden //(051207Kr)
const int c_sin
[] = {0,18,36,54,71,89,107,125,143,160,178,195,213,230,248,265,282,299,316,333,350,367,384,400,
416,433,449,465,481,496,512,527,543,558,573,587,602,616,630,644,658,672,685,698,711,724,737,749,
761,773,784,796,807,818,828,839,849,859,868,878,887,896,904,912,920,928,935,943,949,956,962,968,
974,979,984,989,994,998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024,1024,1024,1023,1023,1022,1020,
1018,1016,1014,1011,1008,1005,1002,998,994,989,984,979,974,968,962,956,949,943,935,928,920,912,904,896,
887,878,868,859,849,839,828,818,807,796,784,773,761,749,737,724,711,698,685,672,658,644,630,616,
602,587,573,558,543,527,512,496,481,465,449,433,416,400,384,367,350,333,316,299,282,265,248,230,
213,195,178,160,143,125,107,89,71,54,36,18,0,-18,-36,-54,-71,-89,-107,-125,-143,-160,-178,-195,
-213,-230,-248,-265,-282,-299,-316,-333,-350,-367,-384,-400,-416,-433,-449,-465,-481,-496,-512,-527,-543,-558,-573,-587,
-602,-616,-630,-644,-658,-672,-685,-698,-711,-724,-737,-749,-761,-773,-784,-796,-807,-818,-828,-839,-849,-859,-868,-878,
-887,-896,-904,-912,-920,-928,-935,-943,-949,-956,-962,-968,-974,-979,-984,-989,-994,-998,-1002,-1005,-1008,-1011,-1014,-1016,
-1018,-1020,-1022,-1023,-1023,-1024,-1024,-1024,-1023,-1023,-1022,-1020,-1018,-1016,-1014,-1011,-1008,-1005,-1002,-998,-994,-989,-984,-979,
-974,-968,-962,-956,-949,-943,-935,-928,-920,-912,-904,-896,-887,-878,-868,-859,-849,-839,-828,-818,-807,-796,-784,-773,
-761,-749,-737,-724,-711,-698,-685,-672,-658,-644,-630,-616,-602,-587,-573,-558,-543,-527,-512,-496,-481,-465,-449,-433,
-416,-400,-384,-367,-350,-333,-316,-299,-282,-265,-248,-230,-213,-195,-178,-160,-143,-125,-107,-89,-71,-54,-36,-18};
//Cosinusfunktion von 0° bis 359°. Ergebnis wurde mit 1024 multipliziert, damit als Definition platzsparend int
//verwendet werden kann. D.h., es muss also in der späteren Berechnung noch durch 1024 dividiert werden //(051207Kr)
const int c_cos
[] = {1024,1024,1023,1023,1022,1020,1018,1016,1014,1011,1008,1005,1002,998,994,989,984,979,974,968,962,956,949,943,
935,928,920,912,904,896,887,878,868,859,849,839,828,818,807,796,784,773,761,749,737,724,711,698,
685,672,658,644,630,616,602,587,573,558,543,527,512,496,481,465,449,433,416,400,384,367,350,333,
316,299,282,265,248,230,213,195,178,160,143,125,107,89,71,54,36,18,0,-18,-36,-54,-71,-89,
-107,-125,-143,-160,-178,-195,-213,-230,-248,-265,-282,-299,-316,-333,-350,-367,-384,-400,-416,-433,-449,-465,-481,-496,
-512,-527,-543,-558,-573,-587,-602,-616,-630,-644,-658,-672,-685,-698,-711,-724,-737,-749,-761,-773,-784,-796,-807,-818,
-828,-839,-849,-859,-868,-878,-887,-896,-904,-912,-920,-928,-935,-943,-949,-956,-962,-968,-974,-979,-984,-989,-994,-998,
-1002,-1005,-1008,-1011,-1014,-1016,-1018,-1020,-1022,-1023,-1023,-1024,-1024,-1024,-1023,-1023,-1022,-1020,-1018,-1016,-1014,-1011,-1008,-1005,
-1002,-998,-994,-989,-984,-979,-974,-968,-962,-956,-949,-943,-935,-928,-920,-912,-904,-896,-887,-878,-868,-859,-849,-839,
-828,-818,-807,-796,-784,-773,-761,-749,-737,-724,-711,-698,-685,-672,-658,-644,-630,-616,-602,-587,-573,-558,-543,-527,
-512,-496,-481,-465,-449,-433,-416,-400,-384,-367,-350,-333,-316,-299,-282,-265,-248,-230,-213,-195,-178,-160,-143,-125,
-107,-89,-71,-54,-36,-18,0,18,36,54,71,89,107,125,143,160,178,195,213,230,248,265,282,299,
316,333,350,367,384,400,416,433,449,465,481,496,512,527,543,558,573,587,602,616,630,644,658,672,
685,698,711,724,737,749,761,773,784,796,807,818,828,839,849,859,868,878,887,896,904,912,920,928,
935,943,949,956,962,968,974,979,984,989,994,998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024};
long Soll_Position_North
= 0;
long Soll_Position_East
= 0;
long GPS_Positionsabweichung_North
= 0;
long GPS_Positionsabweichung_East
= 0;
long GPS_Geschwindigkeit_North
= 0;
long GPS_Geschwindigkeit_East
= 0;
signed int GPS_StickSense
= 15;
long P_Einfluss_North
= 0;
long D_Einfluss_North
= 0;
long P_Einfluss_East
= 0;
long D_Einfluss_East
= 0;
signed int GPS_North
= 0;
signed int GPS_East
= 0;
signed int GPS_Nick
= 0;
signed int GPS_Roll
= 0;
volatile char gethome
= 0; //Flag ob GPS_Home_Position gelernt ist //(280807Kr)
long GPS_Home_North
= 0;
long GPS_Home_East
= 0;
unsigned char blinkcount_LED2
= 0; //Hilfszähler für die blinkende LED (010907Kr)
#define GPS_Limit 35 //(031207Kr)
#define Limit_D_Anteil 30 //(031207Kr)
//**************************** MIRCOS DEFINITIONSANFANG FÜR DIE FILTERUNG
// Filterung der GPS Messwerte vom Ublox-Empfänger. Es wird der gleitende Durchschnitt aus n Messwerten gebildet
#define MITTELWERTZAHL 8 // Anzahl der Messwerte fuer gleitenden Mittelwert
volatile char NeuerMittelwert
= 0; //Flag ob ein neuer Mittelwert vorliegt, der mit GPS Frequenz berechnet wird, da nur dann neue Messwerte vom GPS vorliegen
volatile unsigned char FilterPos
= 0;
volatile long NORTH_MITTEL
= 0;
volatile long EAST_MITTEL
= 0;
volatile long long NORTH_LONG_LONG
= 0;
volatile long long EAST_LONG_LONG
= 0;
volatile int filterfilled
= 0;
typedef struct {
long NORTH
[MITTELWERTZAHL
];
} FILTER_NORTH
; // Filterdefinition für die Positions-Nordwerte aus dem GPS-Empfänger
typedef struct {
long EAST
[MITTELWERTZAHL
];
} FILTER_EAST
; // Filterdefinition für die Positions-Ostwerte aus dem GPS-Empfänger
FILTER_NORTH filter_north
;
FILTER_EAST filter_east
;
//**************************** MIRCOS DEFINITIONSENDE FÜR DIE FILTERUNG
void gps_main
(void)
{
//**************************** MIRCOS FILTERUNG ANFANG
//bilde neuen Positionsmittelwert, wenn 3D-Fix und neue GPS-Daten vorliegen
// if(actualPos.GPSFix == 3 && newData_navPosUtm == 1)
if(actualPos.
GPSFix == 3) // hier umkommentieren, wenn dauernd (also nicht nur mit der GPS-Updaterate) die GPS-Berechnung durchgeführt werden soll
{
filter_north.
NORTH[FilterPos
] = actualPos.
northing;
filter_east.
EAST[FilterPos
] = actualPos.
easting;
FilterPos
++;
if (FilterPos
==MITTELWERTZAHL
) FilterPos
=0;
if (filterfilled
<MITTELWERTZAHL
) filterfilled
++;
NORTH_LONG_LONG
= 0;
EAST_LONG_LONG
= 0;
for (int i
=0; i
<filterfilled
; i
++)
{
NORTH_LONG_LONG
+= filter_north.
NORTH[i
];
EAST_LONG_LONG
+= filter_east.
EAST[i
];
}
NORTH_LONG_LONG
/= filterfilled
;
EAST_LONG_LONG
/= filterfilled
;
NORTH_MITTEL
= NORTH_LONG_LONG
;
EAST_MITTEL
= EAST_LONG_LONG
;
NeuerMittelwert
= 1;
newData_navPosUtm
= 0;
}
//******************************* MIRCOS FILTERUNG ENDE
//wenn kein 3D-Fix vorhanden, über das Poti jedoch irgendeine GPS Funktion aktiv ist, werden die GPS-Steuerbefehle
//auf Null gesetzt, um unkontrolliertes Wegfliegen durch das GPS zu verhindern. Dadurch wird abgesichert
//(auch im laufenden Flug), dass bei einem Verlust der GPS-Verbindung GPS deaktivert wird
if (actualPos.
GPSFix < 3 && Poti3
> 70)
{
GPS_Nick
= 0;
GPS_Roll
= 0;
LED2_OFF
;
beeptime
= 50;
}
// speichert die GPS_Home_Position beim 3D-Fix, MotorEINschalten und wenn noch nicht zu weit weggeflogen wurde
// (modell_fliegt_gps < 2000) einmalig ab. Die Flugzeitabfrage sichert ab, dass bei einem Losfliegen ohne 3D-Fix die Home_Positinen
// irgendwann später dort gelernt werden, wo dann zu ersten mal ein 3D-Fix vorhanden ist
if (actualPos.
GPSFix == 3 && MotorenEin
== 1 && gethome
== 0 && modell_fliegt_gps
< 2000)
{
GPS_Home_North
= NORTH_MITTEL
;
GPS_Home_East
= EAST_MITTEL
;
// Muss hier auch eingelernt werden, damit falls das Poti3 beim Motorenstarten in
// Mittelstellung sein sollte, trotzdem schon eine Sollposition vorliegt. Andernfalls würde
// GPS_Nick und _Roll maximal werden bis zum ersten Mal Stick_Nick und _Roll gesteuert würde
Soll_Position_North
= NORTH_MITTEL
;
Soll_Position_East
= EAST_MITTEL
;
beeptime
= 50;
gethome
= 1;
}
//löscht die GPS_Home_Position beim MotorAUSschalten wieder und resetet das Flag gethome //(280807Kr)
if (MotorenEin
== 0)
{
GPS_Home_North
= 0;
GPS_Home_East
= 0;
gethome
= 0;
modell_fliegt_gps
= 0;
}
// wenn Schalter ausgeschaltet, dann wird die LED2 ausgeschaltet und das GPS Steuerkommando gelöscht
if(Poti3
< 70)
{
// Sollpositionen müssen auch hier beschrieben werden, damit ausgeschlossen ist, dass falls gelandet wird und die GPS-Funktion
// über Poti3 deaktivert und anschließend wieder aktiviert wird keine Soll-Positionen vorliegen.
Soll_Position_North
= NORTH_MITTEL
;
Soll_Position_East
= EAST_MITTEL
;
GPS_Nick
= 0;
GPS_Roll
= 0;
LED2_OFF
;
//hierdurch soll verhindert werden, dass wenn nach schonmal aktiver GPS Funktion (und damit 3D-fix) der GPS-Empfänger
//abgezogen werden sollte weiterhin ein 3D-fix angezeigt wird. Grund ist, dass in diesem Fall in der UART.c nach einem
//Abziehen der Status nicht mehr auf Null zurückgesetzt wird. Dies geschied nun beim Ausschalten der GPS-Funktion.
//Sie ist dann ohne erneuten 3D-Fix nicht mehr aktivierbar. (111107Kr)
actualPos.
GPSFix = 0;
}
// wenn 3D-Fix, Schalter im mittleren Bereich und vom Pilot gesteuert wird, wird ständig neue GPS-Sollposition eingelernt
if(actualPos.
GPSFix == 3 && Poti3
> 70 && Poti3
< 150 && (StickNick
> GPS_StickSense
|| StickNick
< -GPS_StickSense
|| StickRoll
> GPS_StickSense
|| StickRoll
< -GPS_StickSense
))
{
GPS_Nick
= 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert
GPS_Roll
= 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert
Soll_Position_North
= NORTH_MITTEL
; // Soll-Position wird geschrieben
Soll_Position_East
= EAST_MITTEL
; // Soll-Position wird geschrieben
//++++++++++++++++++++++++++++++++++++
//Sorgt lediglich dafür, dass die LED2 langsam blinkt. Dauer ist durch die "> x" Abfrage festgelegt
//++++++++++++++++++++++++++++++++++++
if(blinkcount_LED2
>= 200) // maximal erlaubt 255 wegen unsigned char
{
LED2_FLASH
;
blinkcount_LED2
= 0;
}
blinkcount_LED2
++;
//++++++++++++++++++++++++++++++++++++
}
// wenn 3D-Fix, Schalter im mittleren Bereich (oder höher), neuer Mittelwert vorhanden, wenigstens einmal Sollposition gelernt und der Pilot nicht selbst steuert wird eine GPS-Funktion aktiviert
if (actualPos.
GPSFix == 3 && Poti3
> 70 && NeuerMittelwert
== 1 && Soll_Position_North
!= 0 && Soll_Position_East
!= 0 && StickNick
<= GPS_StickSense
&& StickNick
>= -GPS_StickSense
&& StickRoll
<= GPS_StickSense
&& StickRoll
>= -GPS_StickSense
)
{
//Berechnung der GPS-Positionsabweichung je nach aktivierter GPS-Funktion (GPS_HOLD oder HOMING)
//DYNAMISCHES GPS-HOLD, wenn Schalter noch in Mittelstellung ist wird die zuletzt gelernte Soll_Position angeflogen
if(Poti3
< 150)
{
GPS_Positionsabweichung_North
= (Soll_Position_North
- NORTH_MITTEL
);
GPS_Positionsabweichung_East
= (Soll_Position_East
- EAST_MITTEL
);
LED2_ON
;
}
//andernfalls, wenn Schalter in max. Stellung und zusätzlich die GPS_Home_Positionen beim Motorenstarten gelernt
//werden konnten, wird zur GPS_Home_Position geflogen
if (Poti3
>= 150 && GPS_Home_North
!= 0 && GPS_Home_East
!= 0)
{
GPS_Positionsabweichung_North
= (GPS_Home_North
- NORTH_MITTEL
);
GPS_Positionsabweichung_East
= (GPS_Home_East
- EAST_MITTEL
);
// durch das ständige Neulernen der Soll_Positionen während HOMING wird erreicht, dass der MK nach dem Umschalten
// von HOMING zurück zu GPS-HOLD nicht wieder an die alte Soll_Position zurück fliegt
Soll_Position_North
= NORTH_MITTEL
;
Soll_Position_East
= EAST_MITTEL
;
LED2_ON
;
}
//verhindert, dass beim Umschalten von Mittelstellung (GPS_HOLD) auf Maximalstellung (HOMING) OHNE gelernte Homeposition
//die GPS Funktion unkontrolliert steuert. Daher wird GPS_HOLD aktiv gehalten, die Sollpositon wird bei Stickbefehlen
//jedoch nicht wie beim dynamischen GPS_HOLD überschrieben. Zusätzlich wird der Summer eingeschaltet.
if (Poti3
>= 150 && GPS_Home_North
== 0 && GPS_Home_East
== 0)
{
GPS_Positionsabweichung_North
= (Soll_Position_North
- NORTH_MITTEL
);
GPS_Positionsabweichung_East
= (Soll_Position_East
- EAST_MITTEL
);
LED2_ON
;
beeptime
= 50;
}
//GPS-Geschwindigkeiten
GPS_Geschwindigkeit_North
= actualPos.
velNorth; //actualPos.velNorth ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Nord-Süd Richtung
GPS_Geschwindigkeit_East
= actualPos.
velEast; //actualPos.velEast ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Ost-West Richtung
//Berechnung der Einzelkomponenten des PD-Reglers
P_Einfluss_North
= (P_GPS_Verstaerkung
* GPS_Positionsabweichung_North
) / 2048;
D_Einfluss_North
= (D_GPS_Verstaerkung
* GPS_Geschwindigkeit_North
) / 256;
P_Einfluss_East
= (P_GPS_Verstaerkung
* GPS_Positionsabweichung_East
) / 2048;
D_Einfluss_East
= (D_GPS_Verstaerkung
* GPS_Geschwindigkeit_East
) / 256;
// Begrenzung des maximalen D-Anteils für pos. und neg. Werte. Grenze muss so geählt werden,
// dass das Limit im normalen Position-Hold nicht erreicht wird und somit keinen Einfluss hat.
// Grund: Beim Zufliegen auf weiter entfernte Ziele bremst der D-Anteil kurz vor dem Ziel den Mikrokopter sehr stark ab.
// Dieses Limit wirkt nur, wenn sich der MK im HOMING Modus befindet und sich noch weit (>5m) von der Home-Position weg befindet.
if (Poti3
>= 150 && GPS_Home_North
!= 0 && GPS_Home_East
!= 0 && (abs(GPS_Positionsabweichung_North
) > 500 || abs(GPS_Positionsabweichung_East
) > 500))
{
if (D_Einfluss_North
> Limit_D_Anteil
) D_Einfluss_North
= Limit_D_Anteil
;
if (D_Einfluss_East
> Limit_D_Anteil
) D_Einfluss_East
= Limit_D_Anteil
;
if (D_Einfluss_North
< -1 * Limit_D_Anteil
) D_Einfluss_North
= -1 * Limit_D_Anteil
;
if (D_Einfluss_East
< -1 * Limit_D_Anteil
) D_Einfluss_East
= -1 * Limit_D_Anteil
;
}
// PD-Regler
GPS_North
= (-P_Einfluss_North
+ D_Einfluss_North
);
GPS_East
= (P_Einfluss_East
- D_Einfluss_East
);
//Umrechnen vom globalen North- bzw. East- in das körperfeste X- bzw. Y- Koordinatensystem
GPS_Nick
= ((c_cos
[KompassValue
]*GPS_North
)/1024 - (c_sin
[KompassValue
]*GPS_East
)/1024);
GPS_Roll
= ((c_cos
[KompassValue
]*GPS_East
)/1024 + (c_sin
[KompassValue
]*GPS_North
)/1024);
// Begrenzung des maximalen GPS Einflusses für positive und negative Werte
if (GPS_Nick
> GPS_Limit
) GPS_Nick
= GPS_Limit
;
if (GPS_Roll
> GPS_Limit
) GPS_Roll
= GPS_Limit
;
if (GPS_Nick
< -1 * GPS_Limit
) GPS_Nick
= -1 * GPS_Limit
;
if (GPS_Roll
< -1 * GPS_Limit
) GPS_Roll
= -1 * GPS_Limit
;
//Funktion wird dadurch nur alle 250 ms aufgerufen, wenn neue Mittelwerte aus den GPS-Daten vorliegen.
//FUNKTIONIERT MOMENTAN NOCH NICHT, DA STÄNDIG OHNE UNTERBRECHUNG NEUE MITTELWERTE BERECHNET WERDEN.
//GRUND: DIE ABFRAGE NACH NEUE GPS-DATEN ERFOLGT NOCH NICHT
NeuerMittelwert
= 0;
}
}