// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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"
long Soll_Position_North
= 0;
long Soll_Position_East
= 0;
long GPS_Positionsabweichung_North
= 0;
long GPS_Positionsabweichung_East
= 0;
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;
signed int GPS_Nick_tmp
= 0; //(250308Kr)
signed int GPS_Roll_tmp
= 0; //(250308Kr)
signed int GPS_Nick_ACC
= 0; //(250308Kr)
signed int GPS_Roll_ACC
= 0; //(250308Kr)
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 50 //(031207Kr)
#define GPS_StickSense 12 //(241207Kr)
#define Limit_ACC_Anteil 25 //(250308Kr)
void gps_main
(void)
{
//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
= actualPos.
northing;
GPS_Home_East
= actualPos.
easting;
// 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
= actualPos.
northing;
Soll_Position_East
= actualPos.
easting;
beeptime
= 50;
gethome
= 1;
}
//schaltet bei Verwendung des GPS/MM3-Boards von Walter, GPS an RX von UART0 //(050208Kr)
if (MotorenEin
== 1)
{
SWITCH_GPS_2_RX
;
}
//löscht die GPS_Home_Position beim MotorAUSschalten wieder und resetet das Flag gethome //(280807Kr)
//zusätzlich wird bei Verwendung des GPS/MM3-Boards von Walter BT wieder an RX von UART0 geschaltet //(050208Kr)
if (MotorenEin
== 0)
{
SWITCH_BT_2_RX
;
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
= actualPos.
northing;
Soll_Position_East
= actualPos.
easting;
GPS_Nick
= 0;
GPS_Roll
= 0;
LED2_OFF
;
}
// 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
= actualPos.
northing; // Soll-Position wird geschrieben
Soll_Position_East
= actualPos.
easting; // 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 && 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 in Mittelstellung ist UND KEINE Notlandung, wird die zuletzt gelernte Soll_Position angeflogen
if(Poti3
< 150 && Notlandung
== 0) //(050208Kr)
{
GPS_Positionsabweichung_North
= (Soll_Position_North
- actualPos.
northing);
GPS_Positionsabweichung_East
= (Soll_Position_East
- actualPos.
easting);
LED2_ON
;
}
//andernfalls, wenn Schalter in max. Stellung ODER Notlandung und zusätzlich die GPS_Home_Positionen beim Motorenstarten gelernt
//werden konnten, wird zur GPS_Home_Position geflogen
if ((Poti3
>= 150 || Notlandung
== 1) && GPS_Home_North
!= 0 && GPS_Home_East
!= 0) //(050208Kr)
{
GPS_Positionsabweichung_North
= (GPS_Home_North
- actualPos.
northing);
GPS_Positionsabweichung_East
= (GPS_Home_East
- actualPos.
easting);
// 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
= actualPos.
northing;
Soll_Position_East
= actualPos.
easting;
LED2_ON
;
}
//verhindert, dass beim Umschalten von "GPS-HOLD" auf "HOMING" oder bei einer Notlandung 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 || Notlandung
== 1) && GPS_Home_North
== 0 && GPS_Home_East
== 0) //(050208Kr)
{
GPS_Positionsabweichung_North
= (Soll_Position_North
- actualPos.
northing);
GPS_Positionsabweichung_East
= (Soll_Position_East
- actualPos.
easting);
LED2_ON
;
beeptime
= 50;
}
//Berechnung der Einzelkomponenten des PD-Reglers
P_Einfluss_North
= (P_GPS_Verstaerkung
* GPS_Positionsabweichung_North
) / 2048;
D_Einfluss_North
= (D_GPS_Verstaerkung
* actualPos.
velNorth) / 256;
P_Einfluss_East
= (P_GPS_Verstaerkung
* GPS_Positionsabweichung_East
) / 2048;
D_Einfluss_East
= (D_GPS_Verstaerkung
* actualPos.
velEast) / 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
< -Limit_D_Anteil
) D_Einfluss_North
= -Limit_D_Anteil
;
if (D_Einfluss_East
< -Limit_D_Anteil
) D_Einfluss_East
= -Limit_D_Anteil
;
}
//PD-Regler
GPS_North
= (-P_Einfluss_North
+ D_Einfluss_North
);
GPS_East
= (P_Einfluss_East
- D_Einfluss_East
);
//Diese Variante funktioniert genausogut und ist noch optimierter von der Ausührungszeit und Genauigkeit
GPS_Nick_tmp
= (((long)-sin_i
(KompassValue
)*(long)GPS_East
) + ((long)cos_i
(KompassValue
)*(long)GPS_North
))/4096;
GPS_Roll_tmp
= (((long)cos_i
(KompassValue
)*(long)GPS_East
) + ((long)sin_i
(KompassValue
)*(long)GPS_North
))/4096;
//***************************************
//Einflüsse der ACC Sensoren (körperfest)
GPS_Nick_ACC
= ((long)ACC_GPS_Verstaerkung
* (long)Mittelwert_AccNick
) / 1024; //mindestens durch 23,34 dividiert, um Überläufe zu verhindern
GPS_Roll_ACC
= ((long)ACC_GPS_Verstaerkung
* (long)Mittelwert_AccRoll
) / 1024; //mindestens durch 23,34 dividiert, um Überläufe zu verhindern
if (GPS_Nick_ACC
> Limit_ACC_Anteil
) GPS_Nick_ACC
= Limit_ACC_Anteil
;
if (GPS_Roll_ACC
> Limit_ACC_Anteil
) GPS_Roll_ACC
= Limit_ACC_Anteil
;
if (GPS_Nick_ACC
< -Limit_ACC_Anteil
) GPS_Nick_ACC
= -Limit_ACC_Anteil
;
if (GPS_Roll_ACC
< -Limit_ACC_Anteil
) GPS_Roll_ACC
= -Limit_ACC_Anteil
;
GPS_Nick
= GPS_Nick_tmp
+ GPS_Nick_ACC
;
GPS_Roll
= GPS_Roll_tmp
+ GPS_Roll_ACC
;
//***************************************
// 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
< -GPS_Limit
) GPS_Nick
= -GPS_Limit
;
if (GPS_Roll
< -GPS_Limit
) GPS_Roll
= -GPS_Limit
;
//newData_navPosUtm = 0;
}
}
//***************************
// TO-DO:
// 1.) Vorzeichen von GPS_NICK_ACC und GPS_ROLL_ACC experimentell ermitteln
// 2.) Teilerfaktor (momentan 512) für sinnvollen Wertebereich ermitteln
// 3.) Testen ob Mittelwert_ACCXXX die richtige Signalgröße ist oder evntl. doch z.B. AdWertAccXXXX, ...
// 4.) Zeitverzögerung für P_Einfluss_XXXX nach neutralen Knüppeln integrieren
// 5.) Modularisieren / Unterfunktionen
// 6.) I-Anteil in die GPS-Regelung integrieren
// 7.) Updaterate für die zeitaufwändige Kompassberechnung auf 5Hz reduzieren
//***************************