WebSVN - FlightCtrl - Diff - Rev 158 and 159


/*
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
Regelung fuer GPS noch nicht implementiert
Stand 16.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)
{
        short int n;
        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 
 
        switch (cmd)
        {
                case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
                        if (gps_state != GPS_CRTL_HOLD_ACTIVE)
                        {
                                cnt++;
                                if (cnt > 300) // erst nach Verzoegerung 
                                {
                                        cnt     =       0;
                                        // aktuelle positionsdaten abespeichern
                                        if (gps_rel_act_position.status > 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_HOLD: // Lageregelung beenden
                        cnt                             =       0;
                        GPS_Nick                =       0;
                        GPS_Roll                =       0;
                        gps_state               =       GPS_CRTL_IDLE;
                        gps_int_x               =       0;
                        gps_int_y               =       0;
                        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
 
                                signed int dist_north,dist_east;
                                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;
 
                                //PI Regler
                                gps_int_x       =       gps_int_x + (dist_east  * Parameter_UserParam1)/16; // Integrator
                                gps_int_y       =       gps_int_y + (dist_north * Parameter_UserParam1)/16;
                                if ((gps_int_x >= 4096) || (gps_int_y >= 4096) || (gps_int_x < - 4096) || (gps_int_y <= -4096))
                                {                      
                                        gps_int_x -= (dist_east  * Parameter_UserParam1)/16; // Integrator stoppen
                                        gps_int_y -= (dist_north  * Parameter_UserParam1)/16;
                                }                              
                                gps_reg_x       =       gps_int_x + (dist_east  * Parameter_UserParam2)/16;  // P Anteil dazu
                                gps_reg_y       =       gps_int_y + (dist_north  * Parameter_UserParam2)/16;  
 
                                //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 ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
                                else if ((dist_east <  0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
                                // Relative Richtung in bezug auf Nordachse des Kopters errechen
                                int n,m;
                                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;
                               
                                // Distanz zum Ziel ermitteln
                                GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt
                                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 158	Rev 159
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	Regelung fuer GPS noch nicht implementiert	15	Regelung fuer GPS noch nicht implementiert
16	Stand 16.9.2007	16	Stand 16.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		53	signed int ;
53	static unsigned int rx_len;	54	static unsigned int rx_len;
54	unsigned int cnt0,cnt1,cnt2; //******Provisorisch	55	unsigned int cnt0,cnt1,cnt2; //******Provisorisch
55	static unsigned int ptr_payload_data_end;	56	static unsigned int ptr_payload_data_end;
56		57
57	static uint8_t *ptr_payload_data;	58	static uint8_t *ptr_payload_data;
58	static uint8_t *ptr_pac_status;	59	static uint8_t *ptr_pac_status;
59		60
60	short int Get_GPS_data(void);	61	short int Get_GPS_data(void);
61		62
62	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
63	NAV_STATUS_t actual_status; // Aktueller Nav Status	64	NAV_STATUS_t actual_status; // Aktueller Nav Status
64	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten	65	NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten
65		66
66	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst	67	GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
67	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
68	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
69	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
70		71
71	// Initialisierung	72	// Initialisierung
72	void GPS_Neutral(void)	73	void GPS_Neutral(void)
73	{	74	{
74	short int n;	75	short int n;
75	ublox_msg_state = UBLOX_IDLE;	76	ublox_msg_state = UBLOX_IDLE;
76	gps_state = GPS_CRTL_IDLE;	77	gps_state = GPS_CRTL_IDLE;
77	actual_pos.status = 0;	78	actual_pos.status = 0;
78	actual_speed.status = 0;	79	actual_speed.status = 0;
79	actual_status.status = 0;	80	actual_status.status = 0;
80	gps_home_position.status = 0; // Noch keine gueltige Home Position	81	gps_home_position.status = 0; // Noch keine gueltige Home Position
81	gps_act_position.status = 0;	82	gps_act_position.status = 0;
82	gps_rel_act_position.status = 0;	83	gps_rel_act_position.status = 0;
83	GPS_Nick = 0;	84	GPS_Nick = 0;
84	GPS_Roll = 0;	85	GPS_Roll = 0;
85	gps_updte_flag = 0;	86	gps_updte_flag = 0;
-		87	gps_int_x = 0;
-		88	gps_int_y = 0;
-		89
86	}	90	}
87		91
88	// Home Position sichern falls Daten verfuegbar sind.	92	// Home Position sichern falls Daten verfuegbar sind.
89	void GPS_Save_Home(void)	93	void GPS_Save_Home(void)
90	{	94	{
91	short int n;	95	short int n;
92	n = Get_GPS_data();	96	n = Get_GPS_data();
93	if (n == 0) // Gueltige und aktuelle Daten ?	97	if (n == 0) // Gueltige und aktuelle Daten ?
94	{	98	{
95	// Neue GPS Daten liegen vor	99	// Neue GPS Daten liegen vor
96	gps_home_position.utm_east = gps_act_position.utm_east;	100	gps_home_position.utm_east = gps_act_position.utm_east;
97	gps_home_position.utm_north = gps_act_position.utm_north;	101	gps_home_position.utm_north = gps_act_position.utm_north;
98	gps_home_position.utm_alt = gps_act_position.utm_alt;	102	gps_home_position.utm_alt = gps_act_position.utm_alt;
99	gps_home_position.status = 1; // Home Position gueltig	103	gps_home_position.status = 1; // Home Position gueltig
100	}	104	}
101	}	105	}
102		106
103	// Relative Position zur Home Position bestimmen	107	// Relative Position zur Home Position bestimmen
104	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig	108	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
105	short int Get_Rel_Position(void)	109	short int Get_Rel_Position(void)
106	{	110	{
107	short int n = 0;	111	short int n = 0;
108	n = Get_GPS_data();	112	n = Get_GPS_data();
109	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind	113	if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
110	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt	114	if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
111	{	115	{
112	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);	116	gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east);
113	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);	117	gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north);
114	gps_rel_act_position.status = 1; // gueltige Positionsdaten	118	gps_rel_act_position.status = 1; // gueltige Positionsdaten
115	n = 0;	119	n = 0;
116	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.	120	gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
117	}	121	}
118	else	122	else
119	{	123	{
120	n = 2; //keine gueltigen Daten vorhanden	124	n = 2; //keine gueltigen Daten vorhanden
121	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.	125	gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
122	}	126	}
123	return (n);	127	return (n);
124	}	128	}
125		129
126	// Daten aus aktuellen ublox Messages extrahieren	130	// Daten aus aktuellen ublox Messages extrahieren
127	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig	131	// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
128	short int Get_GPS_data(void)	132	short int Get_GPS_data(void)
129	{	133	{
130	short int n = 1;	134	short int n = 1;
131		135
132	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist	136	if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
133	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))	137	if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
134	{	138	{
135	cnt1++; //**** noch Rausschmeissen	139	cnt1++; //**** noch Rausschmeissen
136	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind	140	if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
137	{	141	{
138	gps_act_position.utm_east = actual_pos.utm_east/10;	142	gps_act_position.utm_east = actual_pos.utm_east/10;
139	gps_act_position.utm_north = actual_pos.utm_north/10;	143	gps_act_position.utm_north = actual_pos.utm_north/10;
140	gps_act_position.utm_alt = actual_pos.utm_alt/10;	144	gps_act_position.utm_alt = actual_pos.utm_alt/10;
141	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;	145	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
142	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;	146	gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
143	gps_act_position.heading = actual_speed.heading/100000;	147	gps_act_position.heading = actual_speed.heading/100000;
144	gps_act_position.status = 1;	148	gps_act_position.status = 1;
145	n = 0; //Daten gueltig	149	n = 0; //Daten gueltig
146	}	150	}
147	else	151	else
148	{	152	{
149	gps_act_position.status = 0; //Keine gueltigen Daten	153	gps_act_position.status = 0; //Keine gueltigen Daten
150	n = 2;	154	n = 2;
151	}	155	}
152	actual_pos.status = 0; //neue ublox Messages anfordern	156	actual_pos.status = 0; //neue ublox Messages anfordern
153	actual_status.status = 0;	157	actual_status.status = 0;
154	actual_speed.status = 0;	158	actual_speed.status = 0;
155	}	159	}
156	return (n);	160	return (n);
157	}	161	}
158		162
159		163
160	/*	164	/*
161	Daten vom GPS im ublox MSG Format auswerten	165	Daten vom GPS im ublox MSG Format auswerten
162	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen	166	Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
163	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein	167	// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
164	*/	168	*/
165	void Get_Ublox_Msg(uint8_t rx)	169	void Get_Ublox_Msg(uint8_t rx)
166	{	170	{
167		171
168	switch (ublox_msg_state)	172	switch (ublox_msg_state)
169	{	173	{
170		174
171	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen	175	case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
172	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;	176	if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
173	else ublox_msg_state = UBLOX_IDLE;	177	else ublox_msg_state = UBLOX_IDLE;
174	break;	178	break;
175		179
176	case UBLOX_SYNC1:	180	case UBLOX_SYNC1:
177		181
178	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;	182	if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
179	else ublox_msg_state = UBLOX_IDLE;	183	else ublox_msg_state = UBLOX_IDLE;
180	chk_a = 0,chk_b = 0;	184	chk_a = 0,chk_b = 0;
181	break;	185	break;
182		186
183	case UBLOX_SYNC2:	187	case UBLOX_SYNC2:
184	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;	188	if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;
185	else ublox_msg_state = UBLOX_IDLE;	189	else ublox_msg_state = UBLOX_IDLE;
186	break;	190	break;
187		191
188	case UBLOX_CLASS: // Nur NAV Meldungen auswerten	192	case UBLOX_CLASS: // Nur NAV Meldungen auswerten
189	switch (rx)	193	switch (rx)
190	{	194	{
191	case UBLOX_NAV_POSUTM:	195	case UBLOX_NAV_POSUTM:
192	ptr_pac_status = &actual_pos.status;	196	ptr_pac_status = &actual_pos.status;
193	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden	197	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
194	else	198	else
195	{	199	{
196	ptr_payload_data = &actual_pos;	200	ptr_payload_data = &actual_pos;
197	ptr_payload_data_end = &actual_pos.status;	201	ptr_payload_data_end = &actual_pos.status;
198	ublox_msg_state = UBLOX_LEN1;	202	ublox_msg_state = UBLOX_LEN1;
199	}	203	}
200	break;	204	break;
201		205
202	case UBLOX_NAV_STATUS:	206	case UBLOX_NAV_STATUS:
203	ptr_pac_status = &actual_status.status;	207	ptr_pac_status = &actual_status.status;
204	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;	208	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
205	else	209	else
206	{	210	{
207	ptr_payload_data = &actual_status;	211	ptr_payload_data = &actual_status;
208	ptr_payload_data_end = &actual_status.status;	212	ptr_payload_data_end = &actual_status.status;
209	ublox_msg_state = UBLOX_LEN1;	213	ublox_msg_state = UBLOX_LEN1;
210	}	214	}
211	break;	215	break;
212		216
213	case UBLOX_NAV_VELED:	217	case UBLOX_NAV_VELED:
214	ptr_pac_status = &actual_speed.status;	218	ptr_pac_status = &actual_speed.status;
215	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;	219	if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
216	else	220	else
217	{	221	{
218	ptr_payload_data = &actual_speed;	222	ptr_payload_data = &actual_speed;
219	ptr_payload_data_end = &actual_speed.status;	223	ptr_payload_data_end = &actual_speed.status;
220	ublox_msg_state = UBLOX_LEN1;	224	ublox_msg_state = UBLOX_LEN1;
221	}	225	}
222	break;	226	break;
223		227
224	default:	228	default:
225	ublox_msg_state = UBLOX_IDLE;	229	ublox_msg_state = UBLOX_IDLE;
226	break;	230	break;
227	}	231	}
228	chk_a = UBLOX_NAV_CLASS + rx;	232	chk_a = UBLOX_NAV_CLASS + rx;
229	chk_b = UBLOX_NAV_CLASS + chk_a;	233	chk_b = UBLOX_NAV_CLASS + chk_a;
230	break;	234	break;
231		235
232	case UBLOX_LEN1: // Laenge auswerten	236	case UBLOX_LEN1: // Laenge auswerten
233	rx_len = rx;	237	rx_len = rx;
234	chk_a += rx;	238	chk_a += rx;
235	chk_b += chk_a;	239	chk_b += chk_a;
236	ublox_msg_state = UBLOX_LEN2;	240	ublox_msg_state = UBLOX_LEN2;
237	break;	241	break;
238		242
239		243
240	case UBLOX_LEN2: // Laenge auswerten	244	case UBLOX_LEN2: // Laenge auswerten
241	rx_len = rx_len + (rx *256); // Laenge ermitteln	245	rx_len = rx_len + (rx *256); // Laenge ermitteln
242	chk_a += rx;	246	chk_a += rx;
243	chk_b += chk_a;	247	chk_b += chk_a;
244	ublox_msg_state = UBLOX_PAYLOAD;	248	ublox_msg_state = UBLOX_PAYLOAD;
245	break;	249	break;
246		250
247	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen	251	case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
248	if (rx_len > 0)	252	if (rx_len > 0)
249	{	253	{
250	*ptr_payload_data = rx;	254	*ptr_payload_data = rx;
251	chk_a += rx;	255	chk_a += rx;
252	chk_b += chk_a;	256	chk_b += chk_a;
253	--rx_len;	257	--rx_len;
254	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))	258	if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
255	{	259	{
256	ptr_payload_data++;	260	ptr_payload_data++;
257	ublox_msg_state = UBLOX_PAYLOAD;	261	ublox_msg_state = UBLOX_PAYLOAD;
258	}	262	}
259	else ublox_msg_state = UBLOX_CKA;	263	else ublox_msg_state = UBLOX_CKA;
260	}	264	}
261	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler	265	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
262	break;	266	break;
263		267
264	case UBLOX_CKA: // Checksum pruefen	268	case UBLOX_CKA: // Checksum pruefen
265	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;	269	if (rx == chk_a) ublox_msg_state = UBLOX_CKB;
266	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler	270	else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
267	break;	271	break;
268		272
269	case UBLOX_CKB: // Checksum pruefen	273	case UBLOX_CKB: // Checksum pruefen
270	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok	274	if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
271	ublox_msg_state = UBLOX_IDLE;	275	ublox_msg_state = UBLOX_IDLE;
272	break;	276	break;
273		277
274	default:	278	default:
275	ublox_msg_state = UBLOX_IDLE;	279	ublox_msg_state = UBLOX_IDLE;
276	break;	280	break;
277	}	281	}
278	}	282	}
279		283
280	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe	284	//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
281	short int GPS_CRTL(short int cmd)	285	short int GPS_CRTL(short int cmd)
282	{	286	{
283	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen	287	static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen
284		288
285	switch (cmd)	289	switch (cmd)
286	{	290	{
287	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.	291	case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
288	if (gps_state != GPS_CRTL_HOLD_ACTIVE)	292	if (gps_state != GPS_CRTL_HOLD_ACTIVE)
289	{	293	{
290	cnt++;	294	cnt++;
291	if (cnt > 300) // erst nach Verzoegerung	295	if (cnt > 300) // erst nach Verzoegerung
292	{	296	{
293	cnt = 0;	297	cnt = 0;
294	// aktuelle positionsdaten abespeichern	298	// aktuelle positionsdaten abespeichern
295	if (gps_rel_act_position.status > 0)	299	if (gps_rel_act_position.status > 0)
296	{	300	{
297	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;	301	gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east;
298	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;	302	gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
299	gps_rel_hold_position.status = 1; // gueltige Positionsdaten	303	gps_rel_hold_position.status = 1; // gueltige Positionsdaten
300	gps_state = GPS_CRTL_HOLD_ACTIVE;	304	gps_state = GPS_CRTL_HOLD_ACTIVE;
301	return (GPS_STST_OK);	305	return (GPS_STST_OK);
302	}	306	}
303	else	307	else
304	{	308	{
305	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar	309	gps_rel_hold_position.status = 0; //Keine Daten verfuegbar
306	gps_state = GPS_CRTL_IDLE;	310	gps_state = GPS_CRTL_IDLE;
307	return(GPS_STST_ERR); // Keine Daten da	311	return(GPS_STST_ERR); // Keine Daten da
308	}	312	}
309	}	313	}
310	else return(GPS_STST_PEND); // noch warten	314	else return(GPS_STST_PEND); // noch warten
311	}	315	}
312	break;	316	break;
313		317
314	case GPS_CMD_STOP_HOLD: // Lageregelung beenden	318	case GPS_CMD_STOP_HOLD: // Lageregelung beenden
315	cnt = 0;	319	cnt = 0;
316	GPS_Nick = 0;	320	GPS_Nick = 0;
317	GPS_Roll = 0;	321	GPS_Roll = 0;
318	gps_state = GPS_CRTL_IDLE;	322	gps_state = GPS_CRTL_IDLE;
-		323	gps_int_x = 0;
-		324	gps_int_y = 0;
319	return (GPS_STST_OK);	325	return (GPS_STST_OK);
320	break;	326	break;
321		327
322	default:	328	default:
323	return (GPS_STST_ERR);	329	return (GPS_STST_ERR);
324	break;	330	break;
325	}	331	}
326		332
327	switch (gps_state)	333	switch (gps_state)
328	{	334	{
329	case GPS_CRTL_IDLE:	335	case GPS_CRTL_IDLE:
330	cnt = 0;	336	cnt = 0;
331	return (GPS_STST_OK);	337	return (GPS_STST_OK);
332	break;	338	break;
333		339
334	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet	340	case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
335	if (gps_updte_flag = 1) //nur wenn neue GPS Daten vorliegen	341	if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen
336	{	342	{
337	gps_updte_flag = 0;	343	gps_updte_flag = 0;
338	// ab hier wird geregelt	344	// ab hier wird geregelt
339	// Richtung zum Ziel ermitteln	-
340	signed int dist_north,dist_east;	345	signed int dist_north,dist_east;
341	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;	346	dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east;
342	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;	347	dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
-		348
-		349	//PI Regler
-		350	gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/16; // Integrator
-		351	gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/16;
-		352	if ((gps_int_x >= 4096) \|\| (gps_int_y >= 4096) \|\| (gps_int_x < - 4096) \|\| (gps_int_y <= -4096))
-		353	{
-		354	gps_int_x -= (dist_east * Parameter_UserParam1)/16; // Integrator stoppen
-		355	gps_int_y -= (dist_north * Parameter_UserParam1)/16;
-		356	}
-		357	gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/16; // P Anteil dazu
-		358	gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/16;
-		359
-		360	//Richtung bezogen auf Nordpol bestimmen
343	GPS_hdng_abs_2trgt = arctan_i((long)dist_east,(long)dist_north);	361	GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y);
-		362
344	//in Winkel 0..360 grad umrechnen	363	//in Winkel 0..360 grad umrechnen
345	if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);	364	if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
346	else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);	365	else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
-		366	// Relative Richtung in bezug auf Nordachse des Kopters errechen
-		367	int n,m;
-		368	n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
-		369	GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n;
-		370	if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180))GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
-		371	else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
-		372	else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
347		373
348	// Distanz zum Ziel ermitteln	374	// Distanz zum Ziel ermitteln
349	GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt	375	GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt
350	return (GPS_STST_OK);	376	return (GPS_STST_OK);
351	}	377	}
352	else return (GPS_STST_OK);	378	else return (GPS_STST_OK);
353	break;	379	break;
354		380
355	default:	381	default:
356	gps_state = GPS_CRTL_IDLE;	382	gps_state = GPS_CRTL_IDLE;
357	return (GPS_STST_ERR);	383	return (GPS_STST_ERR);
358	break;	384	break;
359	}	385	}
360	return (GPS_STST_ERR);	386	return (GPS_STST_ERR);
361		387
362	}	388	}
363		389
364		390

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c - Rev 158 → 159