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@21:37551baf69c6, 2013-05-14 (annotated)
- Committer:
- jekain314
- Date:
- Tue May 14 23:32:15 2013 +0000
- Revision:
- 21:37551baf69c6
- Parent:
- 1:8e24e633f8d8
- Child:
- 28:fcea53fcc712
Moved CRC functions from OEM615.h to crc.cpp/h. Also updated SDShell and un-nested the dependencies because of a merge issue. Will nest later when the code is more fully vetted.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jekain314 | 21:37551baf69c6 | 1 | #include "crc.h" |
jekain314 | 21:37551baf69c6 | 2 | |
jekain314 | 0:432b860b6ff7 | 3 | #pragma pack(1) //this forces the structure to be packed on byte boundaries (with no byte filler) |
jekain314 | 0:432b860b6ff7 | 4 | //set up the GPS message header in a structure to enable easy reading -- see the OEM615 manual (Table 4, page 24) |
jekain314 | 0:432b860b6ff7 | 5 | struct MESSAGEHEADER |
jekain314 | 0:432b860b6ff7 | 6 | { |
jekain314 | 0:432b860b6ff7 | 7 | char synchAA; //1st synch word |
jekain314 | 0:432b860b6ff7 | 8 | char synch44; //2nd synch word |
jekain314 | 0:432b860b6ff7 | 9 | char synch12; //3rd synch word |
jekain314 | 0:432b860b6ff7 | 10 | unsigned char headerLength; //always 28 |
jekain314 | 0:432b860b6ff7 | 11 | unsigned short messageID; //42 (0x2A BESTPOS) , 43 (2B RANGE) , or 99 (x63 BESTVEL), |
jekain314 | 0:432b860b6ff7 | 12 | char messageType; //always = 0 for binary |
jekain314 | 0:432b860b6ff7 | 13 | unsigned char portAddress; //0x20 for COM1 |
jekain314 | 0:432b860b6ff7 | 14 | // from spec: The length in bytes of the body of the message, not including the header nor the CRC |
jekain314 | 0:432b860b6ff7 | 15 | unsigned short messageLength; //not including header or CRC |
jekain314 | 0:432b860b6ff7 | 16 | unsigned short sequence; //typically 0 |
jekain314 | 0:432b860b6ff7 | 17 | unsigned char idleTime; //Time the processor is idle, in the last second |
jekain314 | 0:432b860b6ff7 | 18 | unsigned char timeStatus; //Enum Indicating quality of the GPS reference time |
jekain314 | 0:432b860b6ff7 | 19 | unsigned short GPSweek; //GPS reference week |
jekain314 | 0:432b860b6ff7 | 20 | unsigned long GPSTime_msecs; //from beginning of week |
jekain314 | 0:432b860b6ff7 | 21 | unsigned long receiverStatus; //32-bits representing the status of hardware and software |
jekain314 | 0:432b860b6ff7 | 22 | unsigned short reserved; |
jekain314 | 0:432b860b6ff7 | 23 | unsigned short receiverSWversion; //receiver software build number |
jekain314 | 0:432b860b6ff7 | 24 | //total length in bytes of this header is 28 |
jekain314 | 0:432b860b6ff7 | 25 | }; |
jekain314 | 0:432b860b6ff7 | 26 | MESSAGEHEADER *msgHeader[6]; |
jekain314 | 0:432b860b6ff7 | 27 | |
jekain314 | 0:432b860b6ff7 | 28 | #pragma pack(1) |
jekain314 | 0:432b860b6ff7 | 29 | //structure for OEM615 BESTVEL log message (page 314) |
jekain314 | 0:432b860b6ff7 | 30 | struct OEM615BESTVEL |
jekain314 | 0:432b860b6ff7 | 31 | { |
jekain314 | 0:432b860b6ff7 | 32 | MESSAGEHEADER msgHeader; |
jekain314 | 0:432b860b6ff7 | 33 | int solStatus; //solution status |
jekain314 | 0:432b860b6ff7 | 34 | //solutionStatusOEMStar solStatus; //solution status |
jekain314 | 0:432b860b6ff7 | 35 | int velType; //position or velocity type |
jekain314 | 0:432b860b6ff7 | 36 | //velTypeOEMStar posType; //position or velocity type |
jekain314 | 0:432b860b6ff7 | 37 | float latency; |
jekain314 | 0:432b860b6ff7 | 38 | float age; |
jekain314 | 0:432b860b6ff7 | 39 | double horizontalSpeed; //horizontal velocity vector magnitude (m/s) |
jekain314 | 0:432b860b6ff7 | 40 | double heading; //deg from North called TRK GND in specification |
jekain314 | 0:432b860b6ff7 | 41 | double verticalSpeed; //vertical velocity magnitude (m/s) |
jekain314 | 0:432b860b6ff7 | 42 | float reserved; |
jekain314 | 0:432b860b6ff7 | 43 | unsigned long CRC; |
jekain314 | 0:432b860b6ff7 | 44 | }; |
jekain314 | 0:432b860b6ff7 | 45 | |
jekain314 | 0:432b860b6ff7 | 46 | /* Solution Status descritpion from OEMV manual |
jekain314 | 0:432b860b6ff7 | 47 | 0 SOL_COMPUTED Solution computed |
jekain314 | 0:432b860b6ff7 | 48 | 1 INSUFFICIENT_OBS Insufficient observations |
jekain314 | 0:432b860b6ff7 | 49 | 2 NO_CONVERGENCE No convergence |
jekain314 | 0:432b860b6ff7 | 50 | 3 SINGULARITY Singularity at parameters matrix |
jekain314 | 0:432b860b6ff7 | 51 | 4 COV_TRACE Covariance trace exceeds maximum (trace > 1000 m) |
jekain314 | 0:432b860b6ff7 | 52 | 5 TEST_DIST Test distance exceeded (maximum of 3 rejections if distance >10 km) |
jekain314 | 0:432b860b6ff7 | 53 | 6 COLD_START Not yet converged from cold start |
jekain314 | 0:432b860b6ff7 | 54 | 7 V_H_LIMIT Height or velocity limits exceeded |
jekain314 | 0:432b860b6ff7 | 55 | 8 VARIANCE Variance exceeds limits |
jekain314 | 0:432b860b6ff7 | 56 | 9 RESIDUALS Residuals are too large |
jekain314 | 0:432b860b6ff7 | 57 | 10 DELTA_POS Delta position is too large |
jekain314 | 0:432b860b6ff7 | 58 | 11 NEGATIVE_VAR Negative variance |
jekain314 | 0:432b860b6ff7 | 59 | 12 Reserved |
jekain314 | 0:432b860b6ff7 | 60 | 13 INTEGRITY_WARNING Large residuals make position unreliable |
jekain314 | 0:432b860b6ff7 | 61 | 18 PENDING |
jekain314 | 0:432b860b6ff7 | 62 | 19 INVALID_FIX The fixed position, entered using the FIX POSITION command, is not valid |
jekain314 | 0:432b860b6ff7 | 63 | 20 UNAUTHORIZED Position type is unauthorized - HP or XP |
jekain314 | 0:432b860b6ff7 | 64 | */ |
jekain314 | 0:432b860b6ff7 | 65 | |
jekain314 | 0:432b860b6ff7 | 66 | #pragma pack(1) |
jekain314 | 0:432b860b6ff7 | 67 | //structure for BESTPOS message |
jekain314 | 0:432b860b6ff7 | 68 | struct OEM615BESTPOS |
jekain314 | 0:432b860b6ff7 | 69 | { |
jekain314 | 0:432b860b6ff7 | 70 | MESSAGEHEADER msgHeader; |
jekain314 | 0:432b860b6ff7 | 71 | int solStatus; //solution status |
jekain314 | 0:432b860b6ff7 | 72 | //solutionStatusOEMStar solStatus; //solution status |
jekain314 | 0:432b860b6ff7 | 73 | int posType; //position or velocity type |
jekain314 | 0:432b860b6ff7 | 74 | //posTypeOEMStar posType; //position or velocity type |
jekain314 | 0:432b860b6ff7 | 75 | double latitude; //latitude |
jekain314 | 0:432b860b6ff7 | 76 | double longitude; //longitude |
jekain314 | 0:432b860b6ff7 | 77 | double height; //height above mean sea level |
jekain314 | 0:432b860b6ff7 | 78 | float undulation; //the realtionship between the geoid and the |
jekain314 | 0:432b860b6ff7 | 79 | //ellipsoid of the chosen datum (m) |
jekain314 | 0:432b860b6ff7 | 80 | char datumID[4]; //datum ID is actually an enum that is not implemented |
jekain314 | 0:432b860b6ff7 | 81 | float latitudeSTD; //latitude standard deviation |
jekain314 | 0:432b860b6ff7 | 82 | float longitudeSTD; //longitude standard deviation |
jekain314 | 0:432b860b6ff7 | 83 | float heightSTD; //height standard deviation |
jekain314 | 0:432b860b6ff7 | 84 | char baseStationID[4]; //base station ID |
jekain314 | 0:432b860b6ff7 | 85 | float diffAge; //differential age (s) |
jekain314 | 0:432b860b6ff7 | 86 | float solutionAge; //solution age (s) |
jekain314 | 0:432b860b6ff7 | 87 | unsigned char numSV; //number of satellite vehicles tracked |
jekain314 | 0:432b860b6ff7 | 88 | unsigned char numSolSV; //number of satellite vehicles used in solution |
jekain314 | 0:432b860b6ff7 | 89 | unsigned char numGGL1; //number of GPS plus Glonass L1 |
jekain314 | 0:432b860b6ff7 | 90 | unsigned char res1; |
jekain314 | 0:432b860b6ff7 | 91 | unsigned char res2; |
jekain314 | 0:432b860b6ff7 | 92 | unsigned char extSolStatus; //extended solution status |
jekain314 | 0:432b860b6ff7 | 93 | unsigned char res3; |
jekain314 | 0:432b860b6ff7 | 94 | unsigned char sigMask; //signals used mask |
jekain314 | 0:432b860b6ff7 | 95 | unsigned long CRC; |
jekain314 | 0:432b860b6ff7 | 96 | }; |
jekain314 | 0:432b860b6ff7 | 97 | |
jekain314 | 0:432b860b6ff7 | 98 | //GPS-specific pins |
jekain314 | 0:432b860b6ff7 | 99 | DigitalOut GPS_Reset(p18); //GPS RESET line |
jekain314 | 0:432b860b6ff7 | 100 | InterruptIn PPSInt(p15); // GPS 1PPS (timemark) from the OEM615 |
jekain314 | 0:432b860b6ff7 | 101 | InterruptIn IMUClock(p17); |
jekain314 | 0:432b860b6ff7 | 102 | |
jekain314 | 0:432b860b6ff7 | 103 | Timer timeFromPPS; |
jekain314 | 0:432b860b6ff7 | 104 | unsigned long GPSTimemsecs = 0; |
jekain314 | 0:432b860b6ff7 | 105 | |
jekain314 | 0:432b860b6ff7 | 106 | //mbed tx/rx interface to the GPS COM1 port |
jekain314 | 0:432b860b6ff7 | 107 | MODSERIAL GPS_COM1(p9,p10); //this serial port communicates with the GPS receiver serial port (COM1) |
jekain314 | 0:432b860b6ff7 | 108 | |
jekain314 | 0:432b860b6ff7 | 109 | int test = 0; |
jekain314 | 0:432b860b6ff7 | 110 | unsigned short messageCounter = 0; |
jekain314 | 0:432b860b6ff7 | 111 | unsigned short savedMessageCounter = 0; |
jekain314 | 0:432b860b6ff7 | 112 | const unsigned short maxGPSbytesPerSec = 1536; |
jekain314 | 0:432b860b6ff7 | 113 | unsigned char msgBuffer[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes |
jekain314 | 0:432b860b6ff7 | 114 | const unsigned char maxGPSMessagesPerSec = 12; |
jekain314 | 0:432b860b6ff7 | 115 | unsigned short messageLocation[maxGPSMessagesPerSec] = {0}; //stores the message location start within the message buffer |
jekain314 | 0:432b860b6ff7 | 116 | |
jekain314 | 0:432b860b6ff7 | 117 | unsigned short bytesFromMessageHdrDetect = 0; |
jekain314 | 0:432b860b6ff7 | 118 | union SWAPBYTES { unsigned char b[2]; unsigned short w; }; |
jekain314 | 0:432b860b6ff7 | 119 | volatile SWAPBYTES messageLength; //used to swap the bytes |
jekain314 | 0:432b860b6ff7 | 120 | unsigned long computedCRC = 0; //final resulting computed CRC for this message |
jekain314 | 0:432b860b6ff7 | 121 | unsigned long incrementalCRC = 0; //incrementally-formed CRC over message sequence |
jekain314 | 0:432b860b6ff7 | 122 | unsigned short endByteForCRCcomputation[maxGPSMessagesPerSec]; |
jekain314 | 0:432b860b6ff7 | 123 | bool completeMessageAvailable = false; |
jekain314 | 0:432b860b6ff7 | 124 | |
jekain314 | 0:432b860b6ff7 | 125 | void sendASCII(char* ASCI_message, int numChars) |
jekain314 | 0:432b860b6ff7 | 126 | { |
jekain314 | 0:432b860b6ff7 | 127 | ///////////////////////////////////////////////// |
jekain314 | 0:432b860b6ff7 | 128 | //send an ASCII command to the GPS receiver |
jekain314 | 0:432b860b6ff7 | 129 | ///////////////////////////////////////////////// |
jekain314 | 0:432b860b6ff7 | 130 | |
jekain314 | 0:432b860b6ff7 | 131 | //char ASCI_message[] = "unlogall COM1"; |
jekain314 | 0:432b860b6ff7 | 132 | int as = numChars - 1; |
jekain314 | 0:432b860b6ff7 | 133 | unsigned char CR = 0x0d; //ASCII Carriage Return |
jekain314 | 0:432b860b6ff7 | 134 | unsigned char LF = 0x0a; //ASCII Line Feed |
jekain314 | 0:432b860b6ff7 | 135 | |
jekain314 | 0:432b860b6ff7 | 136 | //printf("%s", ch); |
jekain314 | 0:432b860b6ff7 | 137 | //printf("\n"); |
jekain314 | 0:432b860b6ff7 | 138 | |
jekain314 | 0:432b860b6ff7 | 139 | for (int i=0; i<as; i++) GPS_COM1.putc(ASCI_message[i]); |
jekain314 | 0:432b860b6ff7 | 140 | GPS_COM1.putc(CR); //carriage return at end |
jekain314 | 0:432b860b6ff7 | 141 | GPS_COM1.putc(LF); //line feed at end |
jekain314 | 0:432b860b6ff7 | 142 | }; |
jekain314 | 0:432b860b6ff7 | 143 | |
jekain314 | 0:432b860b6ff7 | 144 | |
jekain314 | 0:432b860b6ff7 | 145 | //see the mbed COOKBOOK for MODSERIAL |
jekain314 | 0:432b860b6ff7 | 146 | //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output. |
jekain314 | 0:432b860b6ff7 | 147 | void readSerialByte(MODSERIAL_IRQ_INFO *q) |
jekain314 | 0:432b860b6ff7 | 148 | { |
jekain314 | 0:432b860b6ff7 | 149 | MODSERIAL *serial = q->serial; //see example of MODSERIAL usage in cookbook |
jekain314 | 0:432b860b6ff7 | 150 | unsigned char synch0 = serial->getc(); //get the next byte |
jekain314 | 0:432b860b6ff7 | 151 | |
jekain314 | 0:432b860b6ff7 | 152 | //byteCounter is zeroed only at a 1PPA event in the 1PPS ISR |
jekain314 | 0:432b860b6ff7 | 153 | //all message bytes stored for a single GPS second |
jekain314 | 0:432b860b6ff7 | 154 | msgBuffer[byteCounter % maxGPSbytesPerSec] = synch0; |
jekain314 | 0:432b860b6ff7 | 155 | |
jekain314 | 0:432b860b6ff7 | 156 | //accumulate the CRC for this message |
jekain314 | 0:432b860b6ff7 | 157 | //incrementalCRC re-initialized after message header is is detected |
jekain314 | 0:432b860b6ff7 | 158 | CRC32Value( incrementalCRC, synch0); |
jekain314 | 0:432b860b6ff7 | 159 | |
jekain314 | 0:432b860b6ff7 | 160 | //Trap the GPS message header byte-string per Novatel OEM615 spec: 0xAA44121C |
jekain314 | 0:432b860b6ff7 | 161 | //generate a 4-byte sliding-window sequence from the input bytes |
jekain314 | 0:432b860b6ff7 | 162 | //shift last 4-byte value left 8 bits & push current-read byte (synch0) into low-order byte |
jekain314 | 0:432b860b6ff7 | 163 | test = (test<<8) | synch0; |
jekain314 | 0:432b860b6ff7 | 164 | |
jekain314 | 0:432b860b6ff7 | 165 | if (test == 0xAA44121C) //test for the Receiver message header signature |
jekain314 | 0:432b860b6ff7 | 166 | { |
jekain314 | 0:432b860b6ff7 | 167 | messageLocation[perSecMessageCounter % maxGPSMessagesPerSec] = byteCounter-3; //store the location of this message (1st of 4 synch word) |
jekain314 | 0:432b860b6ff7 | 168 | perSecMessageCounter++; //counts messages this second |
jekain314 | 0:432b860b6ff7 | 169 | bytesFromMessageHdrDetect = 0; //start byte counter for this message |
jekain314 | 0:432b860b6ff7 | 170 | incrementalCRC = 0x39b0f0e1; //initializes the recursive CRC after the AA44121C header byte sequence |
jekain314 | 0:432b860b6ff7 | 171 | } |
jekain314 | 0:432b860b6ff7 | 172 | else if (bytesFromMessageHdrDetect == 5) messageLength.b[0] = synch0; //first byte of msg length |
jekain314 | 0:432b860b6ff7 | 173 | else if (bytesFromMessageHdrDetect == 6) //second byte of message length |
jekain314 | 0:432b860b6ff7 | 174 | { |
jekain314 | 0:432b860b6ff7 | 175 | messageLength.b[1] = synch0; |
jekain314 | 0:432b860b6ff7 | 176 | endByteForCRCcomputation[perSecMessageCounter] = 24 + messageLength.w; //use union to perform byte-swap from stored bytes |
jekain314 | 0:432b860b6ff7 | 177 | } |
jekain314 | 0:432b860b6ff7 | 178 | else if (bytesFromMessageHdrDetect == endByteForCRCcomputation[perSecMessageCounter]) //stop the CRC recursive computation |
jekain314 | 0:432b860b6ff7 | 179 | computedCRC = incrementalCRC; //store the computed CRC for this message for use in main |
jekain314 | 0:432b860b6ff7 | 180 | else if (bytesFromMessageHdrDetect == (endByteForCRCcomputation[perSecMessageCounter] + 4) ) //detect the end of the message (end of its CRC) |
jekain314 | 0:432b860b6ff7 | 181 | { |
jekain314 | 0:432b860b6ff7 | 182 | savedMessageCounter = perSecMessageCounter; //message counter can be corrupted before use in main |
jekain314 | 0:432b860b6ff7 | 183 | completeMessageAvailable = true; //set flg for the main message processing |
jekain314 | 0:432b860b6ff7 | 184 | } |
jekain314 | 0:432b860b6ff7 | 185 | |
jekain314 | 0:432b860b6ff7 | 186 | //byteCounter reset to zero in main after the 1PPS is detected -- its NOT reset in the 1PPS ISR |
jekain314 | 0:432b860b6ff7 | 187 | byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected) |
jekain314 | 0:432b860b6ff7 | 188 | bytesFromMessageHdrDetect++; //counts the byes received after a message header |
jekain314 | 0:432b860b6ff7 | 189 | }; |
jekain314 | 0:432b860b6ff7 | 190 | |
jekain314 | 0:432b860b6ff7 | 191 | |
jekain314 | 0:432b860b6ff7 | 192 | |
jekain314 | 0:432b860b6ff7 | 193 | |
jekain314 | 0:432b860b6ff7 | 194 | |
jekain314 | 0:432b860b6ff7 | 195 | |
jekain314 | 0:432b860b6ff7 | 196 |