Rev 158 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 158 | Rev 159 | ||
|---|---|---|---|
| Line 47... | Line 47... | ||
| 47 | short int gps_state; |
47 | short int gps_state; |
| 48 | short int gps_updte_flag; |
48 | short int gps_updte_flag; |
| 49 | signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
49 | signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol |
| 50 | signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
50 | signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
| 51 | signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
51 | signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
| - | 52 | signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y; |
|
| 52 | 53 | signed int ; |
|
| 53 | static unsigned int rx_len; |
54 | static unsigned int rx_len; |
| 54 | unsigned int cnt0,cnt1,cnt2; //******Provisorisch |
55 | unsigned int cnt0,cnt1,cnt2; //******Provisorisch |
| 55 | static unsigned int ptr_payload_data_end; |
56 | static unsigned int ptr_payload_data_end; |
| Line 56... | Line 57... | ||
| 56 | 57 | ||
| Line 81... | Line 82... | ||
| 81 | gps_act_position.status = 0; |
82 | gps_act_position.status = 0; |
| 82 | gps_rel_act_position.status = 0; |
83 | gps_rel_act_position.status = 0; |
| 83 | GPS_Nick = 0; |
84 | GPS_Nick = 0; |
| 84 | GPS_Roll = 0; |
85 | GPS_Roll = 0; |
| 85 | gps_updte_flag = 0; |
86 | gps_updte_flag = 0; |
| - | 87 | gps_int_x = 0; |
|
| - | 88 | gps_int_y = 0; |
|
| - | 89 | ||
| 86 | } |
90 | } |
| Line 87... | Line 91... | ||
| 87 | 91 | ||
| 88 | // Home Position sichern falls Daten verfuegbar sind. |
92 | // Home Position sichern falls Daten verfuegbar sind. |
| 89 | void GPS_Save_Home(void) |
93 | void GPS_Save_Home(void) |
| Line 314... | Line 318... | ||
| 314 | case GPS_CMD_STOP_HOLD: // Lageregelung beenden |
318 | case GPS_CMD_STOP_HOLD: // Lageregelung beenden |
| 315 | cnt = 0; |
319 | cnt = 0; |
| 316 | GPS_Nick = 0; |
320 | GPS_Nick = 0; |
| 317 | GPS_Roll = 0; |
321 | GPS_Roll = 0; |
| 318 | gps_state = GPS_CRTL_IDLE; |
322 | gps_state = GPS_CRTL_IDLE; |
| - | 323 | gps_int_x = 0; |
|
| - | 324 | gps_int_y = 0; |
|
| 319 | return (GPS_STST_OK); |
325 | return (GPS_STST_OK); |
| 320 | break; |
326 | break; |
| Line 321... | Line 327... | ||
| 321 | 327 | ||
| 322 | default: |
328 | default: |
| Line 330... | Line 336... | ||
| 330 | cnt = 0; |
336 | cnt = 0; |
| 331 | return (GPS_STST_OK); |
337 | return (GPS_STST_OK); |
| 332 | break; |
338 | break; |
| Line 333... | Line 339... | ||
| 333 | 339 | ||
| 334 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
340 | case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
| 335 | if (gps_updte_flag = 1) //nur wenn neue GPS Daten vorliegen |
341 | if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen |
| 336 | { |
342 | { |
| 337 | gps_updte_flag = 0; |
343 | gps_updte_flag = 0; |
| 338 | // ab hier wird geregelt |
- | |
| 339 | // Richtung zum Ziel ermitteln |
344 | // ab hier wird geregelt |
| 340 | signed int dist_north,dist_east; |
345 | signed int dist_north,dist_east; |
| 341 | 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; |
|
| - | 348 | ||
| - | 349 | //PI Regler |
|
| - | 350 | gps_int_x = gps_int_x + (dist_east * Parameter_UserParam1)/16; // Integrator |
|
| - | 351 | gps_int_y = gps_int_y + (dist_north * Parameter_UserParam1)/16; |
|
| - | 352 | if ((gps_int_x >= 4096) || (gps_int_y >= 4096) || (gps_int_x < - 4096) || (gps_int_y <= -4096)) |
|
| - | 353 | { |
|
| - | 354 | gps_int_x -= (dist_east * Parameter_UserParam1)/16; // Integrator stoppen |
|
| - | 355 | gps_int_y -= (dist_north * Parameter_UserParam1)/16; |
|
| - | 356 | } |
|
| - | 357 | gps_reg_x = gps_int_x + (dist_east * Parameter_UserParam2)/16; // P Anteil dazu |
|
| - | 358 | gps_reg_y = gps_int_y + (dist_north * Parameter_UserParam2)/16; |
|
| - | 359 | ||
| 342 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
360 | //Richtung bezogen auf Nordpol bestimmen |
| - | 361 | GPS_hdng_abs_2trgt = arctan_i((long)gps_reg_x,(long)gps_reg_y); |
|
| 343 | GPS_hdng_abs_2trgt = arctan_i((long)dist_east,(long)dist_north); |
362 | |
| 344 | //in Winkel 0..360 grad umrechnen |
363 | //in Winkel 0..360 grad umrechnen |
| 345 | if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
364 | if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
| - | 365 | else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
|
| - | 366 | // Relative Richtung in bezug auf Nordachse des Kopters errechen |
|
| - | 367 | int n,m; |
|
| - | 368 | n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
|
| - | 369 | GPS_hdng_rel_2trgt = GPS_hdng_abs_2trgt - n; |
|
| - | 370 | if ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180))GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360; |
|
| - | 371 | else if (GPS_hdng_rel_2trgt >180) GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt; |
|
| 346 | else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
372 | else if (GPS_hdng_rel_2trgt <-180) GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt; |
| 347 | 373 | ||
| 348 | // Distanz zum Ziel ermitteln |
374 | // Distanz zum Ziel ermitteln |
| 349 | GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt |
375 | GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt |
| 350 | return (GPS_STST_OK); |
376 | return (GPS_STST_OK); |
| 351 | } |
377 | } |