Subversion Repositories FlightCtrl

Rev

Rev 197 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 197 Rev 209
Line 37... Line 37...
37
#define                 UBLOX_NAV_VELED                 0x12
37
#define                 UBLOX_NAV_VELED                 0x12
38
#define                 UBLOX_NAV_CLASS                 0x01
38
#define                 UBLOX_NAV_CLASS                 0x01
39
#define                 UBLOX_SYNCH1_CHAR               0xB5
39
#define                 UBLOX_SYNCH1_CHAR               0xB5
40
#define                 UBLOX_SYNCH2_CHAR               0x62
40
#define                 UBLOX_SYNCH2_CHAR               0x62
Line 41... Line 41...
41
 
41
 
42
signed int GPS_Nick = 0;
42
signed int      GPS_Nick = 0;
43
signed int GPS_Roll = 0;
43
signed int      GPS_Roll = 0;
44
short int ublox_msg_state = UBLOX_IDLE;
44
short int       ublox_msg_state = UBLOX_IDLE;
45
static uint8_t  chk_a =0; //Checksum
45
static          uint8_t chk_a =0; //Checksum
46
static uint8_t  chk_b =0;
46
static          uint8_t chk_b =0;
47
short int gps_state;
47
short int       gps_state;
48
short int  gps_updte_flag;
48
short int       gps_updte_flag;
49
signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
49
signed int      GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
50
signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
50
signed int      GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
51
signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel 
51
signed int      GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel 
52
signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;                            
-
 
53
signed int ;
52
signed int      gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;                               
54
static unsigned int rx_len;
-
 
55
unsigned int cnt0,cnt1,cnt2; //******Provisorisch
53
static unsigned int rx_len;
56
static unsigned int ptr_payload_data_end;
54
static unsigned int ptr_payload_data_end;
Line 57... Line 55...
57
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
55
unsigned int            gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
58
 
56
 
Line 59... Line 57...
59
static uint8_t *ptr_payload_data;
57
static uint8_t *ptr_payload_data;
Line 85... Line 83...
85
        GPS_Roll                                        =       0;
83
        GPS_Roll                                        =       0;
86
        gps_updte_flag                          =       0;
84
        gps_updte_flag                          =       0;
87
        gps_int_x                                       =       0;
85
        gps_int_x                                       =       0;
88
        gps_int_y                                       =       0;
86
        gps_int_y                                       =       0;
89
        gps_alive_cnt                           =       0;
87
        gps_alive_cnt                           =       0;
90
 
-
 
91
}
88
}
Line 92... Line 89...
92
 
89
 
93
// Home Position sichern falls Daten verfuegbar sind. 
90
// Home Position sichern falls Daten verfuegbar sind. 
94
void GPS_Save_Home(void)
91
void GPS_Save_Home(void)
Line 136... Line 133...
136
        short int n = 1;
133
        short int n = 1;
Line 137... Line 134...
137
 
134
 
138
        if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
135
        if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
139
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
136
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
140
        {
-
 
141
                cnt1++; //**** noch Rausschmeissen
137
        {
142
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
138
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143
                {
139
                {
144
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
140
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
145
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
141
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
Line 168... Line 164...
168
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
164
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
169
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
165
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
170
*/
166
*/
171
void Get_Ublox_Msg(uint8_t rx)
167
void Get_Ublox_Msg(uint8_t rx)
172
{
168
{
173
 
-
 
174
        switch (ublox_msg_state)
169
        switch (ublox_msg_state)
175
        {
170
        {
Line 176... Line 171...
176
 
171
 
177
                case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
172
                case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
Line 382... Line 377...
382
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
377
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
383
                                {
378
                                {
384
                                        int_east        -= dist_east;
379
                                        int_east        -= dist_east;
385
                                        int_north   -= dist_north;                                     
380
                                        int_north   -= dist_north;                                     
386
                                }
381
                                }
387
                                //Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
382
                                // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
388
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. bei 0 ist die Verstaerkung 1
383
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
389
                                #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
384
                                #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
390
                                #define GPS_DIFF_MAX_D 40 //Entfernung bei der maximale Verstaerkung erreicht wird in (dm)
385
                                #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
391
                                signed long dist;
386
                                signed long dist;
392
                                int phi;
387
                                int phi;
393
                                phi = arctan_i(abs(dist_north),abs(dist_east));
388
                                phi = arctan_i(abs(dist_north),abs(dist_east));
394
                                if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln
389
                                if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln
395
                                {
390
                                {
396
                                        dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen
391
                                        dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen
397
                                        dist = abs((dist *1000) / (long) sin_i(phi));
392
                                        dist = abs((dist *1000) / (long) sin_i(phi));
398
                                }
393
                                }
Line 400... Line 395...
400
                                {
395
                                {
401
                                        dist = (long)dist_north;
396
                                        dist = (long)dist_north;
402
                                        dist = abs((dist *1000) / (long) cos_i(phi));
397
                                        dist = abs((dist *1000) / (long) cos_i(phi));
403
                                }
398
                                }
404
                                diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
399
                                diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
405
                                if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V;
400
                                if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
Line 406... Line 401...
406
 
401
 
407
                                //PID Regler Werte aufsummieren
402
                                //PID Regler Werte aufsummieren
408
                                gps_reg_x = ((int_east  * Parameter_UserParam2)/32) + ((dist_east  * Parameter_UserParam1)/8)+ ((diff_east_f  * diff_v * Parameter_UserParam3)/10);  // I + P +D  Anteil X Achse
403
                                gps_reg_x = ((int_east  * Parameter_UserParam2)/32) + ((dist_east  * Parameter_UserParam1)/8)+ ((diff_east_f  * diff_v * Parameter_UserParam3)/10);  // I + P +D  Anteil X Achse