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); |