james kain / Mbed 2 deprecated GPS_Incremental

JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013

Dependencies:   mbed

Fork of GPS_Incremental by Dan Matthews

OEM615.h@9:b45feb91ba38, 2013-04-19 (annotated)

Committer:
jekain314
Date:
Fri Apr 19 16:21:27 2013 +0000
Revision:
9:b45feb91ba38
Parent:
6:2a8486283198 
update to allow better imu gps data collection

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dannyman939 0:c746ee34feae 1
dannyman939 0:c746ee34feae 2 #pragma pack(1) //this forces the structure to be packed on byte boundaries (with no byte filler)
dannyman939 0:c746ee34feae 3 //set up the GPS message header in a structure to enable easy reading -- see the OEM615 manual (Table 4, page 24)
dannyman939 0:c746ee34feae 4 struct MESSAGEHEADER
dannyman939 0:c746ee34feae 5 {
jekain314 9:b45feb91ba38 6 char synchAA; //1st synch word
jekain314 9:b45feb91ba38 7 char synch44; //2nd synch word
jekain314 9:b45feb91ba38 8 char synch12; //3rd synch word
jekain314 9:b45feb91ba38 9 unsigned char headerLength; //always 28
jekain314 9:b45feb91ba38 10 unsigned short messageID; //42 (0x2A BESTPOS) , 43 (2B RANGE) , or 99 (x63 BESTVEL),
jekain314 9:b45feb91ba38 11 char messageType; //always = 0 for binary
jekain314 9:b45feb91ba38 12 unsigned char portAddress; //0x20 for COM1
jekain314 9:b45feb91ba38 13 // from spec: The length in bytes of the body of the message, not including the header nor the CRC
jekain314 9:b45feb91ba38 14 unsigned short messageLength; //not including header or CRC
jekain314 9:b45feb91ba38 15 unsigned short sequence; //typically 0
jekain314 9:b45feb91ba38 16 unsigned char idleTime; //Time the processor is idle, in the last second
jekain314 9:b45feb91ba38 17 unsigned char timeStatus; //Enum Indicating quality of the GPS reference time
jekain314 9:b45feb91ba38 18 unsigned short GPSweek; //GPS reference week
jekain314 9:b45feb91ba38 19 unsigned long GPSTime_msecs; //from beginning of week
jekain314 9:b45feb91ba38 20 unsigned long receiverStatus; //32-bits representing the status of hardware and software
dannyman939 0:c746ee34feae 21 unsigned short reserved;
dannyman939 0:c746ee34feae 22 unsigned short receiverSWversion; //receiver software build number
jekain314 9:b45feb91ba38 23 //total length in bytes of this header is 28
dannyman939 0:c746ee34feae 24 };
jekain314 9:b45feb91ba38 25 MESSAGEHEADER *msgHeader[6];
dannyman939 0:c746ee34feae 26
dannyman939 0:c746ee34feae 27 #pragma pack(1)
dannyman939 0:c746ee34feae 28 //structure for OEM615 BESTVEL log message (page 314)
dannyman939 0:c746ee34feae 29 struct OEM615BESTVEL
dannyman939 0:c746ee34feae 30 {
dannyman939 0:c746ee34feae 31 MESSAGEHEADER msgHeader;
dannyman939 0:c746ee34feae 32 int solStatus; //solution status
dannyman939 0:c746ee34feae 33 //solutionStatusOEMStar solStatus; //solution status
dannyman939 0:c746ee34feae 34 int velType; //position or velocity type
dannyman939 0:c746ee34feae 35 //velTypeOEMStar posType; //position or velocity type
dannyman939 0:c746ee34feae 36 float latency;
dannyman939 0:c746ee34feae 37 float age;
dannyman939 0:c746ee34feae 38 double horizontalSpeed; //horizontal velocity vector magnitude (m/s)
dannyman939 0:c746ee34feae 39 double heading; //deg from North called TRK GND in specification
dannyman939 0:c746ee34feae 40 double verticalSpeed; //vertical velocity magnitude (m/s)
dannyman939 0:c746ee34feae 41 float reserved;
dannyman939 0:c746ee34feae 42 unsigned long CRC;
dannyman939 0:c746ee34feae 43 };
dannyman939 0:c746ee34feae 44
jekain314 6:2a8486283198 45 /* Solution Status descritpion from OEMV manual
jekain314 6:2a8486283198 46 0 SOL_COMPUTED Solution computed
jekain314 6:2a8486283198 47 1 INSUFFICIENT_OBS Insufficient observations
jekain314 6:2a8486283198 48 2 NO_CONVERGENCE No convergence
jekain314 6:2a8486283198 49 3 SINGULARITY Singularity at parameters matrix
jekain314 6:2a8486283198 50 4 COV_TRACE Covariance trace exceeds maximum (trace > 1000 m)
jekain314 6:2a8486283198 51 5 TEST_DIST Test distance exceeded (maximum of 3 rejections if distance >10 km)
jekain314 6:2a8486283198 52 6 COLD_START Not yet converged from cold start
jekain314 6:2a8486283198 53 7 V_H_LIMIT Height or velocity limits exceeded
jekain314 6:2a8486283198 54 8 VARIANCE Variance exceeds limits
jekain314 6:2a8486283198 55 9 RESIDUALS Residuals are too large
jekain314 6:2a8486283198 56 10 DELTA_POS Delta position is too large
jekain314 6:2a8486283198 57 11 NEGATIVE_VAR Negative variance
jekain314 6:2a8486283198 58 12 Reserved
jekain314 6:2a8486283198 59 13 INTEGRITY_WARNING Large residuals make position unreliable
jekain314 6:2a8486283198 60 18 PENDING
jekain314 6:2a8486283198 61 19 INVALID_FIX The fixed position, entered using the FIX POSITION command, is not valid
jekain314 6:2a8486283198 62 20 UNAUTHORIZED Position type is unauthorized - HP or XP
jekain314 6:2a8486283198 63 */
jekain314 6:2a8486283198 64
dannyman939 0:c746ee34feae 65 #pragma pack(1)
dannyman939 0:c746ee34feae 66 //structure for BESTPOS message
dannyman939 0:c746ee34feae 67 struct OEM615BESTPOS
dannyman939 0:c746ee34feae 68 {
dannyman939 0:c746ee34feae 69 MESSAGEHEADER msgHeader;
dannyman939 0:c746ee34feae 70 int solStatus; //solution status
dannyman939 0:c746ee34feae 71 //solutionStatusOEMStar solStatus; //solution status
dannyman939 0:c746ee34feae 72 int posType; //position or velocity type
dannyman939 0:c746ee34feae 73 //posTypeOEMStar posType; //position or velocity type
dannyman939 0:c746ee34feae 74 double latitude; //latitude
dannyman939 0:c746ee34feae 75 double longitude; //longitude
dannyman939 0:c746ee34feae 76 double height; //height above mean sea level
dannyman939 0:c746ee34feae 77 float undulation; //the realtionship between the geoid and the
dannyman939 0:c746ee34feae 78 //ellipsoid of the chosen datum (m)
dannyman939 0:c746ee34feae 79 char datumID[4]; //datum ID is actually an enum that is not implemented
dannyman939 0:c746ee34feae 80 float latitudeSTD; //latitude standard deviation
dannyman939 0:c746ee34feae 81 float longitudeSTD; //longitude standard deviation
dannyman939 0:c746ee34feae 82 float heightSTD; //height standard deviation
dannyman939 0:c746ee34feae 83 char baseStationID[4]; //base station ID
dannyman939 0:c746ee34feae 84 float diffAge; //differential age (s)
dannyman939 0:c746ee34feae 85 float solutionAge; //solution age (s)
dannyman939 0:c746ee34feae 86 unsigned char numSV; //number of satellite vehicles tracked
dannyman939 0:c746ee34feae 87 unsigned char numSolSV; //number of satellite vehicles used in solution
dannyman939 0:c746ee34feae 88 unsigned char numGGL1; //number of GPS plus Glonass L1
dannyman939 0:c746ee34feae 89 unsigned char res1;
dannyman939 0:c746ee34feae 90 unsigned char res2;
dannyman939 0:c746ee34feae 91 unsigned char extSolStatus; //extended solution status
dannyman939 0:c746ee34feae 92 unsigned char res3;
dannyman939 0:c746ee34feae 93 unsigned char sigMask; //signals used mask
dannyman939 0:c746ee34feae 94 unsigned long CRC;
dannyman939 0:c746ee34feae 95 };
dannyman939 0:c746ee34feae 96
jekain314 6:2a8486283198 97 //GPS-specific pins
jekain314 6:2a8486283198 98 DigitalOut GPS_Reset(p18); //GPS RESET line
jekain314 6:2a8486283198 99 InterruptIn PPSInt(p15); // GPS 1PPS (timemark) from the OEM615
jekain314 6:2a8486283198 100 InterruptIn IMUClock(p17);
jekain314 6:2a8486283198 101
jekain314 6:2a8486283198 102 Timer timeFromPPS;
jekain314 6:2a8486283198 103 unsigned long GPSTimemsecs = 0;
jekain314 6:2a8486283198 104 double GPSTime = 0;
jekain314 6:2a8486283198 105 int PPSTimeOffset = 0;
jekain314 6:2a8486283198 106
jekain314 6:2a8486283198 107 //mbed tx/rx interface to the GPS COM1 port
jekain314 6:2a8486283198 108 MODSERIAL GPS_COM1(p9,p10); //this serial port communicates with the GPS receiver serial port (COM1)
jekain314 6:2a8486283198 109
jekain314 6:2a8486283198 110 int test = 0;
jekain314 6:2a8486283198 111 unsigned short messageCounter = 0;
jekain314 6:2a8486283198 112 unsigned short savedMessageCounter = 0;
jekain314 6:2a8486283198 113 const unsigned short maxGPSbytesPerSec = 1536;
jekain314 6:2a8486283198 114 unsigned char msgBuffer[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes
jekain314 6:2a8486283198 115 const unsigned char maxGPSMessagesPerSec = 12;
jekain314 6:2a8486283198 116 unsigned short messageLocation[maxGPSMessagesPerSec] = {0}; //stores the message location start within the message buffer
dannyman939 0:c746ee34feae 117
jekain314 9:b45feb91ba38 118 unsigned short bytesFromMessageHdrDetect = 0;
jekain314 9:b45feb91ba38 119 union SWAPBYTES { unsigned char b[2]; unsigned short w; };
jekain314 9:b45feb91ba38 120 volatile SWAPBYTES messageLength; //used to swap the bytes
jekain314 9:b45feb91ba38 121 unsigned long computedCRC = 0; //final resulting computed CRC for this message
jekain314 9:b45feb91ba38 122 unsigned long incrementalCRC = 0; //incrementally-formed CRC over message sequence
jekain314 9:b45feb91ba38 123 unsigned short endByteForCRCcomputation = 0;
jekain314 9:b45feb91ba38 124 bool completeMessageAvailable = false;
jekain314 9:b45feb91ba38 125
dannyman939 0:c746ee34feae 126 //this code was taken from the Novatel Firmware document page 35
jekain314 9:b45feb91ba38 127 //#define CRC32_POLYNOMIAL 0xEDB88320L
dannyman939 0:c746ee34feae 128 /* --------------------------------------------------------------------------
dannyman939 0:c746ee34feae 129 Calculate a CRC value to be used by CRC calculation functions.
dannyman939 0:c746ee34feae 130 -------------------------------------------------------------------------- */
jekain314 9:b45feb91ba38 131 /*
jekain314 9:b45feb91ba38 132 ////////////////////////////////////////////
jekain314 9:b45feb91ba38 133 //original code from the OEM615 manual
jekain314 9:b45feb91ba38 134 ////////////////////////////////////////////
dannyman939 0:c746ee34feae 135 unsigned long CRC32Value(int i)
dannyman939 0:c746ee34feae 136 {
dannyman939 0:c746ee34feae 137 int j;
dannyman939 0:c746ee34feae 138 unsigned long ulCRC;
dannyman939 0:c746ee34feae 139 ulCRC = i;
dannyman939 0:c746ee34feae 140 for ( j = 8 ; j > 0; j-- )
dannyman939 0:c746ee34feae 141 {
dannyman939 0:c746ee34feae 142 if ( ulCRC & 1 )
dannyman939 0:c746ee34feae 143 ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL;
dannyman939 0:c746ee34feae 144 else
dannyman939 0:c746ee34feae 145 ulCRC >>= 1;
dannyman939 0:c746ee34feae 146 }
dannyman939 0:c746ee34feae 147 return ulCRC;
dannyman939 0:c746ee34feae 148 }
jekain314 9:b45feb91ba38 149 */
jekain314 9:b45feb91ba38 150
jekain314 9:b45feb91ba38 151 #define CRC32_POLYNOMIAL 0xEDB88320L
jekain314 9:b45feb91ba38 152 void CRC32Value(unsigned long &CRC, unsigned char c)
jekain314 9:b45feb91ba38 153 {
jekain314 9:b45feb91ba38 154 /////////////////////////////////////////////////////////////////////////////////////
jekain314 9:b45feb91ba38 155 //CRC must be initialized as zero
jekain314 9:b45feb91ba38 156 //c is a character from the sequence that is used to form the CRC
jekain314 9:b45feb91ba38 157 //this code is a modification of the code from the Novatel OEM615 specification
jekain314 9:b45feb91ba38 158 /////////////////////////////////////////////////////////////////////////////////////
jekain314 9:b45feb91ba38 159 unsigned long ulTemp1 = ( CRC >> 8 ) & 0x00FFFFFFL;
jekain314 9:b45feb91ba38 160 unsigned long ulCRC = ((int) CRC ^ c ) & 0xff ;
jekain314 9:b45feb91ba38 161 for (int j = 8 ; j > 0; j-- )
jekain314 9:b45feb91ba38 162 {
jekain314 9:b45feb91ba38 163 if ( ulCRC & 1 )
jekain314 9:b45feb91ba38 164 ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL;
jekain314 9:b45feb91ba38 165 else
jekain314 9:b45feb91ba38 166 ulCRC >>= 1;
jekain314 9:b45feb91ba38 167 }
jekain314 9:b45feb91ba38 168 CRC = ulTemp1 ^ ulCRC;
jekain314 9:b45feb91ba38 169 }
dannyman939 0:c746ee34feae 170
dannyman939 0:c746ee34feae 171 /* --------------------------------------------------------------------------
dannyman939 0:c746ee34feae 172 Calculates the CRC-32 of a block of data all at once
jekain314 9:b45feb91ba38 173 //the CRC is from the complete message (header plus data)
jekain314 9:b45feb91ba38 174 //but excluding (of course) the CRC at the end
dannyman939 0:c746ee34feae 175 -------------------------------------------------------------------------- */
dannyman939 0:c746ee34feae 176 unsigned long CalculateBlockCRC32(
dannyman939 0:c746ee34feae 177 unsigned long ulCount, /* Number of bytes in the data block */
dannyman939 0:c746ee34feae 178 unsigned char *ucBuffer ) /* Data block */
dannyman939 0:c746ee34feae 179 {
jekain314 9:b45feb91ba38 180 //////////////////////////////////////////////////////////////////////
jekain314 9:b45feb91ba38 181 //the below code tests the CRC32Value procedure used in a markov form
jekain314 9:b45feb91ba38 182 //////////////////////////////////////////////////////////////////////
jekain314 9:b45feb91ba38 183 unsigned long CRC = 0;
jekain314 9:b45feb91ba38 184 for (int i = 0; i<ulCount; i++) CRC32Value( CRC, *ucBuffer++ );
jekain314 9:b45feb91ba38 185 return CRC;
jekain314 9:b45feb91ba38 186 }
jekain314 9:b45feb91ba38 187
jekain314 9:b45feb91ba38 188 /*
jekain314 9:b45feb91ba38 189 unsigned long CalculateBlockCRC32(
jekain314 9:b45feb91ba38 190 unsigned long ulCount,
jekain314 9:b45feb91ba38 191 unsigned char *ucBuffer )
jekain314 9:b45feb91ba38 192 {
jekain314 9:b45feb91ba38 193 ////////////////////////////////////////////
jekain314 9:b45feb91ba38 194 //original code from the OEM615 manual
jekain314 9:b45feb91ba38 195 ////////////////////////////////////////////
dannyman939 0:c746ee34feae 196 unsigned long ulTemp1;
dannyman939 0:c746ee34feae 197 unsigned long ulTemp2;
dannyman939 0:c746ee34feae 198 unsigned long ulCRC = 0;
dannyman939 0:c746ee34feae 199 while ( ulCount-- != 0 )
dannyman939 0:c746ee34feae 200 {
dannyman939 0:c746ee34feae 201 ulTemp1 = ( ulCRC >> 8 ) & 0x00FFFFFFL;
dannyman939 0:c746ee34feae 202 ulTemp2 = CRC32Value( ((int) ulCRC ^ *ucBuffer++ ) & 0xff );
dannyman939 0:c746ee34feae 203 ulCRC = ulTemp1 ^ ulTemp2;
dannyman939 0:c746ee34feae 204 }
dannyman939 0:c746ee34feae 205 return( ulCRC );
dannyman939 0:c746ee34feae 206 }
jekain314 9:b45feb91ba38 207 */
jekain314 6:2a8486283198 208 void sendASCII(char* ASCI_message, int numChars)
jekain314 6:2a8486283198 209 {
jekain314 6:2a8486283198 210 /////////////////////////////////////////////////
jekain314 6:2a8486283198 211 //send an ASCII command to the GPS receiver
jekain314 6:2a8486283198 212 /////////////////////////////////////////////////
jekain314 6:2a8486283198 213
jekain314 6:2a8486283198 214 //char ASCI_message[] = "unlogall COM1";
jekain314 6:2a8486283198 215 int as = numChars - 1;
jekain314 6:2a8486283198 216 unsigned char CR = 0x0d; //ASCII Carriage Return
jekain314 6:2a8486283198 217 unsigned char LF = 0x0a; //ASCII Line Feed
jekain314 6:2a8486283198 218
jekain314 6:2a8486283198 219 //printf("%s", ch);
jekain314 6:2a8486283198 220 //printf("\n");
jekain314 6:2a8486283198 221
jekain314 6:2a8486283198 222 for (int i=0; i<as; i++) GPS_COM1.putc(ASCI_message[i]);
jekain314 6:2a8486283198 223 GPS_COM1.putc(CR); //carriage return at end
jekain314 6:2a8486283198 224 GPS_COM1.putc(LF); //line feed at end
jekain314 6:2a8486283198 225 };
jekain314 6:2a8486283198 226
dannyman939 0:c746ee34feae 227
jekain314 6:2a8486283198 228 //see the mbed COOKBOOK for MODSERIAL
jekain314 6:2a8486283198 229 //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output.
jekain314 6:2a8486283198 230 void readSerialByte(MODSERIAL_IRQ_INFO *q)
jekain314 6:2a8486283198 231 {
jekain314 9:b45feb91ba38 232 MODSERIAL *serial = q->serial; //see example of MODSERIAL usage in cookbook
jekain314 9:b45feb91ba38 233 unsigned char synch0 = serial->getc(); //get the next byte
jekain314 9:b45feb91ba38 234
jekain314 9:b45feb91ba38 235 //byteCounter is zeroed only at a 1PPA event in the 1PPS ISR
jekain314 9:b45feb91ba38 236 //all message bytes stored for a single GPS second
jekain314 6:2a8486283198 237 msgBuffer[byteCounter % maxGPSbytesPerSec] = synch0;
jekain314 9:b45feb91ba38 238
jekain314 9:b45feb91ba38 239 //accumulate the CRC for this message
jekain314 9:b45feb91ba38 240 //incrementalCRC re-initialized after message header is is detected
jekain314 9:b45feb91ba38 241 CRC32Value( incrementalCRC, synch0);
dannyman939 0:c746ee34feae 242
jekain314 9:b45feb91ba38 243 //Trap the GPS message header byte-string per Novatel OEM615 spec: 0xAA44121C
jekain314 6:2a8486283198 244 //generate a 4-byte sliding-window sequence from the input bytes
jekain314 9:b45feb91ba38 245 //shift last 4-byte value left 8 bits & push current-read byte (synch0) into low-order byte
jekain314 9:b45feb91ba38 246 test = (test<<8) | synch0;
jekain314 6:2a8486283198 247
jekain314 6:2a8486283198 248 if (test == 0xAA44121C) //test for the Receiver message header signature
jekain314 6:2a8486283198 249 {
jekain314 9:b45feb91ba38 250 messageLocation[perSecMessageCounter % maxGPSMessagesPerSec] = byteCounter-3; //store the location of this message (1st of 4 synch word)
jekain314 9:b45feb91ba38 251 perSecMessageCounter++; //counts messages this second
jekain314 9:b45feb91ba38 252 bytesFromMessageHdrDetect = 0; //start byte counter for this message
jekain314 9:b45feb91ba38 253 incrementalCRC = 0x39b0f0e1; //initializes the recursive CRC after the AA44121C header byte sequence
jekain314 9:b45feb91ba38 254 }
jekain314 9:b45feb91ba38 255 else if (bytesFromMessageHdrDetect == 5) messageLength.b[0] = synch0; //first byte of msg length
jekain314 9:b45feb91ba38 256 else if (bytesFromMessageHdrDetect == 6) //second byte of message length
jekain314 9:b45feb91ba38 257 {
jekain314 9:b45feb91ba38 258 messageLength.b[1] = synch0;
jekain314 9:b45feb91ba38 259 endByteForCRCcomputation = 24 + messageLength.w; //use union to perform byte-swap from stored bytes
jekain314 9:b45feb91ba38 260 }
jekain314 9:b45feb91ba38 261 else if (bytesFromMessageHdrDetect == endByteForCRCcomputation) //stop the CRC recursive computation
jekain314 9:b45feb91ba38 262 computedCRC = incrementalCRC; //store the computed CRC for this message for use in main
jekain314 9:b45feb91ba38 263 else if (bytesFromMessageHdrDetect == (endByteForCRCcomputation + 4) ) //detect the end of the message (end of its CRC)
jekain314 9:b45feb91ba38 264 {
jekain314 9:b45feb91ba38 265 savedMessageCounter = perSecMessageCounter; //message counter can be corrupted before use in main
jekain314 9:b45feb91ba38 266 completeMessageAvailable = true; //set flg for the main message processing
jekain314 9:b45feb91ba38 267 }
jekain314 9:b45feb91ba38 268
jekain314 6:2a8486283198 269 //byteCounter reset to zero in main after the 1PPS is detected -- its NOT reset in the 1PPS ISR
jekain314 6:2a8486283198 270 byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected)
jekain314 9:b45feb91ba38 271 bytesFromMessageHdrDetect++; //counts the byes received after a message header
jekain314 6:2a8486283198 272 };
dannyman939 0:c746ee34feae 273
dannyman939 0:c746ee34feae 274
dannyman939 0:c746ee34feae 275
jekain314 6:2a8486283198 276
jekain314 6:2a8486283198 277
jekain314 6:2a8486283198 278
jekain314 6:2a8486283198 279