/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; |
} |
} |