/branches/salvo_gps/GPS.c |
---|
54,6 → 54,7 |
static unsigned int rx_len; |
unsigned int cnt0,cnt1,cnt2; //******Provisorisch |
static unsigned int ptr_payload_data_end; |
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
static uint8_t *ptr_payload_data; |
static uint8_t *ptr_pac_status; |
85,6 → 86,7 |
gps_updte_flag = 0; |
gps_int_x = 0; |
gps_int_y = 0; |
gps_alive_cnt = 0; |
} |
110,6 → 112,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_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); |
317,7 → 320,7 |
dist_north = 0; |
diff_east_f = 0; |
diff_north_f = 0; |
diff_east = 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; |
337,11 → 340,11 |
break; |
case GPS_CMD_STOP: // Lageregelung beenden |
cnt = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
gps_int_x = 0; |
gps_int_y = 0; |
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; |
365,7 → 368,7 |
// ab hier wird geregelt |
diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen |
diff_north = -dist_north; |
dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_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; |
399,8 → 402,8 |
dist_north_p = (int) d; |
//PID Regler Werte aufsummieren |
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/6)+ ((diff_east_f * Parameter_UserParam3)/2); // I + P +D Anteil X Achse |
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/6)+ ((diff_north_f * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/4)+ ((diff_east_f * Parameter_UserParam3)/2); // I + P +D Anteil X Achse |
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/4)+ ((diff_north_f * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
//Ziel-Richtung bezogen auf Nordpol bestimmen |
GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
420,18 → 423,18 |
if (abs(gps_reg_x) > abs(gps_reg_y) ) |
{ |
dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
} |
else |
{ |
dist = (long)gps_reg_y; |
dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
} |
GPS_dist_2trgt = (int) dist; |
// Winkel und Distanz in Nick und Rollgroessen umrechnen |
GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
#define GPS_V 8 |
if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
/branches/salvo_gps/fc.c |
---|
328,9 → 328,9 |
EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
EE_Parameter.I_Faktor = 5; |
EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
EE_Parameter.UserParam1 = 0; //zur freien Verwendung, derzeit P-Anteil GPS |
EE_Parameter.UserParam2 = 0; //zur freien Verwendung, derzeit I-Anteil GPS |
EE_Parameter.UserParam3 = 0; //zur freien Verwendung, derzeit P-Anteil GPS |
EE_Parameter.UserParam3 = 0; //zur freien Verwendung, derzeit D-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 |
443,6 → 443,7 |
Mittelwert(); |
//****** GPS Daten holen *************** |
short int n; |
if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout) |
n = Get_Rel_Position(); |
if (n == 0) |
{ |
768,7 → 769,8 |
// 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 ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && ((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) && (gps_alive_cnt > 0)) |
{ |
short int n; |
n= GPS_CRTL(GPS_CMD_REQ_HOLD); |
844,6 → 846,7 |
*/ |
DebugOut.Analog[9] = gps_reg_x; |
DebugOut.Analog[10] = gps_reg_y; |
// DebugOut.Analog[11] = gps_alive_cnt; |
DebugOut.Analog[11] = GPS_hdng_abs_2trgt; |
/branches/salvo_gps/gps.h |
---|
78,6 → 78,7 |
extern signed int GPS_dist_2trgt; |
extern signed int gps_reg_x,gps_reg_y; |
extern signed int GPS_dist_2trgt; |
extern unsigned int gps_alive_cnt; |
// Zustaende der zentralen GPS statemachine |
#define GPS_CRTL_IDLE 0 // |
96,8 → 97,8 |
#define GPS_STST_ERR 2 // Fehler |
// GPS Lageregler |
#define GPS_NICKROLL_MAX 25 //Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
#define GPS_DIST_MAX 300 //Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm) |
#define GPS_NICKROLL_MAX 30 //Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
#define GPS_DIST_MAX 500 //Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm) |
//Salvo 16.9.2007 GPS_STICK_HOLDOFF *************** |
// Laut Datenblatt sind die Werte ueber Zeit und Temperatur sehr stabil. |
#define GPS_STICK_HOLDOFF 25 // Wenn der Nick oder Roll groesser ist, wird GPS_HOLD deaktiviert |
/branches/salvo_gps/math.c |
---|
14,7 → 14,7 |
Winkelfunktionen sin, cos und arctan in |
brute-force Art: Sehr Schnell, nicht sonderlich genau, aber ausreichend |
Sinus Funktion von Nick666 vereinfacht |
Stand 28.9.2007 |
Stand 30.9.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
24,7 → 24,7 |
int arctan_i(long signed int x, long signed int y) |
{ |
short int change_xy = 0; |
signed int i; |
long signed int i; |
long signed int wert; |
int return_value; |
39,7 → 39,8 |
// Quadranten ermitteln |
// Wert durch lineare Interpolation ermitteln |
wert= abs((x*1000)/y); |
if ((y == 0) && (x == 0)) wert =1; // Division durch 0 nicht erlaubt |
else wert= abs((x*1000)/y); |
if (wert <=268) //0...0.0,268 entsprechend 0..15 Grad |
{ |
75,7 → 76,7 |
// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000 |
signed int sin_i(signed int winkel) |
{ |
short int m; |
short int m,n; |
if (abs(winkel) >=360) winkel = winkel % 360; |
if (winkel < 0) |
84,14 → 85,23 |
winkel = abs(winkel); |
} |
else m = +1; |
n =1; |
// Quadranten auswerten |
if ((winkel > 90 ) && (winkel <= 180)) winkel = winkel - 90; |
else if ((winkel > 180 ) && (winkel <= 270)) winkel = winkel - 180; |
else if ((winkel > 270) && (winkel <= 360)) winkel = winkel - 270; |
if ((winkel > 90 ) && (winkel <= 180)) winkel = 180 - winkel; |
else if ((winkel > 180 ) && (winkel <= 270)) |
{ |
winkel = winkel -180; |
n = -1; |
} |
else if ((winkel > 270) && (winkel <= 360)) |
{ |
winkel = 360 - winkel; |
n = -1; |
} |
// else //0 - 90 Grad |
winkel = pgm_read_word(&pgm_sinus[winkel]); |
return (winkel*m); |
return (winkel*m*n); |
} |