Subversion Repositories Projects

Rev

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