Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 209 → Rev 224

/branches/salvo_gps/GPS.c
13,7 → 13,8
Peter Muehlenbrock
Auswertung der Daten vom GPS im ublox Format
Hold Modus mit PID Regler
Stand 2.10.2007
Komm heim zu Papi Funktion
Stand 5.10.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
44,7 → 45,7
short int ublox_msg_state = UBLOX_IDLE;
static uint8_t chk_a =0; //Checksum
static uint8_t chk_b =0;
short int gps_state;
short int gps_state,gps_sub_state;
short int gps_updte_flag;
signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
53,10 → 54,14
static unsigned int rx_len;
static unsigned int ptr_payload_data_end;
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
 
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
 
long int dist_flown,dist_frm_start_east,dist_frm_start_north;
 
short int Get_GPS_data(void);
 
NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
67,6 → 72,7
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
GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode
GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
 
// Initialisierung
void GPS_Neutral(void)
109,7 → 115,7
short int n = 0;
n = Get_GPS_data();
if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in MotorreglerRoutine ueberwacht und dekrementiert
if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
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);
288,12 → 294,53
static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert
signed int n,diff_v;
long signed int dev,n_l;
 
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
if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
{
cnt++;
if (cnt > 500) // erst nach Verzoegerung
{
// Erst mal initialisieren
cnt = 0;
gps_tick = 0;
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
diff_east_f = 0, diff_north_f= 0;
diff_east = 0, diff_north = 0;
dist_flown = 0;
gps_sub_state = GPS_CRTL_IDLE;
// aktuelle positionsdaten abspeichern
if (gps_rel_act_position.status > 0)
{
gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
gps_rel_start_position.status = 1; // gueltige Positionsdaten
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
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
gps_state = GPS_CRTL_HOME_ACTIVE;
return (GPS_STST_OK);
}
else
{
gps_rel_start_position.status = 0; //Keine Daten verfuegbar
gps_state = GPS_CRTL_IDLE;
return(GPS_STST_ERR); // Keine Daten da
}
}
else return(GPS_STST_PEND); // noch warten
}
break;
// ******************************
 
304,19 → 351,14
if (cnt > 500) // erst nach Verzoegerung
{
cnt = 0;
// aktuelle positionsdaten abespeichern
// aktuelle positionsdaten abspeichern
if (gps_rel_act_position.status > 0)
{
int_east = 0;
int_north = 0;
gps_reg_x = 0;
gps_reg_y = 0;
dist_east = 0;
dist_north = 0;
diff_east_f = 0;
diff_north_f = 0;
diff_east = 0;
diff_north = 0;
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
diff_east_f = 0, diff_north_f= 0;
diff_east = 0, diff_north = 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
356,8 → 398,65
return (GPS_STST_OK);
break;
 
case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
//Der Sollwert des Lagereglers wird der Homeposition angenaehert
if (gps_rel_start_position.status >0)
{
if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
{
gps_tick++;
int d1,d2,d3;
d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
if (d3 > GPS_G2T_DIST_MAX_STOP)
{
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
dist_flown += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
}
}
else //Schon ziemlich nahe am Ziel, deswegen abbremsen
{
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
if (d3 > 0)
{
dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
}
else //Ziel erreicht, Regelung beenden
{
gps_rel_hold_position.utm_east = 0;
gps_rel_hold_position.utm_north = 0;
gps_sub_state = GPS_HOME_FINISHED;
}
}
}
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
return (GPS_STST_OK);
}
else return (GPS_STST_OK);
}
else
{
gps_state = GPS_CRTL_IDLE; //Abbruch
return (GPS_STST_ERR);
}
break;
 
 
case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen
if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen
{
gps_updte_flag = 0;
// ab hier wird geregelt
369,10 → 468,10
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
 
*/
#define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung
if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
{
386,22 → 485,14
signed long dist;
int phi;
phi = arctan_i(abs(dist_north),abs(dist_east));
if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln
{
dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen
dist = abs((dist *1000) / (long) sin_i(phi));
}
else
{
dist = (long)dist_north;
dist = abs((dist *1000) / (long) cos_i(phi));
}
dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
 
diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
 
//PID Regler Werte aufsummieren
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil Y Achse
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * 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);
455,12 → 546,21
return (GPS_STST_ERR);
break;
}
else
{
if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
return (GPS_STST_OK);
}
}
else return (GPS_STST_OK);
else
{
if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
return (GPS_STST_OK);
}
break;
 
default:
gps_state = GPS_CRTL_IDLE;
gps_state = GPS_CRTL_IDLE;
return (GPS_STST_ERR);
break;
}