WebSVN - FlightCtrl - Diff - Rev 528 and 529 - /branches/salvo_gps/Basis


/*
This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
either version 3 of the License, or (at your option) any later version.  
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License and GNU Lesser General Public License for more details.
You should have received a copy of GNU General Public License (License_GPL.txt)  and
GNU Lesser General Public License (License_LGPL.txt) along with this program.
If not, see <http://www.gnu.org/licenses/>.
 
Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de
*/
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Peter Muehlenbrock
Auswertung der Daten vom GPS im ublox Format
Hold Modus mit PID Regler
Rückstuerz zur Basis Funktion
Stand 24.10.2007
Anederung: 24.10. Altitude in relativer Position jetzt auch drin
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
#include "math.h"
//#include "gps.h"
 
// Defines fuer ublox Messageformat um Auswertung zu steuern
#define                 UBLOX_IDLE              0
#define                 UBLOX_SYNC1             1
#define                 UBLOX_SYNC2             2
#define                 UBLOX_CLASS             3
#define                 UBLOX_ID                4
#define                 UBLOX_LEN1              5
#define                 UBLOX_LEN2              6
#define                 UBLOX_CKA               7
#define                 UBLOX_CKB               8
#define                 UBLOX_PAYLOAD   9
 
// ublox Protokoll Identifier 
#define                 UBLOX_NAV_POSUTM                0x08
#define                 UBLOX_NAV_STATUS                0x03
#define                 UBLOX_NAV_VELED                 0x12
#define                 UBLOX_NAV_CLASS                 0x01
#define                 UBLOX_SYNCH1_CHAR               0xB5
#define                 UBLOX_SYNCH2_CHAR               0x62
 
signed int                      GPS_Nick = 0;
signed int                      GPS_Roll = 0;
short int                       ublox_msg_state = UBLOX_IDLE;
static                          uint8_t chk_a =0; //Checksum
static                          uint8_t chk_b =0;
short int                       gps_state,gps_sub_state; //Zustaende der Statemachine
short int                       gps_updte_flag;
signed int                      GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
signed int                      GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
signed int                      GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel 
signed int                      gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;                               
static unsigned int rx_len;
static unsigned int ptr_payload_data_end;
unsigned int            gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
signed int                      hdng_2home,dist_2home; //Richtung und Entfernung zur home Position 
static signed           gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt 
static                          short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
static                          uint8_t *ptr_payload_data;
static                          uint8_t *ptr_pac_status;
long int                        dist_flown;
 
short int Get_GPS_data(void);
 
NAV_POSUTM_t actual_pos;    // Aktuelle Nav Daten werden hier im ublox Format abgelegt
NAV_STATUS_t actual_status; // Aktueller Nav Status
NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten
 
GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
GPS_ABS_POSITION_t              gps_home_position;      // Die Startposition, beim Kalibrieren ermittelt
GPS_REL_POSITION_t              gps_rel_act_position;   // Die aktuelle relative Position bezogen auf Home Position
GPS_REL_POSITION_t              gps_rel_hold_position;  // Die gespeicherte Sollposition fuer GPS_ Hold Mode
GPS_REL_POSITION_t              gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
 
// 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;
        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_int_x                                       =       0;
        gps_int_y                                       =       0;
        gps_alive_cnt                           =       0;
}
 
// Home Position sichern falls Daten verfuegbar sind. 
void GPS_Save_Home(void)
{
        short int n;
        n = Get_GPS_data();
        if (n == 0)   // Gueltige  und aktuelle Daten ?   
        {
                // Neue GPS Daten liegen vor
                gps_home_position.utm_east      = gps_act_position.utm_east;   
                gps_home_position.utm_north     = gps_act_position.utm_north;  
                gps_home_position.utm_alt       = gps_act_position.utm_alt;
                gps_home_position.status        = 1; // Home Position gueltig                   
        }
}
 
// Relative Position zur Home Position bestimmen
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
short int Get_Rel_Position(void)
{
        short int n = 0;
        n = Get_GPS_data();
        if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
        if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
        if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
        {
                gps_rel_act_position.utm_east   = (int)  (gps_act_position.utm_east - gps_home_position.utm_east);
                gps_rel_act_position.utm_north  = (int)  (gps_act_position.utm_north - gps_home_position.utm_north);
                gps_rel_act_position.utm_alt    = (int)  (gps_act_position.utm_alt - gps_home_position.utm_alt);
                gps_rel_act_position.status     = 1; // gueltige Positionsdaten
                n = 0;
                gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
        }
        else
        {
                n = 2; //keine gueltigen Daten vorhanden
                gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
        }      
        return (n);
}
 
// Daten aus aktuellen ublox Messages extrahieren 
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
short int Get_GPS_data(void)
{
        short int n = 1;
 
        if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
        {
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
                {
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
                        gps_act_position.heading        = actual_speed.heading/100000;
                        gps_act_position.status         = 1;
                        n                                                       = 0; //Daten gueltig
                }
                else
                {
                        gps_act_position.status = 0; //Keine gueltigen Daten
                        n                                               = 2;
                }
                actual_pos.status               = 0; //neue ublox Messages anfordern
                actual_status.status    = 0;
                actual_speed.status     = 0;
        }      
        return (n);    
}
 
/*
Daten vom GPS im ublox MSG Format auswerten
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
*/
void Get_Ublox_Msg(uint8_t rx)
{
        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;
                        break;
 
                case UBLOX_SYNC1:
 
                        if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
                        else ublox_msg_state = UBLOX_IDLE;
                        chk_a = 0,chk_b = 0;
                        break;
 
                case UBLOX_SYNC2:
                        if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;      
                        else ublox_msg_state = UBLOX_IDLE;
                        break;
 
                case UBLOX_CLASS: // Nur NAV Meldungen auswerten
                        switch (rx)
                        {
                                case UBLOX_NAV_POSUTM:
                                        ptr_pac_status  =       &actual_pos.status;
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
                                        else
                                        {
                                                ptr_payload_data                = &actual_pos;
                                                ptr_payload_data_end    = &actual_pos.status;
                                                ublox_msg_state                 = UBLOX_LEN1;
                                        }
                                        break;
 
                                case UBLOX_NAV_STATUS:
                                        ptr_pac_status  =       &actual_status.status;
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
                                        else
                                        {
                                                ptr_payload_data                = &actual_status;
                                                ptr_payload_data_end    = &actual_status.status;
                                                ublox_msg_state                 = UBLOX_LEN1;
                                        }
                                        break;
 
                                case UBLOX_NAV_VELED:
                                        ptr_pac_status          =       &actual_speed.status;
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
                                        else
                                        {
                                                ptr_payload_data                = &actual_speed;
                                                ptr_payload_data_end    = &actual_speed.status;
                                                ublox_msg_state                 = UBLOX_LEN1;
                                        }
                                        break;
 
                                default:
                                        ublox_msg_state = UBLOX_IDLE;
                                        break; 
                        }
                        chk_a   = UBLOX_NAV_CLASS + rx;
                        chk_b   = UBLOX_NAV_CLASS + chk_a;                     
                        break;
 
                case UBLOX_LEN1: // Laenge auswerten
                        rx_len  = rx;
                        chk_a   += rx;
                        chk_b   += chk_a;              
                        ublox_msg_state = UBLOX_LEN2;
                        break;
 
 
                case UBLOX_LEN2: // Laenge auswerten
                        rx_len = rx_len + (rx *256); // Laenge ermitteln
                        chk_a   += rx;
                        chk_b   += chk_a;      
                        ublox_msg_state = UBLOX_PAYLOAD;
                        break;
 
                case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
                        if (rx_len > 0)
                        {
                                *ptr_payload_data = rx;
                                chk_a   += rx;
                                chk_b   += chk_a;
                                --rx_len;      
                                if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))                          
                                {
                                        ptr_payload_data++;
                                        ublox_msg_state = UBLOX_PAYLOAD;
                            }
                                else ublox_msg_state = UBLOX_CKA;
                        }      
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
                        break;
 
                case UBLOX_CKA: // Checksum pruefen
                        if (rx == chk_a) ublox_msg_state = UBLOX_CKB;                          
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
                        break;
 
                case UBLOX_CKB: // Checksum pruefen
                        if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
                        ublox_msg_state    = UBLOX_IDLE;
                        break;
 
                default:
                        ublox_msg_state = UBLOX_IDLE;          
                        break;
        }
}
       
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
short int GPS_CRTL(short int cmd)
{
        static unsigned int cnt;                                        //Zaehler fuer diverse Verzoegerungen 
        static signed int int_east,int_north;   //Integrierer 
        static signed int dist_north,dist_east;
        static signed int diff_east,diff_north;         // Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)
        static signed int diff_east_f,diff_north_f; // Differenzierer,  gefiltert
        signed int n,diff_v;
        static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
        long signed int dev,n_l;
        signed int dist_frm_start_east,dist_frm_start_north;
        switch (cmd)
        {
 
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. 
                        if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
                        {
                                cnt++;
                                if (cnt > 500) // erst nach Verzoegerung 
                                {
                                        // Erst mal initialisieren
                                        cnt                             = 0;
                                        gps_tick                = 0;                                   
                                        hold_fast               = 0;
                                        hold_reset_int  = 0; // Integrator enablen
                                        int_east                = 0, int_north  = 0;
                                        gps_reg_x               = 0, gps_reg_y  = 0;
                                        dist_east               = 0, dist_north = 0;
                                        diff_east_f             = 0, diff_north_f= 0;
                                        diff_east               = 0, diff_north  = 0;  
                                        dist_flown              = 0;
                                        gps_g2t_act_v   = 0;
                                        gps_sub_state   = GPS_CRTL_IDLE;
                                        // aktuelle positionsdaten abspeichern
                                        if (gps_rel_act_position.status > 0)
                                        {
                                                gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
                                                gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
                                                gps_rel_start_position.status   = 1; // gueltige Positionsdaten 
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
                                                //Richtung zur Home Position bezogen auf Nordpol bestimmen
                                                hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
                                                // in Winkel 0...360 Grad umrechnen
                                                if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
                                                else  hdng_2home = (270 - hdng_2home);
                                                dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
                                                gps_state       = GPS_CRTL_HOME_ACTIVE;
                                                return (GPS_STST_OK);                          
                                        }
                                        else
                                        {
                                                gps_rel_start_position.status   =       0;  //Keine Daten verfuegbar
                                                gps_state                                               = GPS_CRTL_IDLE;
                                                return(GPS_STST_ERR); // Keine Daten da
                                        }
                                }
                                else return(GPS_STST_PEND); // noch warten
                        }
                   break;
// ******************************
 
                case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
                        if (gps_state != GPS_CRTL_HOLD_ACTIVE)
                        {
                                cnt++;
                                if (cnt > 500) // erst nach Verzoegerung 
                                {
                                        cnt     =       0;
                                        // aktuelle positionsdaten abspeichern
                                        if (gps_rel_act_position.status > 0)
                                        {
                                                hold_fast               = 0;
                                                hold_reset_int  = 0; // Integrator enablen
                                                int_east        = 0, int_north  = 0;
                                                gps_reg_x       = 0, gps_reg_y  = 0;
                                                dist_east       = 0, dist_north = 0;
                                                diff_east_f     = 0, diff_north_f= 0;
                                                diff_east       = 0, diff_north  = 0;
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
                                                gps_state                                               = GPS_CRTL_HOLD_ACTIVE;
                                                return (GPS_STST_OK);                          
                                        }
                                        else
                                        {
                                                gps_rel_hold_position.status    =       0;  //Keine Daten verfuegbar
                                                gps_state                                               = GPS_CRTL_IDLE;
                                                return(GPS_STST_ERR); // Keine Daten da
                                        }
                                }
                                else return(GPS_STST_PEND); // noch warten
                        }
                        break;
 
                case GPS_CMD_STOP: // Lageregelung beenden
                        cnt                             =       0;
                        GPS_Nick                =       0;
                        GPS_Roll                =       0;
                        gps_int_x               =       0;
                        gps_int_y               =       0;
                        gps_sub_state   =       GPS_CRTL_IDLE;
                        gps_state               =       GPS_CRTL_IDLE;
                        return (GPS_STST_OK);
                        break;
 
                default:
                        return (GPS_STST_ERR);
                        break;
        }
 
        switch (gps_state)
        {      
                case GPS_CRTL_IDLE:
                        cnt             =       0;
                        return (GPS_STST_OK);
                        break;
 
                case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
                //Der Sollwert des Lagereglers wird der Homeposition angenaehert
                        if (gps_rel_start_position.status >0)
                        {
                                if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
                                {
                                        gps_tick++;
                                        int d1,d2,d3;
                                        d1      = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
                                        d2      = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
                                        d3      = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel       
       
                                        if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
                                        {
                                                if ((d1 < GPS_G2T_FAST_TOL)  && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
                                                {
                                                        if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++;    //Geschwindigkeit langsam erhoehen
                                                        dist_flown              +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
                                                        gps_sub_state   = GPS_HOME_FAST_IN_TOL;
                                                }
                                                else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
                                                {
                                                        if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
                                                        dist_flown++;                                                   //Auch ausserhalb der Toleranz langsam erhoehen
                                                        gps_sub_state   = GPS_HOME_FAST_OUTOF_TOL;
                                                }
                                                hold_reset_int                                  = 0; // Integrator einsschalten  
                                                hold_fast                                               = 1; // Regler fuer schnellen Flug
                                                dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
                                                dist_frm_start_north                    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
                                        }
                                        else if (d3 > GPS_G2T_DIST_HOLD)   //Das Ziel naehert sich, deswegen abbremsen
                                        {
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL))  
                                                {
                                                        dist_flown              +=      GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
                                                        gps_sub_state   =       GPS_HOME_RMPDWN_IN_TOL;
                                                }
                                                else
                                                {
                                                        dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
                                                        gps_sub_state   = GPS_HOME_RMPDWN_OUTOF_TOL;
                                                }                                      
                                                hold_reset_int                                  = 0; // Integrator ausschalten            
                                                hold_fast                                               = 1; // Wieder normal regeln
                                                dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
                                                dist_frm_start_north                    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
                                        }                                                      
                                        else  //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) 
                                        {
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
                                                {
                                                        gps_sub_state   = GPS_HOME_IN_TOL;
                                                        hold_fast               = 0; // Wieder normal regeln
                                                        hold_reset_int  = 0; // Integrator wieder aktivieren              
                                                        if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
                                                        else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
                                                        if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
                                                        else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
                                                        if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
                                                        {
                                                                gps_rel_hold_position.utm_east  = 0;
                                                                gps_rel_hold_position.utm_north = 0;
                                                                gps_sub_state                                   = GPS_HOME_FINISHED;
                                                        }
                                                }
                                                else gps_sub_state      = GPS_HOME_OUTOF_TOL;
                                        }                                      
                                }
                                gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
                                return (GPS_STST_OK);                                  
                        }
                        else  // Keine GPS Daten verfuegbar, deswegen Abbruch
                        {
                                gps_state       =       GPS_CRTL_IDLE; 
                                return (GPS_STST_ERR);
                        }
                        break;
 
 
                case GPS_CRTL_HOLD_ACTIVE:  // Hier werden die Daten fuer Nick und Roll errechnet
                        if (gps_updte_flag >0)  // nur wenn neue GPS Daten vorliegen
                        {
                                gps_updte_flag = 0;
                                // ab hier wird geregelt
                                diff_east       = -dist_east;     //Alten Wert fuer Differenzier schon mal abziehen
                                diff_north      = -dist_north;
                                dist_east       = gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;
                                dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
                                int_east        += dist_east;
                                int_north   += dist_north;
                                diff_east       += dist_east;   // Differenz zur vorhergehenden East Position
                                diff_north      += dist_north;  // Differenz zur vorhergehenden North Position
 
                                if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen
                                {
                                        diff_east_f             = ((diff_east_f  *2)/3) + (diff_east *1/3); //Differenzierer filtern    
                                        diff_north_f    = ((diff_north_f *2)/3) + (diff_north*1/3); //Differenzierer filtern
                                }      
                                else // schwache Filterung
                                {
                                        diff_east_f             = ((diff_east_f  * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern 
                                        diff_north_f    = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern
                                }
 
                                #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung 
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
                                {
                                        int_east        -= dist_east;
                                        int_north   -= dist_north;                                     
                                }
                                if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
                                {
                                        int_east        = 0;   
                                        int_north       = 0;                                   
                                }
 
                                // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
                                signed long dist,int_east1,int_north1;
                                int phi;
                                phi  = arctan_i(abs(dist_north),abs(dist_east));
                                dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
 
                                if (hold_fast == 0)  // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
                                {
                                        diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10
                                        if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen
                                }
                                else
                                {
                                        diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10
                                        if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
                                }
 
                                int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung
                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
                                else diff_p = GPS_PROP_NRML_V;
 
                                //I Werte begrenzen
                                #define INT1_MAX (20 * GPS_V)
                                int_east1 =  ((((long)int_east)   * Parameter_UserParam2)/32);
                                int_north =  ((((long)int_north)  * Parameter_UserParam2)/32);
                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
                                else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;
 
                                //PID Regler Werte aufsummieren
                                gps_reg_x = ((int)int_east1  + ((dist_east  * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east_f  * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil X Achse
                                gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north_f * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil Y Achse
 
                                //Ziel-Richtung bezogen auf Nordpol bestimmen
                                GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
 
                                // in Winkel 0...360 Grad umrechnen
                                if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
                                else  GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
 
                                // Relative Richtung in bezug auf Nordachse des Kopters errechen
                                n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
                                GPS_hdng_rel_2trgt      =       GPS_hdng_abs_2trgt - n;
                                if      ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
                                else if (GPS_hdng_rel_2trgt >180)  GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
                                else if (GPS_hdng_rel_2trgt <-180)  GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
                               
                                // Regelabweichung aus x,y zu Ziel in Distanz umrechnen 
                                if (abs(gps_reg_x) > abs(gps_reg_y) )
                                {
                                        dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
                                        dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
                                }
                                else
                                {
                                        dev = (long)gps_reg_y;
                                        dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
                                }
                                GPS_dist_2trgt  = (int) dev;
                                // Winkel und Distanz in Nick und Rollgroessen umrechnen
                                GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
                                GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
                                       
                                if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
                                else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
                                if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
                                else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
 
                                //Kleine Werte verstaerken, Grosse abschwaechen
                                n                       = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
                                n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
                                GPS_Roll        = (int) n_l;
                                n                       = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));
                                n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
                                GPS_Nick        = (int) n_l;
                                 
                                if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
                                {
                                        GPS_Roll        = 0;
                                        GPS_Nick        = 0;
                                        gps_state       = GPS_CRTL_IDLE;
                                        return (GPS_STST_ERR); 
                                        break;                                 
                                }
                                else
                                {
                                        if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
                                        return (GPS_STST_OK);
                                }
                        }
                        else
                        {
                                if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
                                return (GPS_STST_OK);
                        }
                        break;
 
                default:
                        gps_state = GPS_CRTL_IDLE;
                        return (GPS_STST_ERR);
                        break;
        }      
        return (GPS_STST_ERR);
                       
}
 
 

Rev 528	Rev 529
1	/*	1	/*
2	This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify	2	This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify
3	it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;	3	it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
4	either version 3 of the License, or (at your option) any later version.	4	either version 3 of the License, or (at your option) any later version.
5	This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;	5	This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
6	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	6	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
7	GNU General Public License and GNU Lesser General Public License for more details.	7	GNU General Public License and GNU Lesser General Public License for more details.
8	You should have received a copy of GNU General Public License (License_GPL.txt) and	8	You should have received a copy of GNU General Public License (License_GPL.txt) and
9	GNU Lesser General Public License (License_LGPL.txt) along with this program.	9	GNU Lesser General Public License (License_LGPL.txt) along with this program.
10	If not, see <http://www.gnu.org/licenses/>.	10	If not, see <http://www.gnu.org/licenses/>.
11		11
12	Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de	12	Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de
13	*/	13	*/
14	/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	14	/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15	Peter Muehlenbrock	15	Peter Muehlenbrock
16	Auswertung der Daten vom GPS im ublox Format	16	Auswertung der Daten vom GPS im ublox Format
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	Stand 24.10.2007	19	Stand 24.10.2007
20	Anederung: 24.10. Altitude in relativer Position jetzt auch drin	20	Anederung: 24.10. Altitude in relativer Position jetzt auch drin
21	++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	21	++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
22	*/	22	*/
23	#include "main.h"	23	#include "main.h"
-		24	#include "math.h"
24	//#include "gps.h"	25	//#include "gps.h"
25		26
26	// Defines fuer ublox Messageformat um Auswertung zu steuern	27	// Defines fuer ublox Messageformat um Auswertung zu steuern
27	#define UBLOX_IDLE 0	28	#define UBLOX_IDLE 0
28	#define UBLOX_SYNC1 1	29	#define UBLOX_SYNC1 1
29	#define UBLOX_SYNC2 2	30	#define UBLOX_SYNC2 2
30	#define UBLOX_CLASS 3	31	#define UBLOX_CLASS 3
31	#define UBLOX_ID 4	32	#define UBLOX_ID 4
32	#define UBLOX_LEN1 5	33	#define UBLOX_LEN1 5
33	#define UBLOX_LEN2 6	34	#define UBLOX_LEN2 6
34	#define UBLOX_CKA 7	35	#define UBLOX_CKA 7
35	#define UBLOX_CKB 8	36	#define UBLOX_CKB 8
36	#define UBLOX_PAYLOAD 9	37	#define UBLOX_PAYLOAD 9
37		38
38	// ublox Protokoll Identifier	39	// ublox Protokoll Identifier
39	#define UBLOX_NAV_POSUTM 0x08	40	#define UBLOX_NAV_POSUTM 0x08
40	#define UBLOX_NAV_STATUS 0x03	41	#define UBLOX_NAV_STATUS 0x03
41	#define UBLOX_NAV_VELED 0x12	42	#define UBLOX_NAV_VELED 0x12
42	#define UBLOX_NAV_CLASS 0x01	43	#define UBLOX_NAV_CLASS 0x01
43	#define UBLOX_SYNCH1_CHAR 0xB5	44	#define UBLOX_SYNCH1_CHAR 0xB5
44	#define UBLOX_SYNCH2_CHAR 0x62	45	#define UBLOX_SYNCH2_CHAR 0x62
45		46
46	signed int GPS_Nick = 0;	47	signed int GPS_Nick = 0;
47	signed int GPS_Roll = 0;	48	signed int GPS_Roll = 0;
48	short int ublox_msg_state = UBLOX_IDLE;	49	short int ublox_msg_state = UBLOX_IDLE;
49	static uint8_t chk_a =0; //Checksum	50	static uint8_t chk_a =0; //Checksum
50	static uint8_t chk_b =0;	51	static uint8_t chk_b =0;
51	short int gps_state,gps_sub_state; //Zustaende der Statemachine	52	short int gps_state,gps_sub_state; //Zustaende der Statemachine
52	short int gps_updte_flag;	53	short int gps_updte_flag;
53	signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol	54	signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
54	signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters	55	signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
55	signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel	56	signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
56	signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;	57	signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
57	static unsigned int rx_len;	58	static unsigned int rx_len;
58	static unsigned int ptr_payload_data_end;	59	static unsigned int ptr_payload_data_end;
59	unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt	60	unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
60	signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position	61	signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
61	static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt	62	static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
62	static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler	63	static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
63	static uint8_t *ptr_payload_data;	64	static uint8_t *ptr_payload_data;
64	static uint8_t *ptr_pac_status;	65	static uint8_t *ptr_pac_status;
65	long int dist_flown;	66	long int dist_flown;
66		67
67	short int Get_GPS_data(void);	68	short int Get_GPS_data(void);
68		69
69	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt	70	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
70	NAV_STATUS_t actual_status; // Aktueller Nav Status	71	NAV_STATUS_t actual_status; // Aktueller Nav Status
71	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten	72	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
72		73
73	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst	74	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
74	GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt	75	GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt
75	GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position	76	GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position
76	GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode	77	GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode
77	GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode	78	GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
78		79
79	// Initialisierung	80	// Initialisierung
80	void GPS_Neutral(void)	81	void GPS_Neutral(void)
81	{	82	{
82	ublox_msg_state = UBLOX_IDLE;	83	ublox_msg_state = UBLOX_IDLE;
83	gps_state = GPS_CRTL_IDLE;	84	gps_state = GPS_CRTL_IDLE;
84	gps_sub_state = GPS_CRTL_IDLE;	85	gps_sub_state = GPS_CRTL_IDLE;
85	actual_pos.status = 0;	86	actual_pos.status = 0;
86	actual_speed.status = 0;	87	actual_speed.status = 0;
87	actual_status.status = 0;	88	actual_status.status = 0;
88	gps_home_position.status = 0; // Noch keine gueltige Home Position	89	gps_home_position.status = 0; // Noch keine gueltige Home Position
89	gps_act_position.status = 0;	90	gps_act_position.status = 0;
90	gps_rel_act_position.status = 0;	91	gps_rel_act_position.status = 0;
91	GPS_Nick = 0;	92	GPS_Nick = 0;
92	GPS_Roll = 0;	93	GPS_Roll = 0;
93	gps_updte_flag = 0;	94	gps_updte_flag = 0;
94	gps_int_x = 0;	95	gps_int_x = 0;
95	gps_int_y = 0;	96	gps_int_y = 0;
96	gps_alive_cnt = 0;	97	gps_alive_cnt = 0;
97	}	98	}
98		99
99	// Home Position sichern falls Daten verfuegbar sind.	100	// Home Position sichern falls Daten verfuegbar sind.
100	void GPS_Save_Home(void)	101	void GPS_Save_Home(void)
101	{	102	{
102	short int n;	103	short int n;
103	n = Get_GPS_data();	104	n = Get_GPS_data();
104	if (n == 0) // Gueltige und aktuelle Daten ?	105	if (n == 0) // Gueltige und aktuelle Daten ?
105	{	106	{
106	// Neue GPS Daten liegen vor	107	// Neue GPS Daten liegen vor
107	gps_home_position.utm_east = gps_act_position.utm_east;	108	gps_home_position.utm_east = gps_act_position.utm_east;
108	gps_home_position.utm_north = gps_act_position.utm_north;	109	gps_home_position.utm_north = gps_act_position.utm_north;
109	gps_home_position.utm_alt = gps_act_position.utm_alt;	110	gps_home_position.utm_alt = gps_act_position.utm_alt;
110	gps_home_position.status = 1; // Home Position gueltig	111	gps_home_position.status = 1; // Home Position gueltig
111	}	112	}
112	}	113	}
113		114
114	// Relative Position zur Home Position bestimmen	115	// Relative Position zur Home Position bestimmen
115	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig	116	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
116	short int Get_Rel_Position(void)	117	short int Get_Rel_Position(void)
117	{	118	{
118	short int n = 0;	119	short int n = 0;
119	n = Get_GPS_data();	120	n = Get_GPS_data();
120	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
121	if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert	122	if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
122	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt	123	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
123	{	124	{
124	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);
125	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);
126	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);
127	gps_rel_act_position.status = 1; // gueltige Positionsdaten	128	gps_rel_act_position.status = 1; // gueltige Positionsdaten
128	n = 0;	129	n = 0;
129	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.	130	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
130	}	131	}
131	else	132	else
132	{	133	{
133	n = 2; //keine gueltigen Daten vorhanden	134	n = 2; //keine gueltigen Daten vorhanden
134	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.	135	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
135	}	136	}
136	return (n);	137	return (n);
137	}	138	}
138		139
139	// Daten aus aktuellen ublox Messages extrahieren	140	// Daten aus aktuellen ublox Messages extrahieren
140	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig	141	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
141	short int Get_GPS_data(void)	142	short int Get_GPS_data(void)
142	{	143	{
143	short int n = 1;	144	short int n = 1;
144		145
145	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
146	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))
147	{	148	{
148	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
149	{	150	{
150	gps_act_position.utm_east = actual_pos.utm_east/10;	151	gps_act_position.utm_east = actual_pos.utm_east/10;
151	gps_act_position.utm_north = actual_pos.utm_north/10;	152	gps_act_position.utm_north = actual_pos.utm_north/10;
152	gps_act_position.utm_alt = actual_pos.utm_alt/10;	153	gps_act_position.utm_alt = actual_pos.utm_alt/10;
153	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;	154	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
154	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;	155	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
155	gps_act_position.heading = actual_speed.heading/100000;	156	gps_act_position.heading = actual_speed.heading/100000;
156	gps_act_position.status = 1;	157	gps_act_position.status = 1;
157	n = 0; //Daten gueltig	158	n = 0; //Daten gueltig
158	}	159	}
159	else	160	else
160	{	161	{
161	gps_act_position.status = 0; //Keine gueltigen Daten	162	gps_act_position.status = 0; //Keine gueltigen Daten
162	n = 2;	163	n = 2;
163	}	164	}
164	actual_pos.status = 0; //neue ublox Messages anfordern	165	actual_pos.status = 0; //neue ublox Messages anfordern
165	actual_status.status = 0;	166	actual_status.status = 0;
166	actual_speed.status = 0;	167	actual_speed.status = 0;
167	}	168	}
168	return (n);	169	return (n);
169	}	170	}
170		171
171	/*	172	/*
172	Daten vom GPS im ublox MSG Format auswerten	173	Daten vom GPS im ublox MSG Format auswerten
173	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen	174	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
174	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein	175	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
175	*/	176	*/
176	void Get_Ublox_Msg(uint8_t rx)	177	void Get_Ublox_Msg(uint8_t rx)
177	{	178	{
178	switch (ublox_msg_state)	179	switch (ublox_msg_state)
179	{	180	{
180		181
181	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen	182	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
182	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;	183	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
183	else ublox_msg_state = UBLOX_IDLE;	184	else ublox_msg_state = UBLOX_IDLE;
184	break;	185	break;
185		186
186	case UBLOX_SYNC1:	187	case UBLOX_SYNC1:
187		188
188	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;	189	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
189	else ublox_msg_state = UBLOX_IDLE;	190	else ublox_msg_state = UBLOX_IDLE;
190	chk_a = 0,chk_b = 0;	191	chk_a = 0,chk_b = 0;
191	break;	192	break;
192		193
193	case UBLOX_SYNC2:	194	case UBLOX_SYNC2:
194	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;	195	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
195	else ublox_msg_state = UBLOX_IDLE;	196	else ublox_msg_state = UBLOX_IDLE;
196	break;	197	break;
197		198
198	case UBLOX_CLASS: // Nur NAV Meldungen auswerten	199	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
199	switch (rx)	200	switch (rx)
200	{	201	{
201	case UBLOX_NAV_POSUTM:	202	case UBLOX_NAV_POSUTM:
202	ptr_pac_status = &actual_pos.status;	203	ptr_pac_status = &actual_pos.status;
203	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden	204	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
204	else	205	else
205	{	206	{
206	ptr_payload_data = &actual_pos;	207	ptr_payload_data = &actual_pos;
207	ptr_payload_data_end = &actual_pos.status;	208	ptr_payload_data_end = &actual_pos.status;
208	ublox_msg_state = UBLOX_LEN1;	209	ublox_msg_state = UBLOX_LEN1;
209	}	210	}
210	break;	211	break;
211		212
212	case UBLOX_NAV_STATUS:	213	case UBLOX_NAV_STATUS:
213	ptr_pac_status = &actual_status.status;	214	ptr_pac_status = &actual_status.status;
214	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;	215	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
215	else	216	else
216	{	217	{
217	ptr_payload_data = &actual_status;	218	ptr_payload_data = &actual_status;
218	ptr_payload_data_end = &actual_status.status;	219	ptr_payload_data_end = &actual_status.status;
219	ublox_msg_state = UBLOX_LEN1;	220	ublox_msg_state = UBLOX_LEN1;
220	}	221	}
221	break;	222	break;
222		223
223	case UBLOX_NAV_VELED:	224	case UBLOX_NAV_VELED:
224	ptr_pac_status = &actual_speed.status;	225	ptr_pac_status = &actual_speed.status;
225	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;	226	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
226	else	227	else
227	{	228	{
228	ptr_payload_data = &actual_speed;	229	ptr_payload_data = &actual_speed;
229	ptr_payload_data_end = &actual_speed.status;	230	ptr_payload_data_end = &actual_speed.status;
230	ublox_msg_state = UBLOX_LEN1;	231	ublox_msg_state = UBLOX_LEN1;
231	}	232	}
232	break;	233	break;
233		234
234	default:	235	default:
235	ublox_msg_state = UBLOX_IDLE;	236	ublox_msg_state = UBLOX_IDLE;
236	break;	237	break;
237	}	238	}
238	chk_a = UBLOX_NAV_CLASS + rx;	239	chk_a = UBLOX_NAV_CLASS + rx;
239	chk_b = UBLOX_NAV_CLASS + chk_a;	240	chk_b = UBLOX_NAV_CLASS + chk_a;
240	break;	241	break;
241		242
242	case UBLOX_LEN1: // Laenge auswerten	243	case UBLOX_LEN1: // Laenge auswerten
243	rx_len = rx;	244	rx_len = rx;
244	chk_a += rx;	245	chk_a += rx;
245	chk_b += chk_a;	246	chk_b += chk_a;
246	ublox_msg_state = UBLOX_LEN2;	247	ublox_msg_state = UBLOX_LEN2;
247	break;	248	break;
248		249
249		250
250	case UBLOX_LEN2: // Laenge auswerten	251	case UBLOX_LEN2: // Laenge auswerten
251	rx_len = rx_len + (rx *256); // Laenge ermitteln	252	rx_len = rx_len + (rx *256); // Laenge ermitteln
252	chk_a += rx;	253	chk_a += rx;
253	chk_b += chk_a;	254	chk_b += chk_a;
254	ublox_msg_state = UBLOX_PAYLOAD;	255	ublox_msg_state = UBLOX_PAYLOAD;
255	break;	256	break;
256		257
257	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen	258	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
258	if (rx_len > 0)	259	if (rx_len > 0)
259	{	260	{
260	*ptr_payload_data = rx;	261	*ptr_payload_data = rx;
261	chk_a += rx;	262	chk_a += rx;
262	chk_b += chk_a;	263	chk_b += chk_a;
263	--rx_len;	264	--rx_len;
264	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))	265	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
265	{	266	{
266	ptr_payload_data++;	267	ptr_payload_data++;
267	ublox_msg_state = UBLOX_PAYLOAD;	268	ublox_msg_state = UBLOX_PAYLOAD;
268	}	269	}
269	else ublox_msg_state = UBLOX_CKA;	270	else ublox_msg_state = UBLOX_CKA;
270	}	271	}
271	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler	272	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
272	break;	273	break;
273		274
274	case UBLOX_CKA: // Checksum pruefen	275	case UBLOX_CKA: // Checksum pruefen
275	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;	276	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
276	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler	277	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
277	break;	278	break;
278		279
279	case UBLOX_CKB: // Checksum pruefen	280	case UBLOX_CKB: // Checksum pruefen
280	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok	281	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
281	ublox_msg_state = UBLOX_IDLE;	282	ublox_msg_state = UBLOX_IDLE;
282	break;	283	break;
283		284
284	default:	285	default:
285	ublox_msg_state = UBLOX_IDLE;	286	ublox_msg_state = UBLOX_IDLE;
286	break;	287	break;
287	}	288	}
288	}	289	}
289		290
290	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe	291	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
291	short int GPS_CRTL(short int cmd)	292	short int GPS_CRTL(short int cmd)
292	{	293	{
293	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen	294	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen
294	static signed int int_east,int_north; //Integrierer	295	static signed int int_east,int_north; //Integrierer
295	static signed int dist_north,dist_east;	296	static signed int dist_north,dist_east;
296	static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert)	297	static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert)
297	static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert	298	static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert
298	signed int n,diff_v;	299	signed int n,diff_v;
299	static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion	300	static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
300	long signed int dev,n_l;	301	long signed int dev,n_l;
301	signed int dist_frm_start_east,dist_frm_start_north;	302	signed int dist_frm_start_east,dist_frm_start_north;
302	switch (cmd)	303	switch (cmd)
303	{	304	{
304		305
305	case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.	306	case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
306	if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))	307	if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
307	{	308	{
308	cnt++;	309	cnt++;
309	if (cnt > 500) // erst nach Verzoegerung	310	if (cnt > 500) // erst nach Verzoegerung
310	{	311	{
311	// Erst mal initialisieren	312	// Erst mal initialisieren
312	cnt = 0;	313	cnt = 0;
313	gps_tick = 0;	314	gps_tick = 0;
314	hold_fast = 0;	315	hold_fast = 0;
315	hold_reset_int = 0; // Integrator enablen	316	hold_reset_int = 0; // Integrator enablen
316	int_east = 0, int_north = 0;	317	int_east = 0, int_north = 0;
317	gps_reg_x = 0, gps_reg_y = 0;	318	gps_reg_x = 0, gps_reg_y = 0;
318	dist_east = 0, dist_north = 0;	319	dist_east = 0, dist_north = 0;
319	diff_east_f = 0, diff_north_f= 0;	320	diff_east_f = 0, diff_north_f= 0;
320	diff_east = 0, diff_north = 0;	321	diff_east = 0, diff_north = 0;
321	dist_flown = 0;	322	dist_flown = 0;
322	gps_g2t_act_v = 0;	323	gps_g2t_act_v = 0;
323	gps_sub_state = GPS_CRTL_IDLE;	324	gps_sub_state = GPS_CRTL_IDLE;
324	// aktuelle positionsdaten abspeichern	325	// aktuelle positionsdaten abspeichern
325	if (gps_rel_act_position.status > 0)	326	if (gps_rel_act_position.status > 0)
326	{	327	{
327	gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;	328	gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
328	gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;	329	gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
329	gps_rel_start_position.status = 1; // gueltige Positionsdaten	330	gps_rel_start_position.status = 1; // gueltige Positionsdaten
330	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;	331	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
331	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;	332	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
332	gps_rel_hold_position.status = 1; // gueltige Positionsdaten	333	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
333	//Richtung zur Home Position bezogen auf Nordpol bestimmen	334	//Richtung zur Home Position bezogen auf Nordpol bestimmen
334	hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);	335	hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
335	// in Winkel 0...360 Grad umrechnen	336	// in Winkel 0...360 Grad umrechnen
336	if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);	337	if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
337	else hdng_2home = (270 - hdng_2home);	338	else hdng_2home = (270 - hdng_2home);
338	dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen	339	dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
339	gps_state = GPS_CRTL_HOME_ACTIVE;	340	gps_state = GPS_CRTL_HOME_ACTIVE;
340	return (GPS_STST_OK);	341	return (GPS_STST_OK);
341	}	342	}
342	else	343	else
343	{	344	{
344	gps_rel_start_position.status = 0; //Keine Daten verfuegbar	345	gps_rel_start_position.status = 0; //Keine Daten verfuegbar
345	gps_state = GPS_CRTL_IDLE;	346	gps_state = GPS_CRTL_IDLE;
346	return(GPS_STST_ERR); // Keine Daten da	347	return(GPS_STST_ERR); // Keine Daten da
347	}	348	}
348	}	349	}
349	else return(GPS_STST_PEND); // noch warten	350	else return(GPS_STST_PEND); // noch warten
350	}	351	}
351	break;	352	break;
352	// ******************************	353	// ******************************
353		354
354	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.	355	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
355	if (gps_state != GPS_CRTL_HOLD_ACTIVE)	356	if (gps_state != GPS_CRTL_HOLD_ACTIVE)
356	{	357	{
357	cnt++;	358	cnt++;
358	if (cnt > 500) // erst nach Verzoegerung	359	if (cnt > 500) // erst nach Verzoegerung
359	{	360	{
360	cnt = 0;	361	cnt = 0;
361	// aktuelle positionsdaten abspeichern	362	// aktuelle positionsdaten abspeichern
362	if (gps_rel_act_position.status > 0)	363	if (gps_rel_act_position.status > 0)
363	{	364	{
364	hold_fast = 0;	365	hold_fast = 0;
365	hold_reset_int = 0; // Integrator enablen	366	hold_reset_int = 0; // Integrator enablen
366	int_east = 0, int_north = 0;	367	int_east = 0, int_north = 0;
367	gps_reg_x = 0, gps_reg_y = 0;	368	gps_reg_x = 0, gps_reg_y = 0;
368	dist_east = 0, dist_north = 0;	369	dist_east = 0, dist_north = 0;
369	diff_east_f = 0, diff_north_f= 0;	370	diff_east_f = 0, diff_north_f= 0;
370	diff_east = 0, diff_north = 0;	371	diff_east = 0, diff_north = 0;
371	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;	372	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
372	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;	373	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
373	gps_rel_hold_position.status = 1; // gueltige Positionsdaten	374	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
374	gps_state = GPS_CRTL_HOLD_ACTIVE;	375	gps_state = GPS_CRTL_HOLD_ACTIVE;
375	return (GPS_STST_OK);	376	return (GPS_STST_OK);
376	}	377	}
377	else	378	else
378	{	379	{
379	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar	380	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
380	gps_state = GPS_CRTL_IDLE;	381	gps_state = GPS_CRTL_IDLE;
381	return(GPS_STST_ERR); // Keine Daten da	382	return(GPS_STST_ERR); // Keine Daten da
382	}	383	}
383	}	384	}
384	else return(GPS_STST_PEND); // noch warten	385	else return(GPS_STST_PEND); // noch warten
385	}	386	}
386	break;	387	break;
387		388
388	case GPS_CMD_STOP: // Lageregelung beenden	389	case GPS_CMD_STOP: // Lageregelung beenden
389	cnt = 0;	390	cnt = 0;
390	GPS_Nick = 0;	391	GPS_Nick = 0;
391	GPS_Roll = 0;	392	GPS_Roll = 0;
392	gps_int_x = 0;	393	gps_int_x = 0;
393	gps_int_y = 0;	394	gps_int_y = 0;
394	gps_sub_state = GPS_CRTL_IDLE;	395	gps_sub_state = GPS_CRTL_IDLE;
395	gps_state = GPS_CRTL_IDLE;	396	gps_state = GPS_CRTL_IDLE;
396	return (GPS_STST_OK);	397	return (GPS_STST_OK);
397	break;	398	break;
398		399
399	default:	400	default:
400	return (GPS_STST_ERR);	401	return (GPS_STST_ERR);
401	break;	402	break;
402	}	403	}
403		404
404	switch (gps_state)	405	switch (gps_state)
405	{	406	{
406	case GPS_CRTL_IDLE:	407	case GPS_CRTL_IDLE:
407	cnt = 0;	408	cnt = 0;
408	return (GPS_STST_OK);	409	return (GPS_STST_OK);
409	break;	410	break;
410		411
411	case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis	412	case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
412	//Der Sollwert des Lagereglers wird der Homeposition angenaehert	413	//Der Sollwert des Lagereglers wird der Homeposition angenaehert
413	if (gps_rel_start_position.status >0)	414	if (gps_rel_start_position.status >0)
414	{	415	{
415	if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist	416	if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
416	{	417	{
417	gps_tick++;	418	gps_tick++;
418	int d1,d2,d3;	419	int d1,d2,d3;
419	d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );	420	d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
420	d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );	421	d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
421	d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel	422	d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
422		423
423	if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel	424	if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
424	{	425	{
425	if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz	426	if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
426	{	427	{
427	if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen	428	if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
428	dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit	429	dist_flown +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
429	gps_sub_state = GPS_HOME_FAST_IN_TOL;	430	gps_sub_state = GPS_HOME_FAST_IN_TOL;
430	}	431	}
431	else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz	432	else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
432	{	433	{
433	if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren	434	if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
434	dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen	435	dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
435	gps_sub_state = GPS_HOME_FAST_OUTOF_TOL;	436	gps_sub_state = GPS_HOME_FAST_OUTOF_TOL;
436	}	437	}
437	hold_reset_int = 0; // Integrator einsschalten	438	hold_reset_int = 0; // Integrator einsschalten
438	hold_fast = 1; // Regler fuer schnellen Flug	439	hold_fast = 1; // Regler fuer schnellen Flug
439	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);	440	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
440	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);	441	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
441	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt	442	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
442	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt	443	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
443	}	444	}
444	else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen	445	else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen
445	{	446	{
446	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL))	447	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL))
447	{	448	{
448	dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit	449	dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
449	gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;	450	gps_sub_state = GPS_HOME_RMPDWN_IN_TOL;
450	}	451	}
451	else	452	else
452	{	453	{
453	dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen	454	dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
454	gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL;	455	gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL;
455	}	456	}
456	hold_reset_int = 0; // Integrator ausschalten	457	hold_reset_int = 0; // Integrator ausschalten
457	hold_fast = 1; // Wieder normal regeln	458	hold_fast = 1; // Wieder normal regeln
458	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);	459	dist_frm_start_east = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
459	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);	460	dist_frm_start_north = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
460	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt	461	gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
461	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt	462	gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
462	}	463	}
463	else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)	464	else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
464	{	465	{
465	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln	466	if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
466	{	467	{
467	gps_sub_state = GPS_HOME_IN_TOL;	468	gps_sub_state = GPS_HOME_IN_TOL;
468	hold_fast = 0; // Wieder normal regeln	469	hold_fast = 0; // Wieder normal regeln
469	hold_reset_int = 0; // Integrator wieder aktivieren	470	hold_reset_int = 0; // Integrator wieder aktivieren
470	if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;	471	if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
471	else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;	472	else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
472	if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;	473	if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
473	else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;	474	else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
474	if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))	475	if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
475	{	476	{
476	gps_rel_hold_position.utm_east = 0;	477	gps_rel_hold_position.utm_east = 0;
477	gps_rel_hold_position.utm_north = 0;	478	gps_rel_hold_position.utm_north = 0;
478	gps_sub_state = GPS_HOME_FINISHED;	479	gps_sub_state = GPS_HOME_FINISHED;
479	}	480	}
480	}	481	}
481	else gps_sub_state = GPS_HOME_OUTOF_TOL;	482	else gps_sub_state = GPS_HOME_OUTOF_TOL;
482	}	483	}
483	}	484	}
484	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung	485	gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
485	return (GPS_STST_OK);	486	return (GPS_STST_OK);
486	}	487	}
487	else // Keine GPS Daten verfuegbar, deswegen Abbruch	488	else // Keine GPS Daten verfuegbar, deswegen Abbruch
488	{	489	{
489	gps_state = GPS_CRTL_IDLE;	490	gps_state = GPS_CRTL_IDLE;
490	return (GPS_STST_ERR);	491	return (GPS_STST_ERR);
491	}	492	}
492	break;	493	break;
493		494
494		495
495	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet	496	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
496	if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen	497	if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen
497	{	498	{
498	gps_updte_flag = 0;	499	gps_updte_flag = 0;
499	// ab hier wird geregelt	500	// ab hier wird geregelt

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c @ 633 - Rev 528 → 529