Subversion Repositories FlightCtrl

Rev

Rev 2079 | Rev 2088 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
2039 - 1
#include <inttypes.h>
2
#include "ubx.h"
3
#include <avr/io.h>
2058 - 4
#include "output.h"
2039 - 5
 
6
// ubx protocol parser state machine
7
#define UBXSTATE_IDLE   0
8
#define UBXSTATE_SYNC1  1
9
#define UBXSTATE_SYNC2  2
10
#define UBXSTATE_CLASS  3
11
#define UBXSTATE_LEN1   4
12
#define UBXSTATE_LEN2   5
13
#define UBXSTATE_DATA   6
14
#define UBXSTATE_CKA    7
15
#define UBXSTATE_CKB    8
16
 
17
// ublox protocoll identifier
18
#define UBX_CLASS_NAV   0x01
19
 
20
#define UBX_ID_POSLLH   0x02
21
#define UBX_ID_SOL              0x06
22
#define UBX_ID_VELNED   0x12
23
 
24
#define UBX_SYNC1_CHAR  0xB5
25
#define UBX_SYNC2_CHAR  0x62
26
 
27
typedef struct {
28
  uint32_t ITOW; // ms GPS Millisecond Time of Week
2044 - 29
  int32_t frac; // ns remainder of rounded ms above
2039 - 30
  int16_t week; // GPS week
31
  uint8_t GPSfix; // GPSfix Type, range 0..6
2044 - 32
  uint8_t flags; // Navigation Status Flags
2039 - 33
  int32_t ECEF_X; // cm ECEF X coordinate
34
  int32_t ECEF_Y; // cm ECEF Y coordinate
35
  int32_t ECEF_Z; // cm ECEF Z coordinate
2044 - 36
  uint32_t pAcc; // cm 3D Position Accuracy Estimate
2039 - 37
  int32_t ECEFVX; // cm/s ECEF X velocity
38
  int32_t ECEFVY; // cm/s ECEF Y velocity
39
  int32_t ECEFVZ; // cm/s ECEF Z velocity
2044 - 40
  uint32_t sAcc; // cm/s Speed Accuracy Estimate
2039 - 41
  uint16_t PDOP; // 0.01 Position DOP
42
  uint8_t res1; // reserved
43
  uint8_t numSV; // Number of SVs used in navigation solution
44
  uint32_t res2; // reserved
2044 - 45
  Status_t status;
2039 - 46
} UBX_SOL_t;
47
 
48
typedef struct {
49
  uint32_t ITOW; // ms GPS Millisecond Time of Week
2044 - 50
  int32_t lon; // 1e-07 deg Longitude
51
  int32_t lat; // 1e-07 deg Latitude
52
  int32_t height; // mm Height above Ellipsoid
2039 - 53
  int32_t HMSL; // mm Height above mean sea level
2044 - 54
  uint32_t hAcc; // mm Horizontal Accuracy Estimate
55
  uint32_t vAacc; // mm Vertical Accuracy Estimate
56
  Status_t status;
2039 - 57
} UBX_POSLLH_t;
58
 
59
typedef struct {
60
  uint32_t ITOW; // ms  GPS Millisecond Time of Week
61
  int32_t VEL_N; // cm/s  NED north velocity
62
  int32_t VEL_E; // cm/s  NED east velocity
63
  int32_t VEL_D; // cm/s  NED down velocity
2044 - 64
  uint32_t speed; // cm/s  Speed (3-D)
65
  uint32_t gSpeed; // cm/s  Ground Speed (2-D)
66
  int32_t heading; // 1e-05 deg  Heading 2-D
67
  uint32_t sAcc; // cm/s  Speed Accuracy Estimate
68
  uint32_t cAcc; // deg  Course / Heading Accuracy Estimate
69
  Status_t status;
2039 - 70
} UBX_VELNED_t;
71
 
2044 - 72
UBX_SOL_t ubxSol = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
73
UBX_POSLLH_t ubxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID };
74
UBX_VELNED_t ubxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
2039 - 75
GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
76
 
77
volatile uint8_t GPSTimeout = 0;
2058 - 78
volatile uint16_t GPSDatasetCounter = 0;
2039 - 79
 
80
void updateGPSInfo(void) {
2058 - 81
  if ((ubxSol.status == NEWDATA) && (ubxPosLlh.status == NEWDATA) && (ubxVelNed.status == NEWDATA)) {
2039 - 82
    //RED_FLASH;
83
    // DebugOut.Digital ....blah...
84
    if (GPSInfo.status != NEWDATA) {
2058 - 85
      GPSDatasetCounter++;
2039 - 86
      GPSInfo.status = INVALID;
87
      // NAV SOL
2044 - 88
      GPSInfo.flags =   ubxSol.flags;
89
      GPSInfo.satfix =  ubxSol.GPSfix;
90
      GPSInfo.satnum =  ubxSol.numSV;
91
      GPSInfo.PAcc =    ubxSol.pAcc;
92
      GPSInfo.VAcc =    ubxSol.sAcc;
2084 - 93
      debugOut.analog[31] = GPSInfo.VAcc;
2079 - 94
 
2039 - 95
      // NAV POSLLH
2044 - 96
      GPSInfo.longitude =   ubxPosLlh.lon;
97
      GPSInfo.latitude =    ubxPosLlh.lat;
98
      GPSInfo.altitude =    ubxPosLlh.height;
2039 - 99
 
2044 - 100
      GPSInfo.veleast =     ubxVelNed.VEL_E;
101
      GPSInfo.velnorth =    ubxVelNed.VEL_N;
2051 - 102
      GPSInfo.veltop =     -ubxVelNed.VEL_D;
2044 - 103
      GPSInfo.velground =   ubxVelNed.gSpeed;
2039 - 104
 
105
      GPSInfo.status = NEWDATA;
106
    }
107
 
108
    // set state to collect new data
2044 - 109
    ubxSol.status = PROCESSED; // never update old data
110
    ubxPosLlh.status = PROCESSED; // never update old data
111
    ubxVelNed.status = PROCESSED; // never update old data
2039 - 112
  }
113
}
114
 
115
// this function should be called within the UART RX ISR
116
void ubx_parser(uint8_t c) {
117
  static uint8_t ubxstate = UBXSTATE_IDLE;
118
  static uint8_t cka, ckb;
119
  static uint16_t msglen;
120
  static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
121
 
122
  switch (ubxstate) {
123
  case UBXSTATE_IDLE: // check 1st sync byte
124
    if (c == UBX_SYNC1_CHAR)
125
      ubxstate = UBXSTATE_SYNC1;
126
    else
127
      ubxstate = UBXSTATE_IDLE; // out of synchronization
128
    break;
129
 
130
  case UBXSTATE_SYNC1: // check 2nd sync byte
131
    if (c == UBX_SYNC2_CHAR)
132
      ubxstate = UBXSTATE_SYNC2;
133
    else
134
      ubxstate = UBXSTATE_IDLE; // out of synchronization
135
    break;
136
 
137
  case UBXSTATE_SYNC2: // check msg class to be NAV
138
    if (c == UBX_CLASS_NAV)
139
      ubxstate = UBXSTATE_CLASS;
140
    else
141
      ubxstate = UBXSTATE_IDLE; // unsupported message class
142
    break;
143
 
144
  case UBXSTATE_CLASS: // check message identifier
145
    switch (c) {
146
    case UBX_ID_POSLLH: // geodetic position
2044 - 147
      ubxP = (int8_t *)     &ubxPosLlh; // data start pointer
148
      ubxEp = (int8_t *)   (&ubxPosLlh + 1); // data end pointer
149
      ubxSp = (int8_t *)    &ubxPosLlh.status; // status pointer
2039 - 150
      break;
151
 
152
    case UBX_ID_SOL: // navigation solution
2044 - 153
      ubxP = (int8_t *)     &ubxSol; // data start pointer
154
      ubxEp = (int8_t *)   (&ubxSol + 1); // data end pointer
155
      ubxSp = (int8_t *)    &ubxSol.status; // status pointer
2039 - 156
      break;
157
 
158
    case UBX_ID_VELNED: // velocity vector in tangent plane
2044 - 159
      ubxP = (int8_t *)     &ubxVelNed; // data start pointer
160
      ubxEp = (int8_t *)   (&ubxVelNed + 1); // data end pointer
161
      ubxSp = (int8_t *)    &ubxVelNed.status; // status pointer
2039 - 162
      break;
163
 
164
    default: // unsupported identifier
165
      ubxstate = UBXSTATE_IDLE;
166
      break;
167
    }
168
    if (ubxstate != UBXSTATE_IDLE) {
169
      ubxstate = UBXSTATE_LEN1;
170
      cka = UBX_CLASS_NAV + c;
171
      ckb = UBX_CLASS_NAV + cka;
172
    }
173
    break;
174
 
175
  case UBXSTATE_LEN1: // 1st message length byte
176
    msglen = c;
177
    cka += c;
178
    ckb += cka;
179
    ubxstate = UBXSTATE_LEN2;
180
    break;
181
 
182
  case UBXSTATE_LEN2: // 2nd message length byte
183
    msglen += ((uint16_t) c) << 8;
184
    cka += c;
185
    ckb += cka;
186
    // if the old data are not processed so far then break parsing now
187
    // to avoid writing new data in ISR during reading by another function
188
    if (*ubxSp == NEWDATA) {
189
      updateGPSInfo(); //update GPS info respectively
190
      ubxstate = UBXSTATE_IDLE;
2058 - 191
    } else {// data invalid or already processd
2039 - 192
      *ubxSp = INVALID;
193
      ubxstate = UBXSTATE_DATA;
194
    }
195
    break;
196
 
197
  case UBXSTATE_DATA:
198
    if (ubxP < ubxEp)
199
      *ubxP++ = c; // copy curent data byte if any space is left
200
    cka += c;
201
    ckb += cka;
202
    if (--msglen == 0)
203
      ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
204
    break;
205
 
206
  case UBXSTATE_CKA:
207
    if (c == cka)
208
      ubxstate = UBXSTATE_CKB;
209
    else {
210
      *ubxSp = INVALID;
211
      ubxstate = UBXSTATE_IDLE;
212
    }
213
    break;
214
 
215
  case UBXSTATE_CKB:
216
    if (c == ckb) {
217
      *ubxSp = NEWDATA; // new data are valid
218
      updateGPSInfo(); //update GPS info respectively
219
      GPSTimeout = 255;
220
    } else { // if checksum not match then set data invalid
221
      *ubxSp = INVALID;
222
    }
223
    ubxstate = UBXSTATE_IDLE; // ready to parse new data
224
    break;
225
 
226
  default: // unknown ubx state
227
    ubxstate = UBXSTATE_IDLE;
228
    break;
229
  }
230
}