Rev 185 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 185 | Rev 186 | ||
|---|---|---|---|
| Line 284... | Line 284... | ||
| 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 signed 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 | static signed int diff_east,diff_north; // Differenzierer (Differenz zum vorhergehenden x bzw. y Wert) |
| - | 290 | static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
|
| 290 | int n; |
291 | signed int n; |
| 291 | long int dist; |
292 | long signed int dist,d,n_l; |
| 292 | switch (cmd) |
293 | switch (cmd) |
| 293 | { |
294 | { |
| Line 294... | Line 295... | ||
| 294 | 295 | ||
| 295 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
296 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
| Line 306... | Line 307... | ||
| 306 | { |
307 | { |
| 307 | cnt = 0; |
308 | cnt = 0; |
| 308 | // aktuelle positionsdaten abespeichern |
309 | // aktuelle positionsdaten abespeichern |
| 309 | if (gps_rel_act_position.status > 0) |
310 | if (gps_rel_act_position.status > 0) |
| 310 | { |
311 | { |
| 311 | int_east = 0; |
312 | int_east = 0; |
| 312 | int_north = 0; |
313 | int_north = 0; |
| 313 | gps_reg_x = 0; |
314 | gps_reg_x = 0; |
| 314 | gps_reg_y = 0; |
315 | gps_reg_y = 0; |
| 315 | dist_east = 0; |
316 | dist_east = 0; |
| 316 | dist_north = 0; |
317 | dist_north = 0; |
| - | 318 | diff_east_f = 0; |
|
| - | 319 | diff_north_f = 0; |
|
| - | 320 | diff_east = 0; |
|
| - | 321 | diff_north = 0; |
|
| 317 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
322 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
| 318 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
323 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
| 319 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
324 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
| 320 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
325 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
| 321 | return (GPS_STST_OK); |
326 | return (GPS_STST_OK); |
| Line 351... | Line 356... | ||
| 351 | case GPS_CRTL_IDLE: |
356 | case GPS_CRTL_IDLE: |
| 352 | cnt = 0; |
357 | cnt = 0; |
| 353 | return (GPS_STST_OK); |
358 | return (GPS_STST_OK); |
| 354 | break; |
359 | break; |
| Line 355... | Line 360... | ||
| 355 | 360 | ||
| 356 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
361 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
| 357 | if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
362 | if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
| 358 | { |
363 | { |
| 359 | gps_updte_flag = 0; |
364 | gps_updte_flag = 0; |
| 360 | // ab hier wird geregelt |
- | |
| 361 | 365 | // ab hier wird geregelt |
|
| 362 | diff_east = -dist_east; //Alten Wert schon mal abziehen |
366 | diff_east = -dist_east; //Alten Wert fuer Differenzier schon mal abziehen |
| 363 | diff_north = -dist_north; |
367 | diff_north = -dist_north; |
| 364 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
368 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
| 365 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
369 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
| 366 | int_east += dist_east; |
370 | int_east += dist_east; |
| 367 | int_north += dist_north; |
371 | int_north += dist_north; |
| 368 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
372 | diff_east += dist_east; // Differenz zur vorhergehenden East Position |
| Line -... | Line 373... | ||
| - | 373 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
|
| - | 374 | ||
| - | 375 | diff_east_f = ((diff_east_f )/2) + (diff_east /2); //Differenzierer filtern |
|
| 369 | diff_north += dist_north; // Differenz zur vorhergehenden North Position |
376 | diff_north_f = ((diff_north_f)/2) + (diff_north/2); //Differenzierer filtern |
| 370 | 377 | ||
| 371 | #define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung |
378 | #define GPSINT_MAX 2048 //neuer Wert ab 25.9.2007 Begrenzung |
| 372 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
379 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten |
| 373 | { |
380 | { |
| 374 | int_east -= dist_east; |
381 | int_east -= dist_east; |
| - | 382 | int_north -= dist_north; |
|
| - | 383 | } |
|
| - | 384 | // P-Anteil Kleine Werte verstaerken, grosse abschwaechen |
|
| - | 385 | #define GPS_DIST_P_MAX 200 //maximal 20m |
|
| - | 386 | int dist_east_p,dist_north_p; |
|
| - | 387 | dist_east_p = dist_east; |
|
| - | 388 | dist_north_p = dist_north; |
|
| - | 389 | if (dist_east > GPS_DIST_P_MAX) dist_east_p = GPS_DIST_P_MAX; // P-Anteil begrenzen |
|
| - | 390 | else if (dist_east < - GPS_DIST_P_MAX) dist_east_p = -GPS_DIST_P_MAX; |
|
| - | 391 | if (dist_north > GPS_DIST_P_MAX) dist_north_p = GPS_DIST_P_MAX; // P-Anteil begrenzen |
|
| - | 392 | else if (dist_north < - GPS_DIST_P_MAX) dist_north_p = -GPS_DIST_P_MAX; |
|
| - | 393 | ||
| - | 394 | n = sin_i((dist_east_p*90)/(GPS_DIST_P_MAX)); |
|
| - | 395 | d = (GPS_DIST_P_MAX * (long)n) /1000; |
|
| - | 396 | dist_east_p = (int) d; |
|
| - | 397 | n = sin_i((dist_north_p*90)/(GPS_DIST_P_MAX)); |
|
| - | 398 | d = (GPS_DIST_P_MAX * (long)n) /1000; |
|
| - | 399 | dist_north_p = (int) d; |
|
| - | 400 | ||
| - | 401 | //PID Regler Werte aufsummieren |
|
| Line 375... | Line -... | ||
| 375 | int_north -= dist_north; |
- | |
| 376 | } |
- | |
| 377 | - | ||
| 378 | //PID Regler |
- | |
| 379 | gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * Parameter_UserParam3)/2); // |
402 | gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east_p * Parameter_UserParam1)/6)+ ((diff_east_f * Parameter_UserParam3)/2); // I + P +D Anteil X 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 |
403 | gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north_p * Parameter_UserParam1)/6)+ ((diff_north_f * Parameter_UserParam3)/2); // I + P +D Anteil Y Achse |
| Line 381... | Line 404... | ||
| 381 | 404 | ||
| 382 | //Richtung bezogen auf Nordpol bestimmen |
405 | //Ziel-Richtung bezogen auf Nordpol bestimmen |
| 383 | GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
406 | GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
| Line 384... | Line 407... | ||
| 384 | 407 | ||
| 385 | //in Winkel 0..360 grad umrechnen |
408 | // in Winkel 0...360 Grad umrechnen |
| Line 402... | Line 425... | ||
| 402 | else |
425 | else |
| 403 | { |
426 | { |
| 404 | dist = (long)gps_reg_y; |
427 | dist = (long)gps_reg_y; |
| 405 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
428 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| 406 | } |
429 | } |
| 407 | // if (dist > 200) dist = 200; |
- | |
| 408 | // GPS_dist_2trgt = (int )dist; |
- | |
| Line 409... | Line 430... | ||
| 409 | 430 | ||
| 410 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
431 | // Winkel und Distanz in Nick und Rollgroessen umrechnen |
| 411 | GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
432 | GPS_Roll = (int) +( (dist * (long) sin_i(GPS_hdng_rel_2trgt))/1000); |
| Line 412... | Line 433... | ||
| 412 | GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
433 | GPS_Nick = (int) -( (dist * (long) cos_i(GPS_hdng_rel_2trgt))/1000); |
| 413 | 434 | ||
| 414 | #define GPS_V 8 |
435 | #define GPS_V 8 |
| 415 | if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V); |
436 | 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); |
437 | else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V); |
| Line 417... | Line 438... | ||
| 417 | if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V); |
438 | 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 |
439 | else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V); |
| - | 440 | ||
| - | 441 | //Kleine Werte verstaerken, Grosse abschwaechen |
|
| 421 | long int nick,roll; |
442 | n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| 422 | int n,r; |
- | |
| 423 | r = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
443 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| 424 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
- | |
| 425 | roll = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) r)/1000; |
444 | GPS_Roll = (int) n_l; |
| Line 426... | Line 445... | ||
| 426 | nick = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) n)/1000; |
445 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
| 427 | GPS_Roll = (int) (roll / GPS_V); |
446 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
| 428 | GPS_Nick = (int) (nick / GPS_V); |
447 | GPS_Nick = (int) n_l; |
| 429 | 448 | ||
| 430 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
449 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
| 431 | { |
450 | { |
| - | 451 | GPS_Roll = 0; |
|
| 432 | GPS_Roll = 0; |
452 | GPS_Nick = 0; |
| 433 | GPS_Nick = 0; |
- | |
| 434 | gps_state = GPS_CRTL_IDLE; |
- | |
| 435 | return (GPS_STST_ERR); |
- | |
| 436 | } |
- | |
| 437 | /* else // Distanz ok // kann spaeter entfallen, weil eigentlich schon begrenzt |
- | |
| 438 | { |
- | |
| 439 | if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
- | |
| 440 | else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
- | |
| 441 | if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
453 | gps_state = GPS_CRTL_IDLE; |
| 442 | else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
454 | return (GPS_STST_ERR); |
| 443 | return (GPS_STST_OK); |
455 | break; |
| Line 444... | Line 456... | ||
| 444 | }*/ |
456 | } |