Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 155 → Rev 156

/branches/salvo_gps/GPS.c
13,10 → 13,11
Peter Muehlenbrock
Auswertung der Daten vom GPS im ublox Format
Regelung fuer GPS noch nicht implementiert
Stand 11.9.2007
Stand 14.9.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
//#include "gps.h"
 
// Defines fuer ublox Messageformat um Auswertung zu steuern
#define UBLOX_IDLE 0
51,24 → 52,72
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
 
short int Get_GPS_data(void);
 
NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
NAV_STATUS_t actual_status; // Aktueller Nav Status
NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
 
GPS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt
GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position
 
void GPS_Neutral(void) // Initialisierung
// Initialisierung
void GPS_Neutral(void)
{
ublox_msg_state = UBLOX_IDLE;
actual_pos.status= 0;
actual_speed.status= 0;
actual_status.status= 0;
ublox_msg_state = UBLOX_IDLE;
actual_pos.status = 0;
actual_speed.status = 0;
actual_status.status = 0;
gps_home_position.status= 0; // Noch keine gueltige Home Position
}
 
void Get_GPS_data(void) // Daten aus aktuellen ublox Messages extrahieren
// Home Position sichern falls Daten verfuegbar sind.
void GPS_Save_Home(void)
{
if (actual_pos.status == 0) return; //damit es schnell geht, wenn nix zu tun ist
short int n;
n = Get_GPS_data();
if (n == 0) // Gueltige und aktuelle Daten ?
{
// Neue GPS Daten liegen vor
// beeptime = 500; // Piepsen um korrekte Home Position anzuzeigen
// gps_act_position.status = 0;
gps_home_position.utm_east = gps_act_position.utm_east;
gps_home_position.utm_north = gps_act_position.utm_north;
gps_home_position.utm_alt = gps_act_position.utm_alt;
gps_home_position.status = 1; // Home Position gueltig
}
}
 
// Relative Position zur Home Position bestimmen
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
short int Get_Rel_Position(void)
{
short int n = 0;
n = Get_GPS_data();
if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
{
gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);
gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);
gps_rel_act_position.status = 1; // gueltige Positionsdaten
n = 0;
}
else
{
n = 2; //keine gueltigen Daten vorhanden
gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
}
return (n);
}
 
// Daten aus aktuellen ublox Messages extrahieren
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
short int Get_GPS_data(void)
{
short int n = 1;
 
if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
{
cnt1++; //**** noch Rausschmeissen
81,17 → 130,24
gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
gps_act_position.heading = actual_speed.heading/100000;
gps_act_position.status = 1;
n = 0; //Daten gueltig
}
actual_pos.status = 0;
actual_status.status = 0;
actual_speed.status = 0;
}
else
{
gps_act_position.status = 0; //Keine gueltigen Daten
n = 2;
}
actual_pos.status = 0; //neue ublox Messages anfordern
actual_status.status = 0;
actual_speed.status = 0;
}
return (n);
}
 
 
/*
Daten vom GPS im ublox MSG Format auswerten
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
*/
void Get_Ublox_Msg(uint8_t rx)
143,7 → 199,7
break;
 
case UBLOX_NAV_VELED:
ptr_pac_status = &actual_speed.status;
ptr_pac_status = &actual_speed.status;
if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
else
{
186,7 → 242,7
if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
{
ptr_payload_data++;
ublox_msg_state = UBLOX_PAYLOAD;
ublox_msg_state = UBLOX_PAYLOAD;
}
else ublox_msg_state = UBLOX_CKA;
}
/branches/salvo_gps/fc.c
52,6 → 52,12
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 13.9.2007
/*
Driftkompensation fuer Gyros verbessert
Linearsensor mit fixem neutralwert
Ersatzkompass abgeleitet aus magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion
*/
 
#include "main.h"
 
75,7 → 81,6
unsigned char MAX_GAS,MIN_GAS;
unsigned char Notlandung = 0;
unsigned char HoehenReglerAktiv = 0;
static int SignalSchlecht = 0;
uint8_t magkompass_ok=0;
 
//Salvo 2.9.2007 Ersatzkompass
137,6 → 142,7
AdNeutralNick = 0;
AdNeutralRoll = 0;
AdNeutralGier = 0;
GPS_Neutral();
CalibrierMittelwert();
timer = SetDelay(5);
while (!CheckDelay(timer));
173,7 → 179,6
HoeheD = 0;
Mess_Integral_Hoch = 0;
KompassStartwert = KompassValue;
GPS_Neutral();
beeptime = 50;
//Salvo 2.9.2007 Ersatzkompass
GyroKomp_Int = 0;
201,7 → 206,6
accumulate_AccNick = 0;messanzahl_AccNick = 0;
accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
Integral_Gier = Mess_Integral_Gier;
// Integral_Gier2 = Mess_Integral_Gier2;
IntegralNick = Mess_IntegralNick;
IntegralRoll = Mess_IntegralRoll;
IntegralNick2 = Mess_IntegralNick2;
419,7 → 423,6
MIN_GAS = EE_Parameter.Gas_Min;
}
 
 
//############################################################################
//
void MotorRegler(void)
440,11 → 443,12
static char NeueKompassRichtungMerken = 0;
Mittelwert();
//******PROVISORISCH***************
Get_GPS_data();
if (gps_act_position.status > 0)
short int n;
n = Get_Rel_Position();
if (n == 0)
{
ROT_ON;
gps_act_position.status = 0;
// gps_act_position.status = 0;
}
 
//******PROVISORISCH***************
532,6 → 536,7
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
}
GPS_Save_Home();
}
else delay_neutral = 0;
}
639,7 → 644,7
if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++;
if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--;
}
else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
{
if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR) AdNeutralNick++;
if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR) AdNeutralNick--;
655,19 → 660,6
}
// Salvo End
 
// Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist ***********************
// Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert
// Ohne Driftkompensation ist die Gierachse wesentlich stabiler
// if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht))
// Salvo 13.9.2007
// if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (magkompass_ok >0))
// {
// if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--;
// if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++;
// }
// else Mess_Integral_Gier2 = 0;
// Salvo End ***********************
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralRoll2 = IntegralRoll;
765,7 → 757,6
}
else magkompass_ok = 0;
}
 
// Salvo End *************************
 
 
777,19 → 768,11
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
// Salvo 12.9.2007 Kompassdaempfung abschalten weil Ersatzkompass verwendet wird
// w=0, v=0;
//Salvo End
// if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht)
 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
if ((magkompass_ok > 0) && NeueKompassRichtungMerken)
{
// Salvo 12.9.2007 Ersatzkompass nehmen statt Magnetkompass
// KompassStartwert = KompassValue;
// KompassStartwert = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
// Salvo End *************************
// Salvo 13.9.2007
KompassStartwert = KompassValue;
// Salvo End
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
// Salvo 13.9.2007
804,22 → 787,8
// Salvo Kompasssteuerung **********************
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
// Salvo End
// Salvo 30.8.2007 Winkelbegrenzung **********************
/*
if ((!SignalSchlecht) )
{
if (abs(KompassRichtung) < 135 )
{
Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
}
}
*/
// Salvo End *************************
 
ANALOG_ON; // ADC einschalten
if(SignalSchlecht) SignalSchlecht--;
}
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
843,6 → 812,11
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
DebugOut.Analog[7] = GasMischanteil;
DebugOut.Analog[8] = KompassValue;
 
DebugOut.Analog[9] = gps_rel_act_position.utm_east;
DebugOut.Analog[10] = gps_rel_act_position.utm_north;
DebugOut.Analog[11] = gps_rel_act_position.status;
 
// ******provisorisch
// DebugOut.Analog[9] = cnt1;
// DebugOut.Analog[10] = cnt1;
851,10 → 825,11
// DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000;
// ******provisorisch
 
/*
DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
DebugOut.Analog[10] = magkompass_ok;
DebugOut.Analog[11] = Mess_Integral_Gier2;
*/
/*
DebugOut.Analog[11] = GyroKomp_Inc_Grad;
DebugOut.Analog[12] = GyroKomp_Value;
/branches/salvo_gps/gps.h
8,7 → 8,9
extern void GPS_Neutral(void);
 
extern void Get_Ublox_Msg(uint8_t rx) ;
extern void Get_GPS_data(void);
//extern short int Get_GPS_data(void);
extern short int Get_Rel_Position(void);
extern void GPS_Save_Home(void);
 
typedef struct {
unsigned long utm_itow; // time of week
53,14 → 55,18
unsigned heading; // Richtung in Grad
uint8_t status; // 0: keine gueltigen Daten 1: alles ok
 
} GPS_POSITION_t;
} GPS_ABS_POSITION_t;
 
 
typedef struct { // Struktur fuer Relative GPS Daten (bezogen z.B. auf Home Position)
int utm_east; // UTM Ost in m
int utm_north; // UTM Nord in m
uint8_t status; // 0: keine gueltigen Daten 1: alles ok
 
/*
extern NAV_VELNED_t actual_speed;
extern NAV_STATUS_t actual_status;
extern NAV_POSUTM_t actual_position;
*/
extern GPS_POSITION_t gps_act_position;
} GPS_REL_POSITION_t;
 
 
extern GPS_ABS_POSITION_t gps_act_position;
extern GPS_ABS_POSITION_t gps_home_position;
extern GPS_REL_POSITION_t gps_rel_act_position;
extern unsigned int cnt0,cnt1,cnt2;
/branches/salvo_gps/main.c
148,6 → 148,7
while (!CheckDelay(timer));
printf("OK\n\r");
}
 
SetNeutral();
 
/branches/salvo_gps/timer0.c
21,7 → 21,12
T0_RISING_EDGE = 7
};
 
 
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 13.9.2007
/*
Driftkompensation fuer Gyros verbessert
Linearsensor mit fixem neutralwert
Ersatzkompass abgeleitet aus magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion
*/
SIGNAL (SIG_OVERFLOW0) // 8kHz
{
static unsigned char cnt_1ms = 1,cnt = 0;
69,16 → 74,10
}
// Salvo End
}
// if(cntKompass < 10) cntKompass = 10;
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
//Salvo 12.9.2007 Ersatzkompass als Basis nehmen, nicht den magnetkompass
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
// int i;
// i = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
// if (i < 180) KompassRichtung = ((540 + i - KompassStartwert) % 360) - 180;
// else KompassRichtung = ((540 + (i-360) - KompassStartwert) % 360) - 180;
// Salvo End
//Salvo 13.9.2007 Ok Erkennung des Magnetkompasses
Kompass_Neuer_Wert = 1;
// Salvo End
cntKompass = 0;
}
}