JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
OEM615.h
- Committer:
- jekain314
- Date:
- 2013-04-02
- Revision:
- 6:2a8486283198
- Parent:
- 4:68268737ff89
- Child:
- 9:b45feb91ba38
File content as of revision 6:2a8486283198:
#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 (BESTPOS) , 43 (RANGE) , or 99 (BESTVEL), char messageType; //always = 0 for binary unsigned char portAddress; //0x20 for COM1 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 }; MESSAGEHEADER *msgHeader[4]; #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 //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. -------------------------------------------------------------------------- */ 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; } /* -------------------------------------------------------------------------- Calculates the CRC-32 of a block of data all at once //the CRC is c 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 */ { 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; unsigned char synch0 = serial->getc(); msgBuffer[byteCounter % maxGPSbytesPerSec] = synch0; //we need to trap the GPS message header byte-string 0xAA44121C //generate a 4-byte sliding-window sequence from the input bytes //shift last 4-byte value left 8 bits & push recently 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 (with 4 synch words) perSecMessageCounter++; messageDetected = true; } //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) };