james kain / Mbed 2 deprecated Waldo_Embed_V2

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

main.cpp@2:7039be3daf6e, 2013-05-03 (annotated)

Committer:
jekain314
Date:
Fri May 03 19:27:56 2013 +0000
Revision:
2:7039be3daf6e
Parent:
1:8e24e633f8d8
Child:
3:e1a884e5325a
Added an auto file-open when POSVEL message is received. Added auto-close when the elapsed time between POSVEL messages is > 60sec This reduces the necessary startup procedures on the PC side. Also prevents loss of data if the PC fails.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jekain314 0:432b860b6ff7 1 #include "mbed.h"
jekain314 0:432b860b6ff7 2 #include <string>
jekain314 0:432b860b6ff7 3
jekain314 0:432b860b6ff7 4 //set up the message buffer to be filled by the GPS read process
jekain314 0:432b860b6ff7 5 #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 256
jekain314 0:432b860b6ff7 6
jekain314 0:432b860b6ff7 7 #include "MODSERIAL.h"
jekain314 0:432b860b6ff7 8 #include "SDFileSystem.h" //imported using the import utility
jekain314 0:432b860b6ff7 9
jekain314 0:432b860b6ff7 10 //general digital I/O specifications for this application
jekain314 0:432b860b6ff7 11 //SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
jekain314 0:432b860b6ff7 12 SDFileSystem sd(p11,p12,p13,p14,"sd");
jekain314 0:432b860b6ff7 13 DigitalIn sd_detect(p27);
jekain314 0:432b860b6ff7 14 DigitalOut ppsled(LED1); //blink an LED at the 1PPS
jekain314 0:432b860b6ff7 15 DigitalOut trig1led(LED2); //blink an LED at the camera trigger detection
jekain314 1:8e24e633f8d8 16 DigitalOut rxMsg(LED3);
jekain314 1:8e24e633f8d8 17 DigitalOut txMsg(LED4);
jekain314 1:8e24e633f8d8 18 //DigitalOut recordDataled(LED4); //set the led when the record is on
jekain314 1:8e24e633f8d8 19
jekain314 1:8e24e633f8d8 20 //hardware trigger mechanization for bulb shutter commands
jekain314 1:8e24e633f8d8 21 DigitalInOut fire(p29); //connected to the tip of 2.5mm connector (T2i)
jekain314 1:8e24e633f8d8 22 DigitalInOut pre_fire(p30); //connected to the mid-connection for 2.5mm connector (T2i)
jekain314 1:8e24e633f8d8 23
jekain314 1:8e24e633f8d8 24
jekain314 0:432b860b6ff7 25 //USB serial data stream back to the PC
jekain314 0:432b860b6ff7 26 Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10
jekain314 0:432b860b6ff7 27
jekain314 0:432b860b6ff7 28 Timer timeFromStart;
jekain314 2:7039be3daf6e 29 Timer timeFromPosVelMessageReceipt;
jekain314 0:432b860b6ff7 30
jekain314 0:432b860b6ff7 31 bool detectedGPS1PPS = false; //flag set in the ISR and reset after processing the 1PPS event
jekain314 0:432b860b6ff7 32 int PPSCounter = 0; //counts the 1PPS occurrences
jekain314 0:432b860b6ff7 33 int byteCounter = 0; //byte counter -- zeroed at 1PPS
jekain314 0:432b860b6ff7 34 unsigned short perSecMessageCounter=0; //counts the number of messages in a sec based on the header detection
jekain314 0:432b860b6ff7 35 bool messageDetected = false; //have detected a message header
jekain314 0:432b860b6ff7 36 unsigned long IMUbytesWritten = 0; //counts the IMU bytes written by the fwrite() to the SD card
jekain314 0:432b860b6ff7 37 int savedByteCounter = 0; //save ByteCounter at the 1PPS for display in main
jekain314 0:432b860b6ff7 38 int savedPerSecMessageCounter=0; //saved PerSecMsgCounter for display in main
jekain314 0:432b860b6ff7 39 int savedIMUClockCounter=0; //saved at the 1PPS for later diaplay from main
jekain314 0:432b860b6ff7 40 bool camera1EventDetected = false; //flag from ISR indicating a clock event occurred
jekain314 0:432b860b6ff7 41 double camera1Time; //GPS time of the camera event
jekain314 0:432b860b6ff7 42 int TotalBadCRCmatches = 0; //counter for the bad CRC matches for all GPS messages
jekain314 0:432b860b6ff7 43
jekain314 0:432b860b6ff7 44 volatile int PPSTimeOffset = 0;
jekain314 0:432b860b6ff7 45
jekain314 0:432b860b6ff7 46 //////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 47 // the below should become classes
jekain314 0:432b860b6ff7 48 //////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 49 #include "OEM615.h" //OEM615 GPS activities
jekain314 0:432b860b6ff7 50 #include "ADIS16488.h" //ADIS16488 activities
jekain314 0:432b860b6ff7 51 #include "PCMessaging.h" //PC messaging activities
jekain314 0:432b860b6ff7 52
jekain314 0:432b860b6ff7 53 //ISR for detection of the GPS 1PPS
jekain314 0:432b860b6ff7 54 void detect1PPSISR(void)
jekain314 0:432b860b6ff7 55 {
jekain314 0:432b860b6ff7 56 timeFromPPS.reset(); //reset the 1PPS timer upon 1PPS detection
jekain314 0:432b860b6ff7 57 //note -- the below accounts for time information becoming available AFTER the 1PPS event
jekain314 0:432b860b6ff7 58 PPSTimeOffset++; //counts 1PPS events between matching POS and VEL messages
jekain314 0:432b860b6ff7 59
jekain314 0:432b860b6ff7 60 //covers the case where the PPS ISR interrupts the IMU data ready ISR
jekain314 0:432b860b6ff7 61 if(IMUDataReady) IMUtimeFrom1PPS = 0;
jekain314 0:432b860b6ff7 62
jekain314 0:432b860b6ff7 63 savedByteCounter = byteCounter; //save byteCounter for display in main
jekain314 0:432b860b6ff7 64 savedPerSecMessageCounter = perSecMessageCounter; //save for display un main
jekain314 0:432b860b6ff7 65 byteCounter = 0; //countes bytes between 1PPS events
jekain314 0:432b860b6ff7 66 perSecMessageCounter = 0; //counts GPS messages between 1PPS events
jekain314 0:432b860b6ff7 67
jekain314 0:432b860b6ff7 68 GPS_COM1.rxBufferFlush(); //flush the GPS serial buffer
jekain314 0:432b860b6ff7 69
jekain314 0:432b860b6ff7 70 detectedGPS1PPS = true; //set false in the main when 1PPS actions are complete
jekain314 0:432b860b6ff7 71 PPSCounter++; //count number of 1PPS epoch
jekain314 0:432b860b6ff7 72
jekain314 0:432b860b6ff7 73 ppsled = !ppsled; //blink an LED at the 1PPS
jekain314 0:432b860b6ff7 74 };
jekain314 0:432b860b6ff7 75
jekain314 0:432b860b6ff7 76
jekain314 0:432b860b6ff7 77 ///////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 78 //set up the USB port and the GPS COM port
jekain314 0:432b860b6ff7 79 ///////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 80 FILE *fpNav = NULL; //file pointer to the nav file on the SD card
jekain314 0:432b860b6ff7 81 void setupCOM(void)
jekain314 0:432b860b6ff7 82 {
jekain314 0:432b860b6ff7 83 //system starts with GPS in reset active
jekain314 0:432b860b6ff7 84 //dis-engage the reset to get the GPS started
jekain314 0:432b860b6ff7 85 GPS_Reset=1; wait_ms(1000);
jekain314 0:432b860b6ff7 86
jekain314 0:432b860b6ff7 87 //establish 1PPS ISR
jekain314 0:432b860b6ff7 88 PPSInt.rise(&detect1PPSISR);
jekain314 0:432b860b6ff7 89
jekain314 0:432b860b6ff7 90 //set the USB serial data rate -- rate must be matched at the PC end
jekain314 0:432b860b6ff7 91 //This the serial communication back to the the PC host
jekain314 0:432b860b6ff7 92 //Launch the C++ serial port read program there to catch the ASCII characters
jekain314 0:432b860b6ff7 93 //toPC.baud(9600); wait_ms(100);
jekain314 0:432b860b6ff7 94 toPC.baud(8*115200); wait_ms(100);
jekain314 0:432b860b6ff7 95 //toPC.baud(1*115200); wait_ms(100);
jekain314 0:432b860b6ff7 96 toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n");
jekain314 0:432b860b6ff7 97
jekain314 0:432b860b6ff7 98 //just wait to launch the GPS receiver
jekain314 0:432b860b6ff7 99 for (int i=0; i<5; i++) { toPC.printf(" to start: %3d \n", 4-i); wait(1); }
jekain314 0:432b860b6ff7 100
jekain314 0:432b860b6ff7 101 sd_detect.mode(PullUp);
jekain314 0:432b860b6ff7 102
jekain314 0:432b860b6ff7 103 if (sd_detect == 0)
jekain314 0:432b860b6ff7 104 {
jekain314 0:432b860b6ff7 105 mkdir("/sd/Data", 0777);
jekain314 0:432b860b6ff7 106 }
jekain314 0:432b860b6ff7 107 else
jekain314 0:432b860b6ff7 108 {
jekain314 0:432b860b6ff7 109 toPC.printf(" SD card not present \n");
jekain314 0:432b860b6ff7 110 }
jekain314 0:432b860b6ff7 111
jekain314 0:432b860b6ff7 112 //NOTE: we do not assume that the GPS receiver has been pre-set up for the WALDO_FCS functionality
jekain314 0:432b860b6ff7 113 //we alwsys start with a reset and reprogram the receiver with our data out products
jekain314 0:432b860b6ff7 114 // this prevents failure because of a blown NVRAM as occurred for the older camera systems
jekain314 0:432b860b6ff7 115
jekain314 0:432b860b6ff7 116 //this is the COM1 port from th GPS receiuver to the mbed
jekain314 0:432b860b6ff7 117 //it should be always started at 9600 baud because thats the default for the GPS receiver
jekain314 0:432b860b6ff7 118 GPS_COM1.baud(9600); wait_ms(100);
jekain314 0:432b860b6ff7 119
jekain314 0:432b860b6ff7 120 // this ASCII command sets up the serial data from the GPS receiver on its COM1
jekain314 0:432b860b6ff7 121 char ch7[] = "serialconfig COM1 9600 n 8 1 n off";
jekain314 0:432b860b6ff7 122 // this is a software reset and has the same effect as a hardware reset (why do it?)
jekain314 0:432b860b6ff7 123 //char ch0[] = "RESET";
jekain314 0:432b860b6ff7 124 //this command stops all communication from the GPS receiver on COM1
jekain314 0:432b860b6ff7 125 //logs should still be presented on USB port so the Novatel CDU application can be used on the PC in parallel
jekain314 0:432b860b6ff7 126 char ch1[] = "unlogall COM1";
jekain314 0:432b860b6ff7 127 //set the final baud rate that we will use from here
jekain314 0:432b860b6ff7 128 //allowable baud rate values: 9600 115200 230400 460800 921600
jekain314 0:432b860b6ff7 129 //char ch2[] = "serialconfig COM1 921600 n 8 1 n off";
jekain314 0:432b860b6ff7 130 char ch2[] = "serialconfig COM1 115200 n 8 1 n off";
jekain314 0:432b860b6ff7 131
jekain314 0:432b860b6ff7 132 //the below commands request the POS, VEL, RANGE, and TIME messages
jekain314 0:432b860b6ff7 133 char ch3[] = "log COM1 BESTPOSB ONTIME 1"; //messageID = 42
jekain314 0:432b860b6ff7 134 char ch4[] = "log COM1 BESTVelB ONTIME 1"; //messageID = 99
jekain314 0:432b860b6ff7 135 char ch5[] = "log COM1 RANGEB ONTIME 1"; //messageID = 43
jekain314 0:432b860b6ff7 136 //char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101
jekain314 0:432b860b6ff7 137
jekain314 0:432b860b6ff7 138 //set up VARF to be 100Hz with 1X10^4 * 10^-8 = 10^-4 sec (10usec) pulse width
jekain314 0:432b860b6ff7 139 //in fact, we do not use this output but it is available.
jekain314 0:432b860b6ff7 140 //originally planned to use this to command the IMU data
jekain314 0:432b860b6ff7 141 //char ch8[] = "FREQUENCYOUT enable 10000 1000000";
jekain314 0:432b860b6ff7 142
jekain314 0:432b860b6ff7 143 toPC.printf("set serial config \n");
jekain314 0:432b860b6ff7 144 sendASCII(ch7, sizeof(ch7)); wait_ms(500);
jekain314 0:432b860b6ff7 145 //sendASCII(ch0, sizeof(ch0));
jekain314 0:432b860b6ff7 146 toPC.printf("unlog all messages \n");
jekain314 0:432b860b6ff7 147 sendASCII(ch1, sizeof(ch1)); wait_ms(500);
jekain314 0:432b860b6ff7 148 toPC.printf("log BESTPOSB on COM1 \n");
jekain314 0:432b860b6ff7 149 sendASCII(ch3, sizeof(ch3)); wait_ms(500);
jekain314 0:432b860b6ff7 150 toPC.printf("log BESTVELB on COM1\n");
jekain314 0:432b860b6ff7 151 sendASCII(ch4, sizeof(ch4)); wait_ms(500);
jekain314 0:432b860b6ff7 152 toPC.printf("log RANGEB on COM1\n");
jekain314 0:432b860b6ff7 153 sendASCII(ch5, sizeof(ch5)); wait_ms(500);
jekain314 0:432b860b6ff7 154
jekain314 0:432b860b6ff7 155 //toPC.printf("log TIMEB om COM1 \n");
jekain314 0:432b860b6ff7 156 //sendASCII(ch6, sizeof(ch6)); wait_ms(100);
jekain314 0:432b860b6ff7 157
jekain314 0:432b860b6ff7 158 //toPC.printf("Set up th VARF signal \n");
jekain314 0:432b860b6ff7 159 //sendASCII(ch8, sizeof(ch8)); wait_ms(500);
jekain314 0:432b860b6ff7 160
jekain314 0:432b860b6ff7 161 //set GPS output COM1 to the final high rate
jekain314 0:432b860b6ff7 162 toPC.printf("set the COM ports to high rate\n");
jekain314 0:432b860b6ff7 163 sendASCII(ch2, sizeof(ch2)); wait_ms(500);
jekain314 0:432b860b6ff7 164
jekain314 0:432b860b6ff7 165 //set the mbed COM port to match the GPS transmit rate
jekain314 0:432b860b6ff7 166 //the below baud rate must match the COM1 rate coming from the GPS receiver
jekain314 0:432b860b6ff7 167 GPS_COM1.baud(115200); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL
jekain314 0:432b860b6ff7 168 //GPS_COM1.baud(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL
jekain314 0:432b860b6ff7 169 };
jekain314 0:432b860b6ff7 170
jekain314 0:432b860b6ff7 171
jekain314 0:432b860b6ff7 172 /////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 173 // mbed main to support the Waldo_FCS
jekain314 0:432b860b6ff7 174 /////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 175 int main() {
jekain314 0:432b860b6ff7 176
jekain314 0:432b860b6ff7 177 //these are structures for the to GPS messages that must be parsed
jekain314 0:432b860b6ff7 178 MESSAGEHEADER msgHdr;
jekain314 0:432b860b6ff7 179 OEM615BESTPOS posMsg; //BESTPOS structure in OEMV615.h that has matching time to a BESTVEL message
jekain314 0:432b860b6ff7 180 OEM615BESTPOS curPos; //BESTPOS structure in OEMV615.h
jekain314 0:432b860b6ff7 181 OEM615BESTVEL velMsg; //BESTVEL structure in OEMV615.h that has matching time to a BESTPOS message
jekain314 0:432b860b6ff7 182 OEM615BESTVEL curVel; //BESTVEL structure in OEMV615.h
jekain314 0:432b860b6ff7 183
jekain314 1:8e24e633f8d8 184
jekain314 1:8e24e633f8d8 185 fire.output(); //set the fire pin as outoput
jekain314 1:8e24e633f8d8 186 pre_fire.output(); //set the pre-fire pin as output
jekain314 1:8e24e633f8d8 187 //fire.mode(OpenDrain);
jekain314 1:8e24e633f8d8 188
jekain314 1:8e24e633f8d8 189 //set up for the first trigger
jekain314 1:8e24e633f8d8 190 fire = 1;
jekain314 1:8e24e633f8d8 191 pre_fire = 1;
jekain314 0:432b860b6ff7 192 //set up the GPS and mbed COM ports
jekain314 0:432b860b6ff7 193 setupCOM();
jekain314 0:432b860b6ff7 194
jekain314 0:432b860b6ff7 195 //set up the ADIS16488
jekain314 0:432b860b6ff7 196 setupADIS();
jekain314 0:432b860b6ff7 197
jekain314 0:432b860b6ff7 198 setUpMessages(); //set up the expected text message commands frm the PC
jekain314 0:432b860b6ff7 199
jekain314 0:432b860b6ff7 200 //set up the interrupt to catch the GPS receiver serial bytes as they are presented
jekain314 0:432b860b6ff7 201 GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
jekain314 0:432b860b6ff7 202
jekain314 0:432b860b6ff7 203 timeFromPPS.start(); //start the time for measuring time from 1PPS events
jekain314 0:432b860b6ff7 204 timeFromStart.start();
jekain314 0:432b860b6ff7 205
jekain314 0:432b860b6ff7 206 toPC.printf("\n\n top of the main loop \n\n");
jekain314 0:432b860b6ff7 207
jekain314 0:432b860b6ff7 208 int totalBytesWritten = 0;
jekain314 0:432b860b6ff7 209
jekain314 0:432b860b6ff7 210 /*establish the initial value for the CRC recursion atter the header
jekain314 0:432b860b6ff7 211 unsigned long CRC = 0;
jekain314 0:432b860b6ff7 212 CRC32Value(CRC, 0xAA);
jekain314 0:432b860b6ff7 213 CRC32Value(CRC, 0x44);
jekain314 0:432b860b6ff7 214 CRC32Value(CRC, 0x12);
jekain314 0:432b860b6ff7 215 CRC32Value(CRC, 0x1C);
jekain314 0:432b860b6ff7 216 //this results in a value of: 0x39b0f0e1
jekain314 0:432b860b6ff7 217 toPC.printf(" CRC after AA44121C header: %08x \n", CRC);
jekain314 0:432b860b6ff7 218 wait(20);
jekain314 0:432b860b6ff7 219 */
jekain314 1:8e24e633f8d8 220
jekain314 0:432b860b6ff7 221 recordData = true;
jekain314 0:432b860b6ff7 222 sendRecData = true;
jekain314 0:432b860b6ff7 223
jekain314 0:432b860b6ff7 224 unsigned long cyclesPerSec = 0;
jekain314 0:432b860b6ff7 225 bool GPSdataWritten = false;
jekain314 0:432b860b6ff7 226
jekain314 1:8e24e633f8d8 227 //while(PPSCounter < 300)
jekain314 0:432b860b6ff7 228 ///////////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 229 // top of the while loop
jekain314 0:432b860b6ff7 230 ///////////////////////////////////////////////////////////////////////////
jekain314 1:8e24e633f8d8 231 while(1)
jekain314 0:432b860b6ff7 232 {
jekain314 0:432b860b6ff7 233 //read the USB serial data from the PC to check for commands
jekain314 0:432b860b6ff7 234 //in the primary real-time portion, there are no bytes from the PC so this has no impact
jekain314 0:432b860b6ff7 235 readFromPC();
jekain314 2:7039be3daf6e 236
jekain314 2:7039be3daf6e 237 //this will close the fpNav file on the SD card if the file is open
jekain314 2:7039be3daf6e 238 //and the elapsed time from PosVel messages is > 60 secs
jekain314 2:7039be3daf6e 239 //this prevents loosing the fpNav file if the PC goes down
jekain314 2:7039be3daf6e 240 if (fpNav && (timeFromPosVelMessageReceipt.read() < 60) )
jekain314 2:7039be3daf6e 241 {
jekain314 2:7039be3daf6e 242 sendRecData = true;
jekain314 2:7039be3daf6e 243 recordData = false;
jekain314 2:7039be3daf6e 244 }
jekain314 0:432b860b6ff7 245
jekain314 0:432b860b6ff7 246 processPCmessages(fpNav, posMsg, velMsg);
jekain314 0:432b860b6ff7 247
jekain314 1:8e24e633f8d8 248 if(fireTrigger)
jekain314 1:8e24e633f8d8 249 {
jekain314 1:8e24e633f8d8 250 //pre-fire the trigger using the mid-body 2.5mm connection (T2i)
jekain314 1:8e24e633f8d8 251 pre_fire = 0; //pin30 (midbody of connector) set to zero
jekain314 1:8e24e633f8d8 252 wait(.01f); //wait for 0.25 secs
jekain314 1:8e24e633f8d8 253 fire = 0; //fire the trigger using the tip connection
jekain314 2:7039be3daf6e 254 wait(0.10);
jekain314 1:8e24e633f8d8 255 fire = 1;
jekain314 1:8e24e633f8d8 256 pre_fire = 1;
jekain314 1:8e24e633f8d8 257 unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000 + timeFromPPS.read_us()/1000.0;
jekain314 1:8e24e633f8d8 258 toPC.printf("WMsg TRIGGERTIME %10d \n", triggerTime);
jekain314 1:8e24e633f8d8 259 fireTrigger = false;
jekain314 1:8e24e633f8d8 260 }
jekain314 1:8e24e633f8d8 261
jekain314 0:432b860b6ff7 262 cyclesPerSec++;
jekain314 0:432b860b6ff7 263
jekain314 0:432b860b6ff7 264 //
jekain314 0:432b860b6ff7 265 ////////////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 266 //below is where we process the complete stored GPS message for the second
jekain314 0:432b860b6ff7 267 //The !IMUDataReady test prevents the IMU and GPS data from being written
jekain314 0:432b860b6ff7 268 //to disk on the same pass through this loop
jekain314 0:432b860b6ff7 269 /////////////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 270
jekain314 0:432b860b6ff7 271
jekain314 0:432b860b6ff7 272 if (completeMessageAvailable && !IMUDataReady)
jekain314 0:432b860b6ff7 273 {
jekain314 0:432b860b6ff7 274
jekain314 0:432b860b6ff7 275 msgHdr = *((MESSAGEHEADER*)&msgBuffer[messageLocation[savedMessageCounter-1]]);
jekain314 0:432b860b6ff7 276
jekain314 0:432b860b6ff7 277 //these times are used to tag the IMU sample time. PPSTimeOffset increments by 1 exactly at the 1PPS event (in the 1PPS ISR)
jekain314 0:432b860b6ff7 278 //GPSTimemsecs increments by 1 for each new GPS measurement -- note that the below computations are actually
jekain314 0:432b860b6ff7 279 //done at the receipt of each GPS message. This is OK because each message has the same time in its header.
jekain314 0:432b860b6ff7 280 //Thus GPSTimemsecs increments by 1 here while GPSTimemsecs effectively decrements by 1.
jekain314 0:432b860b6ff7 281 //This handles IMU time tagging between the 1PPS event and the first receipt of a new GPS time.
jekain314 0:432b860b6ff7 282
jekain314 0:432b860b6ff7 283 GPSTimemsecs = msgHdr.GPSTime_msecs; //time in GPS message header
jekain314 0:432b860b6ff7 284 PPSTimeOffset = 0; //incremented by 1 in the PPS ISR
jekain314 0:432b860b6ff7 285
jekain314 0:432b860b6ff7 286 unsigned long msgCRC = *((unsigned long*)&msgBuffer[messageLocation[savedMessageCounter-1] + 28 + msgHdr.messageLength]);
jekain314 0:432b860b6ff7 287
jekain314 0:432b860b6ff7 288 //toPC.printf("tmeFrom1PPS= %5d ID= %3d Ln = %3d computedCRC= %08x msgCRC= %08x msgCntr = %3d CRCerr=%4d\n",
jekain314 0:432b860b6ff7 289 // timeFromPPS.read_us(), msgHdr.messageID, msgHdr.messageLength, computedCRC, msgCRC, savedMessageCounter, TotalBadCRCmatches);
jekain314 0:432b860b6ff7 290
jekain314 0:432b860b6ff7 291 if ( msgCRC != computedCRC)
jekain314 0:432b860b6ff7 292 {
jekain314 0:432b860b6ff7 293 toPC.printf(" bad CRC match for messageID %3d total CRC errors = %4d \n",
jekain314 0:432b860b6ff7 294 msgHdr.messageLength, TotalBadCRCmatches++);
jekain314 0:432b860b6ff7 295 }
jekain314 0:432b860b6ff7 296
jekain314 0:432b860b6ff7 297 if (msgHdr.messageID == 42)
jekain314 0:432b860b6ff7 298 {
jekain314 0:432b860b6ff7 299 curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[savedMessageCounter-1]]);
jekain314 0:432b860b6ff7 300 posMsg = curPos;
jekain314 0:432b860b6ff7 301
jekain314 1:8e24e633f8d8 302 if (streamPos)
jekain314 0:432b860b6ff7 303 {
jekain314 1:8e24e633f8d8 304 toPC.printf("BESTPOS %5d %1d %8.5lf %9.5lf %5.3lf %d %d\n",
jekain314 0:432b860b6ff7 305 curPos.msgHeader.GPSTime_msecs, curPos.solStatus,
jekain314 0:432b860b6ff7 306 curPos.latitude, curPos.longitude, curPos.height,
jekain314 1:8e24e633f8d8 307 curPos.numSV, curPos.numSolSV);
jekain314 0:432b860b6ff7 308 }
jekain314 0:432b860b6ff7 309
jekain314 0:432b860b6ff7 310 }
jekain314 0:432b860b6ff7 311 else if (msgHdr.messageID == 99)
jekain314 0:432b860b6ff7 312 {
jekain314 0:432b860b6ff7 313 curVel = *((OEM615BESTVEL*)&msgBuffer[ messageLocation[savedMessageCounter-1] ]);
jekain314 1:8e24e633f8d8 314 //toPC.printf("BESTVEL vel: horizontalSpeed= %5.3f heading=%5.1f verticalSpeed=%4.2f \n",
jekain314 1:8e24e633f8d8 315 // curVel.horizontalSpeed, curVel.heading, curVel.verticalSpeed );
jekain314 0:432b860b6ff7 316 velMsg = curVel;
jekain314 0:432b860b6ff7 317 }
jekain314 0:432b860b6ff7 318 /*
jekain314 0:432b860b6ff7 319 if (recordData && (fpNav != NULL) && (byteCounter > 0))
jekain314 0:432b860b6ff7 320 {
jekain314 0:432b860b6ff7 321 //wait_us(10);
jekain314 0:432b860b6ff7 322 int totalMessageLength = 28 + msgHdr.messageLength + 4; //header length + message Length + CRC word size
jekain314 0:432b860b6ff7 323 totalBytesWritten += fwrite(&msgBuffer[messageLocation[savedMessageCounter-1]], 1, totalMessageLength, fpNav); // this writes out a complete set of messages for this sec
jekain314 0:432b860b6ff7 324 //wait_us(10);
jekain314 0:432b860b6ff7 325 }
jekain314 0:432b860b6ff7 326
jekain314 0:432b860b6ff7 327
jekain314 0:432b860b6ff7 328 */
jekain314 0:432b860b6ff7 329 completeMessageAvailable = false;
jekain314 0:432b860b6ff7 330 }
jekain314 0:432b860b6ff7 331
jekain314 1:8e24e633f8d8 332 //write the GPS data to the SD card
jekain314 0:432b860b6ff7 333 if (!IMUDataReady && !GPSdataWritten && timeFromPPS.read_us() > 500000 && recordData && (fpNav != NULL))
jekain314 0:432b860b6ff7 334 {
jekain314 0:432b860b6ff7 335 totalBytesWritten += fwrite(&msgBuffer, 1, byteCounter, fpNav);
jekain314 0:432b860b6ff7 336 GPSdataWritten = true;
jekain314 0:432b860b6ff7 337 }
jekain314 0:432b860b6ff7 338
jekain314 0:432b860b6ff7 339 //the IMU data record is read from the SPI in the ISR and the IMUDataReady is set true
jekain314 0:432b860b6ff7 340 //we write the IMU data here
jekain314 0:432b860b6ff7 341 if (IMUDataReady) //IMUDataReady is true if we have a recent IMU data record
jekain314 0:432b860b6ff7 342 {
jekain314 0:432b860b6ff7 343 //write the IMU data
jekain314 0:432b860b6ff7 344 if ( recordData && (fpNav != NULL) )
jekain314 0:432b860b6ff7 345 {
jekain314 1:8e24e633f8d8 346 //delTimeOfWrite = timeFromStart.read_us();
jekain314 0:432b860b6ff7 347
jekain314 0:432b860b6ff7 348 if (fillingPingWritingPong) fwrite(&imuPong, 1, IMUrecArraySize*sizeof(IMUREC), fpNav);
jekain314 0:432b860b6ff7 349 else fwrite(&imuPing, 1, IMUrecArraySize*sizeof(IMUREC), fpNav);
jekain314 0:432b860b6ff7 350
jekain314 1:8e24e633f8d8 351 //delTimeOfWrite = (unsigned long)((unsigned long)timeFromStart.read_us() - delTimeOfWrite);
jekain314 1:8e24e633f8d8 352 //if (delTimeOfWrite > maxWriteTime) maxWriteTime = delTimeOfWrite;
jekain314 0:432b860b6ff7 353 }
jekain314 0:432b860b6ff7 354 IMURecordCounter+=IMUrecArraySize;
jekain314 0:432b860b6ff7 355 IMUDataReady = false;
jekain314 0:432b860b6ff7 356 }
jekain314 0:432b860b6ff7 357
jekain314 0:432b860b6ff7 358 if (camera1EventDetected) //we have detected a camera trigger event
jekain314 0:432b860b6ff7 359 {
jekain314 0:432b860b6ff7 360 toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time);
jekain314 0:432b860b6ff7 361 camera1EventDetected = false;
jekain314 0:432b860b6ff7 362 }
jekain314 0:432b860b6ff7 363
jekain314 0:432b860b6ff7 364 if (detectedGPS1PPS) //true if we are exactly at a 1PPS event detection
jekain314 0:432b860b6ff7 365 {
jekain314 1:8e24e633f8d8 366 //toPC.printf("PPS=%4d stat=%1d bytes=%3d GPSMsgs=%2d #write=%8d cycles=%6d\n",
jekain314 1:8e24e633f8d8 367 // PPSCounter, posMsg.solStatus, savedByteCounter, savedPerSecMessageCounter,
jekain314 1:8e24e633f8d8 368 // totalBytesWritten, cyclesPerSec );
jekain314 0:432b860b6ff7 369
jekain314 0:432b860b6ff7 370 cyclesPerSec = 0;
jekain314 0:432b860b6ff7 371 totalBytesWritten = 0;
jekain314 0:432b860b6ff7 372 GPSdataWritten = false;
jekain314 0:432b860b6ff7 373
jekain314 0:432b860b6ff7 374 IMURecordCounter = 0;
jekain314 0:432b860b6ff7 375 detectedGPS1PPS = false;
jekain314 0:432b860b6ff7 376 }
jekain314 0:432b860b6ff7 377 }
jekain314 0:432b860b6ff7 378
jekain314 0:432b860b6ff7 379 fclose(fpNav);
jekain314 0:432b860b6ff7 380 toPC.printf(" normal termination \n");
jekain314 0:432b860b6ff7 381 }