JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
OEM615.h
- Committer:
- jekain314
- Date:
- 2013-04-19
- Revision:
- 9:b45feb91ba38
- Parent:
- 6:2a8486283198
File content as of revision 9:b45feb91ba38:
#pragma pack(1) //this forces the structure to be packed on byte boundaries (with no byte filler) //set up the GPS message header in a structure to enable easy reading -- see the OEM615 manual (Table 4, page 24) struct MESSAGEHEADER { char synchAA; //1st synch word char synch44; //2nd synch word char synch12; //3rd synch word unsigned char headerLength; //always 28 unsigned short messageID; //42 (0x2A BESTPOS) , 43 (2B RANGE) , or 99 (x63 BESTVEL), char messageType; //always = 0 for binary unsigned char portAddress; //0x20 for COM1 // from spec: The length in bytes of the body of the message, not including the header nor the CRC unsigned short messageLength; //not including header or CRC unsigned short sequence; //typically 0 unsigned char idleTime; //Time the processor is idle, in the last second unsigned char timeStatus; //Enum Indicating quality of the GPS reference time unsigned short GPSweek; //GPS reference week unsigned long GPSTime_msecs; //from beginning of week unsigned long receiverStatus; //32-bits representing the status of hardware and software unsigned short reserved; unsigned short receiverSWversion; //receiver software build number //total length in bytes of this header is 28 }; MESSAGEHEADER *msgHeader[6]; #pragma pack(1) //structure for OEM615 BESTVEL log message (page 314) struct OEM615BESTVEL { MESSAGEHEADER msgHeader; int solStatus; //solution status //solutionStatusOEMStar solStatus; //solution status int velType; //position or velocity type //velTypeOEMStar posType; //position or velocity type float latency; float age; double horizontalSpeed; //horizontal velocity vector magnitude (m/s) double heading; //deg from North called TRK GND in specification double verticalSpeed; //vertical velocity magnitude (m/s) float reserved; unsigned long CRC; }; /* Solution Status descritpion from OEMV manual 0 SOL_COMPUTED Solution computed 1 INSUFFICIENT_OBS Insufficient observations 2 NO_CONVERGENCE No convergence 3 SINGULARITY Singularity at parameters matrix 4 COV_TRACE Covariance trace exceeds maximum (trace > 1000 m) 5 TEST_DIST Test distance exceeded (maximum of 3 rejections if distance >10 km) 6 COLD_START Not yet converged from cold start 7 V_H_LIMIT Height or velocity limits exceeded 8 VARIANCE Variance exceeds limits 9 RESIDUALS Residuals are too large 10 DELTA_POS Delta position is too large 11 NEGATIVE_VAR Negative variance 12 Reserved 13 INTEGRITY_WARNING Large residuals make position unreliable 18 PENDING 19 INVALID_FIX The fixed position, entered using the FIX POSITION command, is not valid 20 UNAUTHORIZED Position type is unauthorized - HP or XP */ #pragma pack(1) //structure for BESTPOS message struct OEM615BESTPOS { MESSAGEHEADER msgHeader; int solStatus; //solution status //solutionStatusOEMStar solStatus; //solution status int posType; //position or velocity type //posTypeOEMStar posType; //position or velocity type double latitude; //latitude double longitude; //longitude double height; //height above mean sea level float undulation; //the realtionship between the geoid and the //ellipsoid of the chosen datum (m) char datumID[4]; //datum ID is actually an enum that is not implemented float latitudeSTD; //latitude standard deviation float longitudeSTD; //longitude standard deviation float heightSTD; //height standard deviation char baseStationID[4]; //base station ID float diffAge; //differential age (s) float solutionAge; //solution age (s) unsigned char numSV; //number of satellite vehicles tracked unsigned char numSolSV; //number of satellite vehicles used in solution unsigned char numGGL1; //number of GPS plus Glonass L1 unsigned char res1; unsigned char res2; unsigned char extSolStatus; //extended solution status unsigned char res3; unsigned char sigMask; //signals used mask unsigned long CRC; }; //GPS-specific pins DigitalOut GPS_Reset(p18); //GPS RESET line InterruptIn PPSInt(p15); // GPS 1PPS (timemark) from the OEM615 InterruptIn IMUClock(p17); Timer timeFromPPS; unsigned long GPSTimemsecs = 0; double GPSTime = 0; int PPSTimeOffset = 0; //mbed tx/rx interface to the GPS COM1 port MODSERIAL GPS_COM1(p9,p10); //this serial port communicates with the GPS receiver serial port (COM1) int test = 0; unsigned short messageCounter = 0; unsigned short savedMessageCounter = 0; const unsigned short maxGPSbytesPerSec = 1536; unsigned char msgBuffer[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes const unsigned char maxGPSMessagesPerSec = 12; unsigned short messageLocation[maxGPSMessagesPerSec] = {0}; //stores the message location start within the message buffer unsigned short bytesFromMessageHdrDetect = 0; union SWAPBYTES { unsigned char b[2]; unsigned short w; }; volatile SWAPBYTES messageLength; //used to swap the bytes unsigned long computedCRC = 0; //final resulting computed CRC for this message unsigned long incrementalCRC = 0; //incrementally-formed CRC over message sequence unsigned short endByteForCRCcomputation = 0; bool completeMessageAvailable = false; //this code was taken from the Novatel Firmware document page 35 //#define CRC32_POLYNOMIAL 0xEDB88320L /* -------------------------------------------------------------------------- Calculate a CRC value to be used by CRC calculation functions. -------------------------------------------------------------------------- */ /* //////////////////////////////////////////// //original code from the OEM615 manual //////////////////////////////////////////// unsigned long CRC32Value(int i) { int j; unsigned long ulCRC; ulCRC = i; for ( j = 8 ; j > 0; j-- ) { if ( ulCRC & 1 ) ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL; else ulCRC >>= 1; } return ulCRC; } */ #define CRC32_POLYNOMIAL 0xEDB88320L void CRC32Value(unsigned long &CRC, unsigned char c) { ///////////////////////////////////////////////////////////////////////////////////// //CRC must be initialized as zero //c is a character from the sequence that is used to form the CRC //this code is a modification of the code from the Novatel OEM615 specification ///////////////////////////////////////////////////////////////////////////////////// unsigned long ulTemp1 = ( CRC >> 8 ) & 0x00FFFFFFL; unsigned long ulCRC = ((int) CRC ^ c ) & 0xff ; for (int j = 8 ; j > 0; j-- ) { if ( ulCRC & 1 ) ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL; else ulCRC >>= 1; } CRC = ulTemp1 ^ ulCRC; } /* -------------------------------------------------------------------------- Calculates the CRC-32 of a block of data all at once //the CRC is from the complete message (header plus data) //but excluding (of course) the CRC at the end -------------------------------------------------------------------------- */ unsigned long CalculateBlockCRC32( unsigned long ulCount, /* Number of bytes in the data block */ unsigned char *ucBuffer ) /* Data block */ { ////////////////////////////////////////////////////////////////////// //the below code tests the CRC32Value procedure used in a markov form ////////////////////////////////////////////////////////////////////// unsigned long CRC = 0; for (int i = 0; i<ulCount; i++) CRC32Value( CRC, *ucBuffer++ ); return CRC; } /* unsigned long CalculateBlockCRC32( unsigned long ulCount, unsigned char *ucBuffer ) { //////////////////////////////////////////// //original code from the OEM615 manual //////////////////////////////////////////// unsigned long ulTemp1; unsigned long ulTemp2; unsigned long ulCRC = 0; while ( ulCount-- != 0 ) { ulTemp1 = ( ulCRC >> 8 ) & 0x00FFFFFFL; ulTemp2 = CRC32Value( ((int) ulCRC ^ *ucBuffer++ ) & 0xff ); ulCRC = ulTemp1 ^ ulTemp2; } return( ulCRC ); } */ void sendASCII(char* ASCI_message, int numChars) { ///////////////////////////////////////////////// //send an ASCII command to the GPS receiver ///////////////////////////////////////////////// //char ASCI_message[] = "unlogall COM1"; int as = numChars - 1; unsigned char CR = 0x0d; //ASCII Carriage Return unsigned char LF = 0x0a; //ASCII Line Feed //printf("%s", ch); //printf("\n"); for (int i=0; i<as; i++) GPS_COM1.putc(ASCI_message[i]); GPS_COM1.putc(CR); //carriage return at end GPS_COM1.putc(LF); //line feed at end }; //see the mbed COOKBOOK for MODSERIAL //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output. void readSerialByte(MODSERIAL_IRQ_INFO *q) { MODSERIAL *serial = q->serial; //see example of MODSERIAL usage in cookbook unsigned char synch0 = serial->getc(); //get the next byte //byteCounter is zeroed only at a 1PPA event in the 1PPS ISR //all message bytes stored for a single GPS second msgBuffer[byteCounter % maxGPSbytesPerSec] = synch0; //accumulate the CRC for this message //incrementalCRC re-initialized after message header is is detected CRC32Value( incrementalCRC, synch0); //Trap the GPS message header byte-string per Novatel OEM615 spec: 0xAA44121C //generate a 4-byte sliding-window sequence from the input bytes //shift last 4-byte value left 8 bits & push current-read byte (synch0) into low-order byte test = (test<<8) | synch0; if (test == 0xAA44121C) //test for the Receiver message header signature { messageLocation[perSecMessageCounter % maxGPSMessagesPerSec] = byteCounter-3; //store the location of this message (1st of 4 synch word) perSecMessageCounter++; //counts messages this second bytesFromMessageHdrDetect = 0; //start byte counter for this message incrementalCRC = 0x39b0f0e1; //initializes the recursive CRC after the AA44121C header byte sequence } else if (bytesFromMessageHdrDetect == 5) messageLength.b[0] = synch0; //first byte of msg length else if (bytesFromMessageHdrDetect == 6) //second byte of message length { messageLength.b[1] = synch0; endByteForCRCcomputation = 24 + messageLength.w; //use union to perform byte-swap from stored bytes } else if (bytesFromMessageHdrDetect == endByteForCRCcomputation) //stop the CRC recursive computation computedCRC = incrementalCRC; //store the computed CRC for this message for use in main else if (bytesFromMessageHdrDetect == (endByteForCRCcomputation + 4) ) //detect the end of the message (end of its CRC) { savedMessageCounter = perSecMessageCounter; //message counter can be corrupted before use in main completeMessageAvailable = true; //set flg for the main message processing } //byteCounter reset to zero in main after the 1PPS is detected -- its NOT reset in the 1PPS ISR byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected) bytesFromMessageHdrDetect++; //counts the byes received after a message header };