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@22:1cbdbc856660, 2013-05-19 (annotated)

Committer:
jekain314
Date:
Sun May 19 16:26:28 2013 +0000
Revision:
22:1cbdbc856660
Parent:
20:3f04a0bde484
Child:
24:353322495742
added WMsg to all messages to PC to improve startup. Blink LEDs on 1PPS to indicate main loop activity

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 16:2aea22130ba1 53
jekain314 16:2aea22130ba1 54 // stuff to send the SD file to the PC
jekain314 16:2aea22130ba1 55 #include "SDShell.h"
jekain314 16:2aea22130ba1 56 void transferFile()
jekain314 16:2aea22130ba1 57 {
jekain314 16:2aea22130ba1 58 SDShell emulate; // create the object
jekain314 16:2aea22130ba1 59 emulate.init(); // init the params inside
jekain314 16:2aea22130ba1 60 GPS_Reset = 0; // low power PCB mode
jekain314 16:2aea22130ba1 61 ADIS_RST = 0; // same here
jekain314 16:2aea22130ba1 62 wait(0.01f); // just make sure that the hardware has time to stop
jekain314 16:2aea22130ba1 63 fflush(stdout); // and clear any TX reminants
jekain314 16:2aea22130ba1 64 toPC.printf("Entering Shell Emulator...\n"); // just for fluf
jekain314 16:2aea22130ba1 65 wait(0.1f); // no reason for this either
jekain314 16:2aea22130ba1 66 emulate.shell(toPC, sd, "/sd"); // now the SDShell object will serve SD files via UNIX commands
jekain314 16:2aea22130ba1 67 }
jekain314 16:2aea22130ba1 68
jekain314 0:432b860b6ff7 69 //ISR for detection of the GPS 1PPS
jekain314 0:432b860b6ff7 70 void detect1PPSISR(void)
jekain314 0:432b860b6ff7 71 {
jekain314 0:432b860b6ff7 72 timeFromPPS.reset(); //reset the 1PPS timer upon 1PPS detection
jekain314 0:432b860b6ff7 73 //note -- the below accounts for time information becoming available AFTER the 1PPS event
jekain314 0:432b860b6ff7 74 PPSTimeOffset++; //counts 1PPS events between matching POS and VEL messages
jekain314 0:432b860b6ff7 75
jekain314 0:432b860b6ff7 76 //covers the case where the PPS ISR interrupts the IMU data ready ISR
jekain314 0:432b860b6ff7 77 if(IMUDataReady) IMUtimeFrom1PPS = 0;
jekain314 0:432b860b6ff7 78
jekain314 0:432b860b6ff7 79 savedByteCounter = byteCounter; //save byteCounter for display in main
jekain314 0:432b860b6ff7 80 savedPerSecMessageCounter = perSecMessageCounter; //save for display un main
jekain314 0:432b860b6ff7 81 byteCounter = 0; //countes bytes between 1PPS events
jekain314 0:432b860b6ff7 82 perSecMessageCounter = 0; //counts GPS messages between 1PPS events
jekain314 0:432b860b6ff7 83
jekain314 0:432b860b6ff7 84 GPS_COM1.rxBufferFlush(); //flush the GPS serial buffer
jekain314 0:432b860b6ff7 85
jekain314 0:432b860b6ff7 86 detectedGPS1PPS = true; //set false in the main when 1PPS actions are complete
jekain314 0:432b860b6ff7 87 PPSCounter++; //count number of 1PPS epoch
jekain314 0:432b860b6ff7 88
jekain314 0:432b860b6ff7 89 ppsled = !ppsled; //blink an LED at the 1PPS
jekain314 0:432b860b6ff7 90 };
jekain314 0:432b860b6ff7 91
jekain314 0:432b860b6ff7 92
jekain314 0:432b860b6ff7 93 ///////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 94 //set up the USB port and the GPS COM port
jekain314 0:432b860b6ff7 95 ///////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 96 FILE *fpNav = NULL; //file pointer to the nav file on the SD card
jekain314 0:432b860b6ff7 97 void setupCOM(void)
jekain314 0:432b860b6ff7 98 {
jekain314 0:432b860b6ff7 99 //system starts with GPS in reset active
jekain314 0:432b860b6ff7 100 //dis-engage the reset to get the GPS started
jekain314 0:432b860b6ff7 101 GPS_Reset=1; wait_ms(1000);
jekain314 0:432b860b6ff7 102
jekain314 0:432b860b6ff7 103 //establish 1PPS ISR
jekain314 0:432b860b6ff7 104 PPSInt.rise(&detect1PPSISR);
jekain314 0:432b860b6ff7 105
jekain314 0:432b860b6ff7 106 //set the USB serial data rate -- rate must be matched at the PC end
jekain314 0:432b860b6ff7 107 //This the serial communication back to the the PC host
jekain314 0:432b860b6ff7 108 //Launch the C++ serial port read program there to catch the ASCII characters
jekain314 0:432b860b6ff7 109 //toPC.baud(9600); wait_ms(100);
jekain314 0:432b860b6ff7 110 toPC.baud(8*115200); wait_ms(100);
jekain314 0:432b860b6ff7 111 //toPC.baud(1*115200); wait_ms(100);
jekain314 6:71da5b99de97 112 //toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n");
jekain314 0:432b860b6ff7 113
jekain314 0:432b860b6ff7 114 //just wait to launch the GPS receiver
jekain314 3:e1a884e5325a 115 for (int i=0; i<5; i++) { toPC.printf("WMsg start: %3d \n", 4-i); wait(1); }
jekain314 0:432b860b6ff7 116
jekain314 0:432b860b6ff7 117 sd_detect.mode(PullUp);
jekain314 0:432b860b6ff7 118
jekain314 0:432b860b6ff7 119 if (sd_detect == 0)
jekain314 0:432b860b6ff7 120 {
jekain314 0:432b860b6ff7 121 mkdir("/sd/Data", 0777);
jekain314 0:432b860b6ff7 122 }
jekain314 0:432b860b6ff7 123 else
jekain314 0:432b860b6ff7 124 {
jekain314 6:71da5b99de97 125 toPC.printf("WMsg SD card not present \n");
jekain314 0:432b860b6ff7 126 }
jekain314 0:432b860b6ff7 127
jekain314 0:432b860b6ff7 128 //NOTE: we do not assume that the GPS receiver has been pre-set up for the WALDO_FCS functionality
jekain314 0:432b860b6ff7 129 //we alwsys start with a reset and reprogram the receiver with our data out products
jekain314 0:432b860b6ff7 130 // this prevents failure because of a blown NVRAM as occurred for the older camera systems
jekain314 0:432b860b6ff7 131
jekain314 0:432b860b6ff7 132 //this is the COM1 port from th GPS receiuver to the mbed
jekain314 0:432b860b6ff7 133 //it should be always started at 9600 baud because thats the default for the GPS receiver
jekain314 0:432b860b6ff7 134 GPS_COM1.baud(9600); wait_ms(100);
jekain314 0:432b860b6ff7 135
jekain314 0:432b860b6ff7 136 // this ASCII command sets up the serial data from the GPS receiver on its COM1
jekain314 0:432b860b6ff7 137 char ch7[] = "serialconfig COM1 9600 n 8 1 n off";
jekain314 0:432b860b6ff7 138 // this is a software reset and has the same effect as a hardware reset (why do it?)
jekain314 0:432b860b6ff7 139 //char ch0[] = "RESET";
jekain314 0:432b860b6ff7 140 //this command stops all communication from the GPS receiver on COM1
jekain314 0:432b860b6ff7 141 //logs should still be presented on USB port so the Novatel CDU application can be used on the PC in parallel
jekain314 0:432b860b6ff7 142 char ch1[] = "unlogall COM1";
jekain314 0:432b860b6ff7 143 //set the final baud rate that we will use from here
jekain314 0:432b860b6ff7 144 //allowable baud rate values: 9600 115200 230400 460800 921600
jekain314 0:432b860b6ff7 145 //char ch2[] = "serialconfig COM1 921600 n 8 1 n off";
jekain314 0:432b860b6ff7 146 char ch2[] = "serialconfig COM1 115200 n 8 1 n off";
jekain314 0:432b860b6ff7 147
jekain314 0:432b860b6ff7 148 //the below commands request the POS, VEL, RANGE, and TIME messages
jekain314 0:432b860b6ff7 149 char ch3[] = "log COM1 BESTPOSB ONTIME 1"; //messageID = 42
jekain314 0:432b860b6ff7 150 char ch4[] = "log COM1 BESTVelB ONTIME 1"; //messageID = 99
jekain314 0:432b860b6ff7 151 char ch5[] = "log COM1 RANGEB ONTIME 1"; //messageID = 43
jekain314 0:432b860b6ff7 152 //char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101
jekain314 0:432b860b6ff7 153
jekain314 0:432b860b6ff7 154 //set up VARF to be 100Hz with 1X10^4 * 10^-8 = 10^-4 sec (10usec) pulse width
jekain314 0:432b860b6ff7 155 //in fact, we do not use this output but it is available.
jekain314 0:432b860b6ff7 156 //originally planned to use this to command the IMU data
jekain314 0:432b860b6ff7 157 //char ch8[] = "FREQUENCYOUT enable 10000 1000000";
jekain314 0:432b860b6ff7 158
jekain314 15:f3b92958cf5a 159 //toPC.printf("WMsg set serial config \n");
jekain314 0:432b860b6ff7 160 sendASCII(ch7, sizeof(ch7)); wait_ms(500);
jekain314 0:432b860b6ff7 161 //sendASCII(ch0, sizeof(ch0));
jekain314 15:f3b92958cf5a 162 //toPC.printf("WMsg unlog all messages \n");
jekain314 0:432b860b6ff7 163 sendASCII(ch1, sizeof(ch1)); wait_ms(500);
jekain314 15:f3b92958cf5a 164 //toPC.printf("WMsg log BESTPOSB on COM1 \n");
jekain314 0:432b860b6ff7 165 sendASCII(ch3, sizeof(ch3)); wait_ms(500);
jekain314 15:f3b92958cf5a 166 //toPC.printf("WMsg log BESTVELB on COM1\n");
jekain314 0:432b860b6ff7 167 sendASCII(ch4, sizeof(ch4)); wait_ms(500);
jekain314 15:f3b92958cf5a 168 //toPC.printf("WMsg log RANGEB on COM1\n");
jekain314 0:432b860b6ff7 169 sendASCII(ch5, sizeof(ch5)); wait_ms(500);
jekain314 0:432b860b6ff7 170
jekain314 0:432b860b6ff7 171 //toPC.printf("log TIMEB om COM1 \n");
jekain314 0:432b860b6ff7 172 //sendASCII(ch6, sizeof(ch6)); wait_ms(100);
jekain314 0:432b860b6ff7 173
jekain314 0:432b860b6ff7 174 //toPC.printf("Set up th VARF signal \n");
jekain314 0:432b860b6ff7 175 //sendASCII(ch8, sizeof(ch8)); wait_ms(500);
jekain314 0:432b860b6ff7 176
jekain314 0:432b860b6ff7 177 //set GPS output COM1 to the final high rate
jekain314 15:f3b92958cf5a 178 //toPC.printf("WMsg set the COM ports to high rate\n");
jekain314 0:432b860b6ff7 179 sendASCII(ch2, sizeof(ch2)); wait_ms(500);
jekain314 0:432b860b6ff7 180
jekain314 0:432b860b6ff7 181 //set the mbed COM port to match the GPS transmit rate
jekain314 0:432b860b6ff7 182 //the below baud rate must match the COM1 rate coming from the GPS receiver
jekain314 0:432b860b6ff7 183 GPS_COM1.baud(115200); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL
jekain314 0:432b860b6ff7 184 //GPS_COM1.baud(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL
jekain314 0:432b860b6ff7 185 };
jekain314 0:432b860b6ff7 186
jekain314 0:432b860b6ff7 187
jekain314 0:432b860b6ff7 188 /////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 189 // mbed main to support the Waldo_FCS
jekain314 0:432b860b6ff7 190 /////////////////////////////////////////////////////////////////////
jekain314 14:009d4671f0e3 191 int main()
jekain314 14:009d4671f0e3 192 {
jekain314 0:432b860b6ff7 193
jekain314 0:432b860b6ff7 194 //these are structures for the to GPS messages that must be parsed
jekain314 0:432b860b6ff7 195 MESSAGEHEADER msgHdr;
jekain314 0:432b860b6ff7 196 OEM615BESTPOS posMsg; //BESTPOS structure in OEMV615.h that has matching time to a BESTVEL message
jekain314 0:432b860b6ff7 197 OEM615BESTPOS curPos; //BESTPOS structure in OEMV615.h
jekain314 0:432b860b6ff7 198 OEM615BESTVEL velMsg; //BESTVEL structure in OEMV615.h that has matching time to a BESTPOS message
jekain314 0:432b860b6ff7 199 OEM615BESTVEL curVel; //BESTVEL structure in OEMV615.h
jekain314 0:432b860b6ff7 200
jekain314 1:8e24e633f8d8 201
jekain314 1:8e24e633f8d8 202 fire.output(); //set the fire pin as outoput
jekain314 1:8e24e633f8d8 203 pre_fire.output(); //set the pre-fire pin as output
jekain314 1:8e24e633f8d8 204 //fire.mode(OpenDrain);
jekain314 1:8e24e633f8d8 205
jekain314 1:8e24e633f8d8 206 //set up for the first trigger
jekain314 1:8e24e633f8d8 207 fire = 1;
jekain314 1:8e24e633f8d8 208 pre_fire = 1;
jekain314 0:432b860b6ff7 209 //set up the GPS and mbed COM ports
jekain314 0:432b860b6ff7 210 setupCOM();
jekain314 0:432b860b6ff7 211
jekain314 0:432b860b6ff7 212 //set up the ADIS16488
jekain314 0:432b860b6ff7 213 setupADIS();
jekain314 0:432b860b6ff7 214
jekain314 0:432b860b6ff7 215 setUpMessages(); //set up the expected text message commands frm the PC
jekain314 0:432b860b6ff7 216
jekain314 0:432b860b6ff7 217 //set up the interrupt to catch the GPS receiver serial bytes as they are presented
jekain314 0:432b860b6ff7 218 GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
jekain314 0:432b860b6ff7 219
jekain314 0:432b860b6ff7 220 timeFromPPS.start(); //start the time for measuring time from 1PPS events
jekain314 0:432b860b6ff7 221 timeFromStart.start();
jekain314 0:432b860b6ff7 222
jekain314 3:e1a884e5325a 223 //toPC.printf("\n\n top of the main loop \n\n");
jekain314 0:432b860b6ff7 224
jekain314 0:432b860b6ff7 225 int totalBytesWritten = 0;
jekain314 0:432b860b6ff7 226
jekain314 0:432b860b6ff7 227 /*establish the initial value for the CRC recursion atter the header
jekain314 0:432b860b6ff7 228 unsigned long CRC = 0;
jekain314 0:432b860b6ff7 229 CRC32Value(CRC, 0xAA);
jekain314 0:432b860b6ff7 230 CRC32Value(CRC, 0x44);
jekain314 0:432b860b6ff7 231 CRC32Value(CRC, 0x12);
jekain314 0:432b860b6ff7 232 CRC32Value(CRC, 0x1C);
jekain314 0:432b860b6ff7 233 //this results in a value of: 0x39b0f0e1
jekain314 0:432b860b6ff7 234 toPC.printf(" CRC after AA44121C header: %08x \n", CRC);
jekain314 0:432b860b6ff7 235 wait(20);
jekain314 0:432b860b6ff7 236 */
jekain314 1:8e24e633f8d8 237
jekain314 0:432b860b6ff7 238 recordData = true;
jekain314 0:432b860b6ff7 239 sendRecData = true;
jekain314 0:432b860b6ff7 240
jekain314 0:432b860b6ff7 241 unsigned long cyclesPerSec = 0;
jekain314 0:432b860b6ff7 242 bool GPSdataWritten = false;
jekain314 4:dda2ab5cc643 243 bool finishTrigger = false;
jekain314 4:dda2ab5cc643 244 Timer triggerInterval;
jekain314 0:432b860b6ff7 245
jekain314 1:8e24e633f8d8 246 //while(PPSCounter < 300)
jekain314 17:71900da6ced6 247
jekain314 17:71900da6ced6 248 bool newMission = true;
jekain314 17:71900da6ced6 249
jekain314 0:432b860b6ff7 250 ///////////////////////////////////////////////////////////////////////////
jekain314 17:71900da6ced6 251 // top of the mission while loop
jekain314 0:432b860b6ff7 252 ///////////////////////////////////////////////////////////////////////////
jekain314 17:71900da6ced6 253 while(newMission)
jekain314 0:432b860b6ff7 254 {
jekain314 14:009d4671f0e3 255
jekain314 0:432b860b6ff7 256 //read the USB serial data from the PC to check for commands
jekain314 0:432b860b6ff7 257 //in the primary real-time portion, there are no bytes from the PC so this has no impact
jekain314 0:432b860b6ff7 258 readFromPC();
jekain314 2:7039be3daf6e 259
jekain314 14:009d4671f0e3 260 //
jekain314 2:7039be3daf6e 261 //this will close the fpNav file on the SD card if the file is open
jekain314 2:7039be3daf6e 262 //and the elapsed time from PosVel messages is > 60 secs
jekain314 2:7039be3daf6e 263 //this prevents loosing the fpNav file if the PC goes down
jekain314 14:009d4671f0e3 264 if (fpNav && (timeFromPosVelMessageReceipt.read() > 10) )
jekain314 2:7039be3daf6e 265 {
jekain314 2:7039be3daf6e 266 sendRecData = true;
jekain314 2:7039be3daf6e 267 recordData = false;
jekain314 2:7039be3daf6e 268 }
jekain314 14:009d4671f0e3 269 //
jekain314 0:432b860b6ff7 270
jekain314 0:432b860b6ff7 271 processPCmessages(fpNav, posMsg, velMsg);
jekain314 0:432b860b6ff7 272
jekain314 20:3f04a0bde484 273 if (get_file_msg)
jekain314 20:3f04a0bde484 274 {
jekain314 20:3f04a0bde484 275 if (fpNav != NULL) fclose(fpNav);
jekain314 20:3f04a0bde484 276 break; //terminate the while loop when we receive this message from the PC
jekain314 20:3f04a0bde484 277 }
jekain314 14:009d4671f0e3 278
jekain314 5:2ce1be9d4bef 279 if(fireTrigger) //comes from a PC request message
jekain314 1:8e24e633f8d8 280 {
jekain314 14:009d4671f0e3 281 unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000 + timeFromPPS.read_us()/1000.0;
jekain314 14:009d4671f0e3 282 toPC.printf("WMsg TRIGGERTIME %10d \n", triggerTime);
jekain314 1:8e24e633f8d8 283 //pre-fire the trigger using the mid-body 2.5mm connection (T2i)
jekain314 1:8e24e633f8d8 284 pre_fire = 0; //pin30 (midbody of connector) set to zero
jekain314 1:8e24e633f8d8 285 wait(.01f); //wait for 0.25 secs
jekain314 1:8e24e633f8d8 286 fire = 0; //fire the trigger using the tip connection
jekain314 4:dda2ab5cc643 287 //wait(0.10);
jekain314 4:dda2ab5cc643 288 fireTrigger = false;
jekain314 4:dda2ab5cc643 289 finishTrigger = true;
jekain314 4:dda2ab5cc643 290 triggerInterval.start();
jekain314 4:dda2ab5cc643 291 }
jekain314 4:dda2ab5cc643 292
jekain314 6:71da5b99de97 293 //the trigger requires a pulse -- the above portion lowers the signal and the below raises it
jekain314 6:71da5b99de97 294 //this has been tested at 50 msecs and it will not fire at that pulse duration
jekain314 4:dda2ab5cc643 295 if(finishTrigger && triggerInterval.read_ms() > 100)
jekain314 4:dda2ab5cc643 296 {
jekain314 1:8e24e633f8d8 297 fire = 1;
jekain314 1:8e24e633f8d8 298 pre_fire = 1;
jekain314 4:dda2ab5cc643 299 triggerInterval.reset();
jekain314 4:dda2ab5cc643 300 finishTrigger = false;
jekain314 1:8e24e633f8d8 301 }
jekain314 1:8e24e633f8d8 302
jekain314 0:432b860b6ff7 303 cyclesPerSec++;
jekain314 0:432b860b6ff7 304
jekain314 0:432b860b6ff7 305 //
jekain314 0:432b860b6ff7 306 ////////////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 307 //below is where we process the complete stored GPS message for the second
jekain314 0:432b860b6ff7 308 //The !IMUDataReady test prevents the IMU and GPS data from being written
jekain314 0:432b860b6ff7 309 //to disk on the same pass through this loop
jekain314 0:432b860b6ff7 310 /////////////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 311
jekain314 0:432b860b6ff7 312
jekain314 0:432b860b6ff7 313 if (completeMessageAvailable && !IMUDataReady)
jekain314 0:432b860b6ff7 314 {
jekain314 0:432b860b6ff7 315
jekain314 0:432b860b6ff7 316 msgHdr = *((MESSAGEHEADER*)&msgBuffer[messageLocation[savedMessageCounter-1]]);
jekain314 0:432b860b6ff7 317
jekain314 0:432b860b6ff7 318 //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 319 //GPSTimemsecs increments by 1 for each new GPS measurement -- note that the below computations are actually
jekain314 0:432b860b6ff7 320 //done at the receipt of each GPS message. This is OK because each message has the same time in its header.
jekain314 0:432b860b6ff7 321 //Thus GPSTimemsecs increments by 1 here while GPSTimemsecs effectively decrements by 1.
jekain314 0:432b860b6ff7 322 //This handles IMU time tagging between the 1PPS event and the first receipt of a new GPS time.
jekain314 0:432b860b6ff7 323
jekain314 0:432b860b6ff7 324 GPSTimemsecs = msgHdr.GPSTime_msecs; //time in GPS message header
jekain314 0:432b860b6ff7 325 PPSTimeOffset = 0; //incremented by 1 in the PPS ISR
jekain314 0:432b860b6ff7 326
jekain314 0:432b860b6ff7 327 unsigned long msgCRC = *((unsigned long*)&msgBuffer[messageLocation[savedMessageCounter-1] + 28 + msgHdr.messageLength]);
jekain314 0:432b860b6ff7 328
jekain314 0:432b860b6ff7 329 //toPC.printf("tmeFrom1PPS= %5d ID= %3d Ln = %3d computedCRC= %08x msgCRC= %08x msgCntr = %3d CRCerr=%4d\n",
jekain314 0:432b860b6ff7 330 // timeFromPPS.read_us(), msgHdr.messageID, msgHdr.messageLength, computedCRC, msgCRC, savedMessageCounter, TotalBadCRCmatches);
jekain314 0:432b860b6ff7 331
jekain314 0:432b860b6ff7 332 if ( msgCRC != computedCRC)
jekain314 0:432b860b6ff7 333 {
jekain314 22:1cbdbc856660 334 toPC.printf("WMsg bad CRC match for messageID %3d total CRC errors = %4d \n",
jekain314 0:432b860b6ff7 335 msgHdr.messageLength, TotalBadCRCmatches++);
jekain314 0:432b860b6ff7 336 }
jekain314 0:432b860b6ff7 337
jekain314 0:432b860b6ff7 338 if (msgHdr.messageID == 42)
jekain314 0:432b860b6ff7 339 {
jekain314 0:432b860b6ff7 340 curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[savedMessageCounter-1]]);
jekain314 0:432b860b6ff7 341 posMsg = curPos;
jekain314 0:432b860b6ff7 342
jekain314 1:8e24e633f8d8 343 if (streamPos)
jekain314 0:432b860b6ff7 344 {
jekain314 1:8e24e633f8d8 345 toPC.printf("BESTPOS %5d %1d %8.5lf %9.5lf %5.3lf %d %d\n",
jekain314 0:432b860b6ff7 346 curPos.msgHeader.GPSTime_msecs, curPos.solStatus,
jekain314 0:432b860b6ff7 347 curPos.latitude, curPos.longitude, curPos.height,
jekain314 1:8e24e633f8d8 348 curPos.numSV, curPos.numSolSV);
jekain314 0:432b860b6ff7 349 }
jekain314 0:432b860b6ff7 350
jekain314 0:432b860b6ff7 351 }
jekain314 0:432b860b6ff7 352 else if (msgHdr.messageID == 99)
jekain314 0:432b860b6ff7 353 {
jekain314 0:432b860b6ff7 354 curVel = *((OEM615BESTVEL*)&msgBuffer[ messageLocation[savedMessageCounter-1] ]);
jekain314 1:8e24e633f8d8 355 //toPC.printf("BESTVEL vel: horizontalSpeed= %5.3f heading=%5.1f verticalSpeed=%4.2f \n",
jekain314 1:8e24e633f8d8 356 // curVel.horizontalSpeed, curVel.heading, curVel.verticalSpeed );
jekain314 0:432b860b6ff7 357 velMsg = curVel;
jekain314 0:432b860b6ff7 358 }
jekain314 5:2ce1be9d4bef 359
jekain314 0:432b860b6ff7 360 completeMessageAvailable = false;
jekain314 0:432b860b6ff7 361 }
jekain314 0:432b860b6ff7 362
jekain314 1:8e24e633f8d8 363 //write the GPS data to the SD card
jekain314 0:432b860b6ff7 364 if (!IMUDataReady && !GPSdataWritten && timeFromPPS.read_us() > 500000 && recordData && (fpNav != NULL))
jekain314 0:432b860b6ff7 365 {
jekain314 0:432b860b6ff7 366 totalBytesWritten += fwrite(&msgBuffer, 1, byteCounter, fpNav);
jekain314 0:432b860b6ff7 367 GPSdataWritten = true;
jekain314 0:432b860b6ff7 368 }
jekain314 0:432b860b6ff7 369
jekain314 0:432b860b6ff7 370 //the IMU data record is read from the SPI in the ISR and the IMUDataReady is set true
jekain314 0:432b860b6ff7 371 //we write the IMU data here
jekain314 0:432b860b6ff7 372 if (IMUDataReady) //IMUDataReady is true if we have a recent IMU data record
jekain314 0:432b860b6ff7 373 {
jekain314 0:432b860b6ff7 374 //write the IMU data
jekain314 0:432b860b6ff7 375 if ( recordData && (fpNav != NULL) )
jekain314 0:432b860b6ff7 376 {
jekain314 1:8e24e633f8d8 377 //delTimeOfWrite = timeFromStart.read_us();
jekain314 0:432b860b6ff7 378
jekain314 20:3f04a0bde484 379 if (fillingPingWritingPong) totalBytesWritten += fwrite(&imuPong, 1, IMUrecArraySize*sizeof(IMUREC), fpNav);
jekain314 20:3f04a0bde484 380 else totalBytesWritten += fwrite(&imuPing, 1, IMUrecArraySize*sizeof(IMUREC), fpNav);
jekain314 0:432b860b6ff7 381
jekain314 1:8e24e633f8d8 382 //delTimeOfWrite = (unsigned long)((unsigned long)timeFromStart.read_us() - delTimeOfWrite);
jekain314 1:8e24e633f8d8 383 //if (delTimeOfWrite > maxWriteTime) maxWriteTime = delTimeOfWrite;
jekain314 0:432b860b6ff7 384 }
jekain314 0:432b860b6ff7 385 IMURecordCounter+=IMUrecArraySize;
jekain314 0:432b860b6ff7 386 IMUDataReady = false;
jekain314 0:432b860b6ff7 387 }
jekain314 0:432b860b6ff7 388
jekain314 0:432b860b6ff7 389 if (camera1EventDetected) //we have detected a camera trigger event
jekain314 0:432b860b6ff7 390 {
jekain314 0:432b860b6ff7 391 toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time);
jekain314 0:432b860b6ff7 392 camera1EventDetected = false;
jekain314 0:432b860b6ff7 393 }
jekain314 0:432b860b6ff7 394
jekain314 0:432b860b6ff7 395 if (detectedGPS1PPS) //true if we are exactly at a 1PPS event detection
jekain314 0:432b860b6ff7 396 {
jekain314 1:8e24e633f8d8 397 //toPC.printf("PPS=%4d stat=%1d bytes=%3d GPSMsgs=%2d #write=%8d cycles=%6d\n",
jekain314 1:8e24e633f8d8 398 // PPSCounter, posMsg.solStatus, savedByteCounter, savedPerSecMessageCounter,
jekain314 1:8e24e633f8d8 399 // totalBytesWritten, cyclesPerSec );
jekain314 0:432b860b6ff7 400
jekain314 0:432b860b6ff7 401 cyclesPerSec = 0;
jekain314 20:3f04a0bde484 402 //totalBytesWritten = 0;
jekain314 0:432b860b6ff7 403 GPSdataWritten = false;
jekain314 22:1cbdbc856660 404 //toPC.printf(" bytesWritten = %5d \n", totalBytesWritten);
jekain314 0:432b860b6ff7 405
jekain314 0:432b860b6ff7 406 IMURecordCounter = 0;
jekain314 0:432b860b6ff7 407 detectedGPS1PPS = false;
jekain314 22:1cbdbc856660 408
jekain314 22:1cbdbc856660 409 rxMsg = !rxMsg;
jekain314 22:1cbdbc856660 410 txMsg = !txMsg;
jekain314 0:432b860b6ff7 411 }
jekain314 0:432b860b6ff7 412 }
jekain314 0:432b860b6ff7 413
jekain314 17:71900da6ced6 414
jekain314 20:3f04a0bde484 415 if (fpNav != NULL)
jekain314 20:3f04a0bde484 416 {
jekain314 20:3f04a0bde484 417 fclose(fpNav); //insurance
jekain314 22:1cbdbc856660 418 toPC.printf("WMsg closeFPNav \n");
jekain314 20:3f04a0bde484 419 }
jekain314 20:3f04a0bde484 420
jekain314 22:1cbdbc856660 421 toPC.printf("WMsg totalBytesWritten %5d \n", totalBytesWritten);
jekain314 20:3f04a0bde484 422 wait_ms(100);
jekain314 20:3f04a0bde484 423
jekain314 17:71900da6ced6 424 transferFile();
jekain314 19:26c5298a7138 425 //rxMsg = txMsg = 0; // just indicate that we're in here
jekain314 17:71900da6ced6 426 // to exit this function the HOST (ie: computer or PC app) must send "exit" otherwise the mbed will act
jekain314 17:71900da6ced6 427 // like a terminal and serve SD file data forever
jekain314 16:2aea22130ba1 428
jekain314 22:1cbdbc856660 429 toPC.printf("WMsg normalTermination \n");
jekain314 20:3f04a0bde484 430 wait_ms(100);
jekain314 17:71900da6ced6 431
jekain314 17:71900da6ced6 432 NVIC_SystemReset();
jekain314 14:009d4671f0e3 433
jekain314 14:009d4671f0e3 434 /*
jekain314 14:009d4671f0e3 435 //at this point we have terminated the dat collection and we need to send the stored nav file to the PC
jekain314 14:009d4671f0e3 436 FILE* fpNavStored = fopen("/sd/Data/NAV.bin", "rb");
jekain314 14:009d4671f0e3 437 unsigned char tempArray[512];
jekain314 14:009d4671f0e3 438 toPC.printf(" beginning the file transfer \n");
jekain314 14:009d4671f0e3 439 int i = 0;
jekain314 14:009d4671f0e3 440 while(!feof(fpNavStored) )
jekain314 14:009d4671f0e3 441 {
jekain314 14:009d4671f0e3 442
jekain314 14:009d4671f0e3 443 toPC.printf(" reading the 512 bytes: %3d \n", i);
jekain314 14:009d4671f0e3 444 fread(tempArray, 1, 512, fpNavStored);
jekain314 14:009d4671f0e3 445 toPC.printf(" writing the 512 bytes: %3d \n", i);
jekain314 14:009d4671f0e3 446 fwrite(tempArray, 1, 512, toPC);
jekain314 14:009d4671f0e3 447 i++;
jekain314 14:009d4671f0e3 448 }
jekain314 14:009d4671f0e3 449 */
jekain314 14:009d4671f0e3 450
jekain314 14:009d4671f0e3 451
jekain314 0:432b860b6ff7 452 }