Subversion Repositories FlightCtrl

Rev

Rev 542 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 542 Rev 565
Line 117... Line 117...
117 
short int Get_Rel_Position(void)
 117 
short int Get_Rel_Position(void)
118 
{
 118 
{
119 
        short int n = 0;
 119 
        short int n = 0;
120 
        n = Get_GPS_data();
 120 
        n = Get_GPS_data();
121 
        if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
 121 
        if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
122 
        if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
 122 
        if (gps_alive_cnt < 1000) gps_alive_cnt += 600; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
123 
        if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
 123 
        if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
124 
        {
 124 
        {
125 
                gps_rel_act_position.utm_east   = (int)  (gps_act_position.utm_east - gps_home_position.utm_east);
 125 
                gps_rel_act_position.utm_east   = (int)  (gps_act_position.utm_east - gps_home_position.utm_east);
126 
                gps_rel_act_position.utm_north  = (int)  (gps_act_position.utm_north - gps_home_position.utm_north);
 126 
                gps_rel_act_position.utm_north  = (int)  (gps_act_position.utm_north - gps_home_position.utm_north);
127 
                gps_rel_act_position.utm_alt    = (int)  (gps_act_position.utm_alt - gps_home_position.utm_alt);
 127 
                gps_rel_act_position.utm_alt    = (int)  (gps_act_position.utm_alt - gps_home_position.utm_alt);
Line 146... Line 146...
146 
        if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
 146 
        if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
147 
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
 147 
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
148 
        {
 148 
        {
149 
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
 149 
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
150 
                {
 150 
                {
- 
 
 151 
                        actual_status.status            = 0;
151 
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
 152 
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
152 
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
 153 
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
153 
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
 154 
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
- 
 
 155 
                        actual_pos.status                       = 0; //neue ublox Messages anfordern
154 
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
 156 
//                      gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
155 
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
 157 
//                      gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
156 
                        gps_act_position.heading        = actual_speed.heading/100000;
 158 
//                      gps_act_position.heading        = actual_speed.heading/100000;
- 
 
 159 
                        actual_speed.status             = 0;
157 
                        gps_act_position.status         = 1;
 160 
                        gps_act_position.status         = 1;
158 
                        n                                                       = 0; //Daten gueltig
 161 
                        n                                                       = 0; //Daten gueltig
159 
                }
 162 
                }
160 
                else
 163 
                else
161 
                {
 164 
                {
162 
                        gps_act_position.status = 0; //Keine gueltigen Daten
 165 
                        gps_act_position.status         = 0; //Keine gueltigen Daten
- 
 
 166 
                        actual_speed.status             = 0;
- 
 
 167 
                        actual_status.status            = 0;
- 
 
 168 
                        actual_pos.status                       = 0; //neue ublox Messages anfordern
163 
                        n                                               = 2;
 169 
                        n                                                       = 2;
164 
                }
 170 
                }
165 
                actual_pos.status               = 0; //neue ublox Messages anfordern
 - 
 
166 
                actual_status.status    = 0;
 - 
 
167 
                actual_speed.status     = 0;
 - 
 
168 
        }      
 171 
        }      
169 
        return (n);    
 172 
        return (n);    
170 
}
 173 
}
Line 171... Line 174...
171 
 
 174 
 
Line 305... Line 308...
305 
 
 308 
 
306 
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
309 
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
307 
                        if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
 310 
                        if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
308 
                        {
 311 
                        {
309 
                                cnt++;
 312 
                                cnt++;
310 
                                if (cnt > 500) // erst nach Verzoegerung
313 
                                if (cnt > 200) // erst nach Verzoegerung
311 
                                {
 314 
                                {
312 
                                        // Erst mal initialisieren
 315 
                                        // Erst mal initialisieren
313 
                                        cnt                             = 0;
 316 
                                        cnt                             = 0;
314 
                                        gps_tick                = 0;                                   
 317 
                                        gps_tick                = 0;                                   
Line 354... Line 357...
354 
 
 357 
 
355 
                case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
 358 
                case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
356 
                        if (gps_state != GPS_CRTL_HOLD_ACTIVE)
 359 
                        if (gps_state != GPS_CRTL_HOLD_ACTIVE)
357 
                        {
 360 
                        {
358 
                                cnt++;
 361 
                                cnt++;
359 
                                if (cnt > 500) // erst nach Verzoegerung
362 
                                if (cnt > 400) // erst nach Verzoegerung
360 
                                {
 363 
                                {
361 
                                        cnt     =       0;
 364 
                                        cnt     =       0;
362 
                                        // aktuelle positionsdaten abspeichern
 365 
                                        // aktuelle positionsdaten abspeichern
363 
                                        if (gps_rel_act_position.status > 0)
 366 
                                        if (gps_rel_act_position.status > 0)
Line 552... Line 555...
552 
                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
 555 
                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
553 
                                else diff_p = GPS_PROP_NRML_V;
 556 
                                else diff_p = GPS_PROP_NRML_V;
Line 554... Line 557...
554 
 
 557 
 
555 
                                //I Werte begrenzen
 558 
                                //I Werte begrenzen
556 
                                #define INT1_MAX (20 * GPS_V)
 559 
                                #define INT1_MAX (20 * GPS_V)
557 
                                int_east1 =  ((((long)int_east)   * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;
 560 
                                int_east1  =  ((((long)int_east)   * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;
558 
                                int_north =  ((((long)int_north)  * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;
 561 
                                int_north1 =  ((((long)int_north)  * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;  //Fehler behoben am 17.12.2007 vorher int_north=
559 
                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
 562 
                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
560 
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
 563 
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
561 
                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
 564 
                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen