Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1050 → Rev 1052

/branches/salvo_gps/Basis_v0070d/trunk/GPS.c
19,9 → 19,9
Umstellung auf NaviParameter an Flight Version 00.70d
GPS_V durch gps_gain ersetzt, damit Einstellung durch MK Tool möglich wird
 
Stand 24.11.2008
Stand 27.11.2008
 
Aenderung 24.11.2008: gps_gain erhoeht
Aenderung 27.11.2008: gps_gain erhoeht
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
68,10 → 68,10
static uint8_t *ptr_pac_status;
static int dist_flown;
//static unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
static int gps_quiet_cnt; // Zaehler fuer GPS Off Time beim Kameraausloesen
static int gps_gain; // // Teilerfaktor Regelabweichung zu Ausgabewert
static int gps_quiet_cnt; // Zaehler fuer GPS Off Time beim Kameraausloesen
static long int limit_gain; // Teilerfaktor Regelabweichung zu Ausgabewert
static long int gps_gain ; // Verstaerkunsgfaktor*10
 
 
short int Get_GPS_data(void);
 
NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
87,19 → 87,19
// Initialisierung
void GPS_Neutral(void)
{
ublox_msg_state = UBLOX_IDLE;
gps_state = GPS_CRTL_IDLE;
gps_sub_state = GPS_CRTL_IDLE;
actual_pos.status = 0;
actual_speed.status = 0;
ublox_msg_state = UBLOX_IDLE;
gps_state = GPS_CRTL_IDLE;
gps_sub_state = GPS_CRTL_IDLE;
actual_pos.status = 0;
actual_speed.status = 0;
actual_status.status = 0;
gps_home_position.status = 0; // Noch keine gueltige Home Position
gps_act_position.status = 0;
gps_rel_act_position.status = 0;
GPS_Nick = 0;
GPS_Roll = 0;
gps_updte_flag = 0;
gps_alive_cnt = 0;
GPS_Nick = 0;
GPS_Roll = 0;
gps_updte_flag = 0;
gps_alive_cnt = 0;
 
}
 
150,8 → 150,9
short int n = 1;
 
if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
gps_gain = (Parameter_NaviGpsGain*8)/20; //maximal Wert ist 255*8/40
// debug_gp_0 = (int)gps_gain; // zum Debuggen
gps_gain = Parameter_NaviGpsGain/10;
limit_gain = 8;
// debug_gp_0 = (int)limit_gain; // zum Debuggen
 
if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
{
190,7 → 191,6
{
switch (ublox_msg_state)
{
 
case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
else ublox_msg_state = UBLOX_IDLE;
305,16 → 305,16
{
static unsigned int cnt; // Zaehler fuer diverse Verzoegerungen
static long int delta_north,delta_east; // Mass fuer Distanz zur Sollposition
signed int n;
signed int n;
static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
signed int dist_frm_start_east,dist_frm_start_north;
int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil
signed int dist_frm_start_east,dist_frm_start_north;
int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil
static signed int int_east,int_north; //Integrierer
long int speed_east,speed_north; //Aktuelle Geschwindigkeit
signed long int_east1,int_north1;
int dist_east,dist_north;
int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung
long ni,ro; // Nick und Roll Zwischenwerte
long int speed_east,speed_north; //Aktuelle Geschwindigkeit
signed long int_east1,int_north1;
int dist_east,dist_north;
int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung
long ni,ro; // Nick und Roll Zwischenwerte
 
 
switch (cmd)
521,8 → 521,8
// ab hier wird geregelt
delta_east = (long) (gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east);
delta_north = (long) (gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north);
int_east += (int)delta_east;
int_north += (int)delta_north;
int_east += (int)(delta_east*gps_gain)/10;
int_north += (int)(delta_north*gps_gain)/10;
speed_east = actual_speed.speed_e;
speed_north = actual_speed.speed_n;
gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden
534,7 → 534,7
long int gpsintmax;
if (Parameter_NaviGpsI > 0)
{
gpsintmax = (GPS_NICKROLL_MAX * gps_gain * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_NaviGpsI; //auf ungefeahren Maximalwert begrenzen
gpsintmax = (GPS_NICKROLL_MAX * limit_gain * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_NaviGpsI; //auf ungefeahren Maximalwert begrenzen
if ((abs(int_east) > (int)gpsintmax) || (abs(int_north)> (int)gpsintmax))
{
// // = 1; // Zahl der Overflows zaehlen
559,7 → 559,7
debug_gp_5 = (int)int_north; // zum Debuggen
 
//I Werte begrenzen
#define INT1_MAX (GPS_NICKROLL_MAX * gps_gain*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen
#define INT1_MAX (GPS_NICKROLL_MAX * limit_gain*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen
int_east1 = ((((long)int_east) * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT;
int_north1 = ((((long)int_north) * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT;
if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen
573,10 → 573,10
amplfy_speed_north = DIFF_Y_F_MAX;
amplfy_speed_east *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
speed_east = (speed_east * (long)amplfy_speed_east) /50;
speed_north = (speed_north * (long)amplfy_speed_north)/50;
speed_east = (speed_east * (long)amplfy_speed_east*gps_gain) /500;
speed_north = (speed_north * (long)amplfy_speed_north*gps_gain)/500;
// D Werte begrenzen
#define D_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
#define D_F_MAX (GPS_NICKROLL_MAX * limit_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
if (speed_east > D_F_MAX) speed_east = D_F_MAX;
else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX;
if (speed_north > D_F_MAX) speed_north = D_F_MAX;
590,10 → 590,10
amplfy_speed_north = DIFF_Y_N_MAX;
amplfy_speed_east *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
speed_east = (speed_east * (long)amplfy_speed_east) /25;
speed_north = (speed_north * (long)amplfy_speed_north)/25;
speed_east = (speed_east * (long)amplfy_speed_east*gps_gain) /250;
speed_north = (speed_north * (long)amplfy_speed_north*gps_gain)/250;
// D Werte begrenzen
#define D_N_MAX (GPS_NICKROLL_MAX * gps_gain*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen
#define D_N_MAX (GPS_NICKROLL_MAX * limit_gain*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen
if (speed_east > D_N_MAX) speed_east = D_N_MAX;
else if (speed_east < -D_N_MAX) speed_east = -D_N_MAX;
if (speed_north > D_N_MAX) speed_north = D_N_MAX;
606,13 → 606,13
// debug_gp_5 = (int)speed_north; // zum Debuggen
 
//P-Werte verstaerken
delta_east = (delta_east * (long)diff_p)/(40);
delta_north = (delta_north * (long)diff_p)/(40);
delta_east = (delta_east * (long)diff_p*gps_gain)/(400);
delta_north = (delta_north * (long)diff_p*gps_gain)/(400);
 
if (hold_fast > 0) //schneller Coming Home Modus
{
// P Werte begrenzen
#define P1_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
#define P1_F_MAX (GPS_NICKROLL_MAX * limit_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
if (delta_east > P1_F_MAX) delta_east = P1_F_MAX;
else if (delta_east < -P1_F_MAX) delta_east = -P1_F_MAX;
if (delta_north > P1_F_MAX) delta_north = P1_F_MAX;
621,7 → 621,7
else // Hold modus
{
// P Werte begrenzen
#define P1_N_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
#define P1_N_MAX (GPS_NICKROLL_MAX * limit_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
if (delta_east > P1_N_MAX) delta_east = P1_N_MAX;
else if (delta_east < -P1_N_MAX) delta_east = -P1_N_MAX;
if (delta_north > P1_N_MAX) delta_north = P1_N_MAX;
640,8 → 640,8
 
// Werte fuer Nick und Roll direkt aus gps_reg_x und gps_reg_y bestimmen
n = GyroKomp_Int/GIER_GRAD_FAKTOR; //Ausrichtung Kopter
ni = -((gps_reg_y * (long)cos_i(n)) + (gps_reg_x * (long)sin_i(n)))/(1000*gps_gain);
ro = ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000*gps_gain);
ni = -((gps_reg_y * (long)cos_i(n)) + (gps_reg_x * (long)sin_i(n)))/(1000L*(long)limit_gain);
ro = ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000L*(long)limit_gain);
if (ni > (GPS_NICKROLL_MAX )) ni = (GPS_NICKROLL_MAX);
else if (ni < -(GPS_NICKROLL_MAX )) ni = -(GPS_NICKROLL_MAX );