Rev 183 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 183 | Rev 184 | ||
|---|---|---|---|
| Line 11... | Line 11... | ||
| 11 | */ |
11 | */ |
| 12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 13 | Peter Muehlenbrock |
13 | Peter Muehlenbrock |
| 14 | Auswertung der Daten vom GPS im ublox Format |
14 | Auswertung der Daten vom GPS im ublox Format |
| 15 | Hold Modus mit PID Regler |
15 | Hold Modus mit PID Regler |
| 16 | Stand 28.9.2007 |
16 | Stand 29.9.2007 |
| 17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 18 | */ |
18 | */ |
| 19 | #include "main.h" |
19 | #include "main.h" |
| 20 | //#include "gps.h" |
20 | //#include "gps.h" |
| Line 281... | Line 281... | ||
| 281 | } |
281 | } |
| Line 282... | Line 282... | ||
| 282 | 282 | ||
| 283 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
283 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
| 284 | short int GPS_CRTL(short int cmd) |
284 | short int GPS_CRTL(short int cmd) |
| 285 | { |
285 | { |
| 286 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
286 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
| 287 | static int int_east,int_north; //Integrierer |
287 | static signed int int_east,int_north; //Integrierer |
| 288 | static signed int dist_north,dist_east; |
288 | static signed int dist_north,dist_east; |
| 289 | int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
289 | int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| 290 | int n; |
290 | int n; |
| 291 | long int dist; |
291 | long int dist; |
| 292 | switch (cmd) |
292 | switch (cmd) |
| Line 310... | Line 310... | ||
| 310 | { |
310 | { |
| 311 | int_east = 0; |
311 | int_east = 0; |
| 312 | int_north = 0; |
312 | int_north = 0; |
| 313 | gps_reg_x = 0; |
313 | gps_reg_x = 0; |
| 314 | gps_reg_y = 0; |
314 | gps_reg_y = 0; |
| - | 315 | dist_east = 0; |
|
| - | 316 | dist_north = 0; |
|
| 315 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
317 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| 316 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
318 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| 317 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
319 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| 318 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
320 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
| 319 | return (GPS_STST_OK); |
321 | return (GPS_STST_OK); |
| Line 355... | Line 357... | ||
| 355 | if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
357 | if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
| 356 | { |
358 | { |
| 357 | gps_updte_flag = 0; |
359 | gps_updte_flag = 0; |
| 358 | // ab hier wird geregelt |
360 | // ab hier wird geregelt |
| Line 359... | Line 361... | ||
| 359 | 361 | ||
| 360 | diff_east = -dist_east; //Alten Wert schon mal abziehen |
362 | diff_east = -dist_east; //Alten Wert schon mal abziehen |
| 361 | diff_north = -dist_north; |
363 | diff_north = -dist_north; |
| 362 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
364 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
| 363 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
365 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| 364 | int_east += dist_east; |
366 | int_east += dist_east; |
| 365 | int_north += dist_north; |
367 | int_north += dist_north; |
| 366 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
368 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
| Line 370... | Line 372... | ||
| 370 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
372 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
| 371 | { |
373 | { |
| 372 | int_east -= dist_east; |
374 | int_east -= dist_east; |
| 373 | int_north -= dist_north; |
375 | int_north -= dist_north; |
| 374 | } |
376 | } |
| 375 | - | ||
| 376 | #define DIST_MAX 100 //neuer Wert ab 24.9.2007 Begrenzung |
- | |
| 377 | if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
- | |
| 378 | if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
- | |
| 379 | if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
- | |
| 380 | if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
- | |
| Line 381... | Line 377... | ||
| 381 | 377 | ||
| 382 | //PID Regler |
378 | //PID Regler |
| 383 | gps_reg_x = ((int_east * Parameter_UserParam1)/32) + ((dist_east * Parameter_UserParam2)/8)+ ((diff_east * Parameter_UserParam3)/2); // |
379 | gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * Parameter_UserParam3)/2); // |
| Line 384... | Line 380... | ||
| 384 | gps_reg_y = ((int_north * Parameter_UserParam1)/32) + ((dist_north * Parameter_UserParam2)/8)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
380 | gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
| 385 | 381 | ||
| Line 386... | Line 382... | ||
| 386 | //Richtung bezogen auf Nordpol bestimmen |
382 | //Richtung bezogen auf Nordpol bestimmen |
| Line 395... | Line 391... | ||
| 395 | GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
391 | GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
| 396 | if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
392 | if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
| 397 | else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt; |
393 | else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt; |
| 398 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
394 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
| Line 399... | Line 395... | ||
| 399 | 395 | ||
| 400 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
- | |
| 401 | 396 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
|
| 402 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
397 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
| 403 | { |
398 | { |
| 404 | dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
399 | dist = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen |
| 405 | dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
400 | dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
| 406 | } |
401 | } |
| 407 | else |
402 | else |
| 408 | { |
403 | { |
| 409 | dist = (long)gps_reg_y; |
404 | dist = (long)gps_reg_y; |
| 410 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
405 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| 411 | } |
406 | } |
| 412 | if (dist > 200) dist = 200; |
407 | // if (dist > 200) dist = 200; |
| Line 413... | Line 408... | ||
| 413 | GPS_dist_2trgt = (int )dist; |
408 | // GPS_dist_2trgt = (int )dist; |
| 414 | 409 | ||
| 415 | // Winkel und Distanz in Nick und Roll groessen umrechnen |
410 | // Winkel und Distanz in Nick und Roll groessen umrechnen |
| Line -... | Line 411... | ||
| - | 411 | GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
|
| - | 412 | GPS_Nick = (int) -((dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
|
| - | 413 | ||
| - | 414 | #define GPS_V 8 |
|
| - | 415 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
|
| - | 416 | else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
|
| - | 417 | if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |
|
| - | 418 | else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V); |
|
| - | 419 | ||
| - | 420 | //Kleine Werte verstaerken, Grosse abschwaechen |
|
| - | 421 | long int nick,roll; |
|
| - | 422 | roll = (((long) GPS_Roll) * ((long)sin_i(abs((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000; |
|
| - | 423 | GPS_Roll = (int) (roll / GPS_V); |
|
| - | 424 | nick = (((long) GPS_Nick) * ((long)sin_i(abs((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000; |
|
| - | 425 | GPS_Nick = (int) (nick / GPS_V); |
|
| - | 426 | ||
| - | 427 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
|
| - | 428 | { |
|
| - | 429 | GPS_Roll = 0; |
|
| - | 430 | GPS_Nick = 0; |
|
| - | 431 | gps_state = GPS_CRTL_IDLE; |
|
| - | 432 | return (GPS_STST_ERR); |
|
| 416 | GPS_Roll = (int) +((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*8)); |
433 | } |
| 417 | GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*8)); |
434 | else // Distanz ok // Die Abfrage kann noch rausfliegen, weil vorher bereits begrenzung war |
| 418 | 435 | { |
|
| 419 | if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
436 | if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
| 420 | else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
437 | else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
| - | 438 | if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
|
| 421 | if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
439 | else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
| 422 | else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
440 | return (GPS_STST_OK); |
| 423 | return (GPS_STST_OK); |
441 | } |
| Line 424... | Line 442... | ||
| 424 | } |
442 | } |