james kain
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
Diff: main.cpp
- Revision:
- 30:96d133f3008e
- Parent:
- 29:dead10cce6e9
--- a/main.cpp Thu Jan 09 14:09:05 2014 +0000
+++ b/main.cpp Mon Mar 03 13:19:31 2014 +0000
@@ -3,6 +3,7 @@
//set up the message buffer to be filled by the GPS read process
#define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 256
+#define CRC32_POLYNOMIAL 0xEDB88320
#include "MODSERIAL.h"
#include "SDFileSystem.h" //imported using the import utility
@@ -28,8 +29,7 @@
Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10
Timer timeFromStart;
-
-
+Timer timeInMessageRead;
bool detectedGPS1PPS = false; //flag set in the ISR and reset after processing the 1PPS event
int PPSCounter = 0; //counts the 1PPS occurrences
@@ -56,8 +56,9 @@
void detect1PPSISR(void)
{
timeFromPPS.reset(); //reset the 1PPS timer upon 1PPS detection
- //note -- the below accounts for time information becoming available AFTER the 1PPS event
- PPSTimeOffset++; //counts 1PPS events between matching POS and VEL messages
+
+ //note -- the below accounts for GPS receiver time information becoming available AFTER the 1PPS event
+ PPSTimeOffset++; //counts 1PPS events -- reset to zero in main after we determine a GPS time
//covers the case where the PPS ISR interrupts the IMU data ready ISR
if(IMUDataReady) IMUtimeFrom1PPS = 0;
@@ -109,8 +110,12 @@
// this ASCII command sets up the serial data from the GPS receiver on its COM1
char ch7[] = "serialconfig COM1 9600 n 8 1 n off";
+
// this is a software reset and has the same effect as a hardware reset (why do it?)
//char ch0[] = "RESET";
+ //toPC.printf("WMsg set RESET command \n");
+ //sendASCII(ch0, sizeof(ch0)); wait_ms(3000);
+
//this command stops all communication from the GPS receiver on COM1
//logs should still be presented on USB port so the Novatel CDU application can be used on the PC in parallel
char ch1[] = "unlogall COM1";
@@ -161,15 +166,76 @@
};
+void CRC32Value_2(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_2(
+ 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_2( CRC, *ucBuffer++ );
+ return CRC;
+}
+
+bool checkMessageCRC(char* message, int byteCounter)
+{
+ //calculate the Novatel OEM615 CRC
+ //length of CRC bytes is string length minus: 8 bytes for long word, 2 bytes for "#" & "*" and 2 bytes for CR, LF
+ // start at byte 1 (not zero) to skip the "#" at the start
+ unsigned long computedCRC = CalculateBlockCRC32_2((unsigned long)(byteCounter-12), (unsigned char*)(&message[1]));
+
+ //get the computed CRC into a zero-padded string so we can compare to the string CRC in the message
+ char CRCCharBuffer [50];
+ int n=sprintf (CRCCharBuffer, "%08x", computedCRC); //note the zero-padding
+ //toPC.printf("n = %d charCRC = %s \n", n, CRCCharBuffer);
+
+ //get a string reresentation of just the CRC from the message
+ char CRCString[10];
+ strncpy(CRCString, &message[byteCounter-10], 8);
+ //toPC.printf(" %s %s \n", CRCCharBuffer, CRCString);
+
+ if (strncmp(CRCCharBuffer, CRCString, 8) != 0)
+ {
+ toPC.printf("mbed bad CRC %d %s %d %s \n", strlen(CRCCharBuffer), CRCCharBuffer, strlen(CRCString), CRCString );
+ return false;
+ }
+
+ return true;
+}
+
+
/////////////////////////////////////////////////////////////////////
// mbed main to support the Waldo_FCS
/////////////////////////////////////////////////////////////////////
int main()
{
- toPC.printf("initiating the mbed app\n");
-
- fire.output(); //set the fire pin as outoput
+ fire.output(); //set the fire pin as output
pre_fire.output(); //set the pre-fire pin as output
//fire.mode(OpenDrain);
@@ -180,16 +246,20 @@
//set up the GPS and mbed COM ports
setupCOM();
- toPC.printf("Completed setting up GPS \n");
+
+ //prints to PC above here wont work cause the PC serial link baud rate has not been set
+ toPC.printf("initiating the mbed app\n");
+ toPC.printf("setting up the ADIS\n");
//set up the ADIS16488 IMU
- //setupADIS();
+ setupADIS();
//initiate the interrupt to catch the GPS receiver serial bytes as they are presented
GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
timeFromPPS.start(); //start the time for measuring time from 1PPS events
timeFromStart.start();
+ timeInMessageRead.start(); //used to prevent hanging in the PC message read loop
unsigned long cyclesPerSec = 0; //main while() loop cycles per GPS sec
@@ -197,40 +267,101 @@
Timer triggerInterval;
bool newMission = true;
+ int workingTime = 0;
+
+ bool triggeringInProcess = false;
+
+ int badRangeMessages = 0;
+ int badBESPOSMessages = 0;
+ int badBESVELMessages = 0;
+
+ int VERSION = 1;
+ double timeAtMessage0 = 0;
///////////////////////////////////////////////////////////////////////////
// top of the mission while loop
///////////////////////////////////////////////////////////////////////////
while(newMission)
{
- if (message0Complete)
+ readFromPC(); //#1
+
+ //process GPS data messages (messages 0, 1, 2)
+ if (toPC.writeable() && message0Complete && !IMUDataReady )
{
- for (int i=0; i<GPSbyteCounter0; i++) toPC.printf("%c", msgBuffer0[i]);
- toPC.printf("\n");
- message0Complete = false;
+ int thisWorkTime = timeFromStart.read_us();
+
+ //get the GPS time at the prior 1PPS from the first header
+ char msgName [15], port [15], clockStatus [15];
+ int a1=0, GPSWeek=0;
+ double b1 = 0.0, GPSTimeFromHeader = 0.0;
+
+ //note the use of the "C" scanset [^,] to read the comma-delimited ASCII message provided by te Novatel OEM615 receiver
+ //these are used only for the character variables in order to avoid detecting the "," as another character
+ //only spaces can be used as delimiters if scanset methodology is not used
+ int N = sscanf(msgBuffer0,"%15[^,],%15[^,],%d,%f,%15[^,],%d,%lf", msgName, port, &a1, &b1, clockStatus, &GPSWeek, &GPSTimeFromHeader);
+ if (N != 7) toPC.printf("scanf failed for message: %s ", msgBuffer0);
+
+ GPSTimemsecs = GPSTimeFromHeader * 1000;
+
+ //this is used to account for the GPS time from the receiver becoming available a few msecs after the actual 1PPS event
+ //A precision timer is set at the 1PPS event that is used to tag the IMU data
+ timeAtMessage0 = timeFromPPS.read_us()/1000.0;
+ PPSTimeOffset = 0.0;
+
+ if (!checkMessageCRC(msgBuffer0, GPSbyteCounter0)) badRangeMessages++;
+
+ N=0;
+ //write the GPS message bytes to the PC
+ for (int i=0; i<GPSbyteCounter0; i++) N += toPC.printf("%c", msgBuffer0[i]);
+ if (N != GPSbyteCounter0) toPC.printf(" error writing msgBuffer0: %d of %d bytes written \n", N, GPSbyteCounter0);
+
+ message0Complete = false; //set up for the next message
+ workingTime += timeFromStart.read_us() - thisWorkTime;
+
+ readFromPC(); //#2
}
- else if (message1Complete)
+ else if (toPC.writeable() && message1Complete && !IMUDataReady)
{
- for (int i=0; i<GPSbyteCounter1; i++) toPC.printf("%c", msgBuffer1[i]);
- toPC.printf("\n");
+ if (!checkMessageCRC(msgBuffer1, GPSbyteCounter1)) badBESPOSMessages++;
+
+ int thisWorkTime = timeFromStart.read_us();
+
+ int N=0;
+ for (int i=0; i<GPSbyteCounter1; i++) N += toPC.printf("%c", msgBuffer1[i]);
+ if (N != GPSbyteCounter1) toPC.printf(" error writing msgBuffer1: %d of %d bytes written \n", N, GPSbyteCounter1);
+
message1Complete = false;
+ workingTime += timeFromStart.read_us() - thisWorkTime;
+
+ readFromPC(); //#3
}
- else if (message2Complete)
+ else if (toPC.writeable() && message2Complete && !IMUDataReady)
{
- for (int i=0; i<GPSbyteCounter2; i++) toPC.printf("%c", msgBuffer2[i]);
- toPC.printf("\n");
+ if( !checkMessageCRC(msgBuffer2, GPSbyteCounter2)) badBESVELMessages++;
+
+ int thisWorkTime = timeFromStart.read_us();
+ int N = 0;
+
+ for (int i=0; i<GPSbyteCounter2; i++) N += toPC.printf("%c", msgBuffer2[i]);
+ if (N != GPSbyteCounter2) toPC.printf(" error writing msgBuffer2: %d of %d bytes written \n", N, GPSbyteCounter2);
+
message2Complete = false;
- }
- /*
- if (writeIMUDataToPC)
+ workingTime += timeFromStart.read_us() - thisWorkTime;
+
+ readFromPC(); //#4
+ }
+ //
+ //
+ if (toPC.writeable() && IMUDataReady)
{
+ int thisWorkTime = timeFromStart.read_us();
//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
+ toPC.printf("IMU %d13,%d13,%d13,%d13,%d13,%d13,%d13 " , //70 bytes
imuPong[i].GPSTime,
imuPong[i].dataWord[0],
imuPong[i].dataWord[1],
@@ -241,7 +372,7 @@
}
else
{
- toPC.printf("%d9 %d9 %d9 %d9 %d9 %d9 %d9",
+ toPC.printf("IMU %d13,%d13,%d13,%d13,%d13,%d13,%d13 " , //70 bytes
imuPing[i].GPSTime,
imuPing[i].dataWord[0],
imuPing[i].dataWord[1],
@@ -250,23 +381,26 @@
imuPing[i].dataWord[4],
imuPing[i].dataWord[5] );
}
+ //
+ readFromPC(); //#5
}
- toPC.printf("/n"); //total of 10 * 70 + 9 = 709 bytes
- writeIMUDataToPC = false;
+ toPC.printf("\n"); //total of 10 * 70 + 9 = 709 bytes
+ IMUDataReady = false;
+ workingTime += timeFromStart.read_us() - thisWorkTime;
}
- */
//read the USB serial data from the PC to check for commands
- //only message we expect will be thr trigger command
+ //only message we expect will be the trigger fire command
readFromPC();
- if(fireTrigger) //comes from a PC request message
+ if(toPC.writeable() && fireTrigger) //comes from a PC request message
{
+ triggeringInProcess = true;
unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0;
- toPC.printf("WMsg TRIGGERTIME %10d \n", triggerTime);
+ toPC.printf("mbedmessage trig1 %d \n", triggerTime);
//pre-fire the trigger using the mid-body 2.5mm connection (T2i)
pre_fire = 0; //pin30 (midbody of connector) set to zero
- wait(.01f); // not sure what this does
+
fire = 0; //fire the trigger using the tip connection
fireTrigger = false; //finished the setup -- but wait to do the actual fire
finishTrigger = true; //set to false after firing the trigger
@@ -275,31 +409,32 @@
//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)
+ if(toPC.writeable() && finishTrigger && triggerInterval.read_ms() > 100)
{
+ unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0;
+ toPC.printf("mbedmessage trig2 %d \n", triggerTime);
+ //pre-fire the trigger using 2the mid-body 2.5mm connec
fire = 1;
pre_fire = 1;
triggerInterval.reset();
finishTrigger = false; //completes the trigger firing pulse definition
+ triggeringInProcess = false;
}
cyclesPerSec++;
-
- //this is a command from the PC to fire a trigger
- if (camera1EventDetected) //we have detected a camera trigger event
- {
- toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time);
- camera1EventDetected = false;
- }
-
- /* managed in GPS class */
if (detectedGPS1PPS) //true if we are exactly at a 1PPS event detection
{
- toPC.printf("detected GPS 1PPS now %5d %10d\n", PPSCounter, totalGPSBytes);
+ if (toPC.writeable() )
+ {
+ toPC.printf("STATUS %03d %04d %06d %02d %06d %5.3f %03d %03d %03d\n",
+ VERSION, PPSCounter, totalGPSBytes, IMURecordCounter, cyclesPerSec, timeAtMessage0, /*workingTime/1000000.0,*/
+ badRangeMessages, badBESVELMessages, badBESVELMessages);
+ }
totalGPSBytes=0;
cyclesPerSec = 0;
+ workingTime= 0.0;
messagePerSecCounter = 0; //GPS message per second counter
@@ -308,6 +443,8 @@
rxMsg = !rxMsg; //flash the lights to make sure the mbed loop is operative
txMsg = !txMsg;
+
+ readFromPC(); //#6
}
///////////////////////////////////////////
} //end of the major while() loop
@@ -315,4 +452,5 @@
-}
\ No newline at end of file
+}
+