Rev 1050 | Rev 1057 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1050 | Rev 1052 | ||
---|---|---|---|
Line 17... | Line 17... | ||
17 | Hold Modus mit PID Regler |
17 | Hold Modus mit PID Regler |
18 | Rückstuerz zur Basis Funktion |
18 | Rückstuerz zur Basis Funktion |
19 | Umstellung auf NaviParameter an Flight Version 00.70d |
19 | Umstellung auf NaviParameter an Flight Version 00.70d |
20 | GPS_V durch gps_gain ersetzt, damit Einstellung durch MK Tool möglich wird |
20 | GPS_V durch gps_gain ersetzt, damit Einstellung durch MK Tool möglich wird |
Line 21... | Line 21... | ||
21 | 21 | ||
Line 22... | Line 22... | ||
22 | Stand 24.11.2008 |
22 | Stand 27.11.2008 |
23 | 23 | ||
24 | Aenderung 24.11.2008: gps_gain erhoeht |
24 | Aenderung 27.11.2008: gps_gain erhoeht |
25 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
25 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
26 | */ |
26 | */ |
27 | #include "main.h" |
27 | #include "main.h" |
Line 66... | Line 66... | ||
66 | static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
66 | static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
67 | static uint8_t *ptr_payload_data; |
67 | static uint8_t *ptr_payload_data; |
68 | static uint8_t *ptr_pac_status; |
68 | static uint8_t *ptr_pac_status; |
69 | static int dist_flown; |
69 | static int dist_flown; |
70 | //static unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
70 | //static unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
71 | static int gps_quiet_cnt; // Zaehler fuer GPS Off Time beim Kameraausloesen |
71 | static int gps_quiet_cnt; // Zaehler fuer GPS Off Time beim Kameraausloesen |
72 | static int gps_gain; // // Teilerfaktor Regelabweichung zu Ausgabewert |
72 | static long int limit_gain; // Teilerfaktor Regelabweichung zu Ausgabewert |
73 | - | ||
- | 73 | static long int gps_gain ; // Verstaerkunsgfaktor*10 |
|
Line 74... | Line 74... | ||
74 | 74 | ||
Line 75... | Line 75... | ||
75 | short int Get_GPS_data(void); |
75 | short int Get_GPS_data(void); |
76 | 76 | ||
Line 85... | Line 85... | ||
85 | GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
85 | GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
Line 86... | Line 86... | ||
86 | 86 | ||
87 | // Initialisierung |
87 | // Initialisierung |
88 | void GPS_Neutral(void) |
88 | void GPS_Neutral(void) |
89 | { |
89 | { |
90 | ublox_msg_state = UBLOX_IDLE; |
90 | ublox_msg_state = UBLOX_IDLE; |
91 | gps_state = GPS_CRTL_IDLE; |
91 | gps_state = GPS_CRTL_IDLE; |
92 | gps_sub_state = GPS_CRTL_IDLE; |
92 | gps_sub_state = GPS_CRTL_IDLE; |
93 | actual_pos.status = 0; |
93 | actual_pos.status = 0; |
94 | actual_speed.status = 0; |
94 | actual_speed.status = 0; |
95 | actual_status.status = 0; |
95 | actual_status.status = 0; |
96 | gps_home_position.status = 0; // Noch keine gueltige Home Position |
96 | gps_home_position.status = 0; // Noch keine gueltige Home Position |
97 | gps_act_position.status = 0; |
97 | gps_act_position.status = 0; |
98 | gps_rel_act_position.status = 0; |
98 | gps_rel_act_position.status = 0; |
99 | GPS_Nick = 0; |
99 | GPS_Nick = 0; |
100 | GPS_Roll = 0; |
100 | GPS_Roll = 0; |
101 | gps_updte_flag = 0; |
101 | gps_updte_flag = 0; |
Line 102... | Line 102... | ||
102 | gps_alive_cnt = 0; |
102 | gps_alive_cnt = 0; |
Line 103... | Line 103... | ||
103 | 103 | ||
104 | } |
104 | } |
Line 148... | Line 148... | ||
148 | short int Get_GPS_data(void) |
148 | short int Get_GPS_data(void) |
149 | { |
149 | { |
150 | short int n = 1; |
150 | short int n = 1; |
Line 151... | Line 151... | ||
151 | 151 | ||
152 | if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist |
152 | if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist |
- | 153 | gps_gain = Parameter_NaviGpsGain/10; |
|
153 | gps_gain = (Parameter_NaviGpsGain*8)/20; //maximal Wert ist 255*8/40 |
154 | limit_gain = 8; |
Line 154... | Line 155... | ||
154 | // debug_gp_0 = (int)gps_gain; // zum Debuggen |
155 | // debug_gp_0 = (int)limit_gain; // zum Debuggen |
155 | 156 | ||
156 | if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0)) |
157 | if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0)) |
157 | { |
158 | { |
Line 188... | Line 189... | ||
188 | */ |
189 | */ |
189 | void Get_Ublox_Msg(uint8_t rx) |
190 | void Get_Ublox_Msg(uint8_t rx) |
190 | { |
191 | { |
191 | switch (ublox_msg_state) |
192 | switch (ublox_msg_state) |
192 | { |
193 | { |
193 | - | ||
194 | case UBLOX_IDLE: // Zuerst Synchcharacters pruefen |
194 | case UBLOX_IDLE: // Zuerst Synchcharacters pruefen |
195 | if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1; |
195 | if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1; |
196 | else ublox_msg_state = UBLOX_IDLE; |
196 | else ublox_msg_state = UBLOX_IDLE; |
197 | break; |
197 | break; |
Line 303... | Line 303... | ||
303 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
303 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
304 | short int GPS_CRTL(short int cmd) |
304 | short int GPS_CRTL(short int cmd) |
305 | { |
305 | { |
306 | static unsigned int cnt; // Zaehler fuer diverse Verzoegerungen |
306 | static unsigned int cnt; // Zaehler fuer diverse Verzoegerungen |
307 | static long int delta_north,delta_east; // Mass fuer Distanz zur Sollposition |
307 | static long int delta_north,delta_east; // Mass fuer Distanz zur Sollposition |
308 | signed int n; |
308 | signed int n; |
309 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
309 | static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
310 | signed int dist_frm_start_east,dist_frm_start_north; |
310 | signed int dist_frm_start_east,dist_frm_start_north; |
311 | int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil |
311 | int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil |
312 | static signed int int_east,int_north; //Integrierer |
312 | static signed int int_east,int_north; //Integrierer |
313 | long int speed_east,speed_north; //Aktuelle Geschwindigkeit |
313 | long int speed_east,speed_north; //Aktuelle Geschwindigkeit |
314 | signed long int_east1,int_north1; |
314 | signed long int_east1,int_north1; |
315 | int dist_east,dist_north; |
315 | int dist_east,dist_north; |
316 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
316 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
317 | long ni,ro; // Nick und Roll Zwischenwerte |
317 | long ni,ro; // Nick und Roll Zwischenwerte |
Line 318... | Line 318... | ||
318 | 318 | ||
319 | 319 | ||
Line 519... | Line 519... | ||
519 | { |
519 | { |
520 | gps_quiet_cnt++; |
520 | gps_quiet_cnt++; |
521 | // ab hier wird geregelt |
521 | // ab hier wird geregelt |
522 | delta_east = (long) (gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east); |
522 | delta_east = (long) (gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east); |
523 | delta_north = (long) (gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north); |
523 | delta_north = (long) (gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north); |
524 | int_east += (int)delta_east; |
524 | int_east += (int)(delta_east*gps_gain)/10; |
525 | int_north += (int)delta_north; |
525 | int_north += (int)(delta_north*gps_gain)/10; |
526 | speed_east = actual_speed.speed_e; |
526 | speed_east = actual_speed.speed_e; |
527 | speed_north = actual_speed.speed_n; |
527 | speed_north = actual_speed.speed_n; |
528 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
528 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
529 | dist_east = (int)delta_east; //merken |
529 | dist_east = (int)delta_east; //merken |
530 | dist_north = (int)delta_north; |
530 | dist_north = (int)delta_north; |
Line 532... | Line 532... | ||
532 | 532 | ||
533 | // #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
533 | // #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
534 | long int gpsintmax; |
534 | long int gpsintmax; |
535 | if (Parameter_NaviGpsI > 0) |
535 | if (Parameter_NaviGpsI > 0) |
536 | { |
536 | { |
537 | gpsintmax = (GPS_NICKROLL_MAX * gps_gain * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_NaviGpsI; //auf ungefeahren Maximalwert begrenzen |
537 | gpsintmax = (GPS_NICKROLL_MAX * limit_gain * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_NaviGpsI; //auf ungefeahren Maximalwert begrenzen |
538 | if ((abs(int_east) > (int)gpsintmax) || (abs(int_north)> (int)gpsintmax)) |
538 | if ((abs(int_east) > (int)gpsintmax) || (abs(int_north)> (int)gpsintmax)) |
539 | { |
539 | { |
540 | // // = 1; // Zahl der Overflows zaehlen |
540 | // // = 1; // Zahl der Overflows zaehlen |
541 | // int_ovfl_cnt -= 1; |
541 | // int_ovfl_cnt -= 1; |
Line 557... | Line 557... | ||
557 | 557 | ||
558 | debug_gp_4 = (int)int_east; // zum Debuggen |
558 | debug_gp_4 = (int)int_east; // zum Debuggen |
Line 559... | Line 559... | ||
559 | debug_gp_5 = (int)int_north; // zum Debuggen |
559 | debug_gp_5 = (int)int_north; // zum Debuggen |
560 | 560 | ||
561 | //I Werte begrenzen |
561 | //I Werte begrenzen |
562 | #define INT1_MAX (GPS_NICKROLL_MAX * gps_gain*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen |
562 | #define INT1_MAX (GPS_NICKROLL_MAX * limit_gain*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen |
563 | int_east1 = ((((long)int_east) * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT; |
563 | int_east1 = ((((long)int_east) * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT; |
564 | int_north1 = ((((long)int_north) * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT; |
564 | int_north1 = ((((long)int_north) * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT; |
565 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
565 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
Line 571... | Line 571... | ||
571 | { |
571 | { |
572 | amplfy_speed_east = DIFF_Y_F_MAX; |
572 | amplfy_speed_east = DIFF_Y_F_MAX; |
573 | amplfy_speed_north = DIFF_Y_F_MAX; |
573 | amplfy_speed_north = DIFF_Y_F_MAX; |
574 | amplfy_speed_east *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
574 | amplfy_speed_east *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
575 | amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
575 | amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
576 | speed_east = (speed_east * (long)amplfy_speed_east) /50; |
576 | speed_east = (speed_east * (long)amplfy_speed_east*gps_gain) /500; |
577 | speed_north = (speed_north * (long)amplfy_speed_north)/50; |
577 | speed_north = (speed_north * (long)amplfy_speed_north*gps_gain)/500; |
578 | // D Werte begrenzen |
578 | // D Werte begrenzen |
579 | #define D_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
579 | #define D_F_MAX (GPS_NICKROLL_MAX * limit_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
580 | if (speed_east > D_F_MAX) speed_east = D_F_MAX; |
580 | if (speed_east > D_F_MAX) speed_east = D_F_MAX; |
581 | else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX; |
581 | else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX; |
582 | if (speed_north > D_F_MAX) speed_north = D_F_MAX; |
582 | if (speed_north > D_F_MAX) speed_north = D_F_MAX; |
583 | else if (speed_north < -D_F_MAX) speed_north = -D_F_MAX; |
583 | else if (speed_north < -D_F_MAX) speed_north = -D_F_MAX; |
Line 588... | Line 588... | ||
588 | { |
588 | { |
589 | amplfy_speed_east = DIFF_Y_N_MAX; |
589 | amplfy_speed_east = DIFF_Y_N_MAX; |
590 | amplfy_speed_north = DIFF_Y_N_MAX; |
590 | amplfy_speed_north = DIFF_Y_N_MAX; |
591 | amplfy_speed_east *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
591 | amplfy_speed_east *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
592 | amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
592 | amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
593 | speed_east = (speed_east * (long)amplfy_speed_east) /25; |
593 | speed_east = (speed_east * (long)amplfy_speed_east*gps_gain) /250; |
594 | speed_north = (speed_north * (long)amplfy_speed_north)/25; |
594 | speed_north = (speed_north * (long)amplfy_speed_north*gps_gain)/250; |
595 | // D Werte begrenzen |
595 | // D Werte begrenzen |
596 | #define D_N_MAX (GPS_NICKROLL_MAX * gps_gain*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen |
596 | #define D_N_MAX (GPS_NICKROLL_MAX * limit_gain*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen |
597 | if (speed_east > D_N_MAX) speed_east = D_N_MAX; |
597 | if (speed_east > D_N_MAX) speed_east = D_N_MAX; |
598 | else if (speed_east < -D_N_MAX) speed_east = -D_N_MAX; |
598 | else if (speed_east < -D_N_MAX) speed_east = -D_N_MAX; |
599 | if (speed_north > D_N_MAX) speed_north = D_N_MAX; |
599 | if (speed_north > D_N_MAX) speed_north = D_N_MAX; |
600 | else if (speed_north < -D_N_MAX) speed_north = -D_N_MAX; |
600 | else if (speed_north < -D_N_MAX) speed_north = -D_N_MAX; |
Line 604... | Line 604... | ||
604 | 604 | ||
605 | // debug_gp_4 = (int)speed_east; // zum Debuggen |
605 | // debug_gp_4 = (int)speed_east; // zum Debuggen |
Line 606... | Line 606... | ||
606 | // debug_gp_5 = (int)speed_north; // zum Debuggen |
606 | // debug_gp_5 = (int)speed_north; // zum Debuggen |
607 | 607 | ||
608 | //P-Werte verstaerken |
608 | //P-Werte verstaerken |
Line 609... | Line 609... | ||
609 | delta_east = (delta_east * (long)diff_p)/(40); |
609 | delta_east = (delta_east * (long)diff_p*gps_gain)/(400); |
610 | delta_north = (delta_north * (long)diff_p)/(40); |
610 | delta_north = (delta_north * (long)diff_p*gps_gain)/(400); |
611 | 611 | ||
612 | if (hold_fast > 0) //schneller Coming Home Modus |
612 | if (hold_fast > 0) //schneller Coming Home Modus |
613 | { |
613 | { |
614 | // P Werte begrenzen |
614 | // P Werte begrenzen |
615 | #define P1_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
615 | #define P1_F_MAX (GPS_NICKROLL_MAX * limit_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
616 | if (delta_east > P1_F_MAX) delta_east = P1_F_MAX; |
616 | if (delta_east > P1_F_MAX) delta_east = P1_F_MAX; |
617 | else if (delta_east < -P1_F_MAX) delta_east = -P1_F_MAX; |
617 | else if (delta_east < -P1_F_MAX) delta_east = -P1_F_MAX; |
618 | if (delta_north > P1_F_MAX) delta_north = P1_F_MAX; |
618 | if (delta_north > P1_F_MAX) delta_north = P1_F_MAX; |
619 | else if (delta_north < -P1_F_MAX) delta_north = -P1_F_MAX; |
619 | else if (delta_north < -P1_F_MAX) delta_north = -P1_F_MAX; |
620 | } |
620 | } |
621 | else // Hold modus |
621 | else // Hold modus |
622 | { |
622 | { |
623 | // P Werte begrenzen |
623 | // P Werte begrenzen |
624 | #define P1_N_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
624 | #define P1_N_MAX (GPS_NICKROLL_MAX * limit_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
625 | if (delta_east > P1_N_MAX) delta_east = P1_N_MAX; |
625 | if (delta_east > P1_N_MAX) delta_east = P1_N_MAX; |
626 | else if (delta_east < -P1_N_MAX) delta_east = -P1_N_MAX; |
626 | else if (delta_east < -P1_N_MAX) delta_east = -P1_N_MAX; |
Line 638... | Line 638... | ||
638 | debug_gp_0 = (int)gps_reg_x; // zum Debuggen |
638 | debug_gp_0 = (int)gps_reg_x; // zum Debuggen |
639 | debug_gp_1 = (int)gps_reg_y; // zum Debuggen |
639 | debug_gp_1 = (int)gps_reg_y; // zum Debuggen |
Line 640... | Line 640... | ||
640 | 640 | ||
641 | // Werte fuer Nick und Roll direkt aus gps_reg_x und gps_reg_y bestimmen |
641 | // Werte fuer Nick und Roll direkt aus gps_reg_x und gps_reg_y bestimmen |
642 | n = GyroKomp_Int/GIER_GRAD_FAKTOR; //Ausrichtung Kopter |
642 | n = GyroKomp_Int/GIER_GRAD_FAKTOR; //Ausrichtung Kopter |
643 | ni = -((gps_reg_y * (long)cos_i(n)) + (gps_reg_x * (long)sin_i(n)))/(1000*gps_gain); |
643 | ni = -((gps_reg_y * (long)cos_i(n)) + (gps_reg_x * (long)sin_i(n)))/(1000L*(long)limit_gain); |
Line 644... | Line 644... | ||
644 | ro = ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000*gps_gain); |
644 | ro = ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000L*(long)limit_gain); |
645 | 645 | ||
646 | if (ni > (GPS_NICKROLL_MAX )) ni = (GPS_NICKROLL_MAX); |
646 | if (ni > (GPS_NICKROLL_MAX )) ni = (GPS_NICKROLL_MAX); |
647 | else if (ni < -(GPS_NICKROLL_MAX )) ni = -(GPS_NICKROLL_MAX ); |
647 | else if (ni < -(GPS_NICKROLL_MAX )) ni = -(GPS_NICKROLL_MAX ); |