JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
OEM615.h@6:2a8486283198, 2013-04-02 (annotated)
- Committer:
- jekain314
- Date:
- Tue Apr 02 15:22:37 2013 +0000
- Revision:
- 6:2a8486283198
- Parent:
- 4:68268737ff89
- Child:
- 9:b45feb91ba38
JEK changes enabling proper recording of IMU/GPS datastreams to the SD-card
Who changed what in which revision?
User | Revision | Line number | New 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 | { |
dannyman939 | 0:c746ee34feae | 6 | char synchAA; //1st synch word |
dannyman939 | 0:c746ee34feae | 7 | char synch44; //2nd synch word |
dannyman939 | 0:c746ee34feae | 8 | char synch12; //3rd synch word |
dannyman939 | 0:c746ee34feae | 9 | unsigned char headerLength; //always 28 |
jekain314 | 4:68268737ff89 | 10 | unsigned short messageID; //42 (BESTPOS) , 43 (RANGE) , or 99 (BESTVEL), |
dannyman939 | 0:c746ee34feae | 11 | char messageType; //always = 0 for binary |
dannyman939 | 0:c746ee34feae | 12 | unsigned char portAddress; //0x20 for COM1 |
dannyman939 | 0:c746ee34feae | 13 | unsigned short messageLength; //not including header or CRC |
dannyman939 | 0:c746ee34feae | 14 | unsigned short sequence; //typically 0 |
dannyman939 | 0:c746ee34feae | 15 | unsigned char idleTime; //Time the processor is idle, in the last second |
dannyman939 | 0:c746ee34feae | 16 | unsigned char timeStatus; //Enum Indicating quality of the GPS reference time |
dannyman939 | 0:c746ee34feae | 17 | unsigned short GPSweek; //GPS reference week |
dannyman939 | 0:c746ee34feae | 18 | unsigned long GPSTime_msecs; //from beginning of week |
dannyman939 | 0:c746ee34feae | 19 | unsigned long receiverStatus; //32-bits representing the status of hardware and software |
dannyman939 | 0:c746ee34feae | 20 | unsigned short reserved; |
dannyman939 | 0:c746ee34feae | 21 | unsigned short receiverSWversion; //receiver software build number |
dannyman939 | 0:c746ee34feae | 22 | }; |
dannyman939 | 0:c746ee34feae | 23 | MESSAGEHEADER *msgHeader[4]; |
dannyman939 | 0:c746ee34feae | 24 | |
dannyman939 | 0:c746ee34feae | 25 | #pragma pack(1) |
dannyman939 | 0:c746ee34feae | 26 | //structure for OEM615 BESTVEL log message (page 314) |
dannyman939 | 0:c746ee34feae | 27 | struct OEM615BESTVEL |
dannyman939 | 0:c746ee34feae | 28 | { |
dannyman939 | 0:c746ee34feae | 29 | MESSAGEHEADER msgHeader; |
dannyman939 | 0:c746ee34feae | 30 | int solStatus; //solution status |
dannyman939 | 0:c746ee34feae | 31 | //solutionStatusOEMStar solStatus; //solution status |
dannyman939 | 0:c746ee34feae | 32 | int velType; //position or velocity type |
dannyman939 | 0:c746ee34feae | 33 | //velTypeOEMStar posType; //position or velocity type |
dannyman939 | 0:c746ee34feae | 34 | float latency; |
dannyman939 | 0:c746ee34feae | 35 | float age; |
dannyman939 | 0:c746ee34feae | 36 | double horizontalSpeed; //horizontal velocity vector magnitude (m/s) |
dannyman939 | 0:c746ee34feae | 37 | double heading; //deg from North called TRK GND in specification |
dannyman939 | 0:c746ee34feae | 38 | double verticalSpeed; //vertical velocity magnitude (m/s) |
dannyman939 | 0:c746ee34feae | 39 | float reserved; |
dannyman939 | 0:c746ee34feae | 40 | unsigned long CRC; |
dannyman939 | 0:c746ee34feae | 41 | }; |
dannyman939 | 0:c746ee34feae | 42 | |
jekain314 | 6:2a8486283198 | 43 | /* Solution Status descritpion from OEMV manual |
jekain314 | 6:2a8486283198 | 44 | 0 SOL_COMPUTED Solution computed |
jekain314 | 6:2a8486283198 | 45 | 1 INSUFFICIENT_OBS Insufficient observations |
jekain314 | 6:2a8486283198 | 46 | 2 NO_CONVERGENCE No convergence |
jekain314 | 6:2a8486283198 | 47 | 3 SINGULARITY Singularity at parameters matrix |
jekain314 | 6:2a8486283198 | 48 | 4 COV_TRACE Covariance trace exceeds maximum (trace > 1000 m) |
jekain314 | 6:2a8486283198 | 49 | 5 TEST_DIST Test distance exceeded (maximum of 3 rejections if distance >10 km) |
jekain314 | 6:2a8486283198 | 50 | 6 COLD_START Not yet converged from cold start |
jekain314 | 6:2a8486283198 | 51 | 7 V_H_LIMIT Height or velocity limits exceeded |
jekain314 | 6:2a8486283198 | 52 | 8 VARIANCE Variance exceeds limits |
jekain314 | 6:2a8486283198 | 53 | 9 RESIDUALS Residuals are too large |
jekain314 | 6:2a8486283198 | 54 | 10 DELTA_POS Delta position is too large |
jekain314 | 6:2a8486283198 | 55 | 11 NEGATIVE_VAR Negative variance |
jekain314 | 6:2a8486283198 | 56 | 12 Reserved |
jekain314 | 6:2a8486283198 | 57 | 13 INTEGRITY_WARNING Large residuals make position unreliable |
jekain314 | 6:2a8486283198 | 58 | 18 PENDING |
jekain314 | 6:2a8486283198 | 59 | 19 INVALID_FIX The fixed position, entered using the FIX POSITION command, is not valid |
jekain314 | 6:2a8486283198 | 60 | 20 UNAUTHORIZED Position type is unauthorized - HP or XP |
jekain314 | 6:2a8486283198 | 61 | */ |
jekain314 | 6:2a8486283198 | 62 | |
dannyman939 | 0:c746ee34feae | 63 | #pragma pack(1) |
dannyman939 | 0:c746ee34feae | 64 | //structure for BESTPOS message |
dannyman939 | 0:c746ee34feae | 65 | struct OEM615BESTPOS |
dannyman939 | 0:c746ee34feae | 66 | { |
dannyman939 | 0:c746ee34feae | 67 | MESSAGEHEADER msgHeader; |
dannyman939 | 0:c746ee34feae | 68 | int solStatus; //solution status |
dannyman939 | 0:c746ee34feae | 69 | //solutionStatusOEMStar solStatus; //solution status |
dannyman939 | 0:c746ee34feae | 70 | int posType; //position or velocity type |
dannyman939 | 0:c746ee34feae | 71 | //posTypeOEMStar posType; //position or velocity type |
dannyman939 | 0:c746ee34feae | 72 | double latitude; //latitude |
dannyman939 | 0:c746ee34feae | 73 | double longitude; //longitude |
dannyman939 | 0:c746ee34feae | 74 | double height; //height above mean sea level |
dannyman939 | 0:c746ee34feae | 75 | float undulation; //the realtionship between the geoid and the |
dannyman939 | 0:c746ee34feae | 76 | //ellipsoid of the chosen datum (m) |
dannyman939 | 0:c746ee34feae | 77 | char datumID[4]; //datum ID is actually an enum that is not implemented |
dannyman939 | 0:c746ee34feae | 78 | float latitudeSTD; //latitude standard deviation |
dannyman939 | 0:c746ee34feae | 79 | float longitudeSTD; //longitude standard deviation |
dannyman939 | 0:c746ee34feae | 80 | float heightSTD; //height standard deviation |
dannyman939 | 0:c746ee34feae | 81 | char baseStationID[4]; //base station ID |
dannyman939 | 0:c746ee34feae | 82 | float diffAge; //differential age (s) |
dannyman939 | 0:c746ee34feae | 83 | float solutionAge; //solution age (s) |
dannyman939 | 0:c746ee34feae | 84 | unsigned char numSV; //number of satellite vehicles tracked |
dannyman939 | 0:c746ee34feae | 85 | unsigned char numSolSV; //number of satellite vehicles used in solution |
dannyman939 | 0:c746ee34feae | 86 | unsigned char numGGL1; //number of GPS plus Glonass L1 |
dannyman939 | 0:c746ee34feae | 87 | unsigned char res1; |
dannyman939 | 0:c746ee34feae | 88 | unsigned char res2; |
dannyman939 | 0:c746ee34feae | 89 | unsigned char extSolStatus; //extended solution status |
dannyman939 | 0:c746ee34feae | 90 | unsigned char res3; |
dannyman939 | 0:c746ee34feae | 91 | unsigned char sigMask; //signals used mask |
dannyman939 | 0:c746ee34feae | 92 | unsigned long CRC; |
dannyman939 | 0:c746ee34feae | 93 | }; |
dannyman939 | 0:c746ee34feae | 94 | |
jekain314 | 6:2a8486283198 | 95 | //GPS-specific pins |
jekain314 | 6:2a8486283198 | 96 | DigitalOut GPS_Reset(p18); //GPS RESET line |
jekain314 | 6:2a8486283198 | 97 | InterruptIn PPSInt(p15); // GPS 1PPS (timemark) from the OEM615 |
jekain314 | 6:2a8486283198 | 98 | InterruptIn IMUClock(p17); |
jekain314 | 6:2a8486283198 | 99 | |
jekain314 | 6:2a8486283198 | 100 | Timer timeFromPPS; |
jekain314 | 6:2a8486283198 | 101 | unsigned long GPSTimemsecs = 0; |
jekain314 | 6:2a8486283198 | 102 | double GPSTime = 0; |
jekain314 | 6:2a8486283198 | 103 | int PPSTimeOffset = 0; |
jekain314 | 6:2a8486283198 | 104 | |
jekain314 | 6:2a8486283198 | 105 | //mbed tx/rx interface to the GPS COM1 port |
jekain314 | 6:2a8486283198 | 106 | MODSERIAL GPS_COM1(p9,p10); //this serial port communicates with the GPS receiver serial port (COM1) |
jekain314 | 6:2a8486283198 | 107 | |
jekain314 | 6:2a8486283198 | 108 | int test = 0; |
jekain314 | 6:2a8486283198 | 109 | unsigned short messageCounter = 0; |
jekain314 | 6:2a8486283198 | 110 | unsigned short savedMessageCounter = 0; |
jekain314 | 6:2a8486283198 | 111 | const unsigned short maxGPSbytesPerSec = 1536; |
jekain314 | 6:2a8486283198 | 112 | unsigned char msgBuffer[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes |
jekain314 | 6:2a8486283198 | 113 | const unsigned char maxGPSMessagesPerSec = 12; |
jekain314 | 6:2a8486283198 | 114 | unsigned short messageLocation[maxGPSMessagesPerSec] = {0}; //stores the message location start within the message buffer |
dannyman939 | 0:c746ee34feae | 115 | |
dannyman939 | 0:c746ee34feae | 116 | //this code was taken from the Novatel Firmware document page 35 |
dannyman939 | 0:c746ee34feae | 117 | #define CRC32_POLYNOMIAL 0xEDB88320L |
dannyman939 | 0:c746ee34feae | 118 | /* -------------------------------------------------------------------------- |
dannyman939 | 0:c746ee34feae | 119 | Calculate a CRC value to be used by CRC calculation functions. |
dannyman939 | 0:c746ee34feae | 120 | -------------------------------------------------------------------------- */ |
dannyman939 | 0:c746ee34feae | 121 | unsigned long CRC32Value(int i) |
dannyman939 | 0:c746ee34feae | 122 | { |
dannyman939 | 0:c746ee34feae | 123 | int j; |
dannyman939 | 0:c746ee34feae | 124 | unsigned long ulCRC; |
dannyman939 | 0:c746ee34feae | 125 | ulCRC = i; |
dannyman939 | 0:c746ee34feae | 126 | for ( j = 8 ; j > 0; j-- ) |
dannyman939 | 0:c746ee34feae | 127 | { |
dannyman939 | 0:c746ee34feae | 128 | if ( ulCRC & 1 ) |
dannyman939 | 0:c746ee34feae | 129 | ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL; |
dannyman939 | 0:c746ee34feae | 130 | else |
dannyman939 | 0:c746ee34feae | 131 | ulCRC >>= 1; |
dannyman939 | 0:c746ee34feae | 132 | } |
dannyman939 | 0:c746ee34feae | 133 | return ulCRC; |
dannyman939 | 0:c746ee34feae | 134 | } |
dannyman939 | 0:c746ee34feae | 135 | |
dannyman939 | 0:c746ee34feae | 136 | /* -------------------------------------------------------------------------- |
dannyman939 | 0:c746ee34feae | 137 | Calculates the CRC-32 of a block of data all at once |
dannyman939 | 0:c746ee34feae | 138 | //the CRC is c from the complete message (header plus data) but excluding (of course) the CRC at the end |
dannyman939 | 0:c746ee34feae | 139 | -------------------------------------------------------------------------- */ |
dannyman939 | 0:c746ee34feae | 140 | unsigned long CalculateBlockCRC32( |
dannyman939 | 0:c746ee34feae | 141 | unsigned long ulCount, /* Number of bytes in the data block */ |
dannyman939 | 0:c746ee34feae | 142 | unsigned char *ucBuffer ) /* Data block */ |
dannyman939 | 0:c746ee34feae | 143 | { |
dannyman939 | 0:c746ee34feae | 144 | unsigned long ulTemp1; |
dannyman939 | 0:c746ee34feae | 145 | unsigned long ulTemp2; |
dannyman939 | 0:c746ee34feae | 146 | unsigned long ulCRC = 0; |
dannyman939 | 0:c746ee34feae | 147 | while ( ulCount-- != 0 ) |
dannyman939 | 0:c746ee34feae | 148 | { |
dannyman939 | 0:c746ee34feae | 149 | ulTemp1 = ( ulCRC >> 8 ) & 0x00FFFFFFL; |
dannyman939 | 0:c746ee34feae | 150 | ulTemp2 = CRC32Value( ((int) ulCRC ^ *ucBuffer++ ) & 0xff ); |
dannyman939 | 0:c746ee34feae | 151 | ulCRC = ulTemp1 ^ ulTemp2; |
dannyman939 | 0:c746ee34feae | 152 | } |
dannyman939 | 0:c746ee34feae | 153 | return( ulCRC ); |
dannyman939 | 0:c746ee34feae | 154 | } |
dannyman939 | 0:c746ee34feae | 155 | |
jekain314 | 6:2a8486283198 | 156 | |
jekain314 | 6:2a8486283198 | 157 | void sendASCII(char* ASCI_message, int numChars) |
jekain314 | 6:2a8486283198 | 158 | { |
jekain314 | 6:2a8486283198 | 159 | ///////////////////////////////////////////////// |
jekain314 | 6:2a8486283198 | 160 | //send an ASCII command to the GPS receiver |
jekain314 | 6:2a8486283198 | 161 | ///////////////////////////////////////////////// |
jekain314 | 6:2a8486283198 | 162 | |
jekain314 | 6:2a8486283198 | 163 | //char ASCI_message[] = "unlogall COM1"; |
jekain314 | 6:2a8486283198 | 164 | int as = numChars - 1; |
jekain314 | 6:2a8486283198 | 165 | unsigned char CR = 0x0d; //ASCII Carriage Return |
jekain314 | 6:2a8486283198 | 166 | unsigned char LF = 0x0a; //ASCII Line Feed |
jekain314 | 6:2a8486283198 | 167 | |
jekain314 | 6:2a8486283198 | 168 | //printf("%s", ch); |
jekain314 | 6:2a8486283198 | 169 | //printf("\n"); |
jekain314 | 6:2a8486283198 | 170 | |
jekain314 | 6:2a8486283198 | 171 | for (int i=0; i<as; i++) GPS_COM1.putc(ASCI_message[i]); |
jekain314 | 6:2a8486283198 | 172 | GPS_COM1.putc(CR); //carriage return at end |
jekain314 | 6:2a8486283198 | 173 | GPS_COM1.putc(LF); //line feed at end |
jekain314 | 6:2a8486283198 | 174 | }; |
jekain314 | 6:2a8486283198 | 175 | |
dannyman939 | 0:c746ee34feae | 176 | |
jekain314 | 6:2a8486283198 | 177 | //see the mbed COOKBOOK for MODSERIAL |
jekain314 | 6:2a8486283198 | 178 | //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output. |
jekain314 | 6:2a8486283198 | 179 | void readSerialByte(MODSERIAL_IRQ_INFO *q) |
jekain314 | 6:2a8486283198 | 180 | { |
jekain314 | 6:2a8486283198 | 181 | MODSERIAL *serial = q->serial; |
jekain314 | 6:2a8486283198 | 182 | unsigned char synch0 = serial->getc(); |
jekain314 | 6:2a8486283198 | 183 | msgBuffer[byteCounter % maxGPSbytesPerSec] = synch0; |
dannyman939 | 0:c746ee34feae | 184 | |
jekain314 | 6:2a8486283198 | 185 | //we need to trap the GPS message header byte-string 0xAA44121C |
jekain314 | 6:2a8486283198 | 186 | //generate a 4-byte sliding-window sequence from the input bytes |
jekain314 | 6:2a8486283198 | 187 | //shift last 4-byte value left 8 bits & push recently read byte (synch0) into low-order byte |
jekain314 | 6:2a8486283198 | 188 | test = (test<<8) | synch0; // |
jekain314 | 6:2a8486283198 | 189 | |
jekain314 | 6:2a8486283198 | 190 | if (test == 0xAA44121C) //test for the Receiver message header signature |
jekain314 | 6:2a8486283198 | 191 | { |
jekain314 | 6:2a8486283198 | 192 | messageLocation[perSecMessageCounter % maxGPSMessagesPerSec] = byteCounter-3; //store the location of this message (with 4 synch words) |
jekain314 | 6:2a8486283198 | 193 | perSecMessageCounter++; |
jekain314 | 6:2a8486283198 | 194 | messageDetected = true; |
jekain314 | 6:2a8486283198 | 195 | } |
jekain314 | 6:2a8486283198 | 196 | //byteCounter reset to zero in main after the 1PPS is detected -- its NOT reset in the 1PPS ISR |
jekain314 | 6:2a8486283198 | 197 | byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected) |
jekain314 | 6:2a8486283198 | 198 | |
jekain314 | 6:2a8486283198 | 199 | }; |
dannyman939 | 0:c746ee34feae | 200 | |
dannyman939 | 0:c746ee34feae | 201 | |
dannyman939 | 0:c746ee34feae | 202 | |
jekain314 | 6:2a8486283198 | 203 | |
jekain314 | 6:2a8486283198 | 204 | |
jekain314 | 6:2a8486283198 | 205 | |
jekain314 | 6:2a8486283198 | 206 |