james kain / Mbed 2 deprecated GPS_Incremental

JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013

Dependencies:   mbed

Fork of GPS_Incremental by Dan Matthews

main.cpp@6:2a8486283198, 2013-04-02 (annotated)

Committer:
jekain314
Date:
Tue Apr 02 15:22:37 2013 +0000
Revision:
6:2a8486283198
Parent:
4:68268737ff89
Child:
7:2e20e4cf53e6
JEK changes enabling proper recording of IMU/GPS datastreams to the SD-card

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dannyman939 0:c746ee34feae 1 #include "mbed.h"
jekain314 6:2a8486283198 2 #include <string>
dannyman939 0:c746ee34feae 3
dannyman939 0:c746ee34feae 4 //set up the message buffer to be filled by the GPS read process
dannyman939 0:c746ee34feae 5 #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 256
dannyman939 0:c746ee34feae 6
dannyman939 0:c746ee34feae 7 #include "MODSERIAL.h"
dannyman939 0:c746ee34feae 8 #include "SDFileSystem.h" //imported using the import utility
dannyman939 0:c746ee34feae 9
jekain314 4:68268737ff89 10 //general digital I/O specifications for this application
dannyman939 0:c746ee34feae 11 //SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
dannyman939 0:c746ee34feae 12 SDFileSystem sd(p11,p12,p13,p14,"sd");
jekain314 6:2a8486283198 13 DigitalIn sd_detect(p27);
jekain314 6:2a8486283198 14 DigitalOut ppsled(LED1); //blink an LED at the 1PPS
jekain314 6:2a8486283198 15 DigitalOut trig1led(LED2); //blink an LED at the camera trigger detection
jekain314 6:2a8486283198 16 DigitalOut recordDataled(LED4); //set the led when the record is on
jekain314 6:2a8486283198 17 InterruptIn camera1Int(p30); // camera interrupt in
jekain314 6:2a8486283198 18 DigitalOut camera2Pin(p29); // We dont use the second camera interrupt
dannyman939 0:c746ee34feae 19 //USB serial data stream back to the PC
jekain314 6:2a8486283198 20 Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10
dannyman939 0:c746ee34feae 21
jekain314 6:2a8486283198 22 bool detectedGPS1PPS = false; //flag set in the ISR and reset after processing the 1PPS event
jekain314 6:2a8486283198 23 int PPSCounter = 0; //counts the 1PPS occurrences
jekain314 6:2a8486283198 24 int byteCounter = 0; //byte counter -- zeroed at 1PPS
jekain314 4:68268737ff89 25 unsigned short perSecMessageCounter=0; //counts the number of messages in a sec based on the header detection
jekain314 6:2a8486283198 26 bool lookingForMessages = true; //set in the PPS ISR and set false after the message processing in the main
jekain314 6:2a8486283198 27 bool messageDetected = false; //have detected a message header
jekain314 6:2a8486283198 28 unsigned long IMUbytesWritten = 0; //counts the IMU bytes written by the fwrite() to the SD card
jekain314 6:2a8486283198 29 int savedByteCounter = 0; //save ByteCounter at the 1PPS for display in main
jekain314 6:2a8486283198 30 int savedPerSecMessageCounter=0; //saved PerSecMsgCounter for display in main
jekain314 6:2a8486283198 31 int IMUClockCounter = 0; //counter for IMU samples per sec
jekain314 6:2a8486283198 32 int savedIMUClockCounter=0; //saved at the 1PPS for later diaplay from main
jekain314 6:2a8486283198 33 bool camera1EventDetected = false; //flag from ISR indicating a clock event occurred
jekain314 6:2a8486283198 34 double camera1Time; //GPS time of the camera event
jekain314 6:2a8486283198 35 int TotalBadCRCmatches = 0; //counter for the bad CRC matches for all GPS messages
jekain314 4:68268737ff89 36
jekain314 6:2a8486283198 37 //////////////////////////////////////////////////////////////////////
jekain314 6:2a8486283198 38 // the below should become classes
jekain314 6:2a8486283198 39 //////////////////////////////////////////////////////////////////////
jekain314 6:2a8486283198 40 #include "OEM615.h" //OEM615 GPS activities
jekain314 6:2a8486283198 41 #include "ADIS16488.h" //ADIS16488 activities
jekain314 6:2a8486283198 42 #include "PCMessaging.h" //PC messaging activities
dannyman939 0:c746ee34feae 43
dannyman939 0:c746ee34feae 44 //ISR for detection of the GPS 1PPS
dannyman939 0:c746ee34feae 45 void detect1PPSISR(void)
dannyman939 0:c746ee34feae 46 {
jekain314 6:2a8486283198 47 timeFromPPS.reset(); //reset the 1PPS timer upon 1PPS detection
jekain314 4:68268737ff89 48 savedIMUClockCounter = IMUClockCounter; //number of IMU clocks received since last 1PPS
jekain314 6:2a8486283198 49 savedByteCounter = byteCounter; //save byteCounter for display in main
jekain314 6:2a8486283198 50 savedPerSecMessageCounter = perSecMessageCounter; //save for display un main
jekain314 6:2a8486283198 51 IMUClockCounter = 0; //counts per-sec IMU samples (between 1PPS events)
jekain314 6:2a8486283198 52 byteCounter = 0; //countes bytes between 1PPS events
jekain314 6:2a8486283198 53 perSecMessageCounter = 0; //counts GPS messages between 1PPS events
jekain314 6:2a8486283198 54
jekain314 6:2a8486283198 55 GPS_COM1.rxBufferFlush(); //flush the GPS serial buffer
dannyman939 0:c746ee34feae 56
jekain314 6:2a8486283198 57 detectedGPS1PPS = true; //set false in the main when 1PPS actions are complete
jekain314 6:2a8486283198 58 lookingForMessages = true; //set false in main after processing messages
jekain314 6:2a8486283198 59 PPSCounter++; //count number of 1PPS epoch
jekain314 6:2a8486283198 60
jekain314 6:2a8486283198 61 //note -- the below accounts for time information becoming available AFTER the 1PPS event
jekain314 6:2a8486283198 62 PPSTimeOffset++; //counts 1PPS events between matching POS and VEL messages
jekain314 6:2a8486283198 63
jekain314 6:2a8486283198 64 ppsled = !ppsled; //blink an LED at the 1PPS
dannyman939 0:c746ee34feae 65 };
dannyman939 0:c746ee34feae 66
dannyman939 0:c746ee34feae 67 //ISR for detection of the hotshoe trigger 1
dannyman939 1:cbb9104d826f 68 void camera1ISR(void)
dannyman939 0:c746ee34feae 69 {
jekain314 6:2a8486283198 70 //GPSTime is from POS message header
jekain314 6:2a8486283198 71 //PPSTimeOffset is an even sec to account for Time becoming known AFTER the 1PPS
jekain314 6:2a8486283198 72 //PPSTimeOffset + timeFromPPS.read() can be as large as 1.02 secs
jekain314 6:2a8486283198 73 camera1Time = GPSTime + PPSTimeOffset + timeFromPPS.read();
jekain314 6:2a8486283198 74 camera1EventDetected = true; //reset to false in main after processing the image detection
jekain314 4:68268737ff89 75 trig1led = !trig1led; //blink an LEWD at the camera event detection
dannyman939 0:c746ee34feae 76 };
dannyman939 0:c746ee34feae 77
jekain314 6:2a8486283198 78 ///////////////////////////////////////////////////////
jekain314 6:2a8486283198 79 //set up the USB port and the GPS COM port
jekain314 6:2a8486283198 80 ///////////////////////////////////////////////////////
jekain314 6:2a8486283198 81 FILE *fpNav = NULL; //file pointer to the nav file on the SD card
dannyman939 0:c746ee34feae 82 void setupCOM(void)
dannyman939 0:c746ee34feae 83 {
dannyman939 0:c746ee34feae 84 //system starts with GPS in reset active
dannyman939 0:c746ee34feae 85 //dis-engage the reset to get the GPS started
dannyman939 0:c746ee34feae 86 GPS_Reset=1; wait_ms(1000);
dannyman939 0:c746ee34feae 87
dannyman939 0:c746ee34feae 88 //establish 1PPS ISR
dannyman939 0:c746ee34feae 89 PPSInt.rise(&detect1PPSISR);
dannyman939 0:c746ee34feae 90
dannyman939 0:c746ee34feae 91 //set the USB serial data rate -- rate must be matched at the PC end
dannyman939 0:c746ee34feae 92 //This the serial communication back to the the PC host
dannyman939 0:c746ee34feae 93 //Launch the C++ serial port read program there to catch the ASCII characters
dannyman939 0:c746ee34feae 94 //toPC.baud(9600); wait_ms(100);
dannyman939 0:c746ee34feae 95 toPC.baud(8*115200); wait_ms(100);
dannyman939 2:7301e63832ee 96 //toPC.baud(1*115200); wait_ms(100);
dannyman939 0:c746ee34feae 97 toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n");
dannyman939 0:c746ee34feae 98
jekain314 4:68268737ff89 99 //just wait to launch the GPS receiver
dannyman939 0:c746ee34feae 100 for (int i=0; i<5; i++) { toPC.printf(" to start: %3d \n", 4-i); wait(1); }
dannyman939 0:c746ee34feae 101
jekain314 6:2a8486283198 102 sd_detect.mode(PullUp);
dannyman939 0:c746ee34feae 103
jekain314 6:2a8486283198 104 if (sd_detect == 0)
jekain314 6:2a8486283198 105 {
jekain314 6:2a8486283198 106 mkdir("/sd/Data", 0777);
jekain314 6:2a8486283198 107 }
jekain314 6:2a8486283198 108 else
jekain314 6:2a8486283198 109 {
jekain314 6:2a8486283198 110 toPC.printf(" SD card not present \n");
jekain314 6:2a8486283198 111 }
jekain314 6:2a8486283198 112
jekain314 6:2a8486283198 113 fpNav = fopen("/sd/Data/IMUGPS.bin", "wb");
jekain314 6:2a8486283198 114 if (fpNav == NULL)
dannyman939 0:c746ee34feae 115 {
dannyman939 0:c746ee34feae 116 toPC.printf(" cannot open the IMUGPS data file \n");
dannyman939 0:c746ee34feae 117 }
dannyman939 0:c746ee34feae 118 else
dannyman939 0:c746ee34feae 119 toPC.printf(" opened the IMUGPS data file \n");
jekain314 4:68268737ff89 120
jekain314 4:68268737ff89 121 //NOTE: we do not assume that the GPS receiver has been pre-set up for the WALDO_FCS functionality
jekain314 4:68268737ff89 122 //we alwsys start with a reset and reprogram the receiver with our data out products
jekain314 4:68268737ff89 123 // this prevents failure because of a blown NVRAM as occurred for the older camera systems
jekain314 4:68268737ff89 124
dannyman939 0:c746ee34feae 125 //this is the COM1 port from th GPS receiuver to the mbed
dannyman939 0:c746ee34feae 126 //it should be always started at 9600 baud because thats the default for the GPS receiver
dannyman939 0:c746ee34feae 127 GPS_COM1.baud(9600); wait_ms(100);
dannyman939 0:c746ee34feae 128
dannyman939 0:c746ee34feae 129 // this ASCII command sets up the serial data from the GPS receiver on its COM1
dannyman939 0:c746ee34feae 130 char ch7[] = "serialconfig COM1 9600 n 8 1 n off";
jekain314 4:68268737ff89 131 // this is a software reset and has the same effect as a hardware reset (why do it?)
jekain314 4:68268737ff89 132 //char ch0[] = "RESET";
dannyman939 0:c746ee34feae 133 //this command stops all communication from the GPS receiver on COM1
jekain314 4:68268737ff89 134 //logs should still be presented on USB port so the Novatel CDU application can be used on the PC in parallel
dannyman939 0:c746ee34feae 135 char ch1[] = "unlogall COM1";
dannyman939 0:c746ee34feae 136 //set the final baud rate that we will use from here
dannyman939 0:c746ee34feae 137 //allowable baud rate values: 9600 115200 230400 460800 921600
jekain314 4:68268737ff89 138 //char ch2[] = "serialconfig COM1 921600 n 8 1 n off";
jekain314 4:68268737ff89 139 char ch2[] = "serialconfig COM1 460800 n 8 1 n off";
dannyman939 0:c746ee34feae 140
dannyman939 0:c746ee34feae 141 //the below commands request the POS, VEL, RANGE, and TIME messages
dannyman939 0:c746ee34feae 142 char ch3[] = "log COM1 BESTPOSB ONTIME 1"; //messageID = 42
dannyman939 0:c746ee34feae 143 char ch4[] = "log COM1 BESTVelB ONTIME 1"; //messageID = 99
dannyman939 0:c746ee34feae 144 char ch5[] = "log COM1 RANGEB ONTIME 1"; //messageID = 43
jekain314 4:68268737ff89 145 //char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101
dannyman939 0:c746ee34feae 146
dannyman939 0:c746ee34feae 147 //set up VARF to be 100Hz with 1X10^4 * 10^-8 = 10^-4 sec (10usec) pulse width
jekain314 4:68268737ff89 148 //in fact, we do not use this output but it is available.
jekain314 4:68268737ff89 149 //originally planned to use this to command the IMU data
jekain314 6:2a8486283198 150 //char ch8[] = "FREQUENCYOUT enable 10000 1000000";
dannyman939 0:c746ee34feae 151
dannyman939 0:c746ee34feae 152 toPC.printf("set serial config \n");
dannyman939 0:c746ee34feae 153 sendASCII(ch7, sizeof(ch7)); wait_ms(500);
dannyman939 0:c746ee34feae 154 //sendASCII(ch0, sizeof(ch0));
dannyman939 0:c746ee34feae 155 toPC.printf("unlog all messages \n");
dannyman939 0:c746ee34feae 156 sendASCII(ch1, sizeof(ch1)); wait_ms(500);
dannyman939 0:c746ee34feae 157 toPC.printf("log BESTPOSB on COM1 \n");
dannyman939 0:c746ee34feae 158 sendASCII(ch3, sizeof(ch3)); wait_ms(500);
dannyman939 0:c746ee34feae 159 toPC.printf("log BESTVELB on COM1\n");
dannyman939 0:c746ee34feae 160 sendASCII(ch4, sizeof(ch4)); wait_ms(500);
dannyman939 0:c746ee34feae 161 toPC.printf("log RANGEB on COM1\n");
dannyman939 0:c746ee34feae 162 sendASCII(ch5, sizeof(ch5)); wait_ms(500);
dannyman939 0:c746ee34feae 163
dannyman939 0:c746ee34feae 164 //toPC.printf("log TIMEB om COM1 \n");
dannyman939 0:c746ee34feae 165 //sendASCII(ch6, sizeof(ch6)); wait_ms(100);
dannyman939 0:c746ee34feae 166
jekain314 6:2a8486283198 167 //toPC.printf("Set up th VARF signal \n");
jekain314 6:2a8486283198 168 //sendASCII(ch8, sizeof(ch8)); wait_ms(500);
dannyman939 0:c746ee34feae 169
dannyman939 0:c746ee34feae 170 //set GPS output COM1 to the final high rate
dannyman939 0:c746ee34feae 171 toPC.printf("set the COM ports to high rate\n");
dannyman939 0:c746ee34feae 172 sendASCII(ch2, sizeof(ch2)); wait_ms(500);
dannyman939 0:c746ee34feae 173
dannyman939 0:c746ee34feae 174 //set the mbed COM port to match the GPS transmit rate
dannyman939 0:c746ee34feae 175 //the below baud rate must match the COM1 rate coming from the GPS receiver
jekain314 4:68268737ff89 176 GPS_COM1.baud(460800); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL
jekain314 4:68268737ff89 177 //GPS_COM1.baud(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL
dannyman939 0:c746ee34feae 178 };
dannyman939 0:c746ee34feae 179
dannyman939 0:c746ee34feae 180 void setupTriggers()
dannyman939 0:c746ee34feae 181 {
dannyman939 1:cbb9104d826f 182 camera1Int.mode(PullUp);
dannyman939 1:cbb9104d826f 183 camera2Pin = 1;
dannyman939 0:c746ee34feae 184 //establish Trigger ISR
dannyman939 1:cbb9104d826f 185 camera1Int.rise(&camera1ISR);
dannyman939 0:c746ee34feae 186
dannyman939 3:5913df46f94a 187 };
dannyman939 0:c746ee34feae 188
jekain314 6:2a8486283198 189 /////////////////////////////////////////////////////////////////////
jekain314 6:2a8486283198 190 // mbed main to support the Waldo_FCS
jekain314 6:2a8486283198 191 /////////////////////////////////////////////////////////////////////
jekain314 6:2a8486283198 192 int main() {
jekain314 4:68268737ff89 193
jekain314 6:2a8486283198 194 //these are structures for the to GPS messages that must be parsed
jekain314 6:2a8486283198 195 OEM615BESTPOS posMsg; //BESTPOS structure in OEMV615.h that has matching time to a BESTVEL message
jekain314 4:68268737ff89 196 OEM615BESTPOS curPos; //BESTPOS structure in OEMV615.h
jekain314 6:2a8486283198 197 OEM615BESTVEL velMsg; //BESTVEL structure in OEMV615.h that has matching time to a BESTPOS message
jekain314 4:68268737ff89 198 OEM615BESTVEL curVel; //BESTVEL structure in OEMV615.h
dannyman939 0:c746ee34feae 199
dannyman939 2:7301e63832ee 200 //set up the GPS and mbed COM ports
dannyman939 0:c746ee34feae 201 setupCOM();
dannyman939 0:c746ee34feae 202
dannyman939 0:c746ee34feae 203 //set up the ADIS16488
dannyman939 0:c746ee34feae 204 setupADIS();
dannyman939 0:c746ee34feae 205
dannyman939 0:c746ee34feae 206 //setup Hotshoe
dannyman939 0:c746ee34feae 207 setupTriggers();
jekain314 6:2a8486283198 208
jekain314 6:2a8486283198 209 setUpMessages(); //set up the expected text message commands frm the PC
dannyman939 0:c746ee34feae 210
jekain314 4:68268737ff89 211 //set up the interrupt to catch the GPS receiver serial bytes as they are presented
dannyman939 0:c746ee34feae 212 GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
dannyman939 0:c746ee34feae 213
jekain314 4:68268737ff89 214 timeFromPPS.start(); //start the time for measuring time from 1PPS events
jekain314 6:2a8486283198 215
jekain314 6:2a8486283198 216 toPC.printf("\n\n top of the main loop \n\n");
jekain314 6:2a8486283198 217
jekain314 6:2a8486283198 218 int totalBytesWritten = 0;
jekain314 6:2a8486283198 219
jekain314 6:2a8486283198 220 //while(PPSCounter < 100)
jekain314 4:68268737ff89 221 ///////////////////////////////////////////////////////////////////////////
jekain314 4:68268737ff89 222 // top of the while loop
jekain314 4:68268737ff89 223 ///////////////////////////////////////////////////////////////////////////
jekain314 6:2a8486283198 224 while(1)
dannyman939 0:c746ee34feae 225 {
dannyman939 0:c746ee34feae 226 //read the USB serial data from the PC to check for commands
jekain314 4:68268737ff89 227 //in the primary real-time portion, there are no bytes from the PC so this has no impact
dannyman939 0:c746ee34feae 228 readFromPC();
jekain314 6:2a8486283198 229 processPCmessages(fpNav, posMsg, velMsg);
dannyman939 0:c746ee34feae 230
jekain314 6:2a8486283198 231 //
jekain314 4:68268737ff89 232 ////////////////////////////////////////////////////////////////////////////
jekain314 4:68268737ff89 233 //below is where we process the complete stored GPS message for the second
jekain314 4:68268737ff89 234 //The !IMUDataReady test prevents the IMU and GPS data from being written
jekain314 4:68268737ff89 235 //to disk on the same pass through thi loop
jekain314 4:68268737ff89 236 /////////////////////////////////////////////////////////////////////////////
jekain314 4:68268737ff89 237 if (!IMUDataReady && lookingForMessages && (timeFromPPS.read_us() > 20000)) //it takes less than 20msec to receive all messages
jekain314 4:68268737ff89 238 {
jekain314 6:2a8486283198 239 //toPC.printf(" num messages = %3d time = %5d \n", perSecMessageCounter, timeFromPPS.read_us());
jekain314 4:68268737ff89 240
jekain314 4:68268737ff89 241 //cycle through all the bytes stored this sec (after the 1PPS as set)
jekain314 4:68268737ff89 242 // perSecMessageCounter is incremented whenever we detect a new message headet 0xAA44121C sequence
jekain314 4:68268737ff89 243 for (int i=0; i<perSecMessageCounter; i++)
dannyman939 0:c746ee34feae 244 {
jekain314 6:2a8486283198 245 msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]];
jekain314 6:2a8486283198 246 //toPC.printf("WMsg MESSAGEINFO %5d %5d \n",
jekain314 6:2a8486283198 247 // msgHeader[i]->messageID,
jekain314 6:2a8486283198 248 // messageLocation[i]);
jekain314 6:2a8486283198 249
jekain314 6:2a8486283198 250 //calculated CRC
jekain314 6:2a8486283198 251 unsigned long CRC1 = CalculateBlockCRC32(28+msgHeader[i]->messageLength, &msgBuffer[messageLocation[i]]);
jekain314 6:2a8486283198 252 unsigned long CRC2 = *((unsigned long*)&msgBuffer[messageLocation[i] + 28 + msgHeader[i]->messageLength]);
jekain314 6:2a8486283198 253
jekain314 6:2a8486283198 254 if (CRC1 != CRC2)
jekain314 6:2a8486283198 255 {
jekain314 6:2a8486283198 256 TotalBadCRCmatches++;
jekain314 6:2a8486283198 257 toPC.printf(" bad CRC match for messageID %3d total CRC errors = %4d \n",
jekain314 6:2a8486283198 258 msgHeader[i]->messageID, TotalBadCRCmatches);
jekain314 6:2a8486283198 259 continue;
jekain314 6:2a8486283198 260 }
jekain314 6:2a8486283198 261
jekain314 4:68268737ff89 262 //test for a message 42 (BESTPOS)
jekain314 4:68268737ff89 263 if (msgHeader[i]->messageID == 42)
dannyman939 2:7301e63832ee 264 {
jekain314 4:68268737ff89 265 curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[i]]);
dannyman939 2:7301e63832ee 266
jekain314 4:68268737ff89 267 if (streamPos)
dannyman939 1:cbb9104d826f 268 {
dannyman939 0:c746ee34feae 269 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",
dannyman939 0:c746ee34feae 270 curPos.msgHeader.GPSTime_msecs,
dannyman939 0:c746ee34feae 271 curPos.solStatus,
dannyman939 0:c746ee34feae 272 curPos.posType,
dannyman939 0:c746ee34feae 273 curPos.latitude,
dannyman939 0:c746ee34feae 274 curPos.longitude,
dannyman939 0:c746ee34feae 275 curPos.height,
dannyman939 0:c746ee34feae 276 curPos.undulation,
dannyman939 0:c746ee34feae 277 curPos.latitudeSTD,
dannyman939 0:c746ee34feae 278 curPos.longitudeSTD,
dannyman939 0:c746ee34feae 279 curPos.heightSTD,
dannyman939 0:c746ee34feae 280 curPos.diffAge,
dannyman939 0:c746ee34feae 281 curPos.solutionAge,
dannyman939 0:c746ee34feae 282 curPos.numSV,
dannyman939 0:c746ee34feae 283 curPos.numSolSV,
dannyman939 0:c746ee34feae 284 curPos.numGGL1,
dannyman939 0:c746ee34feae 285 curPos.extSolStatus,
dannyman939 0:c746ee34feae 286 curPos.sigMask);
dannyman939 0:c746ee34feae 287 }
dannyman939 0:c746ee34feae 288 }
jekain314 4:68268737ff89 289
jekain314 4:68268737ff89 290 //check for a message 99 (BESTVEL) -- and cast it into its message structure
jekain314 4:68268737ff89 291 else if (msgHeader[i]->messageID == 99)
jekain314 4:68268737ff89 292 {
jekain314 4:68268737ff89 293 curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]]);
jekain314 4:68268737ff89 294 }
jekain314 4:68268737ff89 295
jekain314 4:68268737ff89 296 //the below test ensures that the positin and veocity are matched in time
jekain314 4:68268737ff89 297 //not sure the reason for the "250" below
jekain314 4:68268737ff89 298 if ((curVel.msgHeader.GPSTime_msecs+250)/1000 ==
jekain314 4:68268737ff89 299 (curPos.msgHeader.GPSTime_msecs+250)/1000)
dannyman939 2:7301e63832ee 300 {
jekain314 4:68268737ff89 301 // update position and velocity used for calculation
jekain314 4:68268737ff89 302 GPSTimemsecs = curPos.msgHeader.GPSTime_msecs;
jekain314 4:68268737ff89 303 GPSTime = (double)GPSTimemsecs/1000.0;
jekain314 4:68268737ff89 304 velMsg = curVel; //
jekain314 4:68268737ff89 305 posMsg = curPos;
jekain314 4:68268737ff89 306
jekain314 4:68268737ff89 307 /////////////////////////////////////////////////////////////////////////////////////////
jekain314 4:68268737ff89 308 //IMPORTANT: we reset the PPSTimeOffset when we have a matching position and velocity
jekain314 4:68268737ff89 309 PPSTimeOffset = 0;
jekain314 4:68268737ff89 310 /////////////////////////////////////////////////////////////////////////////////////////
dannyman939 2:7301e63832ee 311 }
jekain314 6:2a8486283198 312 } //end of per message loop
jekain314 4:68268737ff89 313 lookingForMessages = false;
jekain314 4:68268737ff89 314
jekain314 6:2a8486283198 315 if (recordData && (fpNav != NULL) && (byteCounter > 0))
jekain314 6:2a8486283198 316 {
jekain314 6:2a8486283198 317 wait_us(1000);
jekain314 6:2a8486283198 318 totalBytesWritten += fwrite(msgBuffer, 1, byteCounter, fpNav); // this writes out a complete set of messages for this sec
jekain314 6:2a8486283198 319 wait_us(1000);
jekain314 6:2a8486283198 320 }
jekain314 6:2a8486283198 321
jekain314 4:68268737ff89 322 } //end of the GPS message processing
jekain314 4:68268737ff89 323
jekain314 6:2a8486283198 324 //
jekain314 6:2a8486283198 325
jekain314 4:68268737ff89 326 //the IMU data record is read from the SPI in the ISR and the IMUDataReady is set true
jekain314 4:68268737ff89 327 //we write the IMU data here
jekain314 4:68268737ff89 328 if (IMUDataReady) //IMUDataReady is true if we have a recent IMU data record
jekain314 4:68268737ff89 329 {
jekain314 6:2a8486283198 330 //GPSTime (secs from midnight) is from the header of the position message
jekain314 6:2a8486283198 331 //PPSTimeOffset accounts for time becoming known ~20msec AFTER the 1PPS
jekain314 6:2a8486283198 332 imuRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + timeFromPPS.read_us()/1000.0;
jekain314 6:2a8486283198 333 wait_us(10);
jekain314 6:2a8486283198 334 spi.write((int) HIGH_REGISTER[0]); wait_us(10); // next read will return results from HIGH_REGITER[0]
jekain314 4:68268737ff89 335 for (int i=0; i<6; i++) //read the 6 rate and accel variables
jekain314 4:68268737ff89 336 {
jekain314 6:2a8486283198 337 wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]); wait_us(10) ;
jekain314 4:68268737ff89 338 if (i<5) // dont this on the last because this was pre-called
jekain314 6:2a8486283198 339 { wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]); wait_us(10); }
jekain314 4:68268737ff89 340 imuRec.dataWord[i] = wd.dataWord; //data word is a signed long
jekain314 4:68268737ff89 341 }
jekain314 4:68268737ff89 342 IMURecordCounter++;
jekain314 4:68268737ff89 343 //write the IMU data
dannyman939 0:c746ee34feae 344 if (recordData && (fpNav != NULL))
jekain314 6:2a8486283198 345 {
jekain314 6:2a8486283198 346 totalBytesWritten += fwrite(&imuRec, 1, sizeof(IMUREC), fpNav);
dannyman939 0:c746ee34feae 347 }
jekain314 4:68268737ff89 348 IMUClockCounter++;
jekain314 4:68268737ff89 349 IMUDataReady = false;
dannyman939 0:c746ee34feae 350 }
jekain314 4:68268737ff89 351
jekain314 6:2a8486283198 352 /*
jekain314 4:68268737ff89 353 if (messageDetected) //some GPS message header has been detected
dannyman939 0:c746ee34feae 354 {
jekain314 4:68268737ff89 355 toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us());
dannyman939 0:c746ee34feae 356 messageDetected = false;
dannyman939 0:c746ee34feae 357 }
jekain314 6:2a8486283198 358 */
jekain314 4:68268737ff89 359
jekain314 4:68268737ff89 360 if (camera1EventDetected) //we have detected a camera trigger event
dannyman939 0:c746ee34feae 361 {
dannyman939 2:7301e63832ee 362 toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time);
dannyman939 0:c746ee34feae 363 camera1EventDetected = false;
dannyman939 0:c746ee34feae 364 }
jekain314 4:68268737ff89 365
jekain314 4:68268737ff89 366 if (detectedGPS1PPS) //true if we are exactly at a 1PPS event detection
dannyman939 0:c746ee34feae 367 {
jekain314 6:2a8486283198 368 //toPC.printf(" PPSCounter=%4d byteCounter=%10d Msgs Received=%3d IMUClock=%4d bytesWritten=%8d\n",
jekain314 6:2a8486283198 369 // PPSCounter, savedByteCounter, savedPerSecMessageCounter, savedIMUClockCounter, totalBytesWritten);
jekain314 6:2a8486283198 370
dannyman939 0:c746ee34feae 371 detectedGPS1PPS = false;
dannyman939 0:c746ee34feae 372 }
dannyman939 0:c746ee34feae 373 }
jekain314 4:68268737ff89 374
jekain314 4:68268737ff89 375 fclose(fpNav);
dannyman939 0:c746ee34feae 376 toPC.printf(" normal termination \n");
dannyman939 0:c746ee34feae 377 }