WebSVN - FlightCtrl - Diff - Rev 184 and 185


/*
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
Stand 29.9.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;
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;                            
signed int ;
static unsigned int rx_len;
unsigned int cnt0,cnt1,cnt2; //******Provisorisch
static unsigned int ptr_payload_data_end;
 
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
 
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
 
// 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;
 
}
 
// 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_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))
        {
                cnt1++; //**** noch Rausschmeissen
                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;
        int diff_east,diff_north;                               // Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)
        int n;
        long int dist;
        switch (cmd)
        {
 
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. 
                // Noch einiges zu ueberlegen und zu tun
                   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 abespeichern
                                        if (gps_rel_act_position.status > 0)
                                        {
                                                int_east        = 0;
                                                int_north       = 0;
                                                gps_reg_x       = 0;
                                                gps_reg_y       = 0;
                                                dist_east       = 0;
                                                dist_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_HOLD_ACTIVE:     // Hier werden die Daten fuer Nick und Roll errechnet
                        if (gps_updte_flag == 1)  //nur wenn neue GPS Daten vorliegen
                        {
                                gps_updte_flag = 0;
                                // ab hier wird geregelt
 
                                diff_east       = -dist_east;    //Alten Wert 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
 
                                #define GPSINT_MAX 2048 //neuer Wert ab 25.9.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;                                     
                                }
 
                                //PID Regler
                                gps_reg_x = ((int_east  * Parameter_UserParam2)/32) + ((dist_east   * Parameter_UserParam1)/8)+ ((diff_east  * Parameter_UserParam3)/2);  // 
                                gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north  * Parameter_UserParam1)/8)+ ((diff_north * Parameter_UserParam3)/2);  // I + P +D  Anteil Y Achse
 
                                //Richtung bezogen auf Nordpol bestimmen
                                GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)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) )
                                {
                                        dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
                                        dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
                                }
                                else
                                {
                                        dist = (long)gps_reg_y;
                                        dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
                                }
//                              if (dist > 200) dist = 200;
//                              GPS_dist_2trgt = (int )dist;
 
                                // Winkel und Distanz in Nick und Rollgroessen umrechnen
                                GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
                                GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
 
                                #define GPS_V 8
                                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
                                long int nick,roll;
                                int n,r;
                                r                       = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
                                n                       = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));
                                roll            = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) r)/1000;
                                nick            = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) n)/1000;
                                GPS_Roll        = (int) (roll / GPS_V);
 
                                GPS_Nick        = (int) (nick / GPS_V);
                                 
                                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);                                         
                                }
/*                              else // Distanz ok // kann spaeter entfallen, weil eigentlich schon begrenzt
                                {
                                        if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen
                                        else if (GPS_Roll <  -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX;
                                        if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX;
                                        else if (GPS_Nick <  - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX;
                                return (GPS_STST_OK);
                                }*/                    
                        }
                        else return (GPS_STST_OK);
                        break;
 
                default:
                        gps_state       = GPS_CRTL_IDLE;
                        return (GPS_STST_ERR);
                        break;
        }      
        return (GPS_STST_ERR);
                       
}
 
 

Rev 184	Rev 185
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	Stand 29.9.2007	16	Stand 29.9.2007
17	++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	17	++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18	*/	18	*/
19	#include "main.h"	19	#include "main.h"
20	//#include "gps.h"	20	//#include "gps.h"
21		21
22	// Defines fuer ublox Messageformat um Auswertung zu steuern	22	// Defines fuer ublox Messageformat um Auswertung zu steuern
23	#define UBLOX_IDLE 0	23	#define UBLOX_IDLE 0
24	#define UBLOX_SYNC1 1	24	#define UBLOX_SYNC1 1
25	#define UBLOX_SYNC2 2	25	#define UBLOX_SYNC2 2
26	#define UBLOX_CLASS 3	26	#define UBLOX_CLASS 3
27	#define UBLOX_ID 4	27	#define UBLOX_ID 4
28	#define UBLOX_LEN1 5	28	#define UBLOX_LEN1 5
29	#define UBLOX_LEN2 6	29	#define UBLOX_LEN2 6
30	#define UBLOX_CKA 7	30	#define UBLOX_CKA 7
31	#define UBLOX_CKB 8	31	#define UBLOX_CKB 8
32	#define UBLOX_PAYLOAD 9	32	#define UBLOX_PAYLOAD 9
33		33
34	// ublox Protokoll Identifier	34	// ublox Protokoll Identifier
35	#define UBLOX_NAV_POSUTM 0x08	35	#define UBLOX_NAV_POSUTM 0x08
36	#define UBLOX_NAV_STATUS 0x03	36	#define UBLOX_NAV_STATUS 0x03
37	#define UBLOX_NAV_VELED 0x12	37	#define UBLOX_NAV_VELED 0x12
38	#define UBLOX_NAV_CLASS 0x01	38	#define UBLOX_NAV_CLASS 0x01
39	#define UBLOX_SYNCH1_CHAR 0xB5	39	#define UBLOX_SYNCH1_CHAR 0xB5
40	#define UBLOX_SYNCH2_CHAR 0x62	40	#define UBLOX_SYNCH2_CHAR 0x62
41		41
42	signed int GPS_Nick = 0;	42	signed int GPS_Nick = 0;
43	signed int GPS_Roll = 0;	43	signed int GPS_Roll = 0;
44	short int ublox_msg_state = UBLOX_IDLE;	44	short int ublox_msg_state = UBLOX_IDLE;
45	static uint8_t chk_a =0; //Checksum	45	static uint8_t chk_a =0; //Checksum
46	static uint8_t chk_b =0;	46	static uint8_t chk_b =0;
47	short int gps_state;	47	short int gps_state;
48	short int gps_updte_flag;	48	short int gps_updte_flag;
49	signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol	49	signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
50	signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters	50	signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
51	signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel	51	signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
52	signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;	52	signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
53	signed int ;	53	signed int ;
54	static unsigned int rx_len;	54	static unsigned int rx_len;
55	unsigned int cnt0,cnt1,cnt2; //******Provisorisch	55	unsigned int cnt0,cnt1,cnt2; //******Provisorisch
56	static unsigned int ptr_payload_data_end;	56	static unsigned int ptr_payload_data_end;
57		57
58	static uint8_t *ptr_payload_data;	58	static uint8_t *ptr_payload_data;
59	static uint8_t *ptr_pac_status;	59	static uint8_t *ptr_pac_status;
60		60
61	short int Get_GPS_data(void);	61	short int Get_GPS_data(void);
62		62
63	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt	63	NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
64	NAV_STATUS_t actual_status; // Aktueller Nav Status	64	NAV_STATUS_t actual_status; // Aktueller Nav Status
65	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten	65	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
66		66
67	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst	67	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
68	GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt	68	GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt
69	GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position	69	GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position
70	GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode	70	GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode
71		71
72	// Initialisierung	72	// Initialisierung
73	void GPS_Neutral(void)	73	void GPS_Neutral(void)
74	{	74	{
75	ublox_msg_state = UBLOX_IDLE;	75	ublox_msg_state = UBLOX_IDLE;
76	gps_state = GPS_CRTL_IDLE;	76	gps_state = GPS_CRTL_IDLE;
77	actual_pos.status = 0;	77	actual_pos.status = 0;
78	actual_speed.status = 0;	78	actual_speed.status = 0;
79	actual_status.status = 0;	79	actual_status.status = 0;
80	gps_home_position.status = 0; // Noch keine gueltige Home Position	80	gps_home_position.status = 0; // Noch keine gueltige Home Position
81	gps_act_position.status = 0;	81	gps_act_position.status = 0;
82	gps_rel_act_position.status = 0;	82	gps_rel_act_position.status = 0;
83	GPS_Nick = 0;	83	GPS_Nick = 0;
84	GPS_Roll = 0;	84	GPS_Roll = 0;
85	gps_updte_flag = 0;	85	gps_updte_flag = 0;
86	gps_int_x = 0;	86	gps_int_x = 0;
87	gps_int_y = 0;	87	gps_int_y = 0;
88		88
89	}	89	}
90		90
91	// Home Position sichern falls Daten verfuegbar sind.	91	// Home Position sichern falls Daten verfuegbar sind.
92	void GPS_Save_Home(void)	92	void GPS_Save_Home(void)
93	{	93	{
94	short int n;	94	short int n;
95	n = Get_GPS_data();	95	n = Get_GPS_data();
96	if (n == 0) // Gueltige und aktuelle Daten ?	96	if (n == 0) // Gueltige und aktuelle Daten ?
97	{	97	{
98	// Neue GPS Daten liegen vor	98	// Neue GPS Daten liegen vor
99	gps_home_position.utm_east = gps_act_position.utm_east;	99	gps_home_position.utm_east = gps_act_position.utm_east;
100	gps_home_position.utm_north = gps_act_position.utm_north;	100	gps_home_position.utm_north = gps_act_position.utm_north;
101	gps_home_position.utm_alt = gps_act_position.utm_alt;	101	gps_home_position.utm_alt = gps_act_position.utm_alt;
102	gps_home_position.status = 1; // Home Position gueltig	102	gps_home_position.status = 1; // Home Position gueltig
103	}	103	}
104	}	104	}
105		105
106	// Relative Position zur Home Position bestimmen	106	// Relative Position zur Home Position bestimmen
107	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig	107	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
108	short int Get_Rel_Position(void)	108	short int Get_Rel_Position(void)
109	{	109	{
110	short int n = 0;	110	short int n = 0;
111	n = Get_GPS_data();	111	n = Get_GPS_data();
112	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind	112	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
113	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt	113	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
114	{	114	{
115	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);	115	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);
116	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);	116	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);
117	gps_rel_act_position.status = 1; // gueltige Positionsdaten	117	gps_rel_act_position.status = 1; // gueltige Positionsdaten
118	n = 0;	118	n = 0;
119	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.	119	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
120	}	120	}
121	else	121	else
122	{	122	{
123	n = 2; //keine gueltigen Daten vorhanden	123	n = 2; //keine gueltigen Daten vorhanden
124	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.	124	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
125	}	125	}
126	return (n);	126	return (n);
127	}	127	}
128		128
129	// Daten aus aktuellen ublox Messages extrahieren	129	// Daten aus aktuellen ublox Messages extrahieren
130	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig	130	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
131	short int Get_GPS_data(void)	131	short int Get_GPS_data(void)
132	{	132	{
133	short int n = 1;	133	short int n = 1;
134		134
135	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist	135	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
136	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))	136	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
137	{	137	{
138	cnt1++; //**** noch Rausschmeissen	138	cnt1++; //**** noch Rausschmeissen
139	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind	139	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
140	{	140	{
141	gps_act_position.utm_east = actual_pos.utm_east/10;	141	gps_act_position.utm_east = actual_pos.utm_east/10;
142	gps_act_position.utm_north = actual_pos.utm_north/10;	142	gps_act_position.utm_north = actual_pos.utm_north/10;
143	gps_act_position.utm_alt = actual_pos.utm_alt/10;	143	gps_act_position.utm_alt = actual_pos.utm_alt/10;
144	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;	144	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
145	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;	145	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
146	gps_act_position.heading = actual_speed.heading/100000;	146	gps_act_position.heading = actual_speed.heading/100000;
147	gps_act_position.status = 1;	147	gps_act_position.status = 1;
148	n = 0; //Daten gueltig	148	n = 0; //Daten gueltig
149	}	149	}
150	else	150	else
151	{	151	{
152	gps_act_position.status = 0; //Keine gueltigen Daten	152	gps_act_position.status = 0; //Keine gueltigen Daten
153	n = 2;	153	n = 2;
154	}	154	}
155	actual_pos.status = 0; //neue ublox Messages anfordern	155	actual_pos.status = 0; //neue ublox Messages anfordern
156	actual_status.status = 0;	156	actual_status.status = 0;
157	actual_speed.status = 0;	157	actual_speed.status = 0;
158	}	158	}
159	return (n);	159	return (n);
160	}	160	}
161		161
162		162
163	/*	163	/*
164	Daten vom GPS im ublox MSG Format auswerten	164	Daten vom GPS im ublox MSG Format auswerten
165	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen	165	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
166	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein	166	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
167	*/	167	*/
168	void Get_Ublox_Msg(uint8_t rx)	168	void Get_Ublox_Msg(uint8_t rx)
169	{	169	{
170		170
171	switch (ublox_msg_state)	171	switch (ublox_msg_state)
172	{	172	{
173		173
174	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen	174	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
175	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;	175	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
176	else ublox_msg_state = UBLOX_IDLE;	176	else ublox_msg_state = UBLOX_IDLE;
177	break;	177	break;
178		178
179	case UBLOX_SYNC1:	179	case UBLOX_SYNC1:
180		180
181	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;	181	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
182	else ublox_msg_state = UBLOX_IDLE;	182	else ublox_msg_state = UBLOX_IDLE;
183	chk_a = 0,chk_b = 0;	183	chk_a = 0,chk_b = 0;
184	break;	184	break;
185		185
186	case UBLOX_SYNC2:	186	case UBLOX_SYNC2:
187	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;	187	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
188	else ublox_msg_state = UBLOX_IDLE;	188	else ublox_msg_state = UBLOX_IDLE;
189	break;	189	break;
190		190
191	case UBLOX_CLASS: // Nur NAV Meldungen auswerten	191	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
192	switch (rx)	192	switch (rx)
193	{	193	{
194	case UBLOX_NAV_POSUTM:	194	case UBLOX_NAV_POSUTM:
195	ptr_pac_status = &actual_pos.status;	195	ptr_pac_status = &actual_pos.status;
196	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden	196	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
197	else	197	else
198	{	198	{
199	ptr_payload_data = &actual_pos;	199	ptr_payload_data = &actual_pos;
200	ptr_payload_data_end = &actual_pos.status;	200	ptr_payload_data_end = &actual_pos.status;
201	ublox_msg_state = UBLOX_LEN1;	201	ublox_msg_state = UBLOX_LEN1;
202	}	202	}
203	break;	203	break;
204		204
205	case UBLOX_NAV_STATUS:	205	case UBLOX_NAV_STATUS:
206	ptr_pac_status = &actual_status.status;	206	ptr_pac_status = &actual_status.status;
207	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;	207	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
208	else	208	else
209	{	209	{
210	ptr_payload_data = &actual_status;	210	ptr_payload_data = &actual_status;
211	ptr_payload_data_end = &actual_status.status;	211	ptr_payload_data_end = &actual_status.status;
212	ublox_msg_state = UBLOX_LEN1;	212	ublox_msg_state = UBLOX_LEN1;
213	}	213	}
214	break;	214	break;
215		215
216	case UBLOX_NAV_VELED:	216	case UBLOX_NAV_VELED:
217	ptr_pac_status = &actual_speed.status;	217	ptr_pac_status = &actual_speed.status;
218	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;	218	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
219	else	219	else
220	{	220	{
221	ptr_payload_data = &actual_speed;	221	ptr_payload_data = &actual_speed;
222	ptr_payload_data_end = &actual_speed.status;	222	ptr_payload_data_end = &actual_speed.status;
223	ublox_msg_state = UBLOX_LEN1;	223	ublox_msg_state = UBLOX_LEN1;
224	}	224	}
225	break;	225	break;
226		226
227	default:	227	default:
228	ublox_msg_state = UBLOX_IDLE;	228	ublox_msg_state = UBLOX_IDLE;
229	break;	229	break;
230	}	230	}
231	chk_a = UBLOX_NAV_CLASS + rx;	231	chk_a = UBLOX_NAV_CLASS + rx;
232	chk_b = UBLOX_NAV_CLASS + chk_a;	232	chk_b = UBLOX_NAV_CLASS + chk_a;
233	break;	233	break;
234		234
235	case UBLOX_LEN1: // Laenge auswerten	235	case UBLOX_LEN1: // Laenge auswerten
236	rx_len = rx;	236	rx_len = rx;
237	chk_a += rx;	237	chk_a += rx;
238	chk_b += chk_a;	238	chk_b += chk_a;
239	ublox_msg_state = UBLOX_LEN2;	239	ublox_msg_state = UBLOX_LEN2;
240	break;	240	break;
241		241
242		242
243	case UBLOX_LEN2: // Laenge auswerten	243	case UBLOX_LEN2: // Laenge auswerten
244	rx_len = rx_len + (rx *256); // Laenge ermitteln	244	rx_len = rx_len + (rx *256); // Laenge ermitteln
245	chk_a += rx;	245	chk_a += rx;
246	chk_b += chk_a;	246	chk_b += chk_a;
247	ublox_msg_state = UBLOX_PAYLOAD;	247	ublox_msg_state = UBLOX_PAYLOAD;
248	break;	248	break;
249		249
250	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen	250	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
251	if (rx_len > 0)	251	if (rx_len > 0)
252	{	252	{
253	*ptr_payload_data = rx;	253	*ptr_payload_data = rx;
254	chk_a += rx;	254	chk_a += rx;
255	chk_b += chk_a;	255	chk_b += chk_a;
256	--rx_len;	256	--rx_len;
257	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))	257	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
258	{	258	{
259	ptr_payload_data++;	259	ptr_payload_data++;
260	ublox_msg_state = UBLOX_PAYLOAD;	260	ublox_msg_state = UBLOX_PAYLOAD;
261	}	261	}
262	else ublox_msg_state = UBLOX_CKA;	262	else ublox_msg_state = UBLOX_CKA;
263	}	263	}
264	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler	264	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
265	break;	265	break;
266		266
267	case UBLOX_CKA: // Checksum pruefen	267	case UBLOX_CKA: // Checksum pruefen
268	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;	268	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
269	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler	269	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
270	break;	270	break;
271		271
272	case UBLOX_CKB: // Checksum pruefen	272	case UBLOX_CKB: // Checksum pruefen
273	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok	273	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
274	ublox_msg_state = UBLOX_IDLE;	274	ublox_msg_state = UBLOX_IDLE;
275	break;	275	break;
276		276
277	default:	277	default:
278	ublox_msg_state = UBLOX_IDLE;	278	ublox_msg_state = UBLOX_IDLE;
279	break;	279	break;
280	}	280	}
281	}	281	}
282		282
283	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe	283	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
284	short int GPS_CRTL(short int cmd)	284	short int GPS_CRTL(short int cmd)
285	{	285	{
286	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen	286	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen
287	static signed int int_east,int_north; //Integrierer	287	static signed int int_east,int_north; //Integrierer
288	static signed int dist_north,dist_east;	288	static signed int dist_north,dist_east;
289	int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert)	289	int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert)
290	int n;	290	int n;
291	long int dist;	291	long int dist;
292	switch (cmd)	292	switch (cmd)
293	{	293	{
294		294
295	case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.	295	case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
296	// Noch einiges zu ueberlegen und zu tun	296	// Noch einiges zu ueberlegen und zu tun
297	return(GPS_STST_PEND); // noch warten	297	return(GPS_STST_PEND); // noch warten
298	break;	298	break;
299	// ******************************	299	// ******************************
300		300
301	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.	301	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
302	if (gps_state != GPS_CRTL_HOLD_ACTIVE)	302	if (gps_state != GPS_CRTL_HOLD_ACTIVE)
303	{	303	{
304	cnt++;	304	cnt++;
305	if (cnt > 500) // erst nach Verzoegerung	305	if (cnt > 500) // erst nach Verzoegerung
306	{	306	{
307	cnt = 0;	307	cnt = 0;
308	// aktuelle positionsdaten abespeichern	308	// aktuelle positionsdaten abespeichern
309	if (gps_rel_act_position.status > 0)	309	if (gps_rel_act_position.status > 0)
310	{	310	{
311	int_east = 0;	311	int_east = 0;
312	int_north = 0;	312	int_north = 0;
313	gps_reg_x = 0;	313	gps_reg_x = 0;
314	gps_reg_y = 0;	314	gps_reg_y = 0;
315	dist_east = 0;	315	dist_east = 0;
316	dist_north = 0;	316	dist_north = 0;
317	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;	317	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
318	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;	318	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
319	gps_rel_hold_position.status = 1; // gueltige Positionsdaten	319	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
320	gps_state = GPS_CRTL_HOLD_ACTIVE;	320	gps_state = GPS_CRTL_HOLD_ACTIVE;
321	return (GPS_STST_OK);	321	return (GPS_STST_OK);
322	}	322	}
323	else	323	else
324	{	324	{
325	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar	325	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
326	gps_state = GPS_CRTL_IDLE;	326	gps_state = GPS_CRTL_IDLE;
327	return(GPS_STST_ERR); // Keine Daten da	327	return(GPS_STST_ERR); // Keine Daten da
328	}	328	}
329	}	329	}
330	else return(GPS_STST_PEND); // noch warten	330	else return(GPS_STST_PEND); // noch warten
331	}	331	}
332	break;	332	break;
333		333
334	case GPS_CMD_STOP: // Lageregelung beenden	334	case GPS_CMD_STOP: // Lageregelung beenden
335	cnt = 0;	335	cnt = 0;
336	GPS_Nick = 0;	336	GPS_Nick = 0;
337	GPS_Roll = 0;	337	GPS_Roll = 0;
338	gps_int_x = 0;	338	gps_int_x = 0;
339	gps_int_y = 0;	339	gps_int_y = 0;
340	gps_state = GPS_CRTL_IDLE;	340	gps_state = GPS_CRTL_IDLE;
341	return (GPS_STST_OK);	341	return (GPS_STST_OK);
342	break;	342	break;
343		343
344	default:	344	default:
345	return (GPS_STST_ERR);	345	return (GPS_STST_ERR);
346	break;	346	break;
347	}	347	}
348		348
349	switch (gps_state)	349	switch (gps_state)
350	{	350	{
351	case GPS_CRTL_IDLE:	351	case GPS_CRTL_IDLE:
352	cnt = 0;	352	cnt = 0;
353	return (GPS_STST_OK);	353	return (GPS_STST_OK);
354	break;	354	break;
355		355
356	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet	356	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
357	if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen	357	if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen
358	{	358	{
359	gps_updte_flag = 0;	359	gps_updte_flag = 0;
360	// ab hier wird geregelt	360	// ab hier wird geregelt
361		361
362	diff_east = -dist_east; //Alten Wert schon mal abziehen	362	diff_east = -dist_east; //Alten Wert schon mal abziehen
363	diff_north = -dist_north;	363	diff_north = -dist_north;
364	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;	364	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
365	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;	365	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
366	int_east += dist_east;	366	int_east += dist_east;
367	int_north += dist_north;	367	int_north += dist_north;
368	diff_east += dist_east; // Differenz zur vorhergehenden East Position	368	diff_east += dist_east; // Differenz zur vorhergehenden East Position
369	diff_north += dist_north; // Differenz zur vorhergehenden North Position	369	diff_north += dist_north; // Differenz zur vorhergehenden North Position
370		370
371	#define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung	371	#define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung
372	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten	372	if ((abs(int_east) > GPSINT_MAX) \|\| (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
373	{	373	{
374	int_east -= dist_east;	374	int_east -= dist_east;
375	int_north -= dist_north;	375	int_north -= dist_north;
376	}	376	}
377		377
378	//PID Regler	378	//PID Regler
379	gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * Parameter_UserParam3)/2); //	379	gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * Parameter_UserParam3)/2); //
380	gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse	380	gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse
381		381
382	//Richtung bezogen auf Nordpol bestimmen	382	//Richtung bezogen auf Nordpol bestimmen
383	GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y);	383	GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y);
384		384
385	//in Winkel 0..360 grad umrechnen	385	//in Winkel 0..360 grad umrechnen
386	if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);	386	if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
387	else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);	387	else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
388		388
389	// Relative Richtung in bezug auf Nordachse des Kopters errechen	389	// Relative Richtung in bezug auf Nordachse des Kopters errechen
390	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;	390	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
391	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;	391	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
392	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;	392	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
393	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;	393	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
394	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;	394	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
395		395
396	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen	396	// Regelabweichung aus x,y zu Ziel in Distanz umrechnen
397	if (abs(gps_reg_x) > abs(gps_reg_y) )	397	if (abs(gps_reg_x) > abs(gps_reg_y) )
398	{	398	{
399	dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen	399	dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
400	dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt));	400	dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
401	}	401	}
402	else	402	else
403	{	403	{
404	dist = (long)gps_reg_y;	404	dist = (long)gps_reg_y;
405	dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt));	405	dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
406	}	406	}
407	// if (dist > 200) dist = 200;	407	// if (dist > 200) dist = 200;
408	// GPS_dist_2trgt = (int )dist;	408	// GPS_dist_2trgt = (int )dist;
409		409
410	// Winkel und Distanz in Nick und Roll groessen umrechnen	410	// Winkel und Distanz in Nick und Rollgroessen umrechnen
411	GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000);	411	GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
412	GPS_Nick = (int) -((dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000);	412	GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
413		413
414	#define GPS_V 8	414	#define GPS_V 8
415	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);	415	if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
416	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);	416	else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
417	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);	417	if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
418	else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);	418	else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
419		419
420	//Kleine Werte verstaerken, Grosse abschwaechen	420	//Kleine Werte verstaerken, Grosse abschwaechen
421	long int nick,roll;	421	long int nick,roll;
-		422	int n,r;
422	roll = (((long) GPS_Roll) * ((long)sin_i(abs((GPS_Roll90)/(GPS_NICKROLL_MAX GPS_V)))))/1000;	423	r = sin_i((GPS_Roll90)/(GPS_NICKROLL_MAX GPS_V));
-		424	n = sin_i((GPS_Nick90)/(GPS_NICKROLL_MAX GPS_V));
-		425	roll = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) r)/1000;
-		426	nick = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) n)/1000;
423	GPS_Roll = (int) (roll / GPS_V);	427	GPS_Roll = (int) (roll / GPS_V);
424	nick = (((long) GPS_Nick) * ((long)sin_i(abs((GPS_Nick90)/(GPS_NICKROLL_MAX GPS_V)))))/1000;	-
425	GPS_Nick = (int) (nick / GPS_V);	428	GPS_Nick = (int) (nick / GPS_V);
426		429
427	if ((abs(dist_east) > GPS_DIST_MAX) \|\| (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen	430	if ((abs(dist_east) > GPS_DIST_MAX) \|\| (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen
428	{	431	{
429	GPS_Roll = 0;	432	GPS_Roll = 0;
430	GPS_Nick = 0;	433	GPS_Nick = 0;
431	gps_state = GPS_CRTL_IDLE;	434	gps_state = GPS_CRTL_IDLE;
432	return (GPS_STST_ERR);	435	return (GPS_STST_ERR);
433	}	436	}
434	else // Distanz ok // Die Abfrage kann noch rausfliegen, weil vorher bereits begrenzung war	437	/* else // Distanz ok // kann spaeter entfallen, weil eigentlich schon begrenzt
435	{	438	{
436	if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen	439	if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen
437	else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX;	440	else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX;
438	if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX;	441	if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX;
439	else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX;	442	else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX;
440	return (GPS_STST_OK);	443	return (GPS_STST_OK);
441	}	444	}*/
442	}	445	}
443	else return (GPS_STST_OK);	446	else return (GPS_STST_OK);
444	break;	447	break;
445		448
446	default:	449	default:
447	gps_state = GPS_CRTL_IDLE;	450	gps_state = GPS_CRTL_IDLE;
448	return (GPS_STST_ERR);	451	return (GPS_STST_ERR);
449	break;	452	break;
450	}	453	}
451	return (GPS_STST_ERR);	454	return (GPS_STST_ERR);
452		455
453	}	456	}
454		457
455		458

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c - Rev 184 → 185