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
Revision 29:dead10cce6e9, committed 2014-01-09
- Comitter:
- jekain314
- Date:
- Thu Jan 09 14:09:05 2014 +0000
- Parent:
- 28:fcea53fcc712
- Child:
- 30:96d133f3008e
- Commit message:
- initial commit
Changed in this revision
--- a/ADIS16488.h Wed Nov 13 22:12:04 2013 +0000 +++ b/ADIS16488.h Thu Jan 09 14:09:05 2014 +0000 @@ -22,16 +22,13 @@ union WD { long dataWord; unsigned short pt[2];} wd; //IMU records are buffered in the IMUDataReady ISR -const unsigned char IMUrecArraySize = 30; +const unsigned char IMUrecArraySize = 10; #pragma pack(1) struct IMUREC { - unsigned long synch; - unsigned short msgID; unsigned long GPSTime; long dataWord[6]; - // 4 + 2 + 4 + 24 = 34 }; IMUREC imuPing[IMUrecArraySize]; @@ -39,7 +36,6 @@ IMUREC tempRec; volatile bool fillingPingWritingPong = true; - unsigned long maxDelIMUmsecs = 0; unsigned long delIMUmsecs = 0; unsigned long lastIMUmsecs = 0; @@ -66,7 +62,8 @@ if ( fillingPingWritingPong) tempRec.dataWord[i] = wd.dataWord; //data word is a signed long else tempRec.dataWord[i] = wd.dataWord; //data word is a signed long } - + + //fill the correct buffer ping or pong if (fillingPingWritingPong) imuPing[IMUClockCounter] = tempRec; else imuPong[IMUClockCounter] = tempRec; @@ -110,13 +107,7 @@ //change the page to 0 to get the data spi.write((int)0x8000); //change to page 0 - - //toPC.printf(" setting the default values\n"); - - //set the IMU synch and message ID - tempRec.synch = 0x1C1244AA; //same as the GPS synch words - tempRec.msgID = 111; //IMU record ID - - //toPC.printf(" finished setting the default values\n"); } + +
--- a/OEM615.h Wed Nov 13 22:12:04 2013 +0000 +++ b/OEM615.h Thu Jan 09 14:09:05 2014 +0000 @@ -1,100 +1,5 @@ #include "crc.h" -#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 @@ -106,21 +11,20 @@ //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 +bool loadingMessageBuffer = false; + +const unsigned short maxGPSbytesPerSec = 512; -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[maxGPSMessagesPerSec]; -bool completeMessageAvailable = false; +int messagePerSecCounter = 0; +unsigned char msgBuffer0[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes +unsigned char msgBuffer1[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes +unsigned char msgBuffer2[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes +int GPSbyteCounter0 = 0; +int GPSbyteCounter1 = 0; +int GPSbyteCounter2 = 0; +bool message0Complete = false; +bool message1Complete = false; +bool message2Complete = false; void sendASCII(char* ASCI_message, int numChars) { @@ -149,48 +53,38 @@ 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); + totalGPSBytes++; - //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; - - //alternative 8-byte test (uses a long long 64bit word) - // AA44121C002A0020 //message 42 (BESTPOS) - // AA44121C002B0020 //message 43 (BESTVEL) - // AA44121C00990020 //message 99 (RANGE) - - if (test == 0xAA44121C) //test for the Receiver message header signature + //all OEM615 GPS ASCII messages begin with the unique character: "#" + //read til we get a "#" and then start storing the message + if (synch0 == '#') { - 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[perSecMessageCounter] = 24 + messageLength.w; //use union to perform byte-swap from stored bytes - } - else if (bytesFromMessageHdrDetect == endByteForCRCcomputation[perSecMessageCounter]) //stop the CRC recursive computation - computedCRC = incrementalCRC; //store the computed CRC for this message for use in main - else if (bytesFromMessageHdrDetect == (endByteForCRCcomputation[perSecMessageCounter] + 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 + if (messagePerSecCounter == 0) GPSbyteCounter0 = 0; + else if(messagePerSecCounter == 1) GPSbyteCounter1 = 0; + else if(messagePerSecCounter == 2) GPSbyteCounter2 = 0; + loadingMessageBuffer = true; + } + + if (messagePerSecCounter == 0) { msgBuffer0[GPSbyteCounter0 % maxGPSbytesPerSec] = synch0; GPSbyteCounter0++; } + else if(messagePerSecCounter == 1) { msgBuffer1[GPSbyteCounter1 % maxGPSbytesPerSec] = synch0; GPSbyteCounter1++; } + else if(messagePerSecCounter == 2) { msgBuffer2[GPSbyteCounter2 % maxGPSbytesPerSec] = synch0; GPSbyteCounter2++; } + + //stop storing the message when we get a LF + if (synch0 == 0x0a /* LF*/) //test for line feed + { + if (messagePerSecCounter == 0) message0Complete = true; + else if(messagePerSecCounter == 1) message1Complete = true; + else if(messagePerSecCounter == 2) message2Complete = true; + messagePerSecCounter++; //count the messages per second + } + + //how this can fail?? + // 1) get noisy # occurrences (unique starting character) + // 2) get noisy LF occurrences (unique ending character) + // 3) get noisy data packet values or extra values + // 4) we will also need to vet the data on the PC side + // 5) here, we should do minimal testing and just pass the data as is + };
--- a/PCMessaging.h Wed Nov 13 22:12:04 2013 +0000 +++ b/PCMessaging.h Thu Jan 09 14:09:05 2014 +0000 @@ -1,382 +1,40 @@ //these are defines for the messages that are sent from the PC across the USB //these messages produce reactions on the mbed -const unsigned char POSVEL_MSG =0; -const unsigned char FIRE_TRIGGER_MSG =1; -const unsigned char STATUS_MSG =2; -const unsigned char STARTDATA_MSG =3; -const unsigned char STOPDATA_MSG =4; -const unsigned char STARTSTREAM_MSG =5; -const unsigned char STOPSTREAM_MSG =6; -const unsigned char STARTLOGINFO_MSG =7; -const unsigned char STOPLOGINFO_MSG =8; -const unsigned char GETFILE_MSG =9; // added to get a file from the SD card - -const double DEGREES_TO_RADIANS = acos(-1.0)/180.0; -const double eccen = 0.0818191908426; //WGS84 earth eccentricity -const double earthRadius = 6378137; //WGS84 earthRadius in meters +const unsigned char FIRE_TRIGGER_MSG = 1; const unsigned short serBuffMax = 18; char serBuf[serBuffMax]; int serBufChars=0; //flags to control the PC command actions -bool sendPosVel =false; -bool sendStatus =false; -bool sendRecData =false; -bool streamPos =false; -bool sendStreamPos =false; -bool logMsgInfo =false; -bool sendLogMsgInfo =false; -bool recordData =false; bool fireTrigger =false; -bool get_file_msg =false; // added for GETFILE command - -const unsigned char numMessages = 10; //number of potential messages (updated to 10 sg-) -char msgList[numMessages][32]; //text array storing the command messages from the PC -char minMessageSize = 11; //minimum size of a text message unsigned char CR = 0x0d; //ASCII Carriage Return unsigned char LF = 0x0a; //ASCII Line Feed -char preamble[5] = "WMsg"; -char testPreamble[5]; bool validMessage = false; -void setUpMessages(void) -{ - //set up the ASCII text records that are candidates to be passed from the PC - sprintf(msgList[STATUS_MSG], "WMsg STATUS"); - sprintf(msgList[POSVEL_MSG], "WMsg POSVEL"); - sprintf(msgList[STARTDATA_MSG], "WMsg RECORDDATA Y"); - sprintf(msgList[STOPDATA_MSG], "WMsg RECORDDATA N"); - sprintf(msgList[STARTSTREAM_MSG], "WMsg POSSTREAM Y"); - sprintf(msgList[STOPSTREAM_MSG], "WMsg POSSTREAM N"); - sprintf(msgList[STARTLOGINFO_MSG], "WMsg LOGINFO Y"); - sprintf(msgList[STOPLOGINFO_MSG], "WMsg LOGINFO N"); - sprintf(msgList[FIRE_TRIGGER_MSG], "WMsg TRIGGER"); - sprintf(msgList[GETFILE_MSG], "WMsg GETFILE"); // added to get the file off of sd cards - //message length is from 10 to 16 chars - - // toPC.printf(" finished setting up messages \n"); -} - void readFromPC() { // should this be a while rather than if ??? -- may have multiple bytes in buffer if (toPC.readable()) //read a PC serial byte and test it for a command { - // Read in next character -- why not read all available?? + // Read in next character + // why not read all available bytes unsigned char inChar = 0; inChar = toPC.getc(); //read char from the USB serial link to the PC - //toPC.printf("%02x ",inChar); //incoming messages will end witb a CR / LF -- disregard these chars if (inChar == CR || inChar == LF) return; //CR is a 0x0a - //all received messages assumed to have a WMsg preamble - //if we have read 4 chars, test these for "WMsg", if they are not WMsg, reset the buffer counter - //if we receive an occasional bad byte that messes up a message, this will resynch fast to a next message - //this will let us miss a message --- but hopefully only one - - // serBuffMax = 18 -- largest serBuffMax is 16 -- but we add one below for the '\0' - // if the following occurs we have had a trash byte following a WMsg header - if (serBufChars >= (serBuffMax-2)) {toPC.printf("WMsg restart message search\n"); serBufChars = 0; } - - serBuf[serBufChars] = inChar; //set this char in a char array for testing complete message - - testPreamble[serBufChars] = inChar; //char array for testing the preamble - serBufChars++; - - if (serBufChars == 4) //four initial chars detected (0, 1, 2, 3) - { - testPreamble[4] = '\0'; //form null-terminated string for compare - if (strncmp(testPreamble, preamble, 5) != 0) //compare the chars to the WMsg preamble - { - toPC.printf("WMsg preamble mismatch \n"); - serBufChars = 0; //if they dont match -- restart the search for a preamble - return; - } - } - - //if we get here, we have found a preamble and we are looking for a complete message - //no need to continue if numChars are less than the shortest candidate message - if (serBufChars < minMessageSize) return; - - // Append end of string - // We always assume we have a complete message string and test for this below - serBuf[serBufChars] = '\0'; - - validMessage = false; - - // Check for valid message -- there are numMessages possible messages - //this assumes that the message buffer contains an exact message - for (int m = 0; m < numMessages && !validMessage; m++) //check for all messages ... - { - //toPC.printf(" \n\n found chars to test %3d %3d %s \n\n\n ", serBufChars, strlen(msgList[m]), serBuf ); - - //check the current partial message against ALL possible messages - //messages must match in both strength length and text - if (serBufChars == strlen(msgList[m]) && strncmp(serBuf, msgList[m], serBufChars) == 0 ) - { - - //toPC.printf( "\n found valid message %s \n\n", serBuf); - rxMsg = !rxMsg; - - validMessage = true; - serBufChars = 0; //reset the character count to reset for next message - - //set programmatic action flags based on the message - switch(m) - { - case STATUS_MSG: - sendStatus = true; //send a status message back to PC - break; - - case POSVEL_MSG: - sendPosVel = true; //send a posvel message back to PC - timeFromPosVelMessageReceipt.reset(); //start time and close SD card file if too long - break; - - case STARTDATA_MSG: //start the data recording to the SD card - recordData = true; - sendRecData = true; - break; - - case STOPDATA_MSG: //stop the data recording to the SD card - recordData = false; - sendRecData = true; - break; - - case STARTSTREAM_MSG: - case STOPSTREAM_MSG: - streamPos = (m == STARTSTREAM_MSG); - sendStreamPos = true; - break; - - case STARTLOGINFO_MSG: - case STOPLOGINFO_MSG: - logMsgInfo = (m == STARTLOGINFO_MSG); - sendLogMsgInfo = true; - break; - - case FIRE_TRIGGER_MSG: - fireTrigger = true; - toPC.printf("WMsg MBED received trigger command \n"); - break; - - case GETFILE_MSG: - get_file_msg = true; // signal to main that we can unload the file that was written - toPC.printf("WMsg request to get SD card file \n"); - break; - - } //end Switch statement - break; - } //end test for a valid message - - } //end message text loop + //all incoming messages will start with "mbedMessage", "messageType", numberDataBytes, and will end with CR & LF + //1) look for the mbedMessage and then get the next byte as numberDataBytes; + //2) read the next numberDataBytes and then look for CR & LF + //if 1) & 2) are successful, declare a valid incoming message + //if message is valid, then send a response as a repeat of the original message + // } //end pc.readable }; -void earthCoefficients(double latitudeRad, double longitudeRad, double height, double &latRateFac, double &lonRateFac) -{ - //compute the lat and lon factors for use in the interpolation of the lat and lon between 1 sec epochs - //latRateFac & lonRateFac multiplied by Vnorth or VEast to get latRate and lonRate - //see this document (page 32) www.fas.org/spp/military/program/nav/basicnav.pdf - - double eccenSinLat = eccen * sin(latitudeRad); - double temp1 = 1.0 - eccenSinLat*eccenSinLat; - double temp2 = sqrt(temp1); - double r_meridian = earthRadius * ( 1.0 - eccen*eccen)/ (temp1 * temp2); - double r_normal = earthRadius / temp2; - - //divide Vnorth by latRateFac to get the latitude rate in deg per sec - latRateFac = (r_meridian + height)* DEGREES_TO_RADIANS; - - //divide VEast by lonRateFac to get the longitude rate in deg per sec - lonRateFac = (r_normal + height) * cos(latitudeRad)* DEGREES_TO_RADIANS; -} - -void sendPosVelMessageToPC(OEM615BESTPOS posMsg, OEM615BESTVEL velMsg) -{ - //north and east velocity from the horizontal speed and heading - //velMsg may not be the "current" message --- but is the one also associated with a position message - double nVel = velMsg.horizontalSpeed*cos(velMsg.heading*DEGREES_TO_RADIANS); - double eVel = velMsg.horizontalSpeed*sin(velMsg.heading*DEGREES_TO_RADIANS); - - double latRateFac; - double lonRateFac; - - earthCoefficients( posMsg.latitude*DEGREES_TO_RADIANS, - posMsg.longitude*DEGREES_TO_RADIANS, - posMsg.height, - latRateFac, lonRateFac); - - //commented calculations are for a spherical earth (Chris's original computation) - // For the 1 second deltas with which we are dealing - // This calculation should be close enough for now - // Approximately 1 nautical mile / minute latitude, 60 minutes/degree, 1852 meters/nautical mile - //double latMetersPerDeg = 60.0*1852.0; - // longitude separation is approximately equal to latitude separation * cosine of latitude - //double lonMetersPerDeg = latMetersPerDeg*cos(posMsg.latitude*DEGREES_TO_RADIANS); - - // Elapsed time since last known GPS position - //PPSTimeOffset is a result of possibly missing a prior GPS position message - // timeFromPPS.read() is always the time from the most recent 1PPS - double elTime = (double)PPSTimeOffset + timeFromPPS.read(); - - //this is the best estimate of the GPS time of the requested POSVEL data - double posTime = GPSTimemsecs/1000.0 + elTime; - - // Position time -- posMsgTime is the time of the last valid GPS position message - // this time may differ from GPSTimemsecs if the last position message (42) was missed due to CRC error - double posMsgTime = posMsg.msgHeader.GPSTime_msecs/1000.0; - - //toPC.printf(" elTime = %6.3f PPSimeOffset = %6.3f \n", elTime, PPSTimeOffset); - //toPC.printf(" latRateFac = %10.3f lonRateFac = %10.3f \n", latRateFac, lonRateFac); - //toPC.printf(" latRateFac = %10.3f lonRateFac = %10.3f \n", latMetersPerDeg, lonMetersPerDeg); - - // Estimated position based on previous position and velocity - // posMsg is the last time when the BESTVEL and BESTPOS messages had identical times - //double latPos = posMsg.latitude + (nVel/latMetersPerDeg)*elTime; - //double lonPos = posMsg.longitude + (eVel/lonMetersPerDeg)*elTime; - - double latPos = posMsg.latitude + (nVel/latRateFac)*(posTime - posMsgTime); - double lonPos = posMsg.longitude + (eVel/lonRateFac)*(posTime - posMsgTime); - double htPos = posMsg.height + velMsg.verticalSpeed/(60*1852)*elTime; - - char solReady = 'N'; - //solStatus - if (posMsg.solStatus == 0) //see description of solution status in OEMV615.h - { - solReady = 'Y'; - } - - toPC.printf("WMsg POSVEL %5.3lf %1d %c %8.6lf %9.6lf %4.3lf %4.3lf %4.3lf %4.3lf\n", - posTime, - posMsg.numSolSV, - solReady, - latPos, - lonPos, - htPos, - nVel, - eVel, - velMsg.verticalSpeed - ); - txMsg = !txMsg; -} - -void processPCmessages(FILE* &fpNav, OEM615BESTPOS posMsg, OEM615BESTVEL velMsg) -{ - - - - //we should put the below stuff into the readPC() procedure. - //only do these actions in response to a command so no need for the tests w/o an inoput byte from the PC - //perform the activities as a response to the commands - if (sendPosVel) //true if we want to return a position solution - { - //if we are receiving POSVEL requests -- always open the file for storage and store the data - //th file is closed(in main) if we dont receive POSVAL messages for 60 secs - if (fpNav == NULL) - { - toPC.printf("WMsg opening the SD card file at first PosVel message \n"); - fpNav = fopen("/sd/Data/NAV.bin", "wb"); - wait_ms(10); - recordData = true; - } - sendPosVel=false; //set to true if a POSVEL is requested from the PC - sendPosVelMessageToPC(posMsg, velMsg); - } - - //all this does is assess the GPS convergence -- really available in the above - if (sendStatus) //send the status message to the PC - { - txMsg = !txMsg; - sendStatus=false; - char solReady = 'N'; - //solStatus - if (posMsg.solStatus == 0) //see description of solution status in OEMV615.h - { - solReady = 'Y'; - } - toPC.printf("WMsg STATUS %5.3lf %c\n", - GPSTimemsecs, - solReady - ); - } - - //should just record ALL the data -- can pick over it in the post-processing - if (sendRecData) //begin to (or stop) record the serial data - { - sendRecData=false; //reset the flag so we dont continue to come through here - char recChar = 'N'; - //recordData set to true only if we receive a STARTDATA from the PC - if (recordData) //here we have received a message to record the data - { - if ((fpNav == NULL)) //if file not opened -- open it - { - toPC.printf(" opening the SD card file from RECORD message \n"); - fpNav = fopen("/sd/Data/NAV.bin", "wb"); - wait_ms(10); - } - if (fpNav != NULL) //if the file was already opened we will respond to the PC with a Y - { - recChar = 'Y'; - } - else //is the file was not opened we will write a message to the PC - { - toPC.printf(" Could not open the SD card \n\n"); - } - } - //recordData set to false only if we receive a STOPDATA from the PC - else //here we have received a message to stop the recording - { - if (fpNav != NULL) //if the file is open -- close it - { - toPC.printf(" closing the SD card file from RECORD message\n\n"); - fflush(fpNav); - fclose(fpNav); - wait_ms(100); - //toPC.printf("\n after closing the SD card file \n\n"); - fpNav = NULL; - } - //if stop recording received and the file is already closed -- just do nothing - recordData = false; - - } - toPC.printf("WMsg RECORDDATA %c\n", - recChar - ); - } - - if (sendStreamPos) //stream the position data to the PC - { - sendStreamPos=false; - char streamChar = 'N'; - if (streamPos) - { - streamChar = 'Y'; - } - toPC.printf("WMsg POSSTREAM %c\n", - streamChar - ); - } - - //not sure this is ever used .. - if (sendLogMsgInfo) //send log info to the PC - { - sendLogMsgInfo=false; - char logChar = 'N'; - if (logMsgInfo) - { - logChar = 'Y'; - } - toPC.printf("WMsg LOGINFO %c\n", - logChar - ); - } - - -}
--- a/main.cpp Wed Nov 13 22:12:04 2013 +0000 +++ b/main.cpp Thu Jan 09 14:09:05 2014 +0000 @@ -8,8 +8,11 @@ #include "SDFileSystem.h" //imported using the import utility //general digital I/O specifications for this application + +//if we dont write to the card we dont need this ... //SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name); SDFileSystem sd(p11,p12,p13,p14,"sd"); + DigitalIn sd_detect(p27); DigitalOut ppsled(LED1); //blink an LED at the 1PPS DigitalOut trig1led(LED2); //blink an LED at the camera trigger detection @@ -21,23 +24,12 @@ DigitalInOut fire(p29); //connected to the tip of 2.5mm connector (T2i) DigitalInOut pre_fire(p30); //connected to the mid-connection for 2.5mm connector (T2i) - - -/* Look at https://mbed.org/users/sam_grove/code/BufferedSerial/ */ -/* I wrote this and it works quite well. IRQ driven UART with software buffers for TX and RX */ -/* Same API as Serial with some enhancements */ -/* This will reduce the loop around main time. Right now the problem is that the delay */ -/* between events changes based on the UART speed */ - //USB serial data stream back to the PC Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10 Timer timeFromStart; -Timer timeFromPosVelMessageReceipt; -/* should make all flags uint32_t for fastest access */ -/* encapsulate all this into a structure */ -/* or several based on what peripheral they belong to */ + bool detectedGPS1PPS = false; //flag set in the ISR and reset after processing the 1PPS event int PPSCounter = 0; //counts the 1PPS occurrences @@ -50,41 +42,16 @@ int savedIMUClockCounter=0; //saved at the 1PPS for later diaplay from main bool camera1EventDetected = false; //flag from ISR indicating a clock event occurred double camera1Time; //GPS time of the camera event -int TotalBadCRCmatches = 0; //counter for the bad CRC matches for all GPS messages volatile int PPSTimeOffset = 0; +int totalGPSBytes = 0; -/* Ive done some of this. Look at: */ -/* https://mbed.org/users/sam_grove/code/OEM615/ */ -/* https://mbed.org/users/sam_grove/code/ADIS16488/ */ -/* a starting point that i'd done back during the hardware tester */ +bool writeIMUDataToPC = false; //used in main loop to cause IMU data write to the PC +bool writeGPSDataToPC = false; //used in main loop to cause GPS data write to the PC -////////////////////////////////////////////////////////////////////// -// the below should become classes -////////////////////////////////////////////////////////////////////// #include "OEM615.h" //OEM615 GPS activities #include "ADIS16488.h" //ADIS16488 activities #include "PCMessaging.h" //PC messaging activities - -/* This should be called from a PC message handler when found. */ - -// stuff to send the SD file to the PC -#include "SDShell.h" -void transferFile() -{ - SDShell emulate; // create the object - emulate.init(); // init the params inside - GPS_Reset = 0; // low power PCB mode - ADIS_RST = 0; // same here - wait(0.01f); // just make sure that the hardware has time to stop - fflush(stdout); // and clear any TX reminants - toPC.printf("Entering Shell Emulator...\n"); // just for fluf - wait(0.1f); // no reason for this either - emulate.shell(toPC, sd, "/sd"); // now the SDShell object will serve SD files via UNIX commands -} - -/* Move into the GPS class */ - //ISR for detection of the GPS 1PPS void detect1PPSISR(void) { @@ -109,14 +76,9 @@ }; - -/* Break up into GPS class and PC message class */ -/* GPS boot up messages should be checking for a response rather than waiting a predefined time */ -/* SD card stuff should also be a seperate class. Allow for dynamic directory and file awareness */ /////////////////////////////////////////////////////// //set up the USB port and the GPS COM port /////////////////////////////////////////////////////// -FILE *fpNav = NULL; //file pointer to the nav file on the SD card void setupCOM(void) { //system starts with GPS in reset active @@ -134,25 +96,14 @@ //toPC.baud(1*115200); wait_ms(100); //toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n"); - //just wait to launch the GPS receiver - for (int i=0; i<5; i++) { toPC.printf("WMsg start: %3d \n", 4-i); wait(1); } + //just wait some time to launch the GPS receiver + for (int i=0; i<5; i++) { toPC.printf("WMessage start countdown: %3d \n", 4-i); wait(1); } - sd_detect.mode(PullUp); - - if (sd_detect == 0) - { - mkdir("/sd/Data", 0777); - } - else - { - toPC.printf("WMsg SD card not present \n"); - } - //NOTE: we do not assume that the GPS receiver has been pre-set up for the WALDO_FCS functionality //we alwsys start with a reset and reprogram the receiver with our data out products // this prevents failure because of a blown NVRAM as occurred for the older camera systems - //this is the COM1 port from th GPS receiuver to the mbed + //this is the COM1 port from th GPS receiver to the mbed //it should be always started at 9600 baud because thats the default for the GPS receiver GPS_COM1.baud(9600); wait_ms(100); @@ -169,15 +120,17 @@ char ch2[] = "serialconfig COM1 115200 n 8 1 n off"; //the below commands request the POS, VEL, RANGE, and TIME messages - char ch3[] = "log COM1 BESTPOSB ONTIME 1"; //messageID = 42 - char ch4[] = "log COM1 BESTVelB ONTIME 1"; //messageID = 99 - char ch5[] = "log COM1 RANGEB ONTIME 1"; //messageID = 43 + //Binary commands are shown below + //char ch3[] = "log COM1 BESTPOSB ONTIME 1"; //messageID = 42 + //char ch4[] = "log COM1 BESTVelB ONTIME 1"; //messageID = 99 + //char ch5[] = "log COM1 RANGEB ONTIME 1"; //messageID = 43 //char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101 - - //set up VARF to be 100Hz with 1X10^4 * 10^-8 = 10^-4 sec (10usec) pulse width - //in fact, we do not use this output but it is available. - //originally planned to use this to command the IMU data - //char ch8[] = "FREQUENCYOUT enable 10000 1000000"; + + //ASCII commands are shown below + char ch3[] = "log COM1 BESTPOSA ONTIME 1"; //messageID = 42 + char ch4[] = "log COM1 BESTVelA ONTIME 1"; //messageID = 99 + char ch5[] = "log COM1 RANGEA ONTIME 1"; //messageID = 43 + //char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101 //toPC.printf("WMsg set serial config \n"); sendASCII(ch7, sizeof(ch7)); wait_ms(500); @@ -214,130 +167,99 @@ int main() { - /* IMO this should all be ASCII (GPS data). Too much processing on the mbed side for messages that arent used */ - /* the parsing and error checking + printf with floating point are EXPENSIVE CPU instructions */ - - //these are structures for the to GPS messages that must be parsed - MESSAGEHEADER msgHdr; - OEM615BESTPOS posMsg; //BESTPOS structure in OEMV615.h that has matching time to a BESTVEL message - OEM615BESTPOS curPos; //BESTPOS structure in OEMV615.h - OEM615BESTVEL velMsg; //BESTVEL structure in OEMV615.h that has matching time to a BESTPOS message - OEM615BESTVEL curVel; //BESTVEL structure in OEMV615.h - - - /* Self explanitory */ + toPC.printf("initiating the mbed app\n"); fire.output(); //set the fire pin as outoput pre_fire.output(); //set the pre-fire pin as output - /* both should be open drain */ - //fire.mode(OpenDrain); - /* not necessary if open drain outputs */ - //set up for the first trigger fire = 1; pre_fire = 1; - /* for lower power modes of operation all this should be different classes and called when the PC requests it */ - /* always on and idle is a waste of battery power. Could probabally add 30-50% life by leaving things off until needed */ - //set up the GPS and mbed COM ports setupCOM(); - - /* same as above */ - - //set up the ADIS16488 - setupADIS(); + toPC.printf("Completed setting up GPS \n"); - setUpMessages(); //set up the expected text message commands from the PC - - /* same as above */ - + //set up the ADIS16488 IMU + //setupADIS(); + //initiate the interrupt to catch the GPS receiver serial bytes as they are presented GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq); - /* same as above */ - timeFromPPS.start(); //start the time for measuring time from 1PPS events timeFromStart.start(); - //toPC.printf("\n\n top of the main loop \n\n"); - - int totalBytesWritten = 0; - - /*establish the initial value for the CRC recursion after the header signature bytes - unsigned long CRC = 0; - CRC32Value(CRC, 0xAA); - CRC32Value(CRC, 0x44); - CRC32Value(CRC, 0x12); - CRC32Value(CRC, 0x1C); - //this results in a value of: 0x39b0f0e1 - toPC.printf(" CRC after AA44121C header: %08x \n", CRC); - wait(20); - */ - - //at the start we do not record the data - recordData = false; - sendRecData = false; - - /* I'd get these off the stack and into a global structure */ - /* If you did have a stack error condition (buffer overwrite) all this could be clobbered */ - unsigned long cyclesPerSec = 0; //main while() loop cycles per GPS sec - bool GPSdataWritten = false; + bool finishTrigger = false; Timer triggerInterval; - //while(PPSCounter < 300) - - /* New command logic will keep this loop from ever exiting */ - bool newMission = true; /////////////////////////////////////////////////////////////////////////// // top of the mission while loop /////////////////////////////////////////////////////////////////////////// while(newMission) - { - - /* right idea. Build messages and parse */ - - //read the USB serial data from the PC to check for commands - readFromPC(); - - /* file should only be open when writing. If you crash and the file is open */ - /* the data will be lost. Should be in a class that takes structures or strings to write to the file */ - - //this will close the fpNav file on the SD card if the file is open - //and the elapsed time from PosVel messages is > 60 secs - //this prevents loosing the fpNav file if the PC goes down - // !!!! timeFromPosVelMessageReceipt !!! was never started - if (fpNav && (timeFromPosVelMessageReceipt.read() > 10) ) + { + if (message0Complete) + { + for (int i=0; i<GPSbyteCounter0; i++) toPC.printf("%c", msgBuffer0[i]); + toPC.printf("\n"); + message0Complete = false; + } + else if (message1Complete) + { + for (int i=0; i<GPSbyteCounter1; i++) toPC.printf("%c", msgBuffer1[i]); + toPC.printf("\n"); + message1Complete = false; + } + else if (message2Complete) + { + for (int i=0; i<GPSbyteCounter2; i++) toPC.printf("%c", msgBuffer2[i]); + toPC.printf("\n"); + message2Complete = false; + } + /* + if (writeIMUDataToPC) { - sendRecData = true; - recordData = false; + //write the IMU data to the PC + toPC.printf("IMURECORD "); // 9 bytes header + for (int i=0; i<IMUrecArraySize; i++) + { + if (fillingPingWritingPong) + { + toPC.printf("%d9 %d9 %d9 %d9 %d9 %d9 %d9", //70 bytes + imuPong[i].GPSTime, + imuPong[i].dataWord[0], + imuPong[i].dataWord[1], + imuPong[i].dataWord[2], + imuPong[i].dataWord[3], + imuPong[i].dataWord[4], + imuPong[i].dataWord[5] ); + } + else + { + toPC.printf("%d9 %d9 %d9 %d9 %d9 %d9 %d9", + imuPing[i].GPSTime, + imuPing[i].dataWord[0], + imuPing[i].dataWord[1], + imuPing[i].dataWord[2], + imuPing[i].dataWord[3], + imuPing[i].dataWord[4], + imuPing[i].dataWord[5] ); + } + } + toPC.printf("/n"); //total of 10 * 70 + 9 = 709 bytes + writeIMUDataToPC = false; } - - /* Should be done when reading and building the message */ - - // for any received PC message, take the appropriate action - processPCmessages(fpNav, posMsg, velMsg); - - /* file should always be closed until needed */ - - //if we receive a "GETFILE" message from the PC -- close the fpNavFile and break from the while() loop - if (get_file_msg) - { - if (fpNav != NULL) fclose(fpNav); - break; //terminate the while loop when we receive this message from the PC - } - - /* wait looks unnecessary (and bad during runtime). During all tests with the camera the pre-fire is */ - /* 250mS before the fire. Not sure if this changes the time til shutter open */ - /* Look at the Timeout class */ - + */ + + //read the USB serial data from the PC to check for commands + //only message we expect will be thr trigger command + readFromPC(); + if(fireTrigger) //comes from a PC request message { unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0; @@ -351,8 +273,6 @@ triggerInterval.start(); } - /* see above */ - //the trigger requires a pulse -- the above portion lowers the signal and the below raises it //this has been tested at 50 msecs and it will not fire at that pulse duration if(finishTrigger && triggerInterval.read_ms() > 100) @@ -363,123 +283,8 @@ finishTrigger = false; //completes the trigger firing pulse definition } - /* who clears this, PPS? */ - cyclesPerSec++; - //////////////////////////////////////////////////////////////////////////// - //below is where we process the complete stored GPS message for the second - //The !IMUDataReady test prevents the IMU and GPS data from being written - //to disk on the same pass through this loop - ///////////////////////////////////////////////////////////////////////////// - - //there are three potential messages and all messages have a header - if (completeMessageAvailable && !IMUDataReady) - { - //must unpack header first to get the message length - msgHdr = *((MESSAGEHEADER*)&msgBuffer[messageLocation[savedMessageCounter-1]]); - - //these times are used to tag the IMU sample time. PPSTimeOffset increments by 1 exactly at the 1PPS event (in the 1PPS ISR) - //GPSTimemsecs increments by 1 for each new GPS measurement -- note that the below computations are actually - //done at the receipt of each GPS message. This is OK because each message has the same time in its header. - //Thus GPSTimemsecs increments by 1 here while GPSTimemsecs effectively decrements by 1. - //This handles IMU time tagging between the 1PPS event and the first receipt of a new GPS time. - - //message header length is 28 -- right side is pointer to the receiver-computed CRC for this record - //CRC is computed while reading in the GPS bytes - unsigned long msgCRC = *((unsigned long*)&msgBuffer[messageLocation[savedMessageCounter-1] + 28 + msgHdr.messageLength]); - - //toPC.printf("tmeFrom1PPS= %5d ID= %3d Ln = %3d computedCRC= %08x msgCRC= %08x msgCntr = %3d CRCerr=%4d\n", - // timeFromPPS.read_us(), msgHdr.messageID, msgHdr.messageLength, computedCRC, msgCRC, savedMessageCounter, TotalBadCRCmatches); - - /* is this really necessary? Seems like keeping it in ascii and sending in ascii would make more sense */ - - if ( msgCRC == computedCRC) //computedCRC is performed as we read each bvyte - { - //if the CRC check is valid -- then get the time from the header - //we get three messages each sec -- does it matter that we do this three times? - GPSTimemsecs = msgHdr.GPSTime_msecs; //time in GPS message header - - //the PPSTimeOffset accounts for the occurrence where we do not get any GPS messages over a sec -- but PPS is still operative - PPSTimeOffset = 0; //incremented by 1 in the PPS ISR - - //We need the pos and vel messages to pass back data to the PC -- error cases that can occur: - // (1) missed 42 (POS) -- use last good pos and extrapolate using last good vel - // (2) missed 99 (VEL) -- use the last good vel since likely not changed much - // (3) missed both 42 and 99 -- must use last good position and extrapolae using last good velocity - // GPS time used to time-tag the IMU data and to do the extrapolttion from last good position to send to PC - // in the position extrapolation, we will use the GPS time that is kept in the header of the POS msg (42) - // see the procedure: sendPosVelMessageToPC() - - if (msgHdr.messageID == 42) //this is the position message (lat, lon, alt) - { - //map the starting record byte index to the record structure - curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[savedMessageCounter-1]]); - posMsg = curPos; - - if (streamPos) // we no longer use this functionality - { - toPC.printf("BESTPOS %5d %1d %8.6lf %9.6lf %5.3lf %d %d\n", - curPos.msgHeader.GPSTime_msecs, curPos.solStatus, - curPos.latitude, curPos.longitude, curPos.height, - curPos.numSV, curPos.numSolSV); - } - - } - else if (msgHdr.messageID == 99) //this is the velocity message - { - curVel = *((OEM615BESTVEL*)&msgBuffer[ messageLocation[savedMessageCounter-1] ]); - //toPC.printf("BESTVEL vel: horizontalSpeed= %5.3f heading=%5.1f verticalSpeed=%4.2f \n", - // curVel.horizontalSpeed, curVel.heading, curVel.verticalSpeed ); - velMsg = curVel; - } - - //below is set to true when we detect that we have received a complete GPS message - completeMessageAvailable = false; - } - else // do this if we do not pass the CRC - { - toPC.printf("WMsg bad CRC match for messageID %3d total CRC errors = %4d \n", - msgHdr.messageLength, TotalBadCRCmatches++); - } - } - - - /* should move into a file management class for protection and error checking */ - - //write the GPS data to the SD card - //NOTE: this is valid only for a once-per-sec GPS message - //for this case, all messages come out well prior to 0.5 secs after the 1PPS - if (!IMUDataReady && !GPSdataWritten && timeFromPPS.read_us() > 500000 && recordData && (fpNav != NULL)) - { - totalBytesWritten += fwrite(&msgBuffer, 1, byteCounter, fpNav); - GPSdataWritten = true; - } - - /* would have 2 arrays of structures and a pointer in the class than changes between the buffers. */ - /* This way there is 1 write and management in 1 place */ - - //the IMU data record is read from the SPI in the ISR and the IMUDataReady is set true - //we write the IMU data here - if (IMUDataReady) //IMUDataReady is true if we have a recent IMU data record - { - //write the IMU data - if ( recordData && (fpNav != NULL) ) - { - //delTimeOfWrite = timeFromStart.read_us(); - - if (fillingPingWritingPong) totalBytesWritten += fwrite(&imuPong, 1, IMUrecArraySize*sizeof(IMUREC), fpNav); - else totalBytesWritten += fwrite(&imuPing, 1, IMUrecArraySize*sizeof(IMUREC), fpNav); - - //delTimeOfWrite = (unsigned long)((unsigned long)timeFromStart.read_us() - delTimeOfWrite); - //if (delTimeOfWrite > maxWriteTime) maxWriteTime = delTimeOfWrite; - } - IMURecordCounter+=IMUrecArraySize; - IMUDataReady = false; - } - - /* should remove floating point if possible and send as milli-sec integers */ - //this is a command from the PC to fire a trigger if (camera1EventDetected) //we have detected a camera trigger event { @@ -491,15 +296,13 @@ if (detectedGPS1PPS) //true if we are exactly at a 1PPS event detection { - //toPC.printf("PPS=%4d stat=%1d bytes=%3d GPSMsgs=%2d #write=%8d cycles=%6d\n", - // PPSCounter, posMsg.solStatus, savedByteCounter, savedPerSecMessageCounter, - // totalBytesWritten, cyclesPerSec ); + toPC.printf("detected GPS 1PPS now %5d %10d\n", PPSCounter, totalGPSBytes); + totalGPSBytes=0; cyclesPerSec = 0; - //totalBytesWritten = 0; - GPSdataWritten = false; - //toPC.printf(" bytesWritten = %5d \n", totalBytesWritten); + messagePerSecCounter = 0; //GPS message per second counter + IMURecordCounter = 0; detectedGPS1PPS = false; @@ -511,33 +314,5 @@ /////////////////////////////////////////// - /* should already be closed by file management class */ - - if (fpNav != NULL) - { - fclose(fpNav); //insurance - toPC.printf("WMsg closeFPNav \n"); - } - - /* accessable by SDShell class */ - /* see: https://mbed.org/users/sam_grove/code/SDShell/ */ - - toPC.printf("WMsg totalBytesWritten %5d \n", totalBytesWritten); - wait_ms(100); - - /* just a state of the communication management class */ - - //send the nav file to the PC - transferFile(); - //rxMsg = txMsg = 0; // just indicate that we're in here - // to exit this function the HOST (ie: computer or PC app) must send "exit" otherwise the mbed will act - // like a terminal and serve SD file data forever - - /* no longer needed */ - - toPC.printf("WMsg normalTermination \n"); - wait_ms(100); - - NVIC_SystemReset(); } \ No newline at end of file