Rev 628 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 628 | Rev 629 | ||
---|---|---|---|
Line 10... | Line 10... | ||
10 | If not, see <http://www.gnu.org/licenses/>. |
10 | If not, see <http://www.gnu.org/licenses/>. |
Line 11... | Line 11... | ||
11 | 11 | ||
12 | 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 |
12 | 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 |
13 | */ |
13 | */ |
14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
14 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
15 | Peter Muehlenbrock |
15 | von Peter Muehlenbrock alias Salvo |
16 | Auswertung der Daten vom GPS im ublox Format |
16 | Auswertung der Daten vom GPS im ublox Format |
17 | Hold Modus mit PID Regler |
17 | Hold Modus mit PID Regler |
18 | Rückstuerz zur Basis Funktion |
18 | Rückstuerz zur Basis Funktion |
Line 19... | Line 19... | ||
19 | Stand 8.1.2008 |
19 | Stand 9.1.2008 |
20 | 20 | ||
21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
21 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
22 | */ |
22 | */ |
Line 75... | Line 75... | ||
75 | 75 | ||
76 | GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
76 | GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
77 | GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
77 | GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
78 | GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
78 | GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
79 | GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
79 | GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
Line 80... | Line 80... | ||
80 | static GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
80 | GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
81 | 81 | ||
82 | // Initialisierung |
82 | // Initialisierung |
83 | void GPS_Neutral(void) |
83 | void GPS_Neutral(void) |
Line 152... | Line 152... | ||
152 | { |
152 | { |
153 | actual_status.status = 0; |
153 | actual_status.status = 0; |
154 | gps_act_position.utm_east = actual_pos.utm_east/10; |
154 | gps_act_position.utm_east = actual_pos.utm_east/10; |
155 | gps_act_position.utm_north = actual_pos.utm_north/10; |
155 | gps_act_position.utm_north = actual_pos.utm_north/10; |
156 | gps_act_position.utm_alt = actual_pos.utm_alt/10; |
156 | gps_act_position.utm_alt = actual_pos.utm_alt/10; |
157 | actual_pos.status = 0; //neue ublox Messages anfordern |
157 | actual_pos.status = 0; //neue ublox Messages anfordern |
158 | gps_act_position.speed_gnd = actual_speed.speed_gnd; |
158 | gps_act_position.speed_gnd = actual_speed.speed_gnd; |
159 | gps_act_position.speed_gnd = actual_speed.speed_gnd; |
159 | gps_act_position.speed_gnd = actual_speed.speed_gnd; |
160 | gps_act_position.heading = actual_speed.heading/100000; |
160 | gps_act_position.heading = actual_speed.heading/100000; |
161 | actual_speed.status = 0; |
161 | actual_speed.status = 0; |
162 | gps_act_position.status = 1; |
162 | gps_act_position.status = 1; |
163 | n = 0; //Daten gueltig |
163 | n = 0; //Daten gueltig |
164 | } |
164 | } |
165 | else |
165 | else |
166 | { |
166 | { |
167 | gps_act_position.status = 0; //Keine gueltigen Daten |
167 | gps_act_position.status = 0; //Keine gueltigen Daten |
168 | actual_speed.status = 0; |
168 | actual_speed.status = 0; |
169 | actual_status.status = 0; |
169 | actual_status.status = 0; |
170 | actual_pos.status = 0; //neue ublox Messages anfordern |
170 | actual_pos.status = 0; //neue ublox Messages anfordern |
171 | n = 2; |
171 | n = 2; |
172 | } |
172 | } |
173 | } |
173 | } |
174 | return (n); |
174 | return (n); |
175 | } |
175 | } |
Line 209... | Line 209... | ||
209 | if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden |
209 | if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden |
210 | else |
210 | else |
211 | { |
211 | { |
212 | ptr_payload_data = &actual_pos; |
212 | ptr_payload_data = &actual_pos; |
213 | ptr_payload_data_end = &actual_pos.status; |
213 | ptr_payload_data_end = &actual_pos.status; |
214 | ublox_msg_state = UBLOX_LEN1; |
214 | ublox_msg_state = UBLOX_LEN1; |
215 | } |
215 | } |
216 | break; |
216 | break; |
Line 217... | Line 217... | ||
217 | 217 | ||
218 | case UBLOX_NAV_STATUS: |
218 | case UBLOX_NAV_STATUS: |
Line 313... | Line 313... | ||
313 | 313 | ||
314 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
314 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
315 | if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE)) |
315 | if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE)) |
316 | { |
316 | { |
317 | cnt++; |
317 | cnt++; |
318 | if (cnt > 200) // erst nach Verzoegerung |
318 | if (cnt > 50) // erst nach Verzoegerung |
319 | { |
319 | { |
320 | // Erst mal initialisieren |
320 | // Erst mal initialisieren |
321 | cnt = 0; |
321 | cnt = 0; |
322 | gps_tick = 0; |
322 | gps_tick = 0; |
323 | hold_fast = 0; |
323 | hold_fast = 0; |
324 | hold_reset_int = 0; // Integrator enablen |
324 | hold_reset_int = 0; // Integrator enablen |
325 | int_east = 0, int_north = 0; |
325 | int_east = 0, int_north = 0; |
Line 347... | Line 347... | ||
347 | return (GPS_STST_OK); |
347 | return (GPS_STST_OK); |
348 | } |
348 | } |
349 | else |
349 | else |
350 | { |
350 | { |
351 | gps_rel_start_position.status = 0; //Keine Daten verfuegbar |
351 | gps_rel_start_position.status = 0; //Keine Daten verfuegbar |
352 | gps_state = GPS_CRTL_IDLE; |
352 | gps_state = GPS_CRTL_IDLE; |
353 | return(GPS_STST_ERR); // Keine Daten da |
353 | return(GPS_STST_ERR); // Keine Daten da |
354 | } |
354 | } |
355 | } |
355 | } |
356 | else return(GPS_STST_PEND); // noch warten |
356 | else return(GPS_STST_PEND); // noch warten |
357 | } |
357 | } |
Line 424... | Line 424... | ||
424 | gps_tick++; |
424 | gps_tick++; |
425 | int d1,d2,d3; |
425 | int d1,d2,d3; |
426 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
426 | d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
427 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
427 | d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
428 | d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel |
428 | d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel |
429 | debug_gp_2 = dist_2home; // zum Debuggen |
429 | debug_gp_2 = dist_2home; // zum Debuggen |
430 | debug_gp_3 = dist_flown; // zum Debuggen |
430 | debug_gp_3 = dist_flown; // zum Debuggen |
431 | debug_gp_4 = hdng_2home; // zum Debuggen |
431 | debug_gp_4 = hdng_2home; // zum Debuggen |
432 | // debug_gp_5 = amplfy_speed_north; // zum Debuggen |
432 | // debug_gp_5 = amplfy_speed_north; // zum Debuggen |
Line 433... | Line 433... | ||
433 | 433 | ||
434 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
434 | if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
435 | { |
435 | { |
436 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
436 | if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
437 | { |
437 | { |
438 | if (gps_g2t_act_v < GPS_G2T_V_MAX-2) gps_g2t_act_v +=2; //Geschwindigkeit langsam erhoehen |
438 | if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit langsam erhoehen |
439 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
439 | dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
440 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
440 | gps_sub_state = GPS_HOME_FAST_IN_TOL; |
441 | } |
441 | } |
442 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
442 | else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
443 | { |
443 | { |
444 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
444 | if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
445 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
445 | // dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
446 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
446 | gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
447 | } |
447 | } |
448 | hold_reset_int = 0; // Integrator einsschalten |
448 | hold_reset_int = 0; // Integrator einsschalten |
449 | hold_fast = 1; // Regler fuer schnellen Flug |
449 | hold_fast = 1; // Regler fuer schnellen Flug |
450 | dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
450 | dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
451 | dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
451 | dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
452 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
452 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
453 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
453 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
454 | } |
454 | } |
455 | else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen |
455 | else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen |
Line 459... | Line 459... | ||
459 | dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit |
459 | dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit |
460 | gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
460 | gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
461 | } |
461 | } |
462 | else |
462 | else |
463 | { |
463 | { |
464 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
464 | dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
465 | gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
465 | gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
466 | } |
466 | } |
467 | hold_reset_int = 0; // Integrator ausschalten |
467 | hold_reset_int = 0; // Integrator einsschalten |
468 | hold_fast = 1; // Wieder normal regeln |
468 | hold_fast = 1; // Regler fuer schnellen Flug |
469 | dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
469 | dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
470 | dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
470 | dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
471 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
471 | gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
472 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
472 | gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
473 | } |
473 | } |
474 | else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
474 | else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
475 | { |
475 | { |
476 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln |
476 | if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln |
477 | { |
477 | { |
478 | gps_sub_state = GPS_HOME_IN_TOL; |
478 | gps_sub_state = GPS_HOME_IN_TOL; |
479 | hold_fast = 0; // Wieder normal regeln |
479 | hold_fast = 0; // Wieder normal regeln |
480 | hold_reset_int = 0; // Integrator wieder aktivieren |
480 | hold_reset_int = 0; // Integrator einsschalten |
481 | if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN; |
481 | if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN; |
482 | else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN; |
482 | else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN; |
483 | if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN; |
483 | if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN; |
484 | else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
484 | else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
485 | if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
485 | if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
486 | { |
486 | { |
487 | gps_rel_hold_position.utm_east = 0; |
487 | gps_rel_hold_position.utm_east = 0; |
488 | gps_rel_hold_position.utm_north = 0; |
488 | gps_rel_hold_position.utm_north = 0; |
489 | gps_sub_state = GPS_HOME_FINISHED; |
489 | gps_sub_state = GPS_HOME_FINISHED; |
490 | } |
490 | } |
491 | } |
491 | } |
492 | else gps_sub_state = GPS_HOME_OUTOF_TOL; |
492 | else gps_sub_state = GPS_HOME_OUTOF_TOL; |
493 | } |
493 | } |
494 | } |
494 | } |
495 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
495 | gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
496 | return (GPS_STST_OK); |
496 | return (GPS_STST_OK); |
497 | } |
497 | } |
498 | else // Keine GPS Daten verfuegbar, deswegen Abbruch |
498 | else // Keine GPS Daten verfuegbar, deswegen Abbruch |
499 | { |
499 | { |
500 | gps_state = GPS_CRTL_IDLE; |
500 | gps_state = GPS_CRTL_IDLE; |
501 | return (GPS_STST_ERR); |
501 | return (GPS_STST_ERR); |
502 | } |
502 | } |
503 | break; |
503 | break; |
Line 508... | Line 508... | ||
508 | { |
508 | { |
509 | // ab hier wird geregelt |
509 | // ab hier wird geregelt |
510 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
510 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
511 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
511 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
512 | int_east += dist_east; |
512 | int_east += dist_east; |
513 | int_north += dist_north; |
513 | int_north += dist_north; |
514 | speed_east = (int) (-actual_speed.speed_e); |
514 | speed_east = (int) (-actual_speed.speed_e); |
515 | speed_north = (int) (-actual_speed.speed_n); |
515 | speed_north = (int) (-actual_speed.speed_n); |
516 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
516 | gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
Line 517... | Line 517... | ||
517 | 517 | ||
518 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
518 | #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
519 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
519 | if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) |
520 | { |
520 | { |
521 | if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
521 | if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen |
522 | } |
522 | } |
523 | if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
523 | if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren |
524 | { |
524 | { |
525 | int_ovfl_cnt -= 1; |
525 | int_ovfl_cnt -= 1; |
526 | int_east = (int_east*7)/8; // Wert reduzieren |
526 | int_east = (int_east*7)/8; // Wert reduzieren |
527 | int_north = (int_north*7)/8; |
527 | int_north = (int_north*7)/8; |
Line 528... | Line 528... | ||
528 | } |
528 | } |
529 | 529 | ||
530 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
530 | if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
531 | { |
531 | { |
532 | int_east = 0; |
532 | int_east = 0; |
533 | int_north = 0; |
533 | int_north = 0; |
534 | } |
534 | } |
535 | if (hold_fast > 0) //schneller Coming Home Modus |
535 | if (hold_fast > 0) //schneller Coming Home Modus Einfluss des D-Anteils verkleinern |
536 | { |
536 | { |
537 | amplfy_speed_east = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
537 | amplfy_speed_east = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
538 | amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
538 | amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT); |
Line 553... | Line 553... | ||
553 | 553 | ||
554 | #define GPS_SPEED_SCALE 5 |
554 | #define GPS_SPEED_SCALE 5 |
555 | speed_east /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
555 | speed_east /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
Line 556... | Line -... | ||
556 | speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
- | |
557 | 556 | speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren |
|
558 | 557 | ||
559 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
558 | int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
Line 560... | Line 559... | ||
560 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
559 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
Line 610... | Line 609... | ||
610 | if (n1 > (GPS_NICKROLL_MAX * GPS_V)) n1 = (GPS_NICKROLL_MAX * GPS_V); |
609 | if (n1 > (GPS_NICKROLL_MAX * GPS_V)) n1 = (GPS_NICKROLL_MAX * GPS_V); |
611 | else if (n1 < -(GPS_NICKROLL_MAX * GPS_V)) n1 = -(GPS_NICKROLL_MAX * GPS_V); |
610 | else if (n1 < -(GPS_NICKROLL_MAX * GPS_V)) n1 = -(GPS_NICKROLL_MAX * GPS_V); |
612 | n = n/GPS_V; |
611 | n = n/GPS_V; |
613 | n1 = n1/GPS_V; |
612 | n1 = n1/GPS_V; |
614 | //Kleine Werte verstaerken, Grosse abschwaechen |
613 | //Kleine Werte verstaerken, Grosse abschwaechen |
615 | /* n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
614 | /* n = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)); |
616 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
615 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
617 | GPS_Roll = (int) n_l; |
616 | GPS_Roll = (int) n_l; |
618 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
617 | n = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)); |
619 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
618 | n_l = ((long) GPS_NICKROLL_MAX * (long) n)/1000; |
620 | GPS_Nick = (int) n_l;*/ |
619 | GPS_Nick = (int) n_l; |
- | 620 | */ |
|
Line 621... | Line 621... | ||
621 | 621 | ||
622 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
622 | if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
623 | { |
623 | { |
624 | GPS_Roll = 0; |
624 | GPS_Roll = 0; |