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

Committer:
jekain314
Date:
Mon Mar 03 13:19:31 2014 +0000
Revision:
30:96d133f3008e
Parent:
29:dead10cce6e9
commit of RT_Download.

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 30:96d133f3008e 6 #define CRC32_POLYNOMIAL 0xEDB88320
jekain314 0:432b860b6ff7 7
jekain314 0:432b860b6ff7 8 #include "MODSERIAL.h"
jekain314 0:432b860b6ff7 9 #include "SDFileSystem.h" //imported using the import utility
jekain314 0:432b860b6ff7 10
jekain314 0:432b860b6ff7 11 //general digital I/O specifications for this application
jekain314 29:dead10cce6e9 12
jekain314 29:dead10cce6e9 13 //if we dont write to the card we dont need this ...
jekain314 0:432b860b6ff7 14 //SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
jekain314 0:432b860b6ff7 15 SDFileSystem sd(p11,p12,p13,p14,"sd");
jekain314 29:dead10cce6e9 16
jekain314 0:432b860b6ff7 17 DigitalIn sd_detect(p27);
jekain314 0:432b860b6ff7 18 DigitalOut ppsled(LED1); //blink an LED at the 1PPS
jekain314 0:432b860b6ff7 19 DigitalOut trig1led(LED2); //blink an LED at the camera trigger detection
jekain314 1:8e24e633f8d8 20 DigitalOut rxMsg(LED3);
jekain314 1:8e24e633f8d8 21 DigitalOut txMsg(LED4);
jekain314 1:8e24e633f8d8 22 //DigitalOut recordDataled(LED4); //set the led when the record is on
jekain314 1:8e24e633f8d8 23
jekain314 1:8e24e633f8d8 24 //hardware trigger mechanization for bulb shutter commands
jekain314 1:8e24e633f8d8 25 DigitalInOut fire(p29); //connected to the tip of 2.5mm connector (T2i)
jekain314 1:8e24e633f8d8 26 DigitalInOut pre_fire(p30); //connected to the mid-connection for 2.5mm connector (T2i)
jekain314 1:8e24e633f8d8 27
jekain314 0:432b860b6ff7 28 //USB serial data stream back to the PC
jekain314 0:432b860b6ff7 29 Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10
jekain314 0:432b860b6ff7 30
jekain314 0:432b860b6ff7 31 Timer timeFromStart;
jekain314 30:96d133f3008e 32 Timer timeInMessageRead;
sam_grove 27:94a6f0589993 33
jekain314 0:432b860b6ff7 34 bool detectedGPS1PPS = false; //flag set in the ISR and reset after processing the 1PPS event
jekain314 0:432b860b6ff7 35 int PPSCounter = 0; //counts the 1PPS occurrences
jekain314 0:432b860b6ff7 36 int byteCounter = 0; //byte counter -- zeroed at 1PPS
jekain314 0:432b860b6ff7 37 unsigned short perSecMessageCounter=0; //counts the number of messages in a sec based on the header detection
jekain314 0:432b860b6ff7 38 bool messageDetected = false; //have detected a message header
jekain314 0:432b860b6ff7 39 unsigned long IMUbytesWritten = 0; //counts the IMU bytes written by the fwrite() to the SD card
jekain314 0:432b860b6ff7 40 int savedByteCounter = 0; //save ByteCounter at the 1PPS for display in main
jekain314 0:432b860b6ff7 41 int savedPerSecMessageCounter=0; //saved PerSecMsgCounter for display in main
jekain314 0:432b860b6ff7 42 int savedIMUClockCounter=0; //saved at the 1PPS for later diaplay from main
jekain314 0:432b860b6ff7 43 bool camera1EventDetected = false; //flag from ISR indicating a clock event occurred
jekain314 0:432b860b6ff7 44 double camera1Time; //GPS time of the camera event
sam_grove 27:94a6f0589993 45 volatile int PPSTimeOffset = 0;
jekain314 29:dead10cce6e9 46 int totalGPSBytes = 0;
jekain314 0:432b860b6ff7 47
jekain314 29:dead10cce6e9 48 bool writeIMUDataToPC = false; //used in main loop to cause IMU data write to the PC
jekain314 29:dead10cce6e9 49 bool writeGPSDataToPC = false; //used in main loop to cause GPS data write to the PC
jekain314 0:432b860b6ff7 50
jekain314 0:432b860b6ff7 51 #include "OEM615.h" //OEM615 GPS activities
jekain314 0:432b860b6ff7 52 #include "ADIS16488.h" //ADIS16488 activities
jekain314 0:432b860b6ff7 53 #include "PCMessaging.h" //PC messaging activities
jekain314 0:432b860b6ff7 54
jekain314 0:432b860b6ff7 55 //ISR for detection of the GPS 1PPS
jekain314 0:432b860b6ff7 56 void detect1PPSISR(void)
jekain314 0:432b860b6ff7 57 {
jekain314 0:432b860b6ff7 58 timeFromPPS.reset(); //reset the 1PPS timer upon 1PPS detection
jekain314 30:96d133f3008e 59
jekain314 30:96d133f3008e 60 //note -- the below accounts for GPS receiver time information becoming available AFTER the 1PPS event
jekain314 30:96d133f3008e 61 PPSTimeOffset++; //counts 1PPS events -- reset to zero in main after we determine a GPS time
jekain314 0:432b860b6ff7 62
jekain314 0:432b860b6ff7 63 //covers the case where the PPS ISR interrupts the IMU data ready ISR
jekain314 0:432b860b6ff7 64 if(IMUDataReady) IMUtimeFrom1PPS = 0;
jekain314 0:432b860b6ff7 65
jekain314 0:432b860b6ff7 66 savedByteCounter = byteCounter; //save byteCounter for display in main
jekain314 0:432b860b6ff7 67 savedPerSecMessageCounter = perSecMessageCounter; //save for display un main
jekain314 0:432b860b6ff7 68 byteCounter = 0; //countes bytes between 1PPS events
jekain314 0:432b860b6ff7 69 perSecMessageCounter = 0; //counts GPS messages between 1PPS events
jekain314 0:432b860b6ff7 70
jekain314 0:432b860b6ff7 71 GPS_COM1.rxBufferFlush(); //flush the GPS serial buffer
jekain314 0:432b860b6ff7 72
jekain314 0:432b860b6ff7 73 detectedGPS1PPS = true; //set false in the main when 1PPS actions are complete
jekain314 0:432b860b6ff7 74 PPSCounter++; //count number of 1PPS epoch
jekain314 0:432b860b6ff7 75
jekain314 0:432b860b6ff7 76 ppsled = !ppsled; //blink an LED at the 1PPS
jekain314 0:432b860b6ff7 77 };
jekain314 0:432b860b6ff7 78
jekain314 0:432b860b6ff7 79
jekain314 0:432b860b6ff7 80 ///////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 81 //set up the USB port and the GPS COM port
jekain314 0:432b860b6ff7 82 ///////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 83 void setupCOM(void)
jekain314 0:432b860b6ff7 84 {
jekain314 0:432b860b6ff7 85 //system starts with GPS in reset active
jekain314 0:432b860b6ff7 86 //dis-engage the reset to get the GPS started
jekain314 0:432b860b6ff7 87 GPS_Reset=1; wait_ms(1000);
jekain314 0:432b860b6ff7 88
jekain314 0:432b860b6ff7 89 //establish 1PPS ISR
jekain314 0:432b860b6ff7 90 PPSInt.rise(&detect1PPSISR);
jekain314 0:432b860b6ff7 91
jekain314 0:432b860b6ff7 92 //set the USB serial data rate -- rate must be matched at the PC end
jekain314 0:432b860b6ff7 93 //This the serial communication back to the the PC host
jekain314 0:432b860b6ff7 94 //Launch the C++ serial port read program there to catch the ASCII characters
jekain314 0:432b860b6ff7 95 //toPC.baud(9600); wait_ms(100);
jekain314 25:2287bd8c9877 96 toPC.baud(8*115200); wait_ms(100);
jekain314 0:432b860b6ff7 97 //toPC.baud(1*115200); wait_ms(100);
jekain314 6:71da5b99de97 98 //toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n");
jekain314 0:432b860b6ff7 99
jekain314 29:dead10cce6e9 100 //just wait some time to launch the GPS receiver
jekain314 29:dead10cce6e9 101 for (int i=0; i<5; i++) { toPC.printf("WMessage start countdown: %3d \n", 4-i); wait(1); }
jekain314 0:432b860b6ff7 102
jekain314 0:432b860b6ff7 103 //NOTE: we do not assume that the GPS receiver has been pre-set up for the WALDO_FCS functionality
jekain314 0:432b860b6ff7 104 //we alwsys start with a reset and reprogram the receiver with our data out products
jekain314 0:432b860b6ff7 105 // this prevents failure because of a blown NVRAM as occurred for the older camera systems
jekain314 0:432b860b6ff7 106
jekain314 29:dead10cce6e9 107 //this is the COM1 port from th GPS receiver to the mbed
jekain314 0:432b860b6ff7 108 //it should be always started at 9600 baud because thats the default for the GPS receiver
jekain314 0:432b860b6ff7 109 GPS_COM1.baud(9600); wait_ms(100);
jekain314 0:432b860b6ff7 110
jekain314 0:432b860b6ff7 111 // this ASCII command sets up the serial data from the GPS receiver on its COM1
jekain314 0:432b860b6ff7 112 char ch7[] = "serialconfig COM1 9600 n 8 1 n off";
jekain314 30:96d133f3008e 113
jekain314 0:432b860b6ff7 114 // this is a software reset and has the same effect as a hardware reset (why do it?)
jekain314 0:432b860b6ff7 115 //char ch0[] = "RESET";
jekain314 30:96d133f3008e 116 //toPC.printf("WMsg set RESET command \n");
jekain314 30:96d133f3008e 117 //sendASCII(ch0, sizeof(ch0)); wait_ms(3000);
jekain314 30:96d133f3008e 118
jekain314 0:432b860b6ff7 119 //this command stops all communication from the GPS receiver on COM1
jekain314 0:432b860b6ff7 120 //logs should still be presented on USB port so the Novatel CDU application can be used on the PC in parallel
jekain314 0:432b860b6ff7 121 char ch1[] = "unlogall COM1";
jekain314 0:432b860b6ff7 122 //set the final baud rate that we will use from here
jekain314 0:432b860b6ff7 123 //allowable baud rate values: 9600 115200 230400 460800 921600
jekain314 0:432b860b6ff7 124 //char ch2[] = "serialconfig COM1 921600 n 8 1 n off";
jekain314 0:432b860b6ff7 125 char ch2[] = "serialconfig COM1 115200 n 8 1 n off";
jekain314 0:432b860b6ff7 126
jekain314 0:432b860b6ff7 127 //the below commands request the POS, VEL, RANGE, and TIME messages
jekain314 29:dead10cce6e9 128 //Binary commands are shown below
jekain314 29:dead10cce6e9 129 //char ch3[] = "log COM1 BESTPOSB ONTIME 1"; //messageID = 42
jekain314 29:dead10cce6e9 130 //char ch4[] = "log COM1 BESTVelB ONTIME 1"; //messageID = 99
jekain314 29:dead10cce6e9 131 //char ch5[] = "log COM1 RANGEB ONTIME 1"; //messageID = 43
jekain314 0:432b860b6ff7 132 //char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101
jekain314 29:dead10cce6e9 133
jekain314 29:dead10cce6e9 134 //ASCII commands are shown below
jekain314 29:dead10cce6e9 135 char ch3[] = "log COM1 BESTPOSA ONTIME 1"; //messageID = 42
jekain314 29:dead10cce6e9 136 char ch4[] = "log COM1 BESTVelA ONTIME 1"; //messageID = 99
jekain314 29:dead10cce6e9 137 char ch5[] = "log COM1 RANGEA ONTIME 1"; //messageID = 43
jekain314 29:dead10cce6e9 138 //char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101
jekain314 0:432b860b6ff7 139
jekain314 15:f3b92958cf5a 140 //toPC.printf("WMsg set serial config \n");
jekain314 0:432b860b6ff7 141 sendASCII(ch7, sizeof(ch7)); wait_ms(500);
jekain314 0:432b860b6ff7 142 //sendASCII(ch0, sizeof(ch0));
jekain314 15:f3b92958cf5a 143 //toPC.printf("WMsg unlog all messages \n");
jekain314 0:432b860b6ff7 144 sendASCII(ch1, sizeof(ch1)); wait_ms(500);
jekain314 15:f3b92958cf5a 145 //toPC.printf("WMsg log BESTPOSB on COM1 \n");
jekain314 0:432b860b6ff7 146 sendASCII(ch3, sizeof(ch3)); wait_ms(500);
jekain314 15:f3b92958cf5a 147 //toPC.printf("WMsg log BESTVELB on COM1\n");
jekain314 0:432b860b6ff7 148 sendASCII(ch4, sizeof(ch4)); wait_ms(500);
jekain314 15:f3b92958cf5a 149 //toPC.printf("WMsg log RANGEB on COM1\n");
jekain314 0:432b860b6ff7 150 sendASCII(ch5, sizeof(ch5)); wait_ms(500);
jekain314 0:432b860b6ff7 151
jekain314 0:432b860b6ff7 152 //toPC.printf("log TIMEB om COM1 \n");
jekain314 0:432b860b6ff7 153 //sendASCII(ch6, sizeof(ch6)); wait_ms(100);
jekain314 0:432b860b6ff7 154
jekain314 0:432b860b6ff7 155 //toPC.printf("Set up th VARF signal \n");
jekain314 0:432b860b6ff7 156 //sendASCII(ch8, sizeof(ch8)); wait_ms(500);
jekain314 0:432b860b6ff7 157
jekain314 0:432b860b6ff7 158 //set GPS output COM1 to the final high rate
jekain314 15:f3b92958cf5a 159 //toPC.printf("WMsg set the COM ports to high rate\n");
jekain314 0:432b860b6ff7 160 sendASCII(ch2, sizeof(ch2)); wait_ms(500);
jekain314 0:432b860b6ff7 161
jekain314 0:432b860b6ff7 162 //set the mbed COM port to match the GPS transmit rate
jekain314 0:432b860b6ff7 163 //the below baud rate must match the COM1 rate coming from the GPS receiver
jekain314 0:432b860b6ff7 164 GPS_COM1.baud(115200); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL
jekain314 0:432b860b6ff7 165 //GPS_COM1.baud(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL
jekain314 0:432b860b6ff7 166 };
jekain314 0:432b860b6ff7 167
jekain314 0:432b860b6ff7 168
jekain314 30:96d133f3008e 169 void CRC32Value_2(unsigned long &CRC, unsigned char c)
jekain314 30:96d133f3008e 170 {
jekain314 30:96d133f3008e 171 /////////////////////////////////////////////////////////////////////////////////////
jekain314 30:96d133f3008e 172 //CRC must be initialized as zero
jekain314 30:96d133f3008e 173 //c is a character from the sequence that is used to form the CRC
jekain314 30:96d133f3008e 174 //this code is a modification of the code from the Novatel OEM615 specification
jekain314 30:96d133f3008e 175 /////////////////////////////////////////////////////////////////////////////////////
jekain314 30:96d133f3008e 176 unsigned long ulTemp1 = ( CRC >> 8 ) & 0x00FFFFFFL;
jekain314 30:96d133f3008e 177 unsigned long ulCRC = ((int) CRC ^ c ) & 0xff ;
jekain314 30:96d133f3008e 178 for (int j = 8 ; j > 0; j-- )
jekain314 30:96d133f3008e 179 {
jekain314 30:96d133f3008e 180 if ( ulCRC & 1 )
jekain314 30:96d133f3008e 181 ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL;
jekain314 30:96d133f3008e 182 else
jekain314 30:96d133f3008e 183 ulCRC >>= 1;
jekain314 30:96d133f3008e 184 }
jekain314 30:96d133f3008e 185 CRC = ulTemp1 ^ ulCRC;
jekain314 30:96d133f3008e 186 }
jekain314 30:96d133f3008e 187
jekain314 30:96d133f3008e 188 /* --------------------------------------------------------------------------
jekain314 30:96d133f3008e 189 Calculates the CRC-32 of a block of data all at once
jekain314 30:96d133f3008e 190 //the CRC is from the complete message (header plus data)
jekain314 30:96d133f3008e 191 //but excluding (of course) the CRC at the end
jekain314 30:96d133f3008e 192 -------------------------------------------------------------------------- */
jekain314 30:96d133f3008e 193 unsigned long CalculateBlockCRC32_2(
jekain314 30:96d133f3008e 194 unsigned long ulCount, /* Number of bytes in the data block */
jekain314 30:96d133f3008e 195 unsigned char *ucBuffer ) /* Data block */
jekain314 30:96d133f3008e 196 {
jekain314 30:96d133f3008e 197 //////////////////////////////////////////////////////////////////////
jekain314 30:96d133f3008e 198 //the below code tests the CRC32Value procedure used in a markov form
jekain314 30:96d133f3008e 199 //////////////////////////////////////////////////////////////////////
jekain314 30:96d133f3008e 200 unsigned long CRC = 0;
jekain314 30:96d133f3008e 201 for (int i = 0; i<ulCount; i++) CRC32Value_2( CRC, *ucBuffer++ );
jekain314 30:96d133f3008e 202 return CRC;
jekain314 30:96d133f3008e 203 }
jekain314 30:96d133f3008e 204
jekain314 30:96d133f3008e 205 bool checkMessageCRC(char* message, int byteCounter)
jekain314 30:96d133f3008e 206 {
jekain314 30:96d133f3008e 207 //calculate the Novatel OEM615 CRC
jekain314 30:96d133f3008e 208 //length of CRC bytes is string length minus: 8 bytes for long word, 2 bytes for "#" & "*" and 2 bytes for CR, LF
jekain314 30:96d133f3008e 209 // start at byte 1 (not zero) to skip the "#" at the start
jekain314 30:96d133f3008e 210 unsigned long computedCRC = CalculateBlockCRC32_2((unsigned long)(byteCounter-12), (unsigned char*)(&message[1]));
jekain314 30:96d133f3008e 211
jekain314 30:96d133f3008e 212 //get the computed CRC into a zero-padded string so we can compare to the string CRC in the message
jekain314 30:96d133f3008e 213 char CRCCharBuffer [50];
jekain314 30:96d133f3008e 214 int n=sprintf (CRCCharBuffer, "%08x", computedCRC); //note the zero-padding
jekain314 30:96d133f3008e 215 //toPC.printf("n = %d charCRC = %s \n", n, CRCCharBuffer);
jekain314 30:96d133f3008e 216
jekain314 30:96d133f3008e 217 //get a string reresentation of just the CRC from the message
jekain314 30:96d133f3008e 218 char CRCString[10];
jekain314 30:96d133f3008e 219 strncpy(CRCString, &message[byteCounter-10], 8);
jekain314 30:96d133f3008e 220 //toPC.printf(" %s %s \n", CRCCharBuffer, CRCString);
jekain314 30:96d133f3008e 221
jekain314 30:96d133f3008e 222 if (strncmp(CRCCharBuffer, CRCString, 8) != 0)
jekain314 30:96d133f3008e 223 {
jekain314 30:96d133f3008e 224 toPC.printf("mbed bad CRC %d %s %d %s \n", strlen(CRCCharBuffer), CRCCharBuffer, strlen(CRCString), CRCString );
jekain314 30:96d133f3008e 225 return false;
jekain314 30:96d133f3008e 226 }
jekain314 30:96d133f3008e 227
jekain314 30:96d133f3008e 228 return true;
jekain314 30:96d133f3008e 229 }
jekain314 30:96d133f3008e 230
jekain314 30:96d133f3008e 231
jekain314 0:432b860b6ff7 232 /////////////////////////////////////////////////////////////////////
jekain314 0:432b860b6ff7 233 // mbed main to support the Waldo_FCS
jekain314 0:432b860b6ff7 234 /////////////////////////////////////////////////////////////////////
jekain314 14:009d4671f0e3 235 int main()
jekain314 14:009d4671f0e3 236 {
jekain314 0:432b860b6ff7 237
jekain314 30:96d133f3008e 238 fire.output(); //set the fire pin as output
jekain314 1:8e24e633f8d8 239 pre_fire.output(); //set the pre-fire pin as output
sam_grove 27:94a6f0589993 240
jekain314 1:8e24e633f8d8 241 //fire.mode(OpenDrain);
jekain314 1:8e24e633f8d8 242
jekain314 1:8e24e633f8d8 243 //set up for the first trigger
jekain314 1:8e24e633f8d8 244 fire = 1;
jekain314 1:8e24e633f8d8 245 pre_fire = 1;
sam_grove 27:94a6f0589993 246
jekain314 0:432b860b6ff7 247 //set up the GPS and mbed COM ports
jekain314 0:432b860b6ff7 248 setupCOM();
jekain314 30:96d133f3008e 249
jekain314 30:96d133f3008e 250 //prints to PC above here wont work cause the PC serial link baud rate has not been set
jekain314 30:96d133f3008e 251 toPC.printf("initiating the mbed app\n");
jekain314 0:432b860b6ff7 252
jekain314 30:96d133f3008e 253 toPC.printf("setting up the ADIS\n");
jekain314 29:dead10cce6e9 254 //set up the ADIS16488 IMU
jekain314 30:96d133f3008e 255 setupADIS();
jekain314 29:dead10cce6e9 256
jekain314 25:2287bd8c9877 257 //initiate the interrupt to catch the GPS receiver serial bytes as they are presented
jekain314 0:432b860b6ff7 258 GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
jekain314 0:432b860b6ff7 259
jekain314 0:432b860b6ff7 260 timeFromPPS.start(); //start the time for measuring time from 1PPS events
jekain314 0:432b860b6ff7 261 timeFromStart.start();
jekain314 30:96d133f3008e 262 timeInMessageRead.start(); //used to prevent hanging in the PC message read loop
jekain314 0:432b860b6ff7 263
jekain314 25:2287bd8c9877 264 unsigned long cyclesPerSec = 0; //main while() loop cycles per GPS sec
jekain314 29:dead10cce6e9 265
jekain314 4:dda2ab5cc643 266 bool finishTrigger = false;
jekain314 4:dda2ab5cc643 267 Timer triggerInterval;
jekain314 0:432b860b6ff7 268
jekain314 17:71900da6ced6 269 bool newMission = true;
jekain314 30:96d133f3008e 270 int workingTime = 0;
jekain314 30:96d133f3008e 271
jekain314 30:96d133f3008e 272 bool triggeringInProcess = false;
jekain314 30:96d133f3008e 273
jekain314 30:96d133f3008e 274 int badRangeMessages = 0;
jekain314 30:96d133f3008e 275 int badBESPOSMessages = 0;
jekain314 30:96d133f3008e 276 int badBESVELMessages = 0;
jekain314 30:96d133f3008e 277
jekain314 30:96d133f3008e 278 int VERSION = 1;
jekain314 30:96d133f3008e 279 double timeAtMessage0 = 0;
jekain314 17:71900da6ced6 280
jekain314 0:432b860b6ff7 281 ///////////////////////////////////////////////////////////////////////////
jekain314 17:71900da6ced6 282 // top of the mission while loop
jekain314 0:432b860b6ff7 283 ///////////////////////////////////////////////////////////////////////////
jekain314 17:71900da6ced6 284 while(newMission)
jekain314 29:dead10cce6e9 285 {
jekain314 30:96d133f3008e 286 readFromPC(); //#1
jekain314 30:96d133f3008e 287
jekain314 30:96d133f3008e 288 //process GPS data messages (messages 0, 1, 2)
jekain314 30:96d133f3008e 289 if (toPC.writeable() && message0Complete && !IMUDataReady )
jekain314 29:dead10cce6e9 290 {
jekain314 30:96d133f3008e 291 int thisWorkTime = timeFromStart.read_us();
jekain314 30:96d133f3008e 292
jekain314 30:96d133f3008e 293 //get the GPS time at the prior 1PPS from the first header
jekain314 30:96d133f3008e 294 char msgName [15], port [15], clockStatus [15];
jekain314 30:96d133f3008e 295 int a1=0, GPSWeek=0;
jekain314 30:96d133f3008e 296 double b1 = 0.0, GPSTimeFromHeader = 0.0;
jekain314 30:96d133f3008e 297
jekain314 30:96d133f3008e 298 //note the use of the "C" scanset [^,] to read the comma-delimited ASCII message provided by te Novatel OEM615 receiver
jekain314 30:96d133f3008e 299 //these are used only for the character variables in order to avoid detecting the "," as another character
jekain314 30:96d133f3008e 300 //only spaces can be used as delimiters if scanset methodology is not used
jekain314 30:96d133f3008e 301 int N = sscanf(msgBuffer0,"%15[^,],%15[^,],%d,%f,%15[^,],%d,%lf", msgName, port, &a1, &b1, clockStatus, &GPSWeek, &GPSTimeFromHeader);
jekain314 30:96d133f3008e 302 if (N != 7) toPC.printf("scanf failed for message: %s ", msgBuffer0);
jekain314 30:96d133f3008e 303
jekain314 30:96d133f3008e 304 GPSTimemsecs = GPSTimeFromHeader * 1000;
jekain314 30:96d133f3008e 305
jekain314 30:96d133f3008e 306 //this is used to account for the GPS time from the receiver becoming available a few msecs after the actual 1PPS event
jekain314 30:96d133f3008e 307 //A precision timer is set at the 1PPS event that is used to tag the IMU data
jekain314 30:96d133f3008e 308 timeAtMessage0 = timeFromPPS.read_us()/1000.0;
jekain314 30:96d133f3008e 309 PPSTimeOffset = 0.0;
jekain314 30:96d133f3008e 310
jekain314 30:96d133f3008e 311 if (!checkMessageCRC(msgBuffer0, GPSbyteCounter0)) badRangeMessages++;
jekain314 30:96d133f3008e 312
jekain314 30:96d133f3008e 313 N=0;
jekain314 30:96d133f3008e 314 //write the GPS message bytes to the PC
jekain314 30:96d133f3008e 315 for (int i=0; i<GPSbyteCounter0; i++) N += toPC.printf("%c", msgBuffer0[i]);
jekain314 30:96d133f3008e 316 if (N != GPSbyteCounter0) toPC.printf(" error writing msgBuffer0: %d of %d bytes written \n", N, GPSbyteCounter0);
jekain314 30:96d133f3008e 317
jekain314 30:96d133f3008e 318 message0Complete = false; //set up for the next message
jekain314 30:96d133f3008e 319 workingTime += timeFromStart.read_us() - thisWorkTime;
jekain314 30:96d133f3008e 320
jekain314 30:96d133f3008e 321 readFromPC(); //#2
jekain314 29:dead10cce6e9 322 }
jekain314 30:96d133f3008e 323 else if (toPC.writeable() && message1Complete && !IMUDataReady)
jekain314 29:dead10cce6e9 324 {
jekain314 30:96d133f3008e 325 if (!checkMessageCRC(msgBuffer1, GPSbyteCounter1)) badBESPOSMessages++;
jekain314 30:96d133f3008e 326
jekain314 30:96d133f3008e 327 int thisWorkTime = timeFromStart.read_us();
jekain314 30:96d133f3008e 328
jekain314 30:96d133f3008e 329 int N=0;
jekain314 30:96d133f3008e 330 for (int i=0; i<GPSbyteCounter1; i++) N += toPC.printf("%c", msgBuffer1[i]);
jekain314 30:96d133f3008e 331 if (N != GPSbyteCounter1) toPC.printf(" error writing msgBuffer1: %d of %d bytes written \n", N, GPSbyteCounter1);
jekain314 30:96d133f3008e 332
jekain314 29:dead10cce6e9 333 message1Complete = false;
jekain314 30:96d133f3008e 334 workingTime += timeFromStart.read_us() - thisWorkTime;
jekain314 30:96d133f3008e 335
jekain314 30:96d133f3008e 336 readFromPC(); //#3
jekain314 29:dead10cce6e9 337 }
jekain314 30:96d133f3008e 338 else if (toPC.writeable() && message2Complete && !IMUDataReady)
jekain314 29:dead10cce6e9 339 {
jekain314 30:96d133f3008e 340 if( !checkMessageCRC(msgBuffer2, GPSbyteCounter2)) badBESVELMessages++;
jekain314 30:96d133f3008e 341
jekain314 30:96d133f3008e 342 int thisWorkTime = timeFromStart.read_us();
jekain314 30:96d133f3008e 343 int N = 0;
jekain314 30:96d133f3008e 344
jekain314 30:96d133f3008e 345 for (int i=0; i<GPSbyteCounter2; i++) N += toPC.printf("%c", msgBuffer2[i]);
jekain314 30:96d133f3008e 346 if (N != GPSbyteCounter2) toPC.printf(" error writing msgBuffer2: %d of %d bytes written \n", N, GPSbyteCounter2);
jekain314 30:96d133f3008e 347
jekain314 29:dead10cce6e9 348 message2Complete = false;
jekain314 30:96d133f3008e 349 workingTime += timeFromStart.read_us() - thisWorkTime;
jekain314 30:96d133f3008e 350
jekain314 30:96d133f3008e 351 readFromPC(); //#4
jekain314 30:96d133f3008e 352 }
jekain314 30:96d133f3008e 353 //
jekain314 30:96d133f3008e 354 //
jekain314 30:96d133f3008e 355 if (toPC.writeable() && IMUDataReady)
jekain314 2:7039be3daf6e 356 {
jekain314 30:96d133f3008e 357 int thisWorkTime = timeFromStart.read_us();
jekain314 29:dead10cce6e9 358 //write the IMU data to the PC
jekain314 29:dead10cce6e9 359 for (int i=0; i<IMUrecArraySize; i++)
jekain314 29:dead10cce6e9 360 {
jekain314 30:96d133f3008e 361 //
jekain314 29:dead10cce6e9 362 if (fillingPingWritingPong)
jekain314 29:dead10cce6e9 363 {
jekain314 30:96d133f3008e 364 toPC.printf("IMU %d13,%d13,%d13,%d13,%d13,%d13,%d13 " , //70 bytes
jekain314 29:dead10cce6e9 365 imuPong[i].GPSTime,
jekain314 29:dead10cce6e9 366 imuPong[i].dataWord[0],
jekain314 29:dead10cce6e9 367 imuPong[i].dataWord[1],
jekain314 29:dead10cce6e9 368 imuPong[i].dataWord[2],
jekain314 29:dead10cce6e9 369 imuPong[i].dataWord[3],
jekain314 29:dead10cce6e9 370 imuPong[i].dataWord[4],
jekain314 29:dead10cce6e9 371 imuPong[i].dataWord[5] );
jekain314 29:dead10cce6e9 372 }
jekain314 29:dead10cce6e9 373 else
jekain314 29:dead10cce6e9 374 {
jekain314 30:96d133f3008e 375 toPC.printf("IMU %d13,%d13,%d13,%d13,%d13,%d13,%d13 " , //70 bytes
jekain314 29:dead10cce6e9 376 imuPing[i].GPSTime,
jekain314 29:dead10cce6e9 377 imuPing[i].dataWord[0],
jekain314 29:dead10cce6e9 378 imuPing[i].dataWord[1],
jekain314 29:dead10cce6e9 379 imuPing[i].dataWord[2],
jekain314 29:dead10cce6e9 380 imuPing[i].dataWord[3],
jekain314 29:dead10cce6e9 381 imuPing[i].dataWord[4],
jekain314 29:dead10cce6e9 382 imuPing[i].dataWord[5] );
jekain314 29:dead10cce6e9 383 }
jekain314 30:96d133f3008e 384 //
jekain314 30:96d133f3008e 385 readFromPC(); //#5
jekain314 29:dead10cce6e9 386 }
jekain314 30:96d133f3008e 387 toPC.printf("\n"); //total of 10 * 70 + 9 = 709 bytes
jekain314 30:96d133f3008e 388 IMUDataReady = false;
jekain314 30:96d133f3008e 389 workingTime += timeFromStart.read_us() - thisWorkTime;
jekain314 2:7039be3daf6e 390 }
jekain314 29:dead10cce6e9 391
jekain314 29:dead10cce6e9 392 //read the USB serial data from the PC to check for commands
jekain314 30:96d133f3008e 393 //only message we expect will be the trigger fire command
jekain314 29:dead10cce6e9 394 readFromPC();
jekain314 29:dead10cce6e9 395
jekain314 30:96d133f3008e 396 if(toPC.writeable() && fireTrigger) //comes from a PC request message
jekain314 1:8e24e633f8d8 397 {
jekain314 30:96d133f3008e 398 triggeringInProcess = true;
jekain314 26:c2208b0ff78b 399 unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0;
jekain314 30:96d133f3008e 400 toPC.printf("mbedmessage trig1 %d \n", triggerTime);
jekain314 1:8e24e633f8d8 401 //pre-fire the trigger using the mid-body 2.5mm connection (T2i)
jekain314 1:8e24e633f8d8 402 pre_fire = 0; //pin30 (midbody of connector) set to zero
jekain314 30:96d133f3008e 403
jekain314 1:8e24e633f8d8 404 fire = 0; //fire the trigger using the tip connection
jekain314 25:2287bd8c9877 405 fireTrigger = false; //finished the setup -- but wait to do the actual fire
jekain314 25:2287bd8c9877 406 finishTrigger = true; //set to false after firing the trigger
jekain314 4:dda2ab5cc643 407 triggerInterval.start();
jekain314 4:dda2ab5cc643 408 }
jekain314 4:dda2ab5cc643 409
jekain314 6:71da5b99de97 410 //the trigger requires a pulse -- the above portion lowers the signal and the below raises it
jekain314 6:71da5b99de97 411 //this has been tested at 50 msecs and it will not fire at that pulse duration
jekain314 30:96d133f3008e 412 if(toPC.writeable() && finishTrigger && triggerInterval.read_ms() > 100)
jekain314 4:dda2ab5cc643 413 {
jekain314 30:96d133f3008e 414 unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0;
jekain314 30:96d133f3008e 415 toPC.printf("mbedmessage trig2 %d \n", triggerTime);
jekain314 30:96d133f3008e 416 //pre-fire the trigger using 2the mid-body 2.5mm connec
jekain314 1:8e24e633f8d8 417 fire = 1;
jekain314 1:8e24e633f8d8 418 pre_fire = 1;
jekain314 4:dda2ab5cc643 419 triggerInterval.reset();
jekain314 25:2287bd8c9877 420 finishTrigger = false; //completes the trigger firing pulse definition
jekain314 30:96d133f3008e 421 triggeringInProcess = false;
jekain314 1:8e24e633f8d8 422 }
jekain314 1:8e24e633f8d8 423
jekain314 0:432b860b6ff7 424 cyclesPerSec++;
sam_grove 27:94a6f0589993 425
jekain314 0:432b860b6ff7 426 if (detectedGPS1PPS) //true if we are exactly at a 1PPS event detection
jekain314 0:432b860b6ff7 427 {
jekain314 30:96d133f3008e 428 if (toPC.writeable() )
jekain314 30:96d133f3008e 429 {
jekain314 30:96d133f3008e 430 toPC.printf("STATUS %03d %04d %06d %02d %06d %5.3f %03d %03d %03d\n",
jekain314 30:96d133f3008e 431 VERSION, PPSCounter, totalGPSBytes, IMURecordCounter, cyclesPerSec, timeAtMessage0, /*workingTime/1000000.0,*/
jekain314 30:96d133f3008e 432 badRangeMessages, badBESVELMessages, badBESVELMessages);
jekain314 30:96d133f3008e 433 }
jekain314 29:dead10cce6e9 434 totalGPSBytes=0;
jekain314 0:432b860b6ff7 435
jekain314 0:432b860b6ff7 436 cyclesPerSec = 0;
jekain314 30:96d133f3008e 437 workingTime= 0.0;
jekain314 0:432b860b6ff7 438
jekain314 29:dead10cce6e9 439 messagePerSecCounter = 0; //GPS message per second counter
jekain314 29:dead10cce6e9 440
jekain314 0:432b860b6ff7 441 IMURecordCounter = 0;
jekain314 0:432b860b6ff7 442 detectedGPS1PPS = false;
jekain314 22:1cbdbc856660 443
jekain314 25:2287bd8c9877 444 rxMsg = !rxMsg; //flash the lights to make sure the mbed loop is operative
jekain314 22:1cbdbc856660 445 txMsg = !txMsg;
jekain314 30:96d133f3008e 446
jekain314 30:96d133f3008e 447 readFromPC(); //#6
jekain314 0:432b860b6ff7 448 }
jekain314 25:2287bd8c9877 449 ///////////////////////////////////////////
jekain314 25:2287bd8c9877 450 } //end of the major while() loop
jekain314 25:2287bd8c9877 451 ///////////////////////////////////////////
jekain314 0:432b860b6ff7 452
jekain314 17:71900da6ced6 453
jekain314 14:009d4671f0e3 454
jekain314 30:96d133f3008e 455 }
jekain314 30:96d133f3008e 456