Rev 157 | Go to most recent revision | 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 | |