james kain
JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of
GPS_Incremental
by 
Dan Matthews
Diff: main.cpp
- Revision:
- 0:c746ee34feae
- Child:
- 1:cbb9104d826f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Mar 19 02:17:40 2013 +0000
@@ -0,0 +1,543 @@
+#include "mbed.h"
+
+//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
+//#include "rtos.h"
+#include "OEM615.h"
+
+#include "ADIS16488.h"
+#include <string>
+
+#define STATUS_MSG 0
+#define POSVEL_MSG 1
+#define STARTDATA_MSG 2
+#define STOPDATA_MSG 3
+#define STARTSTREAM_MSG 4
+#define STOPSTREAM_MSG 5
+#define DEGREES_TO_RADIANS (3.14519/180.0)
+
+//general items for this application
+//SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
+SDFileSystem sd(p11,p12,p13,p14,"sd");
+//Serial debug(USBTX, USBRX); // tx, rx USB communication to the PC for debug purposes
+DigitalOut ppsled(LED1);
+DigitalOut trig1led(LED2);
+DigitalOut recordDataled(LED4);
+InterruptIn Camera1Int(p30);
+InterruptIn Camera2Int(p29);
+//USB serial data stream back to the PC
+Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10
+
+bool detectedGPS1PPS = false;
+bool recordData = false;
+int PPSCounter = 0;
+int byteCounter = 0;
+unsigned short perSecMessageCounter=0;
+bool lookingForMessages = true;
+bool messageDetected = false;
+int savedIMUClockCounter=0;
+int IMUClockCounter = 0;
+bool camera1EventDetected = false;
+bool camera2EventDetected = false;
+double camera1Time;
+double camera2Time;
+char serBuf[64];
+int serBufChars=0;
+bool sendPosVel=false;
+bool sendStatus=false;
+bool sendRecData=false;
+bool streamPos=false;
+bool sendStreamPos=false;
+
+
+//ISR for detection of the GPS 1PPS
+void detect1PPSISR(void)
+{
+ timeFromPPS.reset();
+ savedIMUClockCounter = IMUClockCounter;
+ IMUClockCounter = 0;
+ GPS_COM1.rxBufferFlush();
+
+ //byteCounter=0;
+ detectedGPS1PPS = true;
+ lookingForMessages = true;
+ PPSCounter++;
+ PPSTimeOffset++;
+ ppsled = !ppsled;
+};
+
+//ISR for detection of the hotshoe trigger 1
+void Camera1ISR(void)
+{
+ camera1Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read();
+ //detectedTrigger1 = true;
+ trig1led = !trig1led;
+ camera1EventDetected = true;
+};
+//ISR for detection of the hotshoe trigger 2
+void Camera2ISR(void)
+{
+ camera2Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read();
+ //detectedTrigger1 = true;
+ camera2EventDetected = true;
+};
+
+void readFromPC()
+{
+ if (toPC.readable()) //read a PC serial byte and test it for a command
+ {
+ // Read in next character
+ char inChar = toPC.getc();
+ serBuf[serBufChars++] = inChar;
+ // Append end of string character
+ serBuf[serBufChars] = '\0';
+ // Need to parse message to determine behavior
+ // Need to clean this up
+ char msgList[6][32];
+ 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");
+ // assume an invalid message which needs to be reset
+ bool validMessage = false;
+ bool resetMessage = true;
+ // Check for valid message
+ for (int m = 0; m < 6 && !validMessage; m++)
+ {
+ if (strncmp(serBuf, msgList[m], serBufChars) == 0)
+ {
+ validMessage = true;
+ // buffer length is same as message length
+ if (serBufChars == strlen(msgList[m]))
+ {
+ switch(m)
+ {
+ case STATUS_MSG:
+ sendStatus = true;
+ break;
+ case POSVEL_MSG:
+ sendPosVel = true;
+ break;
+ case STARTDATA_MSG:
+ case STOPDATA_MSG:
+ recordData = (m == STARTDATA_MSG);
+ sendRecData = true;
+ break;
+ case STARTSTREAM_MSG:
+ case STOPSTREAM_MSG:
+ streamPos = (m == STARTSTREAM_MSG);
+ sendStreamPos = true;
+ break;
+
+ }
+ }
+ // message is still in progress, do not reset
+ else
+ {
+ resetMessage = false;
+ }
+ }
+ }
+ // if message should be reset
+ if (resetMessage)
+ {
+ // reset serial buffer character count
+ serBufChars = 0;
+ // if invalid message and most recent character is 'W' (first message character),
+ // possible message collision
+ if ((!validMessage) && (inChar == 'W'))
+ {
+ // start a new message
+ serBuf[serBufChars++] = inChar;
+ serBuf[serBufChars] == '\0';
+ }
+ // Append end of string character
+ serBuf[serBufChars] = '\0';
+ }
+ }
+};
+
+void sendASCII(char* ASCI_message, int numChars)
+{
+ //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
+}
+
+//FILE* fp = NULL;
+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.printf("\n\n released GPS from RESET and set to high baud rate \n\n");
+
+ //just wait to lauinch the GPS receiver
+ for (int i=0; i<5; i++) { toPC.printf(" to start: %3d \n", 4-i); wait(1); }
+
+
+ mkdir("/sd/Data", 0777);
+
+ /*
+ fp = fopen("/sd/Data/IMUGPS.bin", "wb");
+ if (fp == NULL)
+ {
+ toPC.printf(" cannot open the IMUGPS data file \n");
+ }
+ else
+ toPC.printf(" opened the IMUGPS data file \n");
+ */
+
+ //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 CDU can be used 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";
+
+ //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
+ 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(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL
+};
+
+void setupTriggers()
+{
+ Camera1Int.mode(PullUp);
+ Camera2Int.mode(PullUp);
+ //establish Trigger ISR
+ Camera1Int.rise(&Camera1ISR);
+ Camera2Int.rise(&Camera2ISR);
+
+}
+
+int test = 0;
+unsigned short messageCounter = 0;
+unsigned short savedMessageCounter = 0;
+unsigned char msgBuffer[1024];
+unsigned short messageLocation[4] = {0};
+
+//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] = 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] = byteCounter; //store the location of this message (with 4 synch words)
+ perSecMessageCounter++;
+ messageDetected = true;
+ }
+ byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected)
+
+};
+
+int main() {
+
+ //FILE *fpIMU = NULL;
+ //FILE *fpGPS = NULL;
+ FILE *fpNav = NULL;
+ OEM615BESTPOS posMsg;
+ OEM615BESTPOS curPos;
+ OEM615BESTVEL velMsg;
+ OEM615BESTVEL curVel;
+
+ //set up th GPS and mbed COM ports
+ setupCOM();
+
+ //set up the ADIS16488
+ setupADIS();
+
+ //setup Hotshoe
+ setupTriggers();
+ //attempt to use the mbed RTOS library
+ //Thread thread(writeThread);
+
+ toPC.printf("completed setting up COM ports \n");
+
+ //set up the interrupt to catch the serial byte availability
+ GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
+
+ timeFromPPS.start();
+
+ //while(PPSCounter < 1000)
+ while(1)
+ {
+ //read the USB serial data from the PC to check for commands
+ readFromPC();
+
+ if (sendPosVel)
+ {
+ sendPosVel=false;
+ char solReady = 'N';
+ if (posMsg.solStatus == 0)
+ {
+ solReady = 'Y';
+ }
+ double nVel = velMsg.horizontalSpeed*cos(velMsg.heading*DEGREES_TO_RADIANS);
+ double eVel = velMsg.horizontalSpeed*sin(velMsg.heading*DEGREES_TO_RADIANS);
+ // 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
+ double elTime = (double)PPSTimeOffset + timeFromPPS.read();
+ // Position time
+ double posTime = GPSTime + elTime;
+
+ // Estimated position based on previous position and velocity
+ double latPos = posMsg.latitude + (nVel/latMetersPerDeg)*elTime;
+ double lonPos = posMsg.longitude + (eVel/lonMetersPerDeg)*elTime;
+ double htPos = posMsg.height + velMsg.verticalSpeed/(60*1852)*elTime;
+ toPC.printf("WMsg POSVEL %5.3lf %d %c %8.5lf %9.5lf %4.3lf %4.3lf %4.3lf %4.3lf\n",
+ posTime,
+ posMsg.numSolSV,
+ solReady,
+ latPos,
+ lonPos,
+ htPos,
+ nVel,
+ eVel,
+ velMsg.verticalSpeed
+ );
+ }
+ if (sendStatus)
+ {
+ sendStatus=false;
+ char solReady = 'N';
+ if (posMsg.solStatus == 0)
+ {
+ solReady = 'Y';
+ }
+ toPC.printf("WMsg STATUS %5.3lf %c\n",
+ GPSTime,
+ solReady
+ );
+ }
+ if (sendRecData)
+ {
+ sendRecData=false;
+ char recChar = 'N';
+ if (recordData)
+ {
+ if ((fpNav == NULL))
+ {
+ fpNav = fopen("/sd/Data/NAV.bin", "wb");
+ }
+ if (fpNav != NULL)
+ {
+ recChar = 'Y';
+ }
+ //recChar = 'Y';
+ }
+ else
+ {
+ if (fpNav != NULL)
+ {
+ fclose(fpNav);
+ fpNav = NULL;
+ }
+ /*
+ if (fpIMU != NULL)
+ {
+ fclose(fpIMU);
+ fpIMU = NULL;
+ }
+ if (fpGPS != NULL)
+ {
+ fclose(fpGPS);
+ fpGPS = NULL;
+ }
+ */
+ }
+ toPC.printf("WMsg RECORDDATA %c\n",
+ recChar
+ );
+ }
+ recordDataled = recordData;
+ if (sendStreamPos)
+ {
+ sendStreamPos=false;
+ char streamChar = 'N';
+ if (streamPos)
+ {
+ streamChar = 'Y';
+ }
+ toPC.printf("WMsg POSSTREAM %c\n",
+ streamChar
+ );
+ }
+ if (IMUDataReady)
+ {
+ //write the IMU data
+ //if (recordData && (fpIMU != NULL))
+ if (recordData && (fpNav != NULL))
+ {
+ fwrite(imuRec.dataWord, sizeof(IMUREC), 1, fpNav);
+ }
+ IMUDataReady = false;
+ }
+ if (lookingForMessages && (timeFromPPS.read_us() > 15000)) //it takes less than 20msec to receive all messages
+ {
+
+ //toPC.printf(" num messages = %3d time = %5d \n", perSecMessageCounter, timeFromPPS.read_us());
+ for (int i=0; i<perSecMessageCounter; i++)
+ {
+ msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]-3];
+ //toPC.printf(" %5d ", msgHeader[i]->messageID);
+ if ((msgHeader[i]->messageID == 42) ||
+ (msgHeader[i]->messageID == 99))
+ {
+ if (msgHeader[i]->messageID == 42)
+ {
+ // Wait until velocity message has also been received before using as
+ // base position
+ //memcpy(&curPos, &msgBuffer[messageLocation[i]-3], sizeof(OEM615BESTPOS));
+ curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[i]-3]);
+ if (streamPos)
+ {
+ toPC.printf("WMsg BESTPOS %d %d %d %8.5lf %9.5lf %5.3lf %5.3f %5.3f %5.3f %5.3f %5.3f %5.3f %d %d %d %d %d\n",
+ curPos.msgHeader.GPSTime_msecs,
+ curPos.solStatus,
+ curPos.posType,
+ curPos.latitude,
+ curPos.longitude,
+ curPos.height,
+ curPos.undulation,
+ curPos.latitudeSTD,
+ curPos.longitudeSTD,
+ curPos.heightSTD,
+ curPos.diffAge,
+ curPos.solutionAge,
+ curPos.numSV,
+ curPos.numSolSV,
+ curPos.numGGL1,
+ curPos.extSolStatus,
+ curPos.sigMask);
+ }
+ }
+ else if (msgHeader[i]->messageID == 99)
+ {
+ // Wait until position message has also been received before using as
+ // base position
+ //memcpy(&curVel, &msgBuffer[messageLocation[i]-3], sizeof(OEM615BESTVEL));
+ curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]-3]);
+ }
+ if ((curVel.msgHeader.GPSTime_msecs+250)/1000 ==
+ (curPos.msgHeader.GPSTime_msecs+250)/1000)
+ {
+ // update position and velocity used for calculation
+ GPSTimemsecs = curPos.msgHeader.GPSTime_msecs;
+ GPSTime = (double)GPSTimemsecs/1000.0;
+ velMsg = curVel;
+ posMsg = curPos;
+ PPSTimeOffset = 0;
+ }
+ }
+ }
+ //toPC.printf(" %3d %8d \n", msgHeader[0]->timeStatus, GPSTimemsecs);
+ //if (recordData && (fpGPS != NULL))
+ if (recordData && (fpNav != NULL))
+ {
+ fwrite(msgBuffer, byteCounter, 1, fpNav);
+ }
+
+ lookingForMessages = false;
+ }
+ if (messageDetected)
+ {
+ //toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us());
+ messageDetected = false;
+ }
+ if (camera1EventDetected)
+ {
+ toPC.printf("WMsg TRIGGERTIME Camera1 %5.3lf\n", camera1Time);
+ camera1EventDetected = false;
+ }
+ if (camera2EventDetected)
+ {
+ toPC.printf("WMsg TRIGGERTIME Camera2 %5.3lf\n", camera2Time);
+ camera2EventDetected = false;
+ }
+ if (detectedGPS1PPS)
+ {
+ //toPC.printf(" PPSCounter = %4d byteCounter = %10d num Messages Received = %3d IMUClock = %4d\n",
+ // PPSCounter, byteCounter, perSecMessageCounter, savedIMUClockCounter);
+ byteCounter = 0;
+ perSecMessageCounter=0;
+ detectedGPS1PPS = false;
+ }
+ }
+ toPC.printf(" normal termination \n");
+}
\ No newline at end of file