/branches/salvo_gps/GPS.c |
---|
13,7 → 13,7 |
Peter Muehlenbrock |
Auswertung der Daten vom GPS im ublox Format |
Hold Modus |
Stand 20.9.2007 |
Stand 21.9.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
284,10 → 284,18 |
short int GPS_CRTL(short int cmd) |
{ |
static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
static int int_east,int_north; //Integrierer |
int n; |
long int dist; |
switch (cmd) |
{ |
case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
// Noch einiges zu ueberlegen und zu tun |
return(GPS_STST_PEND); // noch warten |
break; |
// ****************************** |
case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
{ |
298,6 → 306,10 |
// aktuelle positionsdaten abespeichern |
if (gps_rel_act_position.status > 0) |
{ |
int_east = 0; |
int_north = 0; |
gps_reg_x = 0; |
gps_reg_y = 0; |
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 |
315,13 → 327,13 |
} |
break; |
case GPS_CMD_STOP_HOLD: // Lageregelung beenden |
cnt = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
gps_state = GPS_CRTL_IDLE; |
gps_int_x = 0; |
gps_int_y = 0; |
case GPS_CMD_STOP: // Lageregelung beenden |
cnt = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
gps_int_x = 0; |
gps_int_y = 0; |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_OK); |
break; |
345,24 → 357,25 |
signed int dist_north,dist_east; |
dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
int_east += dist_east; |
int_north += dist_north; |
#define DIST_MAX 100 //neu ab 19.9. 1900 Begrenzung |
if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
#define GPSINT_MAX 1000 |
if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
{ |
int_east -= dist_east; |
int_north -= dist_north; |
} |
#define DIST_MAX 200 //neu ab 19.9. 1900 Begrenzung |
if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
//PI Regler |
gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/64; // Integrator |
gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/64; |
#define GPSINT_MAX 256 // ************Kleiner machen |
if ((gps_int_x >= GPSINT_MAX) || (gps_int_y >= GPSINT_MAX) || (gps_int_x < -GPSINT_MAX) || (gps_int_y <= -GPSINT_MAX)) |
{ |
gps_int_x -= (dist_east * Parameter_UserParam1)/64; // Integrator stoppen |
gps_int_y -= (dist_north * Parameter_UserParam1)/64; |
} |
gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/8; // P Anteil dazu |
gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/8; |
gps_reg_x = ((int_east * Parameter_UserParam1)/16) + ((dist_east * Parameter_UserParam2)/8); // P+I Anteil |
gps_reg_y = ((int_north * Parameter_UserParam1)/16) + ((dist_north * Parameter_UserParam2)/8); // P+I Anteil |
//Richtung bezogen auf Nordpol bestimmen |
GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
369,7 → 382,7 |
//in Winkel 0..360 grad umrechnen |
if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
else if ((gps_reg_x < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
// Relative Richtung in bezug auf Nordachse des Kopters errechen |
n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
/branches/salvo_gps/fc.c |
---|
317,7 → 317,7 |
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_Min = 20; // Wert : 0-32 |
EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
329,8 → 329,8 |
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.UserParam2 = 2; //zur freien Verwendung, derzeit I-Anteil GPS |
EE_Parameter.UserParam3 = 64; //zur freien Verwendung, derzeit P-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 |
360,7 → 360,7 |
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_Min = 20; // Wert : 0-32 |
EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
724,7 → 724,7 |
ANALOG_ON; // ADC einschalten |
// Salvo Ersatzkompass 8.9.2007 ********************** |
if (Kompass_Neuer_Wert > 0) |
if ((Kompass_Neuer_Wert > 0)) |
{ |
Kompass_Neuer_Wert = 0; |
w = (abs(Mittelwert_AccNick)); |
764,7 → 764,8 |
// Salvo End ************************* |
// Salvo 15.9.2007 GPS Hold Aktiveren mit dem Hoehenschalter aber nur wenn Knueppel in Ruhelage sind |
if ((Parameter_MaxHoehe > 200) && ( (abs(StickRoll)) < GPS_STICK_HOLDOFF) && ( (abs(StickNick)) < GPS_STICK_HOLDOFF)) |
//if ((Parameter_MaxHoehe > 200) && ( (abs(StickRoll)) < GPS_STICK_HOLDOFF) && ( (abs(StickNick)) < GPS_STICK_HOLDOFF)) |
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && ((abs(StickRoll)) < GPS_STICK_HOLDOFF) && ( (abs(StickNick)) < GPS_STICK_HOLDOFF)) |
{ |
short int n; |
n= GPS_CRTL(GPS_CMD_REQ_HOLD); |
771,13 → 772,13 |
} |
else |
{ |
n= GPS_CRTL(GPS_CMD_STOP_HOLD); |
n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0)) |
{ |
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
/branches/salvo_gps/fc.h |
---|
20,7 → 20,7 |
// Salvo End |
//Salvo 16.9.2007 GPS_STICK_HOLDOFF *************** |
// Laut Datenblatt sind die Werte ueber Zeit und Temperatur sehr stabil. |
#define GPS_STICK_HOLDOFF 30 // Wenn der Nick oder Roll groesser ist, wird GPS_HOLD deaktiviert |
#define GPS_STICK_HOLDOFF 25 // Wenn der Nick oder Roll groesser ist, wird GPS_HOLD deaktiviert |
extern unsigned char Parameter_UserParam1 ; |
extern unsigned char Parameter_UserParam2 ; |
/branches/salvo_gps/gps.h |
---|
81,13 → 81,14 |
// Zustaende der zentralen GPS statemachine |
#define GPS_CRTL_IDLE 0 // |
#define GPS_CRTL_HOLD_ACTIVE 3 // Lageregelung aktiv |
#define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv |
#define GPS_CRTL_HOME_ACTIVE 2 // Rueckglug zur Basis Aktiv |
// Kommandokonstanten fuer die zentrale GPS statemachine |
#define GPS_CMD_REQ_INIT 0 // Initialisierung |
#define GPS_CMD_REQ_HOLD 1 // Lageregelung soll aktiviert werden |
#define GPS_CMD_STOP_HOLD 2 // Lageregelung soll deaktiviert werden |
#define GPS_CMD_WAIT 4 // nix tun, nur Status abfragen |
#define GPS_CMD_STOP 1 // Lageregelung soll deaktiviert werden |
#define GPS_CMD_REQ_HOLD 3 // Lageregelung soll aktiviert werden |
#define GPS_CMD_REQ_HOME 4 // Das Heimfliegen soll aktiviert werden |
// Statusmeldungen der zentralen GPS statemachine |
#define GPS_STST_OK 0 // Kommando erfolgreich und abgeschlossen |
95,5 → 96,5 |
#define GPS_STST_ERR 2 // Fehler |
// GPS Lageregler |
#define GPS_NICKROLL_MAX 30 //Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
#define GPS_NICKROLL_MAX 15 //Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
/branches/salvo_gps/timer0.c |
---|
10,6 → 10,9 |
volatile uint8_t Kompass_Neuer_Wert= 0; |
volatile unsigned int Kompass_Value_Old = 0; |
// Salvo End |
//Salvo 21.9.2007 |
short unsigned int Kompass_present= 0; //>0 bedeutet dass der Kompass vorhanden ist |
// Salvo End |
enum { |
STOP = 0, |
CK = 1, |
21,11 → 24,11 |
T0_RISING_EDGE = 7 |
}; |
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 13.9.2007 |
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 21.9.2007 |
/* |
Driftkompensation fuer Gyros verbessert |
Linearsensor mit fixem neutralwert |
Ersatzkompass abgeleitet aus magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion |
Linearsensor mit fixem Neutralwert |
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion |
*/ |
SIGNAL (SIG_OVERFLOW0) // 8kHz |
{ |
34,6 → 37,7 |
if(!cnt--) |
{ |
if (Kompass_present > 0) Kompass_present--; //Runterzaehlen. Wenn 0 ist der Kompass nicht vorhanden |
cnt = 9; |
cnt_1ms++; |
cnt_1ms %= 2; |
60,7 → 64,8 |
{ |
if((cntKompass) && (cntKompass < 4000)) |
{ |
// Salvo Kompassoffset 30.8.2007 *********** |
// Salvo Kompassoffset 30.8.2007 und 21.9.2007 *********** |
Kompass_present = 255; |
Kompass_Value_Old = KompassValue; |
KompassValue = cntKompass -KOMPASS_OFFSET; |
/branches/salvo_gps/timer0.h |
---|
17,7 → 17,8 |
extern volatile unsigned int cntKompass; |
extern int ServoValue; |
//Salvo 9.9.2007 |
//Salvo 21.9.2007 |
extern volatile uint8_t Kompass_Neuer_Wert; |
extern volatile unsigned int Kompass_Value_Old; |
extern unsigned short int Kompass_present; |
// Salvo End |