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:
- 29:dead10cce6e9
- Parent:
- 27:94a6f0589993
- Child:
- 30:96d133f3008e
--- 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
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 30 Apr 2013 |
| Imports: | 21 |
| Forks: | 0 |
| Commits: | 31 |
| Dependents: | 0 |
| Dependencies: | 9 |
| Followers: | 2 |
