Rev 157 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 157 | Rev 158 | ||
---|---|---|---|
Line 11... | Line 11... | ||
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 | Regelung fuer GPS noch nicht implementiert |
16 | Stand 15.9.2007 |
16 | Stand 16.9.2007 |
17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
18 | */ |
18 | */ |
19 | #include "main.h" |
19 | #include "main.h" |
20 | //#include "gps.h" |
20 | //#include "gps.h" |
Line 39... | Line 39... | ||
39 | #define UBLOX_SYNCH1_CHAR 0xB5 |
39 | #define UBLOX_SYNCH1_CHAR 0xB5 |
40 | #define UBLOX_SYNCH2_CHAR 0x62 |
40 | #define UBLOX_SYNCH2_CHAR 0x62 |
Line 41... | Line 41... | ||
41 | 41 | ||
42 | signed int GPS_Nick = 0; |
42 | signed int GPS_Nick = 0; |
43 | signed int GPS_Roll = 0; |
43 | signed int GPS_Roll = 0; |
44 | static uint8_t ublox_msg_state = UBLOX_IDLE; |
44 | short int ublox_msg_state = UBLOX_IDLE; |
45 | static uint8_t chk_a =0; //Checksum |
45 | static uint8_t chk_a =0; //Checksum |
- | 46 | static uint8_t chk_b =0; |
|
- | 47 | short int gps_state; |
|
- | 48 | short int gps_updte_flag; |
|
- | 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 |
|
Line 46... | Line 51... | ||
46 | static uint8_t chk_b =0; |
51 | signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
47 | 52 | ||
48 | static unsigned int rx_len; |
53 | static unsigned int rx_len; |
Line 59... | Line 64... | ||
59 | NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten |
64 | NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten |
Line 60... | Line 65... | ||
60 | 65 | ||
61 | GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
66 | GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
62 | GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
67 | GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
- | 68 | GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
|
Line 63... | Line 69... | ||
63 | GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
69 | GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
64 | 70 | ||
65 | // Initialisierung |
71 | // Initialisierung |
- | 72 | void GPS_Neutral(void) |
|
66 | void GPS_Neutral(void) |
73 | { |
- | 74 | short int n; |
|
67 | { |
75 | ublox_msg_state = UBLOX_IDLE; |
68 | ublox_msg_state = UBLOX_IDLE; |
76 | gps_state = GPS_CRTL_IDLE; |
69 | actual_pos.status = 0; |
77 | actual_pos.status = 0; |
70 | actual_speed.status = 0; |
78 | actual_speed.status = 0; |
71 | actual_status.status = 0; |
79 | actual_status.status = 0; |
72 | gps_home_position.status = 0; // Noch keine gueltige Home Position |
80 | gps_home_position.status = 0; // Noch keine gueltige Home Position |
73 | gps_act_position.status = 0; |
- | |
- | 81 | gps_act_position.status = 0; |
|
- | 82 | gps_rel_act_position.status = 0; |
|
- | 83 | GPS_Nick = 0; |
|
74 | gps_rel_act_position.status = 0; |
84 | GPS_Roll = 0; |
Line 75... | Line 85... | ||
75 | 85 | gps_updte_flag = 0; |
|
76 | } |
86 | } |
77 | 87 | ||
Line 101... | Line 111... | ||
101 | { |
111 | { |
102 | gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east); |
112 | gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east); |
103 | gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north); |
113 | gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north); |
104 | gps_rel_act_position.status = 1; // gueltige Positionsdaten |
114 | gps_rel_act_position.status = 1; // gueltige Positionsdaten |
105 | n = 0; |
115 | n = 0; |
- | 116 | gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen. |
|
106 | } |
117 | } |
107 | else |
118 | else |
108 | { |
119 | { |
109 | n = 2; //keine gueltigen Daten vorhanden |
120 | n = 2; //keine gueltigen Daten vorhanden |
110 | gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist. |
121 | gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist. |
Line 261... | Line 272... | ||
261 | break; |
272 | break; |
Line 262... | Line 273... | ||
262 | 273 | ||
263 | default: |
274 | default: |
264 | ublox_msg_state = UBLOX_IDLE; |
275 | ublox_msg_state = UBLOX_IDLE; |
- | 276 | break; |
|
- | 277 | } |
|
- | 278 | } |
|
- | 279 | ||
- | 280 | //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
|
- | 281 | short int GPS_CRTL(short int cmd) |
|
- | 282 | { |
|
- | 283 | static unsigned int cnt; //Zaehler fuer diverse Verzoegerungen |
|
- | 284 | ||
- | 285 | switch (cmd) |
|
- | 286 | { |
|
- | 287 | case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
|
- | 288 | if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
|
- | 289 | { |
|
- | 290 | cnt++; |
|
- | 291 | if (cnt > 300) // erst nach Verzoegerung |
|
- | 292 | { |
|
- | 293 | cnt = 0; |
|
- | 294 | // aktuelle positionsdaten abespeichern |
|
- | 295 | if (gps_rel_act_position.status > 0) |
|
- | 296 | { |
|
- | 297 | gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
|
- | 298 | gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
|
- | 299 | gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
|
- | 300 | gps_state = GPS_CRTL_HOLD_ACTIVE; |
|
- | 301 | return (GPS_STST_OK); |
|
- | 302 | } |
|
- | 303 | else |
|
- | 304 | { |
|
- | 305 | gps_rel_hold_position.status = 0; //Keine Daten verfuegbar |
|
- | 306 | gps_state = GPS_CRTL_IDLE; |
|
- | 307 | return(GPS_STST_ERR); // Keine Daten da |
|
- | 308 | } |
|
- | 309 | } |
|
- | 310 | else return(GPS_STST_PEND); // noch warten |
|
- | 311 | } |
|
- | 312 | break; |
|
- | 313 | ||
- | 314 | case GPS_CMD_STOP_HOLD: // Lageregelung beenden |
|
- | 315 | cnt = 0; |
|
- | 316 | GPS_Nick = 0; |
|
- | 317 | GPS_Roll = 0; |
|
- | 318 | gps_state = GPS_CRTL_IDLE; |
|
- | 319 | return (GPS_STST_OK); |
|
- | 320 | break; |
|
- | 321 | ||
- | 322 | default: |
|
- | 323 | return (GPS_STST_ERR); |
|
- | 324 | break; |
|
- | 325 | } |
|
- | 326 | ||
- | 327 | switch (gps_state) |
|
- | 328 | { |
|
- | 329 | case GPS_CRTL_IDLE: |
|
- | 330 | cnt = 0; |
|
- | 331 | return (GPS_STST_OK); |
|
- | 332 | break; |
|
- | 333 | ||
- | 334 | 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 |
|
- | 336 | { |
|
- | 337 | gps_updte_flag = 0; |
|
- | 338 | // ab hier wird geregelt |
|
- | 339 | // Richtung zum Ziel ermitteln |
|
- | 340 | signed int dist_north,dist_east; |
|
- | 341 | dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
|
- | 342 | dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
|
- | 343 | GPS_hdng_abs_2trgt = arctan_i((long)dist_east,(long)dist_north); |
|
- | 344 | //in Winkel 0..360 grad umrechnen |
|
- | 345 | if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
|
- | 346 | else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
|
- | 347 | ||
- | 348 | // Distanz zum Ziel ermitteln |
|
- | 349 | GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt |
|
- | 350 | return (GPS_STST_OK); |
|
- | 351 | } |
|
- | 352 | else return (GPS_STST_OK); |
|
- | 353 | break; |
|
- | 354 | ||
- | 355 | default: |
|
- | 356 | gps_state = GPS_CRTL_IDLE; |
|
- | 357 | return (GPS_STST_ERR); |
|
265 | break; |
358 | break; |
- | 359 | } |
|
- | 360 | return (GPS_STST_ERR); |
|
266 | } |
361 |