james kain / Mbed 2 deprecated Waldo_Embed_V2

this version has all of Jim's fixes for reading the GPS and IMU data synchronously

Dependencies:   MODSERIAL SDFileSystem mbed SDShell CRC CommHandler FP LinkedList LogUtil

OEM615.h@1:8e24e633f8d8, 2013-04-30 (annotated)

Committer:
jekain314
Date:
Tue Apr 30 19:59:43 2013 +0000
Revision:
1:8e24e633f8d8
Parent:
0:432b860b6ff7
Child:
21:37551baf69c6
Jim's version dated 30-apr-13

Who changed what in which revision?

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