/branches/salvo_gps/GPS.c |
---|
16,7 → 16,7 |
Auswertung der Daten vom GPS im ublox Format |
Hold Modus mit PID Regler |
Rückstuerz zur Basis Funktion |
Stand 8.10.2007 |
Stand 12.10.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
328,12 → 328,12 |
gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
//Richtung zur Home Positionbezogen auf Nordpol bestimmen |
//Richtung zur Home Position bezogen auf Nordpol bestimmen |
hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
// in Winkel 0...360 Grad umrechnen |
if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
else hdng_2home = (270 - hdng_2home); |
dist_2home = (int)get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
gps_state = GPS_CRTL_HOME_ACTIVE; |
return (GPS_STST_OK); |
} |
423,8 → 423,8 |
if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
{ |
if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen |
dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
gps_sub_state = GPS_HOME_FAST_IN_TOL; |
dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
gps_sub_state = GPS_HOME_FAST_IN_TOL; |
} |
else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
{ |
503,10 → 503,20 |
int_north += dist_north; |
diff_east += dist_east; // Differenz zur vorhergehenden East Position |
diff_north += dist_north; // Differenz zur vorhergehenden North Position |
/* |
diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern |
diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern |
*/ |
if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen |
{ |
diff_east_f = ((diff_east_f *3)/4) + (diff_east *1/4); //Differenzierer filtern |
diff_north_f = ((diff_north_f *3)/4) + (diff_north*1/4); //Differenzierer filtern |
} |
else // schwache Filterung |
{ |
// diff_east_f = diff_east; |
// diff_north_f = diff_north; |
diff_east_f = ((diff_east_f *2)/4) + (diff_east *2/4); //Differenzierer filtern |
diff_north_f = ((diff_north_f *2)/4) + (diff_north*2/4); //Differenzierer filtern |
} |
#define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung |
if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
{ |
551,8 → 561,8 |
else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
//PID Regler Werte aufsummieren |
gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
gps_reg_x = ((int)int_east1 + ((dist_east * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east_f * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil X Achse |
gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north_f * diff_v * Parameter_UserParam3)/10)); // I + P +D Anteil Y Achse |
//Ziel-Richtung bezogen auf Nordpol bestimmen |
GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y); |
/branches/salvo_gps/README_Gps.txt |
---|
1,6 → 1,6 |
********************************************************************* |
GPS Implementierung von Peter Muehlenbrock ("Salvo") für Mikrokopter/FlightCrtl |
Stand 9.10.2007 |
Stand 13.10.2007 |
Verwendung der SW ohne Gewaehr. Siehe auch die Lizenzbedingungen in File Licensce_LPGL.txt und Licensce_GPL.txt |
********************************************************************* |
Hardware-Voraussetzungen: |
28,7 → 28,7 |
Hier kann und muss gespielt werden.Alle Parameter koennen direkt im Mikrokoptertool in den Settings eingestellt werden. |
Standardwerte bei kaum Wind sind 8 für den P-Anteil, 1 für den I-Anteil und 12 für den D-Anteil. |
Standardwerte bei leichtem verhältnissen sind 12 für den P-Anteil, 2 für den I-Anteil und 18 für den D-Anteil. |
Standardwerte bei rauheren verhältnissen sind 16 für den P-Anteil, 2 für den I-Anteil und 24 für den D-Anteil. |
Standardwerte bei rauheren verhältnissen sind 16 für den P-Anteil, 2 für den I-Anteil und 20 für den D-Anteil. |
Je größer die Werte des "ruckeliger" reagiert die Regelung aber desto schneller und stärker greift sie auch. |
Wenn alle 0 sind, ist der Regler deaktiviert. |
53,12 → 53,15 |
GPS Rücksturz zur Basis (GPS Home) Funktion |
Voraussetzungen wie bei GPS Hold. |
Die Funktion kann im Flug aktivert werden durch Setzen des Hoehenreglerschalters. |
Die Funktion kann im Flug aktiviert werden durch Setzen des Hoehenreglerschalters. |
Holgers Code habe ich so abgeändert daß der Höhenregler mit "Parameter_MaxHoehe" ab 50 aktiviert wird. |
Mit einem Schalter mit Neutrallage können damit die Funktionen |
Alles aus - Hoehenregler ein, GPS Home Aus - Hoehenregler Ein UND GPS Home ein |
aktiviert werden. Ist noch nicht die beste Lösung und werde ich noch anpassen |
Weitere Änderungen: |
Bei Unterschreiten der eingestellten Warnschwelle für UBAT ertönt zunächst wie gewohnt der Piepser. Geht die Spannung weiter |
runter wird zwansgweise die Gas einstellung langsam reduziert um den Kopter zum Landen zu bringen |
Bekannte Schwächen: |
Bei längerer Neigung weicht der Ersatzkompass ab, was zu Lageregelungsfehlern bis zum Ausbrechen führen kann. |
/branches/salvo_gps/fc.c |
---|
81,9 → 81,11 |
unsigned char MAX_GAS,MIN_GAS; |
unsigned char Notlandung = 0; |
unsigned char HoehenReglerAktiv = 0; |
//Salvo 4.10.2007 |
//Salvo 12.10.2007 |
uint8_t magkompass_ok=0; |
uint8_t gps_cmd = GPS_CMD_STOP; |
static int ubat_cnt =0; |
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung |
//Salvo End |
//Salvo 2.9.2007 Ersatzkompass |
183,8 → 185,10 |
Mess_Integral_Hoch = 0; |
KompassStartwert = KompassValue; |
beeptime = 50; |
//Salvo 4.9.2007 Ersatzkompass |
//Salvo 13.10.2007 Ersatzkompass |
GyroKomp_Int = 0; |
gas_mittel = 30; |
gas_actual = gas_mittel; |
// Salvo End |
} |
326,14 → 330,14 |
EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 102; // 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 = 0; //zur freien Verwendung, derzeit P-Anteil GPS |
EE_Parameter.UserParam2 = 0; //zur freien Verwendung, derzeit I-Anteil GPS |
EE_Parameter.UserParam3 = 0; //zur freien Verwendung, derzeit D-Anteil GPS |
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 |
369,14 → 373,14 |
EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 102; // 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 = 0; //zur freien Verwendung |
EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
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.UserParam3 = 0; //zur freien Verwendung |
EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
457,12 → 461,45 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gaswert ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen |
// und dieser dann langsam zwangsweise reduziert |
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
if (UBat <= EE_Parameter.UnterspannungsWarnung - 2) //Unterhalb der Piepser Schwelle aktivieren |
{ |
if (ubat_cnt > 800) |
{ |
ubat_cnt = 0; |
if (gas_actual > ((gas_mittel*13)/15)) gas_actual--; |
} |
else ubat_cnt++; |
if (GasMischanteil > gas_actual) GasMischanteil = gas_actual; |
} |
else //Falls UBAT wieder ok ist |
{ |
if (ubat_cnt > 1000) |
{ |
gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern |
gas_actual = GasMischanteil; |
} |
else |
{ |
ubat_cnt++; |
if ((ubat_cnt % 10) == 0) |
{ |
if (gas_actual < GasMischanteil) gas_actual++; |
else gas_actual = GasMischanteil; |
} |
} |
GasMischanteil = gas_actual; |
} |
ANALOG_ON; // ADC einschalten |
// Salvo End |
if(GasMischanteil < 0) GasMischanteil = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang schlecht |
// Empfang schlecht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay < 100) |
{ |
833,28 → 870,29 |
TimerWerteausgabe = 49; |
// DebugOut.Analog[0] = MesswertNick; |
// DebugOut.Analog[1] = MesswertRoll; |
DebugOut.Analog[0] = gps_sub_state; |
// DebugOut.Analog[0] = gps_sub_state; |
DebugOut.Analog[1] = dist_2home; |
/* DebugOut.Analog[1] = dist_2home; |
DebugOut.Analog[2] = hdng_2home; |
*/ |
DebugOut.Analog[3] = gps_rel_hold_position.utm_east; |
DebugOut.Analog[4] = gps_rel_hold_position.utm_north; |
/* DebugOut.Analog[2] = MesswertGier; |
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
/* DebugOut.Analog[3] = gps_rel_hold_position.utm_east; |
DebugOut.Analog[4] = gps_rel_hold_position.utm_north; |
*/ |
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[2] = MesswertGier; |
DebugOut.Analog[7] = GasMischanteil; |
*/ |
DebugOut.Analog[7] = dist_flown; |
// DebugOut.Analog[7] = dist_flown; |
DebugOut.Analog[8] = KompassValue; |
// DebugOut.Analog[8] = dist_frm_start_north; |
935,7 → 973,7 |
{ |
//Salvo 10.10.2007 Um Absacken beim Einschalten zu verhindern |
// SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
SollHoehe = HoehenWert -10; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
SollHoehe = HoehenWert - 0; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
// Salvo End |
HoehenReglerAktiv = 0; |
} |
/branches/salvo_gps/gps.h |
---|
1,7 → 1,7 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Peter Muehlenbrock ("Salvo") |
// Definitionen fuer Modul GPS |
// Stand 9.10.007 |
// Stand 12.10.007 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
extern signed int GPS_Nick; |
extern signed int GPS_Roll; |
118,15 → 118,15 |
#define GPS_DIFF_FAST_MAX_D 80 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm im Fast mode |
// P-Regler Verstaerkung |
#define GPS_PROP_NRML_V 2 //maximale Verstaerkung * 2 |
#define GPS_PROP_FAST_V 2 //maximale Verstaerkung * 2 |
#define GPS_PROP_FAST_V 2 //maximale Verstaerkung * 2 im Fast mode |
// GPS G2T /Go to Target Regler |
#define GPS_G2T_DIST_MAX_STOP 60 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm) |
#define GPS_G2T_DIST_HOLD 30 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler geschwindigkeit eingeregelt |
#define GPS_G2T_V_MAX 8 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird. |
#define GPS_G2T_V_RAMP_DWN 4 // Geschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen |
#define GPS_G2T_DIST_MAX_STOP 50 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm) |
#define GPS_G2T_DIST_HOLD 30 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler Geschwindigkeit eingeregelt |
#define GPS_G2T_V_MAX 10 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird. |
#define GPS_G2T_V_RAMP_DWN 4 // Geschwindigkeit (in 10cm/0.25ekunden) in der Naehe der Home Position um abzubremsen |
#define GPS_G2T_V_MIN 2 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition. |
#define GPS_G2T_FAST_TOL 80 // Bei grosser Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
#define GPS_G2T_NRML_TOL 50 // Bei kleiner Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
#define GPS_G2T_FAST_TOL 60 // Bei grosser Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
#define GPS_G2T_NRML_TOL 40 // Bei kleiner Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
/branches/salvo_gps/math.c |
---|
16,7 → 16,7 |
Winkelfunktionen sin, cos und arctan in |
brute-force Art: Sehr Schnell, nicht sonderlich genau, aber ausreichend |
get_dist Funktion fuer Entfernungsermittlung |
Stand 1.10.2007 |
Stand 12.10.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |