Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
143 salvo 1
/*
2
This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify
242 salvo 3
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
143 salvo 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;
6
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
242 salvo 7
GNU General Public License and GNU Lesser General Public License for more details.
258 salvo 8
You should have received a copy of GNU General Public License (License_GPL.txt)  and
242 salvo 9
GNU Lesser General Public License (License_LGPL.txt) along with this program.
10
If not, see <http://www.gnu.org/licenses/>.
143 salvo 11
 
149 salvo 12
Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de
143 salvo 13
*/
14
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15
Peter Muehlenbrock
16
Auswertung der Daten vom GPS im ublox Format
182 salvo 17
Hold Modus mit PID Regler
236 salvo 18
Rückstuerz zur Basis Funktion
293 salvo 19
Stand 13.10.2007
143 salvo 20
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21
*/
1 ingob 22
#include "main.h"
156 salvo 23
//#include "gps.h"
1 ingob 24
 
143 salvo 25
// Defines fuer ublox Messageformat um Auswertung zu steuern
183 salvo 26
#define                 UBLOX_IDLE              0
27
#define                 UBLOX_SYNC1             1
28
#define                 UBLOX_SYNC2             2
29
#define                 UBLOX_CLASS             3
30
#define                 UBLOX_ID                4
31
#define                 UBLOX_LEN1              5
32
#define                 UBLOX_LEN2              6
33
#define                 UBLOX_CKA               7
34
#define                 UBLOX_CKB               8
143 salvo 35
#define                 UBLOX_PAYLOAD   9
36
 
37
// ublox Protokoll Identifier 
38
#define                 UBLOX_NAV_POSUTM                0x08
39
#define                 UBLOX_NAV_STATUS                0x03
40
#define                 UBLOX_NAV_VELED                 0x12
41
#define                 UBLOX_NAV_CLASS                 0x01
42
#define                 UBLOX_SYNCH1_CHAR               0xB5
43
#define                 UBLOX_SYNCH2_CHAR               0x62
44
 
258 salvo 45
signed int                      GPS_Nick = 0;
46
signed int                      GPS_Roll = 0;
47
short int                       ublox_msg_state = UBLOX_IDLE;
48
static                          uint8_t chk_a =0; //Checksum
49
static                          uint8_t chk_b =0;
50
short int                       gps_state,gps_sub_state; //Zustaende der Statemachine
51
short int                       gps_updte_flag;
52
signed int                      GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
53
signed int                      GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
54
signed int                      GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel 
55
signed int                      gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;                               
143 salvo 56
static unsigned int rx_len;
57
static unsigned int ptr_payload_data_end;
209 salvo 58
unsigned int            gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
258 salvo 59
signed int                      hdng_2home,dist_2home; //Richtung und Entfernung zur home Position 
60
static signed           gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt 
61
static                          short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
62
static                          uint8_t *ptr_payload_data;
63
static                          uint8_t *ptr_pac_status;
64
long int                        dist_flown;
143 salvo 65
 
156 salvo 66
short int Get_GPS_data(void);
143 salvo 67
 
68
NAV_POSUTM_t actual_pos;    // Aktuelle Nav Daten werden hier im ublox Format abgelegt
69
NAV_STATUS_t actual_status; // Aktueller Nav Status
70
NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten
71
 
156 salvo 72
GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
73
GPS_ABS_POSITION_t              gps_home_position;      // Die Startposition, beim Kalibrieren ermittelt
74
GPS_REL_POSITION_t              gps_rel_act_position;   // Die aktuelle relative Position bezogen auf Home Position
158 salvo 75
GPS_REL_POSITION_t              gps_rel_hold_position;  // Die gespeicherte Sollposition fuer GPS_ Hold Mode
258 salvo 76
GPS_REL_POSITION_t              gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
143 salvo 77
 
156 salvo 78
// Initialisierung
79
void GPS_Neutral(void)
1 ingob 80
{
158 salvo 81
        ublox_msg_state                         =       UBLOX_IDLE;
82
        gps_state                                       =       GPS_CRTL_IDLE;
266 salvo 83
        gps_sub_state                           =       GPS_CRTL_IDLE;
158 salvo 84
        actual_pos.status                       =       0;
85
        actual_speed.status                     =       0;
86
        actual_status.status            =       0;
87
        gps_home_position.status        =       0; // Noch keine gueltige Home Position
88
        gps_act_position.status         =       0;
89
        gps_rel_act_position.status     =       0;     
90
        GPS_Nick                                        =       0;
91
        GPS_Roll                                        =       0;
92
        gps_updte_flag                          =       0;
159 salvo 93
        gps_int_x                                       =       0;
94
        gps_int_y                                       =       0;
190 salvo 95
        gps_alive_cnt                           =       0;
1 ingob 96
}
97
 
156 salvo 98
// Home Position sichern falls Daten verfuegbar sind. 
99
void GPS_Save_Home(void)
1 ingob 100
{
156 salvo 101
        short int n;
102
        n = Get_GPS_data();
103
        if (n == 0)   // Gueltige  und aktuelle Daten ?   
104
        {
105
                // Neue GPS Daten liegen vor
157 salvo 106
                gps_home_position.utm_east      = gps_act_position.utm_east;   
107
                gps_home_position.utm_north     = gps_act_position.utm_north;  
108
                gps_home_position.utm_alt       = gps_act_position.utm_alt;
109
                gps_home_position.status        = 1; // Home Position gueltig                   
156 salvo 110
        }
111
}
112
 
113
// Relative Position zur Home Position bestimmen
114
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
115
short int Get_Rel_Position(void)
116
{
117
        short int n = 0;
118
        n = Get_GPS_data();
119
        if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
224 salvo 120
        if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
156 salvo 121
        if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
122
        {
123
                gps_rel_act_position.utm_east   = (int)  (gps_act_position.utm_east - gps_home_position.utm_east);
124
                gps_rel_act_position.utm_north  = (int)  (gps_act_position.utm_north - gps_home_position.utm_north);
125
                gps_rel_act_position.status     = 1; // gueltige Positionsdaten
126
                n = 0;
158 salvo 127
                gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
156 salvo 128
        }
129
        else
130
        {
131
                n = 2; //keine gueltigen Daten vorhanden
132
                gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
133
        }      
134
        return (n);
135
}
136
 
137
// Daten aus aktuellen ublox Messages extrahieren 
138
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
139
short int Get_GPS_data(void)
140
{
141
        short int n = 1;
142
 
143
        if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
143 salvo 144
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
145
        {
149 salvo 146
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143 salvo 147
                {
148
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
149
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
150
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
151
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
152
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
153
                        gps_act_position.heading        = actual_speed.heading/100000;
154
                        gps_act_position.status         = 1;
156 salvo 155
                        n                                                       = 0; //Daten gueltig
143 salvo 156
                }
156 salvo 157
                else
158
                {
159
                        gps_act_position.status = 0; //Keine gueltigen Daten
160
                        n                                               = 2;
161
                }
162
                actual_pos.status               = 0; //neue ublox Messages anfordern
163
                actual_status.status    = 0;
164
                actual_speed.status     = 0;
165
        }      
166
        return (n);    
1 ingob 167
}
168
 
143 salvo 169
/*
170
Daten vom GPS im ublox MSG Format auswerten
156 salvo 171
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
143 salvo 172
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
173
*/
174
void Get_Ublox_Msg(uint8_t rx)
175
{
176
        switch (ublox_msg_state)
177
        {
1 ingob 178
 
143 salvo 179
                case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
180
                        if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
181
                        else ublox_msg_state = UBLOX_IDLE;
182
                        break;
183
 
184
                case UBLOX_SYNC1:
185
 
186
                        if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
187
                        else ublox_msg_state = UBLOX_IDLE;
188
                        chk_a = 0,chk_b = 0;
189
                        break;
190
 
191
                case UBLOX_SYNC2:
192
                        if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;      
193
                        else ublox_msg_state = UBLOX_IDLE;
194
                        break;
195
 
196
                case UBLOX_CLASS: // Nur NAV Meldungen auswerten
197
                        switch (rx)
198
                        {
199
                                case UBLOX_NAV_POSUTM:
200
                                        ptr_pac_status  =       &actual_pos.status;
201
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
202
                                        else
203
                                        {
204
                                                ptr_payload_data                = &actual_pos;
205
                                                ptr_payload_data_end    = &actual_pos.status;
206
                                                ublox_msg_state                 = UBLOX_LEN1;
207
                                        }
208
                                        break;
209
 
210
                                case UBLOX_NAV_STATUS:
211
                                        ptr_pac_status  =       &actual_status.status;
212
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
213
                                        else
214
                                        {
215
                                                ptr_payload_data                = &actual_status;
216
                                                ptr_payload_data_end    = &actual_status.status;
217
                                                ublox_msg_state                 = UBLOX_LEN1;
218
                                        }
219
                                        break;
220
 
221
                                case UBLOX_NAV_VELED:
156 salvo 222
                                        ptr_pac_status          =       &actual_speed.status;
143 salvo 223
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
224
                                        else
225
                                        {
226
                                                ptr_payload_data                = &actual_speed;
227
                                                ptr_payload_data_end    = &actual_speed.status;
228
                                                ublox_msg_state                 = UBLOX_LEN1;
229
                                        }
230
                                        break;
231
 
232
                                default:
233
                                        ublox_msg_state = UBLOX_IDLE;
234
                                        break; 
235
                        }
236
                        chk_a   = UBLOX_NAV_CLASS + rx;
237
                        chk_b   = UBLOX_NAV_CLASS + chk_a;                     
238
                        break;
239
 
240
                case UBLOX_LEN1: // Laenge auswerten
241
                        rx_len  = rx;
242
                        chk_a   += rx;
243
                        chk_b   += chk_a;              
244
                        ublox_msg_state = UBLOX_LEN2;
245
                        break;
246
 
247
 
248
                case UBLOX_LEN2: // Laenge auswerten
249
                        rx_len = rx_len + (rx *256); // Laenge ermitteln
250
                        chk_a   += rx;
251
                        chk_b   += chk_a;      
252
                        ublox_msg_state = UBLOX_PAYLOAD;
253
                        break;
254
 
255
                case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
256
                        if (rx_len > 0)
257
                        {
258
                                *ptr_payload_data = rx;
259
                                chk_a   += rx;
260
                                chk_b   += chk_a;
261
                                --rx_len;      
262
                                if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))                          
263
                                {
264
                                        ptr_payload_data++;
156 salvo 265
                                        ublox_msg_state = UBLOX_PAYLOAD;
143 salvo 266
                            }
267
                                else ublox_msg_state = UBLOX_CKA;
268
                        }      
269
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
270
                        break;
271
 
272
                case UBLOX_CKA: // Checksum pruefen
273
                        if (rx == chk_a) ublox_msg_state = UBLOX_CKB;                          
274
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
275
                        break;
276
 
277
                case UBLOX_CKB: // Checksum pruefen
278
                        if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
279
                        ublox_msg_state    = UBLOX_IDLE;
280
                        break;
281
 
282
                default:
283
                        ublox_msg_state = UBLOX_IDLE;          
284
                        break;
158 salvo 285
        }
143 salvo 286
}
158 salvo 287
 
288
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
289
short int GPS_CRTL(short int cmd)
290
{
236 salvo 291
        static unsigned int cnt;                                        //Zaehler fuer diverse Verzoegerungen 
184 salvo 292
        static signed int int_east,int_north;   //Integrierer 
293
        static signed int dist_north,dist_east;
186 salvo 294
        static signed int diff_east,diff_north;         // Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)
295
        static signed int diff_east_f,diff_north_f; // Differenzierer,  gefiltert
197 salvo 296
        signed int n,diff_v;
236 salvo 297
        static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
197 salvo 298
        long signed int dev,n_l;
225 salvo 299
        signed int dist_frm_start_east,dist_frm_start_north;
158 salvo 300
        switch (cmd)
301
        {
162 salvo 302
 
303
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. 
224 salvo 304
                        if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
305
                        {
306
                                cnt++;
307
                                if (cnt > 500) // erst nach Verzoegerung 
308
                                {
309
                                        // Erst mal initialisieren
310
                                        cnt                             = 0;
311
                                        gps_tick                = 0;                                   
225 salvo 312
                                        hold_fast               = 0;
236 salvo 313
                                        hold_reset_int  = 0; // Integrator enablen
224 salvo 314
                                        int_east                = 0, int_north  = 0;
315
                                        gps_reg_x               = 0, gps_reg_y  = 0;
316
                                        dist_east               = 0, dist_north = 0;
317
                                        diff_east_f             = 0, diff_north_f= 0;
318
                                        diff_east               = 0, diff_north  = 0;  
319
                                        dist_flown              = 0;
236 salvo 320
                                        gps_g2t_act_v   = 0;
224 salvo 321
                                        gps_sub_state   = GPS_CRTL_IDLE;
322
                                        // aktuelle positionsdaten abspeichern
323
                                        if (gps_rel_act_position.status > 0)
324
                                        {
325
                                                gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
326
                                                gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
327
                                                gps_rel_start_position.status   = 1; // gueltige Positionsdaten 
328
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
329
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
330
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
291 salvo 331
                                                //Richtung zur Home Position bezogen auf Nordpol bestimmen
224 salvo 332
                                                hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
333
                                                // in Winkel 0...360 Grad umrechnen
241 salvo 334
                                                if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
224 salvo 335
                                                else  hdng_2home = (270 - hdng_2home);
291 salvo 336
                                                dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
224 salvo 337
                                                gps_state       = GPS_CRTL_HOME_ACTIVE;
338
                                                return (GPS_STST_OK);                          
339
                                        }
340
                                        else
341
                                        {
342
                                                gps_rel_start_position.status   =       0;  //Keine Daten verfuegbar
343
                                                gps_state                                               = GPS_CRTL_IDLE;
344
                                                return(GPS_STST_ERR); // Keine Daten da
345
                                        }
346
                                }
347
                                else return(GPS_STST_PEND); // noch warten
348
                        }
162 salvo 349
                   break;
350
// ******************************
351
 
158 salvo 352
                case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
353
                        if (gps_state != GPS_CRTL_HOLD_ACTIVE)
354
                        {
355
                                cnt++;
160 salvo 356
                                if (cnt > 500) // erst nach Verzoegerung 
158 salvo 357
                                {
358
                                        cnt     =       0;
224 salvo 359
                                        // aktuelle positionsdaten abspeichern
158 salvo 360
                                        if (gps_rel_act_position.status > 0)
361
                                        {
236 salvo 362
                                                hold_fast               = 0;
363
                                                hold_reset_int  = 0; // Integrator enablen
224 salvo 364
                                                int_east        = 0, int_north  = 0;
365
                                                gps_reg_x       = 0, gps_reg_y  = 0;
366
                                                dist_east       = 0, dist_north = 0;
367
                                                diff_east_f     = 0, diff_north_f= 0;
368
                                                diff_east       = 0, diff_north  = 0;
158 salvo 369
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
370
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
371
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
372
                                                gps_state                                               = GPS_CRTL_HOLD_ACTIVE;
373
                                                return (GPS_STST_OK);                          
374
                                        }
375
                                        else
376
                                        {
377
                                                gps_rel_hold_position.status    =       0;  //Keine Daten verfuegbar
378
                                                gps_state                                               = GPS_CRTL_IDLE;
379
                                                return(GPS_STST_ERR); // Keine Daten da
380
                                        }
381
                                }
382
                                else return(GPS_STST_PEND); // noch warten
383
                        }
384
                        break;
385
 
162 salvo 386
                case GPS_CMD_STOP: // Lageregelung beenden
190 salvo 387
                        cnt                             =       0;
388
                        GPS_Nick                =       0;
389
                        GPS_Roll                =       0;
390
                        gps_int_x               =       0;
391
                        gps_int_y               =       0;
266 salvo 392
                        gps_sub_state   =       GPS_CRTL_IDLE;
393
                        gps_state               =       GPS_CRTL_IDLE;
158 salvo 394
                        return (GPS_STST_OK);
395
                        break;
396
 
397
                default:
398
                        return (GPS_STST_ERR);
399
                        break;
400
        }
401
 
402
        switch (gps_state)
403
        {      
404
                case GPS_CRTL_IDLE:
405
                        cnt             =       0;
406
                        return (GPS_STST_OK);
407
                        break;
408
 
224 salvo 409
                case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
410
                //Der Sollwert des Lagereglers wird der Homeposition angenaehert
411
                        if (gps_rel_start_position.status >0)
412
                        {
413
                                if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
414
                                {
415
                                        gps_tick++;
416
                                        int d1,d2,d3;
225 salvo 417
                                        d1      = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
418
                                        d2      = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
224 salvo 419
                                        d3      = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel       
420
 
258 salvo 421
                                        if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
224 salvo 422
                                        {
258 salvo 423
                                                if ((d1 < GPS_G2T_FAST_TOL)  && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
225 salvo 424
                                                {
266 salvo 425
                                                        if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++;    //Geschwindigkeit langsam erhoehen
291 salvo 426
                                                        dist_flown              +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
427
                                                        gps_sub_state   = GPS_HOME_FAST_IN_TOL;
224 salvo 428
                                                }
236 salvo 429
                                                else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
430
                                                {
258 salvo 431
                                                        if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
266 salvo 432
                                                        dist_flown++;                                                   //Auch ausserhalb der Toleranz langsam erhoehen
241 salvo 433
                                                        gps_sub_state   = GPS_HOME_FAST_OUTOF_TOL;
236 salvo 434
                                                }
266 salvo 435
                                                hold_fast                                               = 1; // Regler fuer schnellen Flug
436
                                                hold_reset_int                                  = 1; // Integrator ausschalten            
437
                                                dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
438
                                                dist_frm_start_north                    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
439
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
440
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
224 salvo 441
                                        }
258 salvo 442
                                        else if (d3 > GPS_G2T_DIST_HOLD)   //Das Ziel naehert sich, deswegen abbremsen
224 salvo 443
                                        {
266 salvo 444
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL))  
224 salvo 445
                                                {
266 salvo 446
                                                        dist_flown              +=      GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
447
                                                        gps_sub_state   =       GPS_HOME_RMPDWN_IN_TOL;
258 salvo 448
                                                }
449
                                                else
450
                                                {
266 salvo 451
                                                        dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
258 salvo 452
                                                        gps_sub_state   = GPS_HOME_RMPDWN_OUTOF_TOL;
453
                                                }                                      
266 salvo 454
                                                hold_fast                                               = 0; // Wieder normal regeln
455
                                                hold_reset_int                                  = 1; // Integrator ausschalten            
456
                                                dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
457
                                                dist_frm_start_north                    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
458
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
459
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
258 salvo 460
                                        }                                                      
461
                                        else  //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) 
462
                                        {
463
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
464
                                                {
465
                                                        gps_sub_state   = GPS_HOME_IN_TOL;
466
                                                        hold_fast               = 0; // Wieder normal regeln
467
                                                        hold_reset_int  = 0; // Integrator wieder aktivieren              
468
                                                        if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
469
                                                        else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
470
                                                        if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
471
                                                        else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
472
                                                        if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
224 salvo 473
                                                        {
258 salvo 474
                                                                gps_rel_hold_position.utm_east  = 0;
475
                                                                gps_rel_hold_position.utm_north = 0;
476
                                                                gps_sub_state                                   = GPS_HOME_FINISHED;
224 salvo 477
                                                        }
258 salvo 478
                                                }
266 salvo 479
                                                else gps_sub_state      = GPS_HOME_OUTOF_TOL;
258 salvo 480
                                        }                                      
224 salvo 481
                                }
258 salvo 482
                                gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
483
                                return (GPS_STST_OK);                                  
224 salvo 484
                        }
258 salvo 485
                        else  // Keine GPS Daten verfuegbar, deswegen Abbruch
224 salvo 486
                        {
258 salvo 487
                                gps_state       =       GPS_CRTL_IDLE; 
224 salvo 488
                                return (GPS_STST_ERR);
489
                        }
490
                        break;
491
 
492
 
236 salvo 493
                case GPS_CRTL_HOLD_ACTIVE:  // Hier werden die Daten fuer Nick und Roll errechnet
224 salvo 494
                        if (gps_updte_flag >0)  // nur wenn neue GPS Daten vorliegen
158 salvo 495
                        {
496
                                gps_updte_flag = 0;
497
                                // ab hier wird geregelt
186 salvo 498
                                diff_east       = -dist_east;     //Alten Wert fuer Differenzier schon mal abziehen
499
                                diff_north      = -dist_north;
190 salvo 500
                                dist_east       = gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;
184 salvo 501
                                dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
162 salvo 502
                                int_east        += dist_east;
503
                                int_north   += dist_north;
167 salvo 504
                                diff_east       += dist_east;   // Differenz zur vorhergehenden East Position
505
                                diff_north      += dist_north;  // Differenz zur vorhergehenden North Position
291 salvo 506
 
507
                                if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen
508
                                {
293 salvo 509
                                        diff_east_f             = ((diff_east_f  *2)/3) + (diff_east *1/3); //Differenzierer filtern    
510
                                        diff_north_f    = ((diff_north_f *2)/3) + (diff_north*1/3); //Differenzierer filtern
291 salvo 511
                                }      
512
                                else // schwache Filterung
513
                                {
293 salvo 514
                                        diff_east_f             = ((diff_east_f  * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern 
515
                                        diff_north_f    = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern
291 salvo 516
                                }
517
 
236 salvo 518
                                #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung 
162 salvo 519
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
520
                                {
521
                                        int_east        -= dist_east;
522
                                        int_north   -= dist_north;                                     
523
                                }
236 salvo 524
                                if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
225 salvo 525
                                {
526
                                        int_east        = 0;   
527
                                        int_north       = 0;                                   
528
                                }
529
 
209 salvo 530
                                // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
531
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
236 salvo 532
                                signed long dist,int_east1,int_north1;
197 salvo 533
                                int phi;
266 salvo 534
                                phi  = arctan_i(abs(dist_north),abs(dist_east));
535
                                dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
224 salvo 536
 
258 salvo 537
                                if (hold_fast == 0)  // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
538
                                {
539
                                        diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10
540
                                        if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen
541
                                }
542
                                else
543
                                {
544
                                        diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10
545
                                        if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
546
                                }
161 salvo 547
 
266 salvo 548
                                int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung
549
                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
550
                                else diff_p = GPS_PROP_NRML_V;
551
 
236 salvo 552
                                //I Werte begrenzen
553
                                #define INT1_MAX (20 * GPS_V)
554
                                int_east1 =  ((((long)int_east)   * Parameter_UserParam2)/32);
555
                                int_north =  ((((long)int_north)  * Parameter_UserParam2)/32);
556
                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
557
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
558
                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
559
                                else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;
560
 
186 salvo 561
                                //PID Regler Werte aufsummieren
291 salvo 562
                                gps_reg_x = ((int)int_east1  + ((dist_east  * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east_f  * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil X Achse
563
                                gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north_f * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil Y Achse
159 salvo 564
 
186 salvo 565
                                //Ziel-Richtung bezogen auf Nordpol bestimmen
194 salvo 566
                                GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
159 salvo 567
 
186 salvo 568
                                // in Winkel 0...360 Grad umrechnen
161 salvo 569
                                if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
162 salvo 570
                                else  GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
161 salvo 571
 
159 salvo 572
                                // Relative Richtung in bezug auf Nordachse des Kopters errechen
573
                                n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
574
                                GPS_hdng_rel_2trgt      =       GPS_hdng_abs_2trgt - n;
167 salvo 575
                                if      ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
159 salvo 576
                                else if (GPS_hdng_rel_2trgt >180)  GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
577
                                else if (GPS_hdng_rel_2trgt <-180)  GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
578
 
184 salvo 579
                                // Regelabweichung aus x,y zu Ziel in Distanz umrechnen 
160 salvo 580
                                if (abs(gps_reg_x) > abs(gps_reg_y) )
581
                                {
197 salvo 582
                                        dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
583
                                        dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
160 salvo 584
                                }
585
                                else
586
                                {
197 salvo 587
                                        dev = (long)gps_reg_y;
588
                                        dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
160 salvo 589
                                }
197 salvo 590
                                GPS_dist_2trgt  = (int) dev;
185 salvo 591
                                // Winkel und Distanz in Nick und Rollgroessen umrechnen
197 salvo 592
                                GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
593
                                GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
190 salvo 594
 
184 salvo 595
                                if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
596
                                else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
597
                                if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
598
                                else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
599
 
600
                                //Kleine Werte verstaerken, Grosse abschwaechen
186 salvo 601
                                n                       = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
602
                                n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
603
                                GPS_Roll        = (int) n_l;
185 salvo 604
                                n                       = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));
186 salvo 605
                                n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
606
                                GPS_Nick        = (int) n_l;
184 salvo 607
 
608
                                if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
609
                                {
610
                                        GPS_Roll        = 0;
611
                                        GPS_Nick        = 0;
612
                                        gps_state       = GPS_CRTL_IDLE;
186 salvo 613
                                        return (GPS_STST_ERR); 
614
                                        break;                                 
184 salvo 615
                                }
224 salvo 616
                                else
617
                                {
618
                                        if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
619
                                        return (GPS_STST_OK);
620
                                }
158 salvo 621
                        }
224 salvo 622
                        else
623
                        {
624
                                if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
625
                                return (GPS_STST_OK);
626
                        }
158 salvo 627
                        break;
628
 
629
                default:
224 salvo 630
                        gps_state = GPS_CRTL_IDLE;
158 salvo 631
                        return (GPS_STST_ERR);
632
                        break;
633
        }      
634
        return (GPS_STST_ERR);
635
 
636
}
637