Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 161 → Rev 162

/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