WebSVN - FlightCtrl - Diff - Rev 236 and 241


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

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 492 - Rev 236 → 241