Subversion Repositories FlightCtrl

Rev

Rev 2039 | Rev 2051 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2039 Rev 2044
Line 24... Line 24...
24 
#define UBX_SYNC1_CHAR  0xB5
 24 
#define UBX_SYNC1_CHAR  0xB5
25 
#define UBX_SYNC2_CHAR  0x62
 25 
#define UBX_SYNC2_CHAR  0x62
Line 26... Line 26...
26 
 
 26 
 
27 
typedef struct {
 27 
typedef struct {
28 
  uint32_t ITOW; // ms GPS Millisecond Time of Week
 28 
  uint32_t ITOW; // ms GPS Millisecond Time of Week
29 
  int32_t Frac; // ns remainder of rounded ms above
 29 
  int32_t frac; // ns remainder of rounded ms above
30 
  int16_t week; // GPS week
 30 
  int16_t week; // GPS week
31 
  uint8_t GPSfix; // GPSfix Type, range 0..6
 31 
  uint8_t GPSfix; // GPSfix Type, range 0..6
32 
  uint8_t Flags; // Navigation Status Flags
 32 
  uint8_t flags; // Navigation Status Flags
33 
  int32_t ECEF_X; // cm ECEF X coordinate
 33 
  int32_t ECEF_X; // cm ECEF X coordinate
34 
  int32_t ECEF_Y; // cm ECEF Y coordinate
 34 
  int32_t ECEF_Y; // cm ECEF Y coordinate
35 
  int32_t ECEF_Z; // cm ECEF Z coordinate
 35 
  int32_t ECEF_Z; // cm ECEF Z coordinate
36 
  uint32_t PAcc; // cm 3D Position Accuracy Estimate
 36 
  uint32_t pAcc; // cm 3D Position Accuracy Estimate
37 
  int32_t ECEFVX; // cm/s ECEF X velocity
 37 
  int32_t ECEFVX; // cm/s ECEF X velocity
38 
  int32_t ECEFVY; // cm/s ECEF Y velocity
 38 
  int32_t ECEFVY; // cm/s ECEF Y velocity
39 
  int32_t ECEFVZ; // cm/s ECEF Z velocity
 39 
  int32_t ECEFVZ; // cm/s ECEF Z velocity
40 
  uint32_t SAcc; // cm/s Speed Accuracy Estimate
 40 
  uint32_t sAcc; // cm/s Speed Accuracy Estimate
41 
  uint16_t PDOP; // 0.01 Position DOP
 41 
  uint16_t PDOP; // 0.01 Position DOP
42 
  uint8_t res1; // reserved
 42 
  uint8_t res1; // reserved
43 
  uint8_t numSV; // Number of SVs used in navigation solution
 43 
  uint8_t numSV; // Number of SVs used in navigation solution
44 
  uint32_t res2; // reserved
 44 
  uint32_t res2; // reserved
45 
  Status_t Status;
 45 
  Status_t status;
Line 46... Line 46...
46 
} UBX_SOL_t;
 46 
} UBX_SOL_t;
47 
 
 47 
 
48 
typedef struct {
 48 
typedef struct {
49 
  uint32_t ITOW; // ms GPS Millisecond Time of Week
 49 
  uint32_t ITOW; // ms GPS Millisecond Time of Week
50 
  int32_t LON; // 1e-07 deg Longitude
 50 
  int32_t lon; // 1e-07 deg Longitude
51 
  int32_t LAT; // 1e-07 deg Latitude
 51 
  int32_t lat; // 1e-07 deg Latitude
52 
  int32_t HEIGHT; // mm Height above Ellipsoid
 52 
  int32_t height; // mm Height above Ellipsoid
53 
  int32_t HMSL; // mm Height above mean sea level
 53 
  int32_t HMSL; // mm Height above mean sea level
54 
  uint32_t Hacc; // mm Horizontal Accuracy Estimate
 54 
  uint32_t hAcc; // mm Horizontal Accuracy Estimate
55 
  uint32_t Vacc; // mm Vertical Accuracy Estimate
 55 
  uint32_t vAacc; // mm Vertical Accuracy Estimate
Line 56... Line 56...
56 
  Status_t Status;
 56 
  Status_t status;
57 
} UBX_POSLLH_t;
 57 
} UBX_POSLLH_t;
58 
 
 58 
 
59 
typedef struct {
 59 
typedef struct {
60 
  uint32_t ITOW; // ms  GPS Millisecond Time of Week
 60 
  uint32_t ITOW; // ms  GPS Millisecond Time of Week
61 
  int32_t VEL_N; // cm/s  NED north velocity
 61 
  int32_t VEL_N; // cm/s  NED north velocity
62 
  int32_t VEL_E; // cm/s  NED east velocity
 62 
  int32_t VEL_E; // cm/s  NED east velocity
63 
  int32_t VEL_D; // cm/s  NED down velocity
 63 
  int32_t VEL_D; // cm/s  NED down velocity
64 
  uint32_t Speed; // cm/s  Speed (3-D)
 64 
  uint32_t speed; // cm/s  Speed (3-D)
65 
  uint32_t GSpeed; // cm/s  Ground Speed (2-D)
 65 
  uint32_t gSpeed; // cm/s  Ground Speed (2-D)
66 
  int32_t Heading; // 1e-05 deg  Heading 2-D
 66 
  int32_t heading; // 1e-05 deg  Heading 2-D
67 
  uint32_t SAcc; // cm/s  Speed Accuracy Estimate
 67 
  uint32_t sAcc; // cm/s  Speed Accuracy Estimate
Line 68... Line -...
68 
  uint32_t CAcc; // deg  Course / Heading Accuracy Estimate
 - 
 
69 
  Status_t Status;
 68 
  uint32_t cAcc; // deg  Course / Heading Accuracy Estimate
70 
} UBX_VELNED_t;
 69 
  Status_t status;
71 
 
 70 
} UBX_VELNED_t;
72 
UBX_SOL_t UbxSol =
 71 
 
Line 73... Line 72...
73 
    { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
 72 
UBX_SOL_t ubxSol = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
Line 74... Line 73...
74 
UBX_POSLLH_t UbxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID };
 73 
UBX_POSLLH_t ubxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID };
75 
UBX_VELNED_t UbxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
 74 
UBX_VELNED_t ubxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
76 
GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
 75 
GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
77 
 
 76 
 
78 
volatile uint8_t GPSTimeout = 0;
 77 
volatile uint8_t GPSTimeout = 0;
79 
 
 78 
 
80 
void updateGPSInfo(void) {
 79 
void updateGPSInfo(void) {
81 
  if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA)
 80 
  if ((ubxSol.status == NEWDATA) && (ubxPosLlh.status == NEWDATA)
82 
      && (UbxVelNed.Status == NEWDATA)) {
 81 
      && (ubxVelNed.status == NEWDATA)) {
83 
    //RED_FLASH;
 82 
    //RED_FLASH;
84 
    // DebugOut.Digital ....blah...
 83 
    // DebugOut.Digital ....blah...
85 
    if (GPSInfo.status != NEWDATA) {
 84 
    if (GPSInfo.status != NEWDATA) {
86 
      GPSInfo.status = INVALID;
 85 
      GPSInfo.status = INVALID;
87 
      // NAV SOL
 86 
      // NAV SOL
88 
      GPSInfo.flags = UbxSol.Flags;
 87 
      GPSInfo.flags =   ubxSol.flags;
89 
      GPSInfo.satfix = UbxSol.GPSfix;
 88 
      GPSInfo.satfix =  ubxSol.GPSfix;
90 
      GPSInfo.satnum = UbxSol.numSV;
 89 
      GPSInfo.satnum =  ubxSol.numSV;
91 
      GPSInfo.PAcc = UbxSol.PAcc;
 90 
      GPSInfo.PAcc =    ubxSol.pAcc;
92 
      GPSInfo.VAcc = UbxSol.SAcc;
 91 
      GPSInfo.VAcc =    ubxSol.sAcc;
93 
      // NAV POSLLH
 92 
      // NAV POSLLH
94 
      GPSInfo.longitude = UbxPosLlh.LON;
 93 
      GPSInfo.longitude =   ubxPosLlh.lon;
95 
      GPSInfo.latitude = UbxPosLlh.LAT;
 94 
      GPSInfo.latitude =    ubxPosLlh.lat;
Line 96... Line 95...
96 
      GPSInfo.altitude = UbxPosLlh.HEIGHT;
 95 
      GPSInfo.altitude =    ubxPosLlh.height;
97 
 
 96 
 
Line 98... Line 97...
98 
      GPSInfo.veleast = UbxVelNed.VEL_E;
 97 
      GPSInfo.veleast =     ubxVelNed.VEL_E;
99 
      GPSInfo.velnorth = UbxVelNed.VEL_N;
 98 
      GPSInfo.velnorth =    ubxVelNed.VEL_N;
100 
      GPSInfo.veltop = -UbxVelNed.VEL_D;
 99 
      GPSInfo.veltop =      -ubxVelNed.VEL_D;
101 
      GPSInfo.velground = UbxVelNed.GSpeed;
 100 
      GPSInfo.velground =   ubxVelNed.gSpeed;
102 
 
 101 
 
103 
      GPSInfo.status = NEWDATA;
 102 
      GPSInfo.status = NEWDATA;
Line 104... Line 103...
104 
    }
 103 
    }
105 
 
 104 
 
Line 140... Line 139...
140 
    break;
 139 
    break;
Line 141... Line 140...
141 
 
 140 
 
142 
  case UBXSTATE_CLASS: // check message identifier
 141 
  case UBXSTATE_CLASS: // check message identifier
143 
    switch (c) {
 142 
    switch (c) {
144 
    case UBX_ID_POSLLH: // geodetic position
 143 
    case UBX_ID_POSLLH: // geodetic position
145 
      ubxP = (int8_t *) &UbxPosLlh; // data start pointer
 144 
      ubxP = (int8_t *)     &ubxPosLlh; // data start pointer
146 
      ubxEp = (int8_t *) (&UbxPosLlh + 1); // data end pointer
 145 
      ubxEp = (int8_t *)   (&ubxPosLlh + 1); // data end pointer
147 
      ubxSp = (int8_t *) &UbxPosLlh.Status; // status pointer
 146 
      ubxSp = (int8_t *)    &ubxPosLlh.status; // status pointer
Line 148... Line 147...
148 
      break;
 147 
      break;
149 
 
 148 
 
150 
    case UBX_ID_SOL: // navigation solution
 149 
    case UBX_ID_SOL: // navigation solution
151 
      ubxP = (int8_t *) &UbxSol; // data start pointer
 150 
      ubxP = (int8_t *)     &ubxSol; // data start pointer
152 
      ubxEp = (int8_t *) (&UbxSol + 1); // data end pointer
 151 
      ubxEp = (int8_t *)   (&ubxSol + 1); // data end pointer
Line 153... Line 152...
153 
      ubxSp = (int8_t *) &UbxSol.Status; // status pointer
 152 
      ubxSp = (int8_t *)    &ubxSol.status; // status pointer
154 
      break;
 153 
      break;
155 
 
 154 
 
156 
    case UBX_ID_VELNED: // velocity vector in tangent plane
 155 
    case UBX_ID_VELNED: // velocity vector in tangent plane
157 
      ubxP = (int8_t *) &UbxVelNed; // data start pointer
 156 
      ubxP = (int8_t *)     &ubxVelNed; // data start pointer
Line 158... Line 157...
158 
      ubxEp = (int8_t *) (&UbxVelNed + 1); // data end pointer
 157 
      ubxEp = (int8_t *)   (&ubxVelNed + 1); // data end pointer
159 
      ubxSp = (int8_t *) &UbxVelNed.Status; // status pointer
 158 
      ubxSp = (int8_t *)    &ubxVelNed.status; // status pointer
160 
      break;
 159 
      break;
Line 225... Line 224...
225 
  default: // unknown ubx state
 224 
  default: // unknown ubx state
226 
    ubxstate = UBXSTATE_IDLE;
 225 
    ubxstate = UBXSTATE_IDLE;
227 
    break;
 226 
    break;
228 
  }
 227 
  }
229 
}
 228 
}
230 
 
 -