Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 189 → Rev 190

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