Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 224 → Rev 225

/branches/salvo_gps/GPS.c
14,7 → 14,7
Auswertung der Daten vom GPS im ublox Format
Hold Modus mit PID Regler
Komm heim zu Papi Funktion
Stand 5.10.2007
Stand 6.10.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
45,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,gps_sub_state;
short int gps_state,gps_sub_state; //Zustaende der Statemachine
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
56,12 → 56,11
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 short int hold_fast; //Flag fuer Hold Regler
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
long int dist_flown;
 
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
294,7 → 293,7
static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert
signed int n,diff_v;
long signed int dev,n_l;
 
signed int dist_frm_start_east,dist_frm_start_north;
switch (cmd)
{
 
307,6 → 306,7
// Erst mal initialisieren
cnt = 0;
gps_tick = 0;
hold_fast = 0;
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
354,6 → 354,7
// aktuelle positionsdaten abspeichern
if (gps_rel_act_position.status > 0)
{
hold_fast = 0;
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
406,41 → 407,45
{
gps_tick++;
int d1,d2,d3;
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 );
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
{
{
hold_fast = 1; // Schnell Regeln
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
dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
}
else hold_fast = 0; // Wieder normal regeln
}
else //Schon ziemlich nahe am Ziel, deswegen abbremsen
{
hold_fast = 0; // Wieder normal regeln
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
if (d3 > 0)
if (d3 > GPS_G2T_DIST_HOLD)
{
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_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
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
else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
{
gps_rel_hold_position.utm_east = 0;
gps_rel_hold_position.utm_north = 0;
gps_sub_state = GPS_HOME_FINISHED;
if (gps_rel_hold_position.utm_east > 1) gps_rel_hold_position.utm_east--;
else if (gps_rel_hold_position.utm_east <-1 ) gps_rel_hold_position.utm_east++;
if (gps_rel_hold_position.utm_north > 1) gps_rel_hold_position.utm_north--;
else if (gps_rel_hold_position.utm_north < -1 ) gps_rel_hold_position.utm_north++;
if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state = GPS_HOME_FINISHED;
}
}
}
}
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
return (GPS_STST_OK);
478,6 → 483,12
int_east -= dist_east;
int_north -= dist_north;
}
if (hold_fast > 0) //Im Schnellen Mode Integrator abschalten
{
int_east = 0;
int_north = 0;
}
 
// Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
// desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
#define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
489,6 → 500,7
 
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
if (hold_fast > 0) diff_v = 10; // im schnellen Modus keine staerkere Wirkung des Differenzierers
 
//PID Regler Werte aufsummieren
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