Rev 542 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 542 | Rev 565 | ||
---|---|---|---|
Line 117... | Line 117... | ||
117 | short int Get_Rel_Position(void) |
117 | short int Get_Rel_Position(void) |
118 | { |
118 | { |
119 | short int n = 0; |
119 | short int n = 0; |
120 | n = Get_GPS_data(); |
120 | n = Get_GPS_data(); |
121 | if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind |
121 | if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind |
122 | if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert |
122 | if (gps_alive_cnt < 1000) gps_alive_cnt += 600; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert |
123 | if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt |
123 | if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt |
124 | { |
124 | { |
125 | gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east); |
125 | gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east); |
126 | gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north); |
126 | gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north); |
127 | gps_rel_act_position.utm_alt = (int) (gps_act_position.utm_alt - gps_home_position.utm_alt); |
127 | gps_rel_act_position.utm_alt = (int) (gps_act_position.utm_alt - gps_home_position.utm_alt); |
Line 146... | Line 146... | ||
146 | if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist |
146 | if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist |
147 | if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0)) |
147 | if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0)) |
148 | { |
148 | { |
149 | if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind |
149 | if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind |
150 | { |
150 | { |
- | 151 | actual_status.status = 0; |
|
151 | gps_act_position.utm_east = actual_pos.utm_east/10; |
152 | gps_act_position.utm_east = actual_pos.utm_east/10; |
152 | gps_act_position.utm_north = actual_pos.utm_north/10; |
153 | gps_act_position.utm_north = actual_pos.utm_north/10; |
153 | gps_act_position.utm_alt = actual_pos.utm_alt/10; |
154 | gps_act_position.utm_alt = actual_pos.utm_alt/10; |
- | 155 | actual_pos.status = 0; //neue ublox Messages anfordern |
|
154 | gps_act_position.speed_gnd = actual_speed.speed_gnd/10; |
156 | // gps_act_position.speed_gnd = actual_speed.speed_gnd/10; |
155 | gps_act_position.speed_gnd = actual_speed.speed_gnd/10; |
157 | // gps_act_position.speed_gnd = actual_speed.speed_gnd/10; |
156 | gps_act_position.heading = actual_speed.heading/100000; |
158 | // gps_act_position.heading = actual_speed.heading/100000; |
- | 159 | actual_speed.status = 0; |
|
157 | gps_act_position.status = 1; |
160 | gps_act_position.status = 1; |
158 | n = 0; //Daten gueltig |
161 | n = 0; //Daten gueltig |
159 | } |
162 | } |
160 | else |
163 | else |
161 | { |
164 | { |
162 | gps_act_position.status = 0; //Keine gueltigen Daten |
165 | gps_act_position.status = 0; //Keine gueltigen Daten |
- | 166 | actual_speed.status = 0; |
|
- | 167 | actual_status.status = 0; |
|
- | 168 | actual_pos.status = 0; //neue ublox Messages anfordern |
|
163 | n = 2; |
169 | n = 2; |
164 | } |
170 | } |
165 | actual_pos.status = 0; //neue ublox Messages anfordern |
- | |
166 | actual_status.status = 0; |
- | |
167 | actual_speed.status = 0; |
- | |
168 | } |
171 | } |
169 | return (n); |
172 | return (n); |
170 | } |
173 | } |
Line 171... | Line 174... | ||
171 | 174 | ||
Line 305... | Line 308... | ||
305 | 308 | ||
306 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
309 | case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
307 | if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE)) |
310 | if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE)) |
308 | { |
311 | { |
309 | cnt++; |
312 | cnt++; |
310 | if (cnt > 500) // erst nach Verzoegerung |
313 | if (cnt > 200) // erst nach Verzoegerung |
311 | { |
314 | { |
312 | // Erst mal initialisieren |
315 | // Erst mal initialisieren |
313 | cnt = 0; |
316 | cnt = 0; |
314 | gps_tick = 0; |
317 | gps_tick = 0; |
Line 354... | Line 357... | ||
354 | 357 | ||
355 | case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
358 | case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
356 | if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
359 | if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
357 | { |
360 | { |
358 | cnt++; |
361 | cnt++; |
359 | if (cnt > 500) // erst nach Verzoegerung |
362 | if (cnt > 400) // erst nach Verzoegerung |
360 | { |
363 | { |
361 | cnt = 0; |
364 | cnt = 0; |
362 | // aktuelle positionsdaten abspeichern |
365 | // aktuelle positionsdaten abspeichern |
363 | if (gps_rel_act_position.status > 0) |
366 | if (gps_rel_act_position.status > 0) |
Line 552... | Line 555... | ||
552 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
555 | if (hold_fast > 0) diff_p = GPS_PROP_FAST_V; |
553 | else diff_p = GPS_PROP_NRML_V; |
556 | else diff_p = GPS_PROP_NRML_V; |
Line 554... | Line 557... | ||
554 | 557 | ||
555 | //I Werte begrenzen |
558 | //I Werte begrenzen |
556 | #define INT1_MAX (20 * GPS_V) |
559 | #define INT1_MAX (20 * GPS_V) |
557 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
560 | int_east1 = ((((long)int_east) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
558 | int_north = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; |
561 | int_north1 = ((((long)int_north) * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT; //Fehler behoben am 17.12.2007 vorher int_north= |
559 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
562 | if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
560 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
563 | else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
561 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
564 | if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |