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