Rev 108 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 108 | Rev 143 | ||
|---|---|---|---|
| Line -... | Line 1... | ||
| - | 1 | /* |
|
| - | 2 | This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify |
|
| - | 3 | it under the terms of the GNU General Public License as published by the Free Software Foundation; |
|
| - | 4 | either version 3 of the License, or (at your option) any later version. |
|
| - | 5 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
|
| - | 6 | without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
| - | 7 | GNU General Public License for more details. You should have received a copy of the GNU General Public License |
|
| - | 8 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
| - | 9 | ||
| - | 10 | Please note: All the other files for the project "Mikrokopter" by H.Buss are under the licencse published by www.mikrokopter.de |
|
| - | 11 | */ |
|
| 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 13 | Peter Muehlenbrock |
|
| 2 | // + Copyright (c) 04.2007 Holger Buss |
14 | Auswertung der Daten vom GPS im ublox Format |
| 3 | // + only for non-profit use |
15 | Regelung fuer GPS noch nicht implementiert |
| 4 | // + www.MikroKopter.com |
16 | Stand 10.9.2007 |
| 5 | // + see the File "License.txt" for further Informations |
- | |
| 6 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
17 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 18 | */ |
|
| 7 | #include "main.h" |
19 | #include "main.h" |
| Line -... | Line 20... | ||
| - | 20 | ||
| - | 21 | // Defines fuer ublox Messageformat um Auswertung zu steuern |
|
| - | 22 | #define UBLOX_IDLE 0 |
|
| - | 23 | #define UBLOX_SYNC1 1 |
|
| - | 24 | #define UBLOX_SYNC2 2 |
|
| - | 25 | #define UBLOX_CLASS 3 |
|
| - | 26 | #define UBLOX_ID 4 |
|
| - | 27 | #define UBLOX_LEN1 5 |
|
| - | 28 | #define UBLOX_LEN2 6 |
|
| - | 29 | #define UBLOX_CKA 7 |
|
| - | 30 | #define UBLOX_CKB 8 |
|
| - | 31 | #define UBLOX_PAYLOAD 9 |
|
| - | 32 | ||
| - | 33 | // ublox Protokoll Identifier |
|
| - | 34 | #define UBLOX_NAV_POSUTM 0x08 |
|
| - | 35 | #define UBLOX_NAV_STATUS 0x03 |
|
| - | 36 | #define UBLOX_NAV_VELED 0x12 |
|
| - | 37 | #define UBLOX_NAV_CLASS 0x01 |
|
| - | 38 | #define UBLOX_SYNCH1_CHAR 0xB5 |
|
| - | 39 | #define UBLOX_SYNCH2_CHAR 0x62 |
|
| 8 | 40 | ||
| 9 | signed int GPS_Nick = 0; |
41 | signed int GPS_Nick = 0; |
| - | 42 | signed int GPS_Roll = 0; |
|
| 10 | signed int GPS_Roll = 0; |
43 | static uint8_t ublox_msg_state = UBLOX_IDLE; |
| 11 | long GpsAktuell_X = 0; |
44 | static uint8_t chk_a =0; //Checksum |
| - | 45 | static uint8_t chk_b =0; |
|
| 12 | long GpsAktuell_Y = 0; |
46 | |
| - | 47 | static unsigned int rx_len; |
|
| - | 48 | unsigned int cnt0,cnt1,cnt2; //******Provisorisch |
|
| - | 49 | static unsigned int ptr_payload_data_end; |
|
| - | 50 | ||
| 13 | long GpsZiel_X = 0; |
51 | static uint8_t *ptr_payload_data; |
| - | 52 | static uint8_t *ptr_pac_status; |
|
| - | 53 | ||
| - | 54 | ||
| - | 55 | NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt |
|
| - | 56 | NAV_STATUS_t actual_status; // Aktueller Nav Status |
|
| - | 57 | NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten |
|
| - | 58 | ||
| 14 | long GpsZiel_Y = 0; |
59 | GPS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
| 15 | 60 | ||
| 16 | void GPS_Neutral(void) |
61 | void GPS_Neutral(void) // Initialisierung |
| - | 62 | { |
|
| - | 63 | ublox_msg_state = UBLOX_IDLE; |
|
| 17 | { |
64 | actual_pos.status= 0; |
| 18 | GpsZiel_X = GpsAktuell_X; |
65 | actual_speed.status= 0; |
| 19 | GpsZiel_Y = GpsAktuell_Y; |
66 | actual_status.status= 0; |
| Line 20... | Line 67... | ||
| 20 | } |
67 | } |
| 21 | 68 | ||
| - | 69 | void Get_GPS_data(void) // Daten aus aktuellen ublox Messages extrahieren |
|
| - | 70 | { |
|
| - | 71 | if (actual_pos.status == 0) return; //damit es schnell geht, wenn nix zu tun ist |
|
| - | 72 | if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0)) |
|
| - | 73 | { |
|
| - | 74 | cnt1++; //**** noch Rausschmeissen |
|
| - | 75 | if ((actual_status.gpsfix_type & 0x0f) >=2) // nur wenn Daten aktuell sind |
|
| - | 76 | { |
|
| - | 77 | gps_act_position.utm_east = actual_pos.utm_east/10; |
|
| - | 78 | gps_act_position.utm_north = actual_pos.utm_north/10; |
|
| - | 79 | gps_act_position.utm_alt = actual_pos.utm_alt/10; |
|
| - | 80 | gps_act_position.speed_gnd = actual_speed.speed_gnd/10; |
|
| - | 81 | gps_act_position.speed_gnd = actual_speed.speed_gnd/10; |
|
| - | 82 | gps_act_position.heading = actual_speed.heading/100000; |
|
| 22 | void GPS_BerechneZielrichtung(void) |
83 | gps_act_position.status = 1; |
| - | 84 | } |
|
| 23 | { |
85 | actual_pos.status = 0; |
| - | 86 | actual_status.status = 0; |
|
| 24 | GPS_Nick = 0; |
87 | actual_speed.status = 0; |
| Line -... | Line 88... | ||
| - | 88 | } |
|
| - | 89 | } |
|
| - | 90 | ||
| - | 91 | ||
| - | 92 | /* |
|
| - | 93 | Daten vom GPS im ublox MSG Format auswerten |
|
| - | 94 | Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen |
|
| Line -... | Line 95... | ||
| - | 95 | // Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein |
|
| - | 96 | */ |
|
| - | 97 | void Get_Ublox_Msg(uint8_t rx) |
|
| - | 98 | { |
|
| - | 99 | ||
| - | 100 | switch (ublox_msg_state) |
|
| - | 101 | { |
|
| - | 102 | ||
| - | 103 | case UBLOX_IDLE: // Zuerst Synchcharacters pruefen |
|
| - | 104 | if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1; |
|
| - | 105 | else ublox_msg_state = UBLOX_IDLE; |
|
| - | 106 | break; |
|
| - | 107 | ||
| - | 108 | case UBLOX_SYNC1: |
|
| - | 109 | ||
| - | 110 | if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2; |
|
| - | 111 | else ublox_msg_state = UBLOX_IDLE; |
|
| - | 112 | chk_a = 0,chk_b = 0; |
|
| - | 113 | break; |
|
| - | 114 | ||
| - | 115 | case UBLOX_SYNC2: |
|
| - | 116 | if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS; |
|
| - | 117 | else ublox_msg_state = UBLOX_IDLE; |
|
| - | 118 | break; |
|
| - | 119 | ||
| - | 120 | case UBLOX_CLASS: // Nur NAV Meldungen auswerten |
|
| - | 121 | switch (rx) |
|
| - | 122 | { |
|
| - | 123 | case UBLOX_NAV_POSUTM: |
|
| - | 124 | ptr_pac_status = &actual_pos.status; |
|
| - | 125 | if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden |
|
| - | 126 | else |
|
| - | 127 | { |
|
| - | 128 | ptr_payload_data = &actual_pos; |
|
| - | 129 | ptr_payload_data_end = &actual_pos.status; |
|
| - | 130 | ublox_msg_state = UBLOX_LEN1; |
|
| - | 131 | } |
|
| - | 132 | break; |
|
| - | 133 | ||
| - | 134 | case UBLOX_NAV_STATUS: |
|
| - | 135 | ptr_pac_status = &actual_status.status; |
|
| - | 136 | if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; |
|
| - | 137 | else |
|
| - | 138 | { |
|
| - | 139 | ptr_payload_data = &actual_status; |
|
| - | 140 | ptr_payload_data_end = &actual_status.status; |
|
| - | 141 | ublox_msg_state = UBLOX_LEN1; |
|
| - | 142 | } |
|
| - | 143 | break; |
|
| - | 144 | ||
| - | 145 | case UBLOX_NAV_VELED: |
|
| - | 146 | ptr_pac_status = &actual_speed.status; |
|
| - | 147 | if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; |
|
| - | 148 | else |
|
| - | 149 | { |
|
| - | 150 | ptr_payload_data = &actual_speed; |
|
| - | 151 | ptr_payload_data_end = &actual_speed.status; |
|
| - | 152 | ublox_msg_state = UBLOX_LEN1; |
|
| - | 153 | } |
|
| - | 154 | break; |
|
| - | 155 | ||
| - | 156 | default: |
|
| - | 157 | ublox_msg_state = UBLOX_IDLE; |
|
| - | 158 | break; |
|
| - | 159 | } |
|
| - | 160 | chk_a = UBLOX_NAV_CLASS + rx; |
|
| - | 161 | chk_b = UBLOX_NAV_CLASS + chk_a; |
|
| - | 162 | break; |
|
| - | 163 | ||
| - | 164 | case UBLOX_LEN1: // Laenge auswerten |
|
| - | 165 | rx_len = rx; |
|
| - | 166 | chk_a += rx; |
|
| - | 167 | chk_b += chk_a; |
|
| - | 168 | ublox_msg_state = UBLOX_LEN2; |
|
| - | 169 | break; |
|
| - | 170 | ||
| - | 171 | ||
| - | 172 | case UBLOX_LEN2: // Laenge auswerten |
|
| - | 173 | rx_len = rx_len + (rx *256); // Laenge ermitteln |
|
| - | 174 | chk_a += rx; |
|
| - | 175 | chk_b += chk_a; |
|
| - | 176 | ublox_msg_state = UBLOX_PAYLOAD; |
|
| - | 177 | break; |
|
| - | 178 | ||
| - | 179 | case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen |
|
| - | 180 | if (rx_len > 0) |
|
| - | 181 | { |
|
| - | 182 | *ptr_payload_data = rx; |
|
| - | 183 | chk_a += rx; |
|
| - | 184 | chk_b += chk_a; |
|
| - | 185 | --rx_len; |
|
| - | 186 | if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end)) |
|
| - | 187 | { |
|
| - | 188 | ptr_payload_data++; |
|
| - | 189 | ublox_msg_state = UBLOX_PAYLOAD; |
|
| - | 190 | } |
|
| - | 191 | else ublox_msg_state = UBLOX_CKA; |
|
| - | 192 | } |
|
| - | 193 | else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler |
|
| - | 194 | break; |
|
| - | 195 | ||
| - | 196 | case UBLOX_CKA: // Checksum pruefen |
|
| - | 197 | if (rx == chk_a) ublox_msg_state = UBLOX_CKB; |
|
| - | 198 | else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler |
|
| - | 199 | break; |
|
| - | 200 | ||
| - | 201 | case UBLOX_CKB: // Checksum pruefen |
|
| - | 202 | if (rx == chk_b) *ptr_pac_status = 1; // Paket ok |
|
| - | 203 | ublox_msg_state = UBLOX_IDLE; |
|
| - | 204 | break; |
|
| - | 205 | ||