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

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c - Rev 628 → 629