Subversion Repositories FlightCtrl

Rev

Rev 2084 | 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 };
2088 - 75
GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
2039 - 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;
2079 - 91
 
2088 - 92
      GPSInfo.position3DAcc =  ubxSol.pAcc;
93
      GPSInfo.verticalAcc =    ubxPosLlh.vAacc;
94
      GPSInfo.velocityAcc =    ubxSol.sAcc;
95
 
96
      debugOut.analog[30] = ubxPosLlh.HMSL/1000L;
97
      debugOut.analog[31] = GPSInfo.verticalAcc/1000L;
98
 
2039 - 99
      // NAV POSLLH
2044 - 100
      GPSInfo.longitude =   ubxPosLlh.lon;
101
      GPSInfo.latitude =    ubxPosLlh.lat;
102
      GPSInfo.altitude =    ubxPosLlh.height;
2039 - 103
 
2044 - 104
      GPSInfo.veleast =     ubxVelNed.VEL_E;
105
      GPSInfo.velnorth =    ubxVelNed.VEL_N;
2051 - 106
      GPSInfo.veltop =     -ubxVelNed.VEL_D;
2044 - 107
      GPSInfo.velground =   ubxVelNed.gSpeed;
2039 - 108
 
109
      GPSInfo.status = NEWDATA;
110
    }
111
 
112
    // set state to collect new data
2044 - 113
    ubxSol.status = PROCESSED; // never update old data
114
    ubxPosLlh.status = PROCESSED; // never update old data
115
    ubxVelNed.status = PROCESSED; // never update old data
2039 - 116
  }
117
}
118
 
119
// this function should be called within the UART RX ISR
120
void ubx_parser(uint8_t c) {
121
  static uint8_t ubxstate = UBXSTATE_IDLE;
122
  static uint8_t cka, ckb;
123
  static uint16_t msglen;
124
  static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
125
 
126
  switch (ubxstate) {
127
  case UBXSTATE_IDLE: // check 1st sync byte
128
    if (c == UBX_SYNC1_CHAR)
129
      ubxstate = UBXSTATE_SYNC1;
130
    else
131
      ubxstate = UBXSTATE_IDLE; // out of synchronization
132
    break;
133
 
134
  case UBXSTATE_SYNC1: // check 2nd sync byte
135
    if (c == UBX_SYNC2_CHAR)
136
      ubxstate = UBXSTATE_SYNC2;
137
    else
138
      ubxstate = UBXSTATE_IDLE; // out of synchronization
139
    break;
140
 
141
  case UBXSTATE_SYNC2: // check msg class to be NAV
142
    if (c == UBX_CLASS_NAV)
143
      ubxstate = UBXSTATE_CLASS;
144
    else
145
      ubxstate = UBXSTATE_IDLE; // unsupported message class
146
    break;
147
 
148
  case UBXSTATE_CLASS: // check message identifier
149
    switch (c) {
150
    case UBX_ID_POSLLH: // geodetic position
2044 - 151
      ubxP = (int8_t *)     &ubxPosLlh; // data start pointer
152
      ubxEp = (int8_t *)   (&ubxPosLlh + 1); // data end pointer
153
      ubxSp = (int8_t *)    &ubxPosLlh.status; // status pointer
2039 - 154
      break;
155
 
156
    case UBX_ID_SOL: // navigation solution
2044 - 157
      ubxP = (int8_t *)     &ubxSol; // data start pointer
158
      ubxEp = (int8_t *)   (&ubxSol + 1); // data end pointer
159
      ubxSp = (int8_t *)    &ubxSol.status; // status pointer
2039 - 160
      break;
161
 
162
    case UBX_ID_VELNED: // velocity vector in tangent plane
2044 - 163
      ubxP = (int8_t *)     &ubxVelNed; // data start pointer
164
      ubxEp = (int8_t *)   (&ubxVelNed + 1); // data end pointer
165
      ubxSp = (int8_t *)    &ubxVelNed.status; // status pointer
2039 - 166
      break;
167
 
168
    default: // unsupported identifier
169
      ubxstate = UBXSTATE_IDLE;
170
      break;
171
    }
172
    if (ubxstate != UBXSTATE_IDLE) {
173
      ubxstate = UBXSTATE_LEN1;
174
      cka = UBX_CLASS_NAV + c;
175
      ckb = UBX_CLASS_NAV + cka;
176
    }
177
    break;
178
 
179
  case UBXSTATE_LEN1: // 1st message length byte
180
    msglen = c;
181
    cka += c;
182
    ckb += cka;
183
    ubxstate = UBXSTATE_LEN2;
184
    break;
185
 
186
  case UBXSTATE_LEN2: // 2nd message length byte
187
    msglen += ((uint16_t) c) << 8;
188
    cka += c;
189
    ckb += cka;
190
    // if the old data are not processed so far then break parsing now
191
    // to avoid writing new data in ISR during reading by another function
192
    if (*ubxSp == NEWDATA) {
193
      updateGPSInfo(); //update GPS info respectively
194
      ubxstate = UBXSTATE_IDLE;
2058 - 195
    } else {// data invalid or already processd
2039 - 196
      *ubxSp = INVALID;
197
      ubxstate = UBXSTATE_DATA;
198
    }
199
    break;
200
 
201
  case UBXSTATE_DATA:
202
    if (ubxP < ubxEp)
203
      *ubxP++ = c; // copy curent data byte if any space is left
204
    cka += c;
205
    ckb += cka;
206
    if (--msglen == 0)
207
      ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
208
    break;
209
 
210
  case UBXSTATE_CKA:
211
    if (c == cka)
212
      ubxstate = UBXSTATE_CKB;
213
    else {
214
      *ubxSp = INVALID;
215
      ubxstate = UBXSTATE_IDLE;
216
    }
217
    break;
218
 
219
  case UBXSTATE_CKB:
220
    if (c == ckb) {
221
      *ubxSp = NEWDATA; // new data are valid
222
      updateGPSInfo(); //update GPS info respectively
223
      GPSTimeout = 255;
224
    } else { // if checksum not match then set data invalid
225
      *ubxSp = INVALID;
226
    }
227
    ubxstate = UBXSTATE_IDLE; // ready to parse new data
228
    break;
229
 
230
  default: // unknown ubx state
231
    ubxstate = UBXSTATE_IDLE;
232
    break;
233
  }
234
}