Diff: main.cpp

Revision:

0:432b860b6ff7

Child:

1:8e24e633f8d8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 22 21:26:04 2013 +0000
@@ -0,0 +1,371 @@
+#include "mbed.h" 
+#include <string>
+
+//set up the message buffer to be filled by the GPS read process
+#define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 256 
+
+#include "MODSERIAL.h"
+#include "SDFileSystem.h"      //imported using the import utility    
+
+//general digital I/O specifications for this application
+//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
+DigitalOut recordDataled(LED4); //set the led when the record is on
+InterruptIn camera1Int(p30);    // camera interrupt in
+DigitalOut camera2Pin(p29);     // We dont use the second camera interrupt
+//USB serial data stream back to the PC
+Serial toPC(USBTX, USBRX);      //connect the GPS TX, RX to p9 and p10
+
+Timer timeFromStart;
+
+bool detectedGPS1PPS = false;       //flag set in the ISR and reset after processing the 1PPS event
+int PPSCounter = 0;                 //counts the 1PPS occurrences
+int byteCounter = 0;                //byte counter -- zeroed at 1PPS
+unsigned short perSecMessageCounter=0; //counts the number of messages in a sec based on the header detection
+bool messageDetected = false;       //have detected a message header
+unsigned long IMUbytesWritten = 0;  //counts the IMU bytes written by the fwrite() to the SD card 
+int savedByteCounter = 0;           //save ByteCounter at the 1PPS for display in main
+int savedPerSecMessageCounter=0;    //saved PerSecMsgCounter for display in main
+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;
+
+//////////////////////////////////////////////////////////////////////
+// the below should become classes
+//////////////////////////////////////////////////////////////////////
+#include "OEM615.h"         //OEM615 GPS activities
+#include "ADIS16488.h"      //ADIS16488 activities
+#include "PCMessaging.h"    //PC messaging activities
+
+//ISR for detection of the GPS 1PPS
+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
+    
+    //covers the case where the PPS ISR interrupts the IMU data ready ISR
+    if(IMUDataReady) IMUtimeFrom1PPS = 0;
+            
+    savedByteCounter = byteCounter;         //save byteCounter for display in main
+    savedPerSecMessageCounter = perSecMessageCounter;   //save for display un main
+    byteCounter = 0;                        //countes bytes between 1PPS events 
+    perSecMessageCounter = 0;               //counts GPS messages between 1PPS events
+    
+    GPS_COM1.rxBufferFlush();               //flush the GPS serial buffer
+    
+    detectedGPS1PPS = true;         //set false in the main when 1PPS actions are complete
+    PPSCounter++;                   //count number of 1PPS epoch
+    
+    ppsled = !ppsled;               //blink an LED at the 1PPS
+};
+
+//ISR for detection of the hotshoe trigger 1
+void camera1ISR(void)
+{
+    //GPSTime is from POS message header
+    //PPSTimeOffset is an even sec to account for Time becoming known AFTER the 1PPS
+    //PPSTimeOffset + timeFromPPS.read() can be as large as 1.02 secs
+    camera1Time = GPSTime + PPSTimeOffset + timeFromPPS.read();    
+    camera1EventDetected = true;  //reset to false in main after processing the image detection
+    trig1led = !trig1led;  //blink an LEWD at the camera event detection
+};
+
+///////////////////////////////////////////////////////
+//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
+    //dis-engage the reset to get the GPS started
+    GPS_Reset=1; wait_ms(1000); 
+    
+    //establish 1PPS ISR 
+    PPSInt.rise(&detect1PPSISR);
+    
+    //set the USB serial data rate -- rate must be matched at the PC end
+    //This the serial communication back to the the PC host
+    //Launch the C++ serial port read program there to catch the ASCII characters
+    //toPC.baud(9600); wait_ms(100);    
+    toPC.baud(8*115200); wait_ms(100);
+    //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(" to start: %3d \n", 4-i); wait(1); }
+
+    sd_detect.mode(PullUp);
+    
+    if (sd_detect == 0)
+    {
+        mkdir("/sd/Data", 0777);
+    }
+    else
+    {
+        toPC.printf(" 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
+    //it should be always started at 9600 baud because thats the default for the GPS receiver 
+    GPS_COM1.baud(9600); wait_ms(100);
+   
+    // 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"; 
+    //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";
+    //set the final baud rate that we will use from here  
+    //allowable baud rate values: 9600 115200 230400 460800 921600
+    //char ch2[] = "serialconfig COM1 921600 n 8 1 n off";
+    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
+    //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";
+    
+    toPC.printf("set serial config \n");
+    sendASCII(ch7, sizeof(ch7)); wait_ms(500);
+    //sendASCII(ch0, sizeof(ch0));  
+    toPC.printf("unlog all messages \n");
+    sendASCII(ch1, sizeof(ch1)); wait_ms(500);
+    toPC.printf("log BESTPOSB on COM1 \n");
+    sendASCII(ch3, sizeof(ch3)); wait_ms(500);
+    toPC.printf("log BESTVELB on COM1\n");
+    sendASCII(ch4, sizeof(ch4)); wait_ms(500);
+    toPC.printf("log RANGEB on COM1\n");
+    sendASCII(ch5, sizeof(ch5)); wait_ms(500);
+    
+    //toPC.printf("log TIMEB om COM1 \n");
+    //sendASCII(ch6, sizeof(ch6)); wait_ms(100);
+    
+    //toPC.printf("Set up th VARF signal \n"); 
+    //sendASCII(ch8, sizeof(ch8)); wait_ms(500);
+       
+    //set GPS output COM1 to the final high rate
+    toPC.printf("set the COM ports to high rate\n");
+    sendASCII(ch2, sizeof(ch2)); wait_ms(500);
+    
+    //set the mbed COM port to match the GPS transmit rate
+    //the below baud rate must match the COM1 rate coming from the GPS receiver 
+    GPS_COM1.baud(115200); wait_ms(500);  //without this wait -- the baud rate is not detected when using MODSERIAL     
+    //GPS_COM1.baud(921600); wait_ms(500);  //without this wait -- the baud rate is not detected when using MODSERIAL     
+};
+
+void setupTriggers()
+{
+    camera1Int.mode(PullUp);
+    camera2Pin = 1;
+    //establish Trigger ISR 
+    camera1Int.rise(&camera1ISR);
+    
+};
+
+/////////////////////////////////////////////////////////////////////
+//  mbed main to support the Waldo_FCS
+/////////////////////////////////////////////////////////////////////
+int main() {
+    
+    //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
+
+    //set up the GPS and mbed COM ports
+    setupCOM(); 
+    
+    //set up the ADIS16488 
+    setupADIS();
+    
+    //setup Hotshoe
+    setupTriggers();
+
+    setUpMessages();  //set up the expected text message commands frm the PC 
+    
+    //set up 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();
+    
+    toPC.printf("\n\n top of the main loop \n\n");
+    
+    int totalBytesWritten = 0;
+    
+    /*establish the initial value for the CRC recursion atter the header
+    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);
+    */
+    
+    int CRCerrors = 0;
+    
+    recordData = true;
+    sendRecData = true;     
+    
+    unsigned long cyclesPerSec = 0;
+    unsigned long delTimeOfWrite = 0;
+    unsigned long maxWriteTime = 0;
+    bool GPSdataWritten = false;
+    
+    while(PPSCounter < 1000)
+    ///////////////////////////////////////////////////////////////////////////
+    // top of the while loop
+    ///////////////////////////////////////////////////////////////////////////
+    //while(1)
+    {        
+        //read the USB serial data from the PC to check for commands
+        //in the primary real-time portion, there are no bytes from the PC so this has no impact
+        readFromPC();
+                
+        processPCmessages(fpNav, posMsg, velMsg);
+        
+        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  
+        /////////////////////////////////////////////////////////////////////////////
+        
+        
+        if (completeMessageAvailable && !IMUDataReady)
+        {
+            
+            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.
+
+            GPSTimemsecs = msgHdr.GPSTime_msecs;  //time in GPS message header
+            PPSTimeOffset = 0;                    //incremented by 1 in the PPS ISR
+            
+            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);
+                
+            if ( msgCRC != computedCRC)
+            {
+                toPC.printf(" bad CRC match for messageID %3d total CRC errors = %4d \n",  
+                    msgHdr.messageLength, TotalBadCRCmatches++);
+            }
+                     
+            if      (msgHdr.messageID == 42)
+            {
+                curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[savedMessageCounter-1]]);
+                posMsg = curPos;
+                
+                //if (streamPos)
+                {
+                        toPC.printf("BESTPOS %5d %1d %8.5lf %9.5lf %5.3lf %d %d %d\n",
+                                          curPos.msgHeader.GPSTime_msecs,  curPos.solStatus,
+                                          curPos.latitude, curPos.longitude, curPos.height,
+                                          curPos.numSV, curPos.numSolSV, curPos.numGGL1);
+                }
+                
+            } 
+            else if (msgHdr.messageID == 99)  
+            {
+                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;
+            }
+            /*
+            if (recordData && (fpNav != NULL) && (byteCounter > 0))
+            {
+                //wait_us(10);
+                int totalMessageLength = 28 + msgHdr.messageLength + 4;  //header length + message Length + CRC word size
+                totalBytesWritten += fwrite(&msgBuffer[messageLocation[savedMessageCounter-1]], 1, totalMessageLength, fpNav);  // this writes out a complete set of messages for this sec
+                //wait_us(10);
+            }
+                
+            
+            */
+            completeMessageAvailable = false;
+        }
+        
+        if (!IMUDataReady && !GPSdataWritten && timeFromPPS.read_us() > 500000 && recordData && (fpNav != NULL))
+        {
+                totalBytesWritten += fwrite(&msgBuffer, 1, byteCounter, fpNav);
+                GPSdataWritten = true;
+        }
+        
+        //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) fwrite(&imuPong, 1, IMUrecArraySize*sizeof(IMUREC), fpNav);
+                else                        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;
+        }
+        
+        if (camera1EventDetected)  //we have detected a camera trigger event
+        {
+            toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time);
+            camera1EventDetected = false;
+        }
+        
+        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  MR=%5u\n", 
+                            PPSCounter, posMsg.solStatus, savedByteCounter, savedPerSecMessageCounter, 
+                            totalBytesWritten, cyclesPerSec, maxWriteTime );
+                            
+            //if ( (savedIMUClockCounter - IMURecordCounter) > 2) toPC.printf(" IMU diffs = %2d \n", savedIMUClockCounter - IMURecordCounter);
+            //completeMessageAvailable = false;   
+            cyclesPerSec = 0;
+            maxDelIMUmsecs = 0;
+            maxWriteTime = 0;
+            totalBytesWritten = 0;
+            GPSdataWritten = false;
+            
+            IMURecordCounter = 0;
+            detectedGPS1PPS = false;
+        }
+    }
+      
+    fclose(fpNav);
+    toPC.printf(" normal termination \n");
+}
\ No newline at end of file