Rev 161 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 161 | Rev 162 | ||
|---|---|---|---|
| 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 |
15 | Hold Modus |
| 16 | Stand 20.9.2007 |
16 | Stand 21.9.2007 |
| 17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 18 | */ |
18 | */ |
| 19 | #include "main.h" |
19 | #include "main.h" |
| 20 | //#include "gps.h" |
20 | //#include "gps.h" |
| 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 | int n; |
288 | int n; |
| 288 | long int dist; |
289 | long int dist; |
| 289 | switch (cmd) |
290 | switch (cmd) |
| - | 291 | { |
|
| - | 292 | ||
| - | 293 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
|
| - | 294 | // Noch einiges zu ueberlegen und zu tun |
|
| - | 295 | return(GPS_STST_PEND); // noch warten |
|
| - | 296 | break; |
|
| - | 297 | // ****************************** |
|
| 290 | { |
298 | |
| 291 | case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
299 | case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
| 292 | if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
300 | if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
| 293 | { |
301 | { |
| 294 | cnt++; |
302 | cnt++; |
| 295 | if (cnt > 500) // erst nach Verzoegerung |
303 | if (cnt > 500) // erst nach Verzoegerung |
| 296 | { |
304 | { |
| 297 | cnt = 0; |
305 | cnt = 0; |
| 298 | // aktuelle positionsdaten abespeichern |
306 | // aktuelle positionsdaten abespeichern |
| 299 | if (gps_rel_act_position.status > 0) |
307 | if (gps_rel_act_position.status > 0) |
| - | 308 | { |
|
| - | 309 | int_east = 0; |
|
| - | 310 | int_north = 0; |
|
| - | 311 | gps_reg_x = 0; |
|
| 300 | { |
312 | gps_reg_y = 0; |
| 301 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
313 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| 302 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
314 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| 303 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
315 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| 304 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
316 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
| Line 313... | Line 325... | ||
| 313 | } |
325 | } |
| 314 | else return(GPS_STST_PEND); // noch warten |
326 | else return(GPS_STST_PEND); // noch warten |
| 315 | } |
327 | } |
| 316 | break; |
328 | break; |
| Line 317... | Line 329... | ||
| 317 | 329 | ||
| 318 | case GPS_CMD_STOP_HOLD: // Lageregelung beenden |
330 | case GPS_CMD_STOP: // Lageregelung beenden |
| 319 | cnt = 0; |
331 | cnt = 0; |
| 320 | GPS_Nick = 0; |
332 | GPS_Nick = 0; |
| 321 | GPS_Roll = 0; |
333 | GPS_Roll = 0; |
| 322 | gps_state = GPS_CRTL_IDLE; |
334 | gps_int_x = 0; |
| 323 | gps_int_x = 0; |
335 | gps_int_y = 0; |
| 324 | gps_int_y = 0; |
336 | gps_state = GPS_CRTL_IDLE; |
| 325 | return (GPS_STST_OK); |
337 | return (GPS_STST_OK); |
| Line 326... | Line 338... | ||
| 326 | break; |
338 | break; |
| 327 | 339 | ||
| Line 343... | Line 355... | ||
| 343 | gps_updte_flag = 0; |
355 | gps_updte_flag = 0; |
| 344 | // ab hier wird geregelt |
356 | // ab hier wird geregelt |
| 345 | signed int dist_north,dist_east; |
357 | signed int dist_north,dist_east; |
| 346 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
358 | 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; |
359 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| - | 360 | int_east += dist_east; |
|
| - | 361 | int_north += dist_north; |
|
| Line -... | Line 362... | ||
| - | 362 | ||
| - | 363 | #define GPSINT_MAX 1000 |
|
| - | 364 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
|
| - | 365 | { |
|
| - | 366 | int_east -= dist_east; |
|
| - | 367 | int_north -= dist_north; |
|
| - | 368 | } |
|
| 348 | 369 | ||
| 349 | #define DIST_MAX 100 //neu ab 19.9. 1900 Begrenzung |
370 | #define DIST_MAX 200 //neu ab 19.9. 1900 Begrenzung |
| 350 | if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
371 | if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
| 351 | if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
372 | if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
| 352 | if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
373 | if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
| Line 353... | Line 374... | ||
| 353 | if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
374 | if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
| 354 | - | ||
| 355 | //PI Regler |
- | |
| 356 | gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/64; // Integrator |
- | |
| 357 | gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/64; |
- | |
| 358 | #define GPSINT_MAX 256 // ************Kleiner machen |
- | |
| 359 | if ((gps_int_x >= GPSINT_MAX) || (gps_int_y >= GPSINT_MAX) || (gps_int_x < -GPSINT_MAX) || (gps_int_y <= -GPSINT_MAX)) |
- | |
| 360 | { |
- | |
| 361 | gps_int_x -= (dist_east * Parameter_UserParam1)/64; // Integrator stoppen |
- | |
| 362 | gps_int_y -= (dist_north * Parameter_UserParam1)/64; |
375 | |
| 363 | } |
376 | //PI Regler |
| Line 364... | Line 377... | ||
| 364 | gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/8; // P Anteil dazu |
377 | gps_reg_x = ((int_east * Parameter_UserParam1)/16) + ((dist_east * Parameter_UserParam2)/8); // P+I Anteil |
| 365 | gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/8; |
378 | gps_reg_y = ((int_north * Parameter_UserParam1)/16) + ((dist_north * Parameter_UserParam2)/8); // P+I Anteil |
| Line 366... | Line 379... | ||
| 366 | 379 | ||
| 367 | //Richtung bezogen auf Nordpol bestimmen |
380 | //Richtung bezogen auf Nordpol bestimmen |
| 368 | GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
381 | GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
| Line 369... | Line 382... | ||
| 369 | 382 | ||
| 370 | //in Winkel 0..360 grad umrechnen |
383 | //in Winkel 0..360 grad umrechnen |
| 371 | if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
384 | if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
| 372 | else if ((gps_reg_x < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
385 | else GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |