Rev 160 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 160 | Rev 161 | ||
|---|---|---|---|
| Line 10... | Line 10... | ||
| 10 | Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
10 | Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
| 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 | Regelung fuer GPS noch nicht implementiert |
15 | Hold Modus |
| 16 | Stand 16.9.2007 |
16 | Stand 20.9.2007 |
| 17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 18 | */ |
18 | */ |
| 19 | #include "main.h" |
19 | #include "main.h" |
| 20 | //#include "gps.h" |
20 | //#include "gps.h" |
| 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 | #define DIST_MAX 100 //neu ab 19.9. 1900 Begrenzung |
|
| - | 350 | if (dist_east > DIST_MAX) dist_east = DIST_MAX; |
|
| - | 351 | if (dist_east <-DIST_MAX) dist_east = -DIST_MAX; |
|
| - | 352 | if (dist_north > DIST_MAX) dist_north = DIST_MAX; |
|
| - | 353 | if (dist_north <-DIST_MAX) dist_north = -DIST_MAX; |
|
| - | 354 | ||
| 349 | //PI Regler |
355 | //PI Regler |
| 350 | gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/16; // Integrator |
356 | gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/64; // Integrator |
| 351 | gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/16; |
357 | gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/64; |
| - | 358 | #define GPSINT_MAX 256 // ************Kleiner machen |
|
| 352 | if ((gps_int_x >= 4096) || (gps_int_y >= 4096) || (gps_int_x < - 4096) || (gps_int_y <= -4096)) |
359 | if ((gps_int_x >= GPSINT_MAX) || (gps_int_y >= GPSINT_MAX) || (gps_int_x < -GPSINT_MAX) || (gps_int_y <= -GPSINT_MAX)) |
| 353 | { |
360 | { |
| 354 | gps_int_x -= (dist_east * Parameter_UserParam1)/16; // Integrator stoppen |
361 | gps_int_x -= (dist_east * Parameter_UserParam1)/64; // Integrator stoppen |
| 355 | gps_int_y -= (dist_north * Parameter_UserParam1)/16; |
362 | gps_int_y -= (dist_north * Parameter_UserParam1)/64; |
| 356 | } |
363 | } |
| 357 | gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/16; // P Anteil dazu |
364 | gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/8; // P Anteil dazu |
| 358 | gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/16; |
365 | gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/8; |
| Line 359... | Line 366... | ||
| 359 | 366 | ||
| 360 | //Richtung bezogen auf Nordpol bestimmen |
367 | //Richtung bezogen auf Nordpol bestimmen |
| Line 361... | Line 368... | ||
| 361 | GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
368 | GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
| 362 | 369 | ||
| 363 | //in Winkel 0..360 grad umrechnen |
370 | //in Winkel 0..360 grad umrechnen |
| - | 371 | if ((gps_reg_x >= 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); |
372 | else if ((gps_reg_x < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
| 365 | else if ((gps_reg_x < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
373 | |
| 366 | // Relative Richtung in bezug auf Nordachse des Kopters errechen |
374 | // Relative Richtung in bezug auf Nordachse des Kopters errechen |
| 367 | n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
375 | n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
| 368 | GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
376 | GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
| Line 373... | Line 381... | ||
| 373 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
381 | // Regelabweichung aus x,y zu Ziel in Distanz umrechnen |
| Line 374... | Line 382... | ||
| 374 | 382 | ||
| 375 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
383 | if (abs(gps_reg_x) > abs(gps_reg_y) ) |
| 376 | { |
384 | { |
| 377 | dist = (long)gps_reg_x; //Groesseren Wert wegen besserer genauigkeit nehmen |
385 | dist = (long)gps_reg_x; //Groesseren Wert wegen besserer genauigkeit nehmen |
| 378 | dist = abs((dist *1000) / (long) cos_i(GPS_hdng_rel_2trgt)); |
386 | dist = abs((dist *1000) / (long) cos_i(GPS_hdng_abs_2trgt)); |
| 379 | } |
387 | } |
| 380 | else |
388 | else |
| 381 | { |
389 | { |
| 382 | dist = (long)gps_reg_y; |
390 | dist = (long)gps_reg_y; |
| 383 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_rel_2trgt)); |
391 | dist = abs((dist *1000) / (long) sin_i(GPS_hdng_abs_2trgt)); |
| 384 | } |
392 | } |
| 385 | if (dist > 30000) dist = 30000; |
393 | if (dist > 200) dist = 200; |
| Line 386... | Line 394... | ||
| 386 | GPS_dist_2trgt = (int )dist; |
394 | GPS_dist_2trgt = (int )dist; |
| 387 | 395 | ||
| 388 | // Winkel und Distanz in Nick und Roll groessen umrechnen |
396 | // Winkel und Distanz in Nick und Roll groessen umrechnen |
| 389 | long int a,b; |
397 | long int a,b; |
| Line 390... | Line 398... | ||
| 390 | GPS_Roll = (int) -((dist * sin_i(GPS_hdng_rel_2trgt))/(1000*4)); |
398 | 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)); |
399 | GPS_Nick = (int) -((dist * cos_i(GPS_hdng_rel_2trgt))/(1000*4)); |
| 392 | 400 | ||
| 393 | if (GPS_Roll > GPS_ROLL_MAX) GPS_Roll = GPS_ROLL_MAX; //Auf Maxwerte begrenzen |
401 | if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen |
| Line 394... | Line 402... | ||
| 394 | else if (GPS_Roll < -GPS_ROLL_MAX) GPS_Roll = - GPS_ROLL_MAX; |
402 | else if (GPS_Roll < -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX; |
| 395 | if (GPS_Nick > GPS_NICK_MAX) GPS_Nick = GPS_NICK_MAX; |
403 | if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX; |
| 396 | else if (GPS_Nick < -GPS_NICK_MAX) GPS_Nick = - GPS_NICK_MAX; |
404 | else if (GPS_Nick < - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX; |
| 397 | 405 | ||