Changes to allow hardware camera trigger
Fork of GPS_Incremental by
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:1730ee31d181
- Child:
- 8:2e20e4cf53e6
JEK changes enabling proper recording of IMU/GPS datastreams to the SD-card
Who changed what in which revision?
User | Revision | Line number | New 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 | } |