Go to most recent revision | Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1734 | - | 1 | /* |
| 2 | * tracking.c |
||
| 3 | * |
||
| 4 | * Created on: 13.02.2012 |
||
| 5 | * Author: cebra |
||
| 6 | */ |
||
| 7 | |||
| 8 | #include "cpu.h" |
||
| 9 | #include <string.h> |
||
| 10 | #include <util/delay.h> |
||
| 11 | #include <avr/interrupt.h> |
||
| 12 | #include <stdlib.h> |
||
| 13 | #include "main.h" |
||
| 14 | #include "tracking.h" |
||
| 15 | #include <avr/pgmspace.h> |
||
| 16 | #include "fifo.h" |
||
| 17 | #include "bluetooth.h" |
||
| 18 | |||
| 19 | #ifdef HWVERSION3_9 |
||
| 20 | |||
| 21 | #define MAX_POWER 10 |
||
| 22 | #define getPower(x) (int32_t)pgm_read_dword(&powers[x]) |
||
| 23 | const int32_t PROGMEM powers[MAX_POWER] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; |
||
| 24 | |||
| 25 | HomePos_t MK_pos; // Home position of station |
||
| 26 | //GPS_Pos_t currentPos; // Current position of flying object |
||
| 27 | int8_t NMEAsatsInUse; // Number of satelites currently in use |
||
| 28 | int32_t NMEAlatitude, NMEAlongitude; |
||
| 29 | char NMEATime[9] = "GP:Ti:me"; |
||
| 30 | //char NMEADate [6]; |
||
| 31 | |||
| 32 | // Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit |
||
| 33 | // i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450 |
||
| 34 | int32_t floatStrToInt(const char *s, int32_t power1) |
||
| 35 | { char *endPtr; |
||
| 36 | int32_t v = strtol(s, &endPtr, 10); |
||
| 37 | |||
| 38 | if (*endPtr == '.') { |
||
| 39 | for (s = endPtr + 1; *s && power1; s++) { |
||
| 40 | v = v * 10 + (*s - '0'); |
||
| 41 | --power1; |
||
| 42 | } |
||
| 43 | } |
||
| 44 | if (power1) { |
||
| 45 | // Table to avoid multiple multiplications |
||
| 46 | v = v * getPower(power1); |
||
| 47 | } |
||
| 48 | return v; |
||
| 49 | } |
||
| 50 | |||
| 51 | // NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps |
||
| 52 | int32_t getLatitude(const char *s, const char *NS) |
||
| 53 | { int32_t deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees |
||
| 54 | int32_t min = floatStrToInt(&s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
||
| 55 | |||
| 56 | deg = deg * 10000000 + min; |
||
| 57 | if (*NS == 'S') deg = -deg; |
||
| 58 | return deg; |
||
| 59 | } |
||
| 60 | |||
| 61 | |||
| 62 | // NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps |
||
| 63 | int32_t getLongitude(const char *s, const char *WE) |
||
| 64 | { int32_t deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees |
||
| 65 | int32_t min = floatStrToInt(&s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps |
||
| 66 | |||
| 67 | deg = deg * 10000000 + min; |
||
| 68 | if (*WE == 'W') deg = -deg; |
||
| 69 | return deg; |
||
| 70 | } |
||
| 71 | |||
| 72 | void getNMEATime( const char *s) |
||
| 73 | { |
||
| 74 | uint8_t sem = 0; |
||
| 75 | uint8_t i; |
||
| 76 | |||
| 77 | for ( i=0;i < 6; i++ ) |
||
| 78 | { |
||
| 79 | NMEATime[sem++] = s[i]; |
||
| 80 | if (i==1 || i==3) NMEATime[sem++] = ':'; |
||
| 81 | |||
| 82 | } |
||
| 83 | NMEATime[sem] = '\0'; |
||
| 84 | } |
||
| 85 | |||
| 86 | |||
| 87 | |||
| 88 | |||
| 89 | //$GPGGA,191410.000,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45 |
||
| 90 | // ^ ^ ^ ^ ^ ^ ^ ^ |
||
| 91 | // | | | | | | | | |
||
| 92 | // | | | | | | | Höhe Geoid minus |
||
| 93 | // | | | | | | | Höhe Ellipsoid (WGS84) |
||
| 94 | // | | | | | | | in Metern (48.0,M) |
||
| 95 | // | | | | | | | |
||
| 96 | // | | | | | | Höhe über Meer (über Geoid)in Metern (351.5,M) |
||
| 97 | // | | | | | | |
||
| 98 | // | | | | | HDOP (horizontal dilution |
||
| 99 | // | | | | | of precision) Genauigkeit |
||
| 100 | // | | | | | |
||
| 101 | // | | | | Anzahl der erfassten Satelliten |
||
| 102 | // | | | | |
||
| 103 | // | | | Qualität der Messung |
||
| 104 | // | | | (0 = ungültig) |
||
| 105 | // | | | (1 = GPS) |
||
| 106 | // | | | (2 = DGPS) |
||
| 107 | // | | | (6 = geschätzt nur NMEA-0183 2.3) |
||
| 108 | // | | | |
||
| 109 | // | | Längengrad |
||
| 110 | // | | |
||
| 111 | // | Breitengrad |
||
| 112 | // | |
||
| 113 | // Uhrzeit |
||
| 114 | |||
| 115 | void Tracking_NMEA(void) |
||
| 116 | { |
||
| 117 | char *token; |
||
| 118 | |||
| 119 | |||
| 120 | if (decodeNMEA()) { |
||
| 121 | token = strtok((char*)data_decode, ","); |
||
| 122 | if (!strcmp(token, "GPGGA")) |
||
| 123 | { |
||
| 124 | // $GPGGA,220613.400,4843.5080,N,00922.9583,E,1,7,2.23,287.1,M,48.0,M,, |
||
| 125 | // Skip time |
||
| 126 | getNMEATime(strtok(0, ".")); //Zeit |
||
| 127 | |||
| 128 | strtok(0, ","); // Skip Rest von der Zeit |
||
| 129 | // Latitude |
||
| 130 | NMEAlatitude = getLatitude(strtok(0, ","), strtok(0, ",")); //N |
||
| 131 | // Longitude |
||
| 132 | NMEAlongitude = getLongitude(strtok(0, ","), strtok(0, ","));//E |
||
| 133 | // Signal valid? (Position Fix Indicator) |
||
| 134 | if (*strtok(0, ",") != '0')// Qualität |
||
| 135 | { |
||
| 136 | // Satellites in use |
||
| 137 | NMEAsatsInUse = atoi(strtok(0, ",")); //Anzahl Sats |
||
| 138 | |||
| 139 | // // Skip dilution |
||
| 140 | // strtok(0, ","); //Dilution |
||
| 141 | // // Altitude |
||
| 142 | // currentPos.Altitude = floatStrToInt(strtok(0, ","), 3); |
||
| 143 | // currentPos.Latitude = latitude; |
||
| 144 | // currentPos.Longitude = longitude; |
||
| 145 | // |
||
| 146 | // if ((coldstart) && (satsInUse > 5)) { |
||
| 147 | // // First position after reboot (or change of mode) will be the home position (facing north) |
||
| 148 | // MK_pos.Home_Lon = (double)currentPos.Longitude / 10000000.0; |
||
| 149 | // MK_pos.Home_Lat = (double)currentPos.Latitude / 10000000.0; |
||
| 150 | // MK_pos.Home_Lon7 = currentPos.Longitude; |
||
| 151 | // MK_pos.Home_Lat7 = currentPos.Latitude; |
||
| 152 | // MK_pos.Home_Alt = currentPos.Altitude; |
||
| 153 | // MK_pos.direction = 0; |
||
| 154 | // coldstart = 0; |
||
| 155 | // Double_Beep(DBEEPNMEAFIX, DBEEPMEAFIXP); |
||
| 156 | } |
||
| 157 | // do_tracking(); |
||
| 158 | // } |
||
| 159 | } |
||
| 160 | } |
||
| 161 | // Displ_GPS(); // letzte empfangene Daten auch bei ausgeschalteter NMEA sichtbar |
||
| 162 | } |
||
| 163 | |||
| 164 | |||
| 165 | uint8_t hexDigitToInt(uint8_t digit) |
||
| 166 | { |
||
| 167 | if (digit >= '0' && digit <= '9') return digit - '0'; |
||
| 168 | if (digit >= 'a' && digit <= 'f') return digit - 'a' + 10; |
||
| 169 | if (digit >= 'A' && digit <= 'F') return digit - 'A' + 10; |
||
| 170 | return 0; |
||
| 171 | } |
||
| 172 | |||
| 173 | |||
| 174 | uint8_t decodeNMEA(void) |
||
| 175 | { |
||
| 176 | uint8_t ret = 0; |
||
| 177 | uint8_t crc; |
||
| 178 | uint8_t tmpCRC = 0; |
||
| 179 | uint8_t i; |
||
| 180 | |||
| 181 | if (rx_ready == 1 && rx_len > 0) { |
||
| 182 | // Calculate checksum |
||
| 183 | for (i = 1; i < rx_len && rx_buffer[i] != '*'; i++) { |
||
| 184 | tmpCRC ^= rx_buffer[i]; |
||
| 185 | } |
||
| 186 | if (rx_len >= i + 3) { |
||
| 187 | crc = hexDigitToInt(rx_buffer[i + 1]) << 4 | hexDigitToInt(rx_buffer[i + 2]); |
||
| 188 | if (crc == tmpCRC) { |
||
| 189 | rx_buffer[i] = 0; |
||
| 190 | strcpy(data_decode, &rx_buffer[1]); // Data without $, crc |
||
| 191 | ret = 1; |
||
| 192 | // wi232RX = 1; // So antenna-symbol will blink |
||
| 193 | // cli(); |
||
| 194 | // rx_timeout = 0; // Got valid data, reset counter |
||
| 195 | // sei(); |
||
| 196 | } |
||
| 197 | } |
||
| 198 | } |
||
| 199 | // if (rx_timeout < RX_TIME_OLD) wi232RX = 1; |
||
| 200 | rx_ready = 0; // Unlock buffer, next NMEA string can be received |
||
| 201 | return ret; |
||
| 202 | } |
||
| 203 | #endif |