Subversion Repositories FlightCtrl

Rev

Rev 159 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 159 Rev 160
Line 70... Line 70...
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
Line 71... Line 71...
71 
 
 71 
 
72 
// Initialisierung
 72 
// Initialisierung
73 
void GPS_Neutral(void)
 73 
void GPS_Neutral(void)
74 
{
 - 
 
75 
        short int n;
 74 
{
76 
        ublox_msg_state                         =       UBLOX_IDLE;
 75 
        ublox_msg_state                         =       UBLOX_IDLE;
77 
        gps_state                                       =       GPS_CRTL_IDLE;
 76 
        gps_state                                       =       GPS_CRTL_IDLE;
78 
        actual_pos.status                       =       0;
 77 
        actual_pos.status                       =       0;
79 
        actual_speed.status                     =       0;
 78 
        actual_speed.status                     =       0;
Line 283... Line 282...
283 
       
 282 
       
284 
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
 283 
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
285 
short int GPS_CRTL(short int cmd)
 284 
short int GPS_CRTL(short int cmd)
286 
{
 285 
{
287 
        static unsigned int cnt;        //Zaehler fuer diverse Verzoegerungen
286 
        static unsigned int cnt;        //Zaehler fuer diverse Verzoegerungen
- 
 
 287 
        int n;
288 
 
 288 
        long int dist;
289 
        switch (cmd)
 289 
        switch (cmd)
290 
        {
 290 
        {
291 
                case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
 291 
                case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
292 
                        if (gps_state != GPS_CRTL_HOLD_ACTIVE)
 292 
                        if (gps_state != GPS_CRTL_HOLD_ACTIVE)
293 
                        {
 293 
                        {
294 
                                cnt++;
 294 
                                cnt++;
295 
                                if (cnt > 300) // erst nach Verzoegerung
295 
                                if (cnt > 500) // erst nach Verzoegerung
296 
                                {
 296 
                                {
297 
                                        cnt     =       0;
 297 
                                        cnt     =       0;
298 
                                        // aktuelle positionsdaten abespeichern
 298 
                                        // aktuelle positionsdaten abespeichern
299 
                                        if (gps_rel_act_position.status > 0)
 299 
                                        if (gps_rel_act_position.status > 0)
Line 343... Line 343...
343 
                                gps_updte_flag = 0;
 343 
                                gps_updte_flag = 0;
344 
                                // ab hier wird geregelt
 344 
                                // ab hier wird geregelt
345 
                                signed int dist_north,dist_east;
 345 
                                signed int dist_north,dist_east;
346 
                                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;
347 
                                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 
 
 348 
                               
349 
                                //PI Regler
 349 
                                //PI Regler
350 
                                gps_int_x       =       gps_int_x + (dist_east  * Parameter_UserParam1)/16; // Integrator
 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;
 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))
 352 
                                if ((gps_int_x >= 4096) || (gps_int_y >= 4096) || (gps_int_x < - 4096) || (gps_int_y <= -4096))
353 
                                {                      
 353 
                                {                      
Line 359... Line 359...
359 
 
 359 
 
360 
                                //Richtung bezogen auf Nordpol bestimmen
 360 
                                //Richtung bezogen auf Nordpol bestimmen
Line 361... Line 361...
361 
                                GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y);
 361 
                                GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y);
362 
 
 362 
 
363 
                                //in Winkel 0..360 grad umrechnen
 363 
                                //in Winkel 0..360 grad umrechnen
364 
                                if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
 364 
                                if ((gps_reg_x >= 0) && (gps_reg_y < 0)) GPS_hdng_abs_2trgt = ( 90-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
 365 
                                else if ((gps_reg_x <  0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
367 
                                int n,m;
 366 
                                // Relative Richtung in bezug auf Nordachse des Kopters errechen
368 
                                n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
 367 
                                n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
369 
                                GPS_hdng_rel_2trgt      =       GPS_hdng_abs_2trgt - n;
 368 
                                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;
 369 
                                if      ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180))GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
Line -... Line 370...
- 
 
 370 
                                else if (GPS_hdng_rel_2trgt >180)  GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
- 
 
 371 
                                else if (GPS_hdng_rel_2trgt <-180)  GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
- 
 
 372 
                               
- 
 
 373 
                                // Regelabweichung aus x,y zu Ziel  in Distanz umrechnen
- 
 
 374 
       
- 
 
 375 
                                if (abs(gps_reg_x) > abs(gps_reg_y) )
- 
 
 376 
                                {
- 
 
 377 
                                        dist = (long)gps_reg_x; //Groesseren Wert wegen besserer genauigkeit nehmen
- 
 
 378 
                                        dist = abs((dist *1000) / (long) cos_i(GPS_hdng_rel_2trgt));
- 
 
 379 
                                }
- 
 
 380 
                                else
- 
 
 381 
                                {
- 
 
 382 
                                        dist    = (long)gps_reg_y;
371 
                                else if (GPS_hdng_rel_2trgt >180)  GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
 383 
                                        dist = abs((dist *1000) / (long) sin_i(GPS_hdng_rel_2trgt));
- 
 
 384 
                                }
- 
 
 385 
                                if (dist > 30000) dist = 30000;
- 
 
 386 
                                GPS_dist_2trgt = (int )dist;
- 
 
 387 
 
372 
                                else if (GPS_hdng_rel_2trgt <-180)  GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
 388 
                                // Winkel und Distanz in Nick und Roll groessen umrechnen
- 
 
 389 
                                long int a,b;
- 
 
 390 
                                GPS_Roll = (int) -((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*4));
- 
 
 391 
                                GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*4));
- 
 
 392 
 
- 
 
 393 
                                if (GPS_Roll > GPS_ROLL_MAX) GPS_Roll = GPS_ROLL_MAX; //Auf Maxwerte begrenzen
- 
 
 394 
                                else if (GPS_Roll <  -GPS_ROLL_MAX) GPS_Roll = - GPS_ROLL_MAX;
373 
                               
 395 
                                if (GPS_Nick > GPS_NICK_MAX) GPS_Nick = GPS_NICK_MAX;
374 
                                // Distanz zum Ziel ermitteln
 396 
                                else if (GPS_Nick <  -GPS_NICK_MAX) GPS_Nick = - GPS_NICK_MAX;
375 
                                GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt
 397 
 
376 
                                return (GPS_STST_OK);                  
 398 
                                return (GPS_STST_OK);