| /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" |