Changes to allow hardware camera trigger
Fork of GPS_Incremental by
main.cpp@2:7301e63832ee, 2013-03-20 (annotated)
- Committer:
- dannyman939
- Date:
- Wed Mar 20 21:52:48 2013 +0000
- Revision:
- 2:7301e63832ee
- Parent:
- 1:cbb9104d826f
- Child:
- 3:5913df46f94a
Last Version from Chris Casey - JEK and NDM are updating this version from this point forward.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dannyman939 | 0:c746ee34feae | 1 | #include "mbed.h" |
dannyman939 | 0:c746ee34feae | 2 | |
dannyman939 | 0:c746ee34feae | 3 | //set up the message buffer to be filled by the GPS read process |
dannyman939 | 0:c746ee34feae | 4 | #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 256 |
dannyman939 | 0:c746ee34feae | 5 | |
dannyman939 | 0:c746ee34feae | 6 | #include "MODSERIAL.h" |
dannyman939 | 0:c746ee34feae | 7 | #include "SDFileSystem.h" //imported using the import utility |
dannyman939 | 0:c746ee34feae | 8 | //#include "rtos.h" |
dannyman939 | 0:c746ee34feae | 9 | #include "OEM615.h" |
dannyman939 | 0:c746ee34feae | 10 | |
dannyman939 | 0:c746ee34feae | 11 | #include "ADIS16488.h" |
dannyman939 | 0:c746ee34feae | 12 | #include <string> |
dannyman939 | 0:c746ee34feae | 13 | |
dannyman939 | 0:c746ee34feae | 14 | #define STATUS_MSG 0 |
dannyman939 | 0:c746ee34feae | 15 | #define POSVEL_MSG 1 |
dannyman939 | 0:c746ee34feae | 16 | #define STARTDATA_MSG 2 |
dannyman939 | 0:c746ee34feae | 17 | #define STOPDATA_MSG 3 |
dannyman939 | 0:c746ee34feae | 18 | #define STARTSTREAM_MSG 4 |
dannyman939 | 0:c746ee34feae | 19 | #define STOPSTREAM_MSG 5 |
dannyman939 | 2:7301e63832ee | 20 | #define STARTLOGINFO_MSG 6 |
dannyman939 | 2:7301e63832ee | 21 | #define STOPLOGINFO_MSG 7 |
dannyman939 | 0:c746ee34feae | 22 | #define DEGREES_TO_RADIANS (3.14519/180.0) |
dannyman939 | 0:c746ee34feae | 23 | |
dannyman939 | 0:c746ee34feae | 24 | //general items for this application |
dannyman939 | 0:c746ee34feae | 25 | //SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name); |
dannyman939 | 0:c746ee34feae | 26 | SDFileSystem sd(p11,p12,p13,p14,"sd"); |
dannyman939 | 0:c746ee34feae | 27 | //Serial debug(USBTX, USBRX); // tx, rx USB communication to the PC for debug purposes |
dannyman939 | 0:c746ee34feae | 28 | DigitalOut ppsled(LED1); |
dannyman939 | 0:c746ee34feae | 29 | DigitalOut trig1led(LED2); |
dannyman939 | 0:c746ee34feae | 30 | DigitalOut recordDataled(LED4); |
dannyman939 | 1:cbb9104d826f | 31 | InterruptIn camera1Int(p30); |
dannyman939 | 1:cbb9104d826f | 32 | DigitalOut camera2Pin(p29); |
dannyman939 | 0:c746ee34feae | 33 | //USB serial data stream back to the PC |
dannyman939 | 0:c746ee34feae | 34 | Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10 |
dannyman939 | 0:c746ee34feae | 35 | |
dannyman939 | 0:c746ee34feae | 36 | bool detectedGPS1PPS = false; |
dannyman939 | 0:c746ee34feae | 37 | bool recordData = false; |
dannyman939 | 0:c746ee34feae | 38 | int PPSCounter = 0; |
dannyman939 | 0:c746ee34feae | 39 | int byteCounter = 0; |
dannyman939 | 0:c746ee34feae | 40 | unsigned short perSecMessageCounter=0; |
dannyman939 | 0:c746ee34feae | 41 | bool lookingForMessages = true; |
dannyman939 | 0:c746ee34feae | 42 | bool messageDetected = false; |
dannyman939 | 0:c746ee34feae | 43 | int savedIMUClockCounter=0; |
dannyman939 | 0:c746ee34feae | 44 | int IMUClockCounter = 0; |
dannyman939 | 0:c746ee34feae | 45 | bool camera1EventDetected = false; |
dannyman939 | 0:c746ee34feae | 46 | double camera1Time; |
dannyman939 | 2:7301e63832ee | 47 | char serBuf[128]; |
dannyman939 | 0:c746ee34feae | 48 | int serBufChars=0; |
dannyman939 | 0:c746ee34feae | 49 | bool sendPosVel=false; |
dannyman939 | 0:c746ee34feae | 50 | bool sendStatus=false; |
dannyman939 | 0:c746ee34feae | 51 | bool sendRecData=false; |
dannyman939 | 0:c746ee34feae | 52 | bool streamPos=false; |
dannyman939 | 0:c746ee34feae | 53 | bool sendStreamPos=false; |
dannyman939 | 2:7301e63832ee | 54 | bool logMsgInfo=false; |
dannyman939 | 2:7301e63832ee | 55 | bool sendLogMsgInfo=false; |
dannyman939 | 0:c746ee34feae | 56 | |
dannyman939 | 0:c746ee34feae | 57 | |
dannyman939 | 0:c746ee34feae | 58 | //ISR for detection of the GPS 1PPS |
dannyman939 | 0:c746ee34feae | 59 | void detect1PPSISR(void) |
dannyman939 | 0:c746ee34feae | 60 | { |
dannyman939 | 0:c746ee34feae | 61 | timeFromPPS.reset(); |
dannyman939 | 0:c746ee34feae | 62 | savedIMUClockCounter = IMUClockCounter; |
dannyman939 | 0:c746ee34feae | 63 | IMUClockCounter = 0; |
dannyman939 | 0:c746ee34feae | 64 | GPS_COM1.rxBufferFlush(); |
dannyman939 | 0:c746ee34feae | 65 | |
dannyman939 | 0:c746ee34feae | 66 | detectedGPS1PPS = true; |
dannyman939 | 0:c746ee34feae | 67 | lookingForMessages = true; |
dannyman939 | 0:c746ee34feae | 68 | PPSCounter++; |
dannyman939 | 0:c746ee34feae | 69 | PPSTimeOffset++; |
dannyman939 | 0:c746ee34feae | 70 | ppsled = !ppsled; |
dannyman939 | 0:c746ee34feae | 71 | }; |
dannyman939 | 0:c746ee34feae | 72 | |
dannyman939 | 0:c746ee34feae | 73 | //ISR for detection of the hotshoe trigger 1 |
dannyman939 | 1:cbb9104d826f | 74 | void camera1ISR(void) |
dannyman939 | 0:c746ee34feae | 75 | { |
dannyman939 | 0:c746ee34feae | 76 | camera1Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read(); |
dannyman939 | 0:c746ee34feae | 77 | trig1led = !trig1led; |
dannyman939 | 0:c746ee34feae | 78 | camera1EventDetected = true; |
dannyman939 | 0:c746ee34feae | 79 | }; |
dannyman939 | 0:c746ee34feae | 80 | |
dannyman939 | 0:c746ee34feae | 81 | void readFromPC() |
dannyman939 | 0:c746ee34feae | 82 | { |
dannyman939 | 0:c746ee34feae | 83 | if (toPC.readable()) //read a PC serial byte and test it for a command |
dannyman939 | 0:c746ee34feae | 84 | { |
dannyman939 | 0:c746ee34feae | 85 | // Read in next character |
dannyman939 | 0:c746ee34feae | 86 | char inChar = toPC.getc(); |
dannyman939 | 0:c746ee34feae | 87 | serBuf[serBufChars++] = inChar; |
dannyman939 | 0:c746ee34feae | 88 | // Append end of string character |
dannyman939 | 0:c746ee34feae | 89 | serBuf[serBufChars] = '\0'; |
dannyman939 | 0:c746ee34feae | 90 | // Need to parse message to determine behavior |
dannyman939 | 0:c746ee34feae | 91 | // Need to clean this up |
dannyman939 | 2:7301e63832ee | 92 | char msgList[8][32]; |
dannyman939 | 0:c746ee34feae | 93 | sprintf(msgList[STATUS_MSG], "WMsg STATUS"); |
dannyman939 | 0:c746ee34feae | 94 | sprintf(msgList[POSVEL_MSG], "WMsg POSVEL"); |
dannyman939 | 0:c746ee34feae | 95 | sprintf(msgList[STARTDATA_MSG], "WMsg RECORDDATA Y"); |
dannyman939 | 0:c746ee34feae | 96 | sprintf(msgList[STOPDATA_MSG], "WMsg RECORDDATA N"); |
dannyman939 | 0:c746ee34feae | 97 | sprintf(msgList[STARTSTREAM_MSG], "WMsg POSSTREAM Y"); |
dannyman939 | 0:c746ee34feae | 98 | sprintf(msgList[STOPSTREAM_MSG], "WMsg POSSTREAM N"); |
dannyman939 | 2:7301e63832ee | 99 | sprintf(msgList[STARTLOGINFO_MSG], "WMsg LOGINFO Y"); |
dannyman939 | 2:7301e63832ee | 100 | sprintf(msgList[STOPLOGINFO_MSG], "WMsg LOGINFO N"); |
dannyman939 | 0:c746ee34feae | 101 | // assume an invalid message which needs to be reset |
dannyman939 | 0:c746ee34feae | 102 | bool validMessage = false; |
dannyman939 | 0:c746ee34feae | 103 | bool resetMessage = true; |
dannyman939 | 0:c746ee34feae | 104 | // Check for valid message |
dannyman939 | 2:7301e63832ee | 105 | for (int m = 0; m < 8 && !validMessage; m++) |
dannyman939 | 0:c746ee34feae | 106 | { |
dannyman939 | 0:c746ee34feae | 107 | if (strncmp(serBuf, msgList[m], serBufChars) == 0) |
dannyman939 | 0:c746ee34feae | 108 | { |
dannyman939 | 0:c746ee34feae | 109 | validMessage = true; |
dannyman939 | 0:c746ee34feae | 110 | // buffer length is same as message length |
dannyman939 | 0:c746ee34feae | 111 | if (serBufChars == strlen(msgList[m])) |
dannyman939 | 0:c746ee34feae | 112 | { |
dannyman939 | 0:c746ee34feae | 113 | switch(m) |
dannyman939 | 0:c746ee34feae | 114 | { |
dannyman939 | 0:c746ee34feae | 115 | case STATUS_MSG: |
dannyman939 | 0:c746ee34feae | 116 | sendStatus = true; |
dannyman939 | 0:c746ee34feae | 117 | break; |
dannyman939 | 0:c746ee34feae | 118 | case POSVEL_MSG: |
dannyman939 | 0:c746ee34feae | 119 | sendPosVel = true; |
dannyman939 | 0:c746ee34feae | 120 | break; |
dannyman939 | 0:c746ee34feae | 121 | case STARTDATA_MSG: |
dannyman939 | 0:c746ee34feae | 122 | case STOPDATA_MSG: |
dannyman939 | 0:c746ee34feae | 123 | recordData = (m == STARTDATA_MSG); |
dannyman939 | 0:c746ee34feae | 124 | sendRecData = true; |
dannyman939 | 0:c746ee34feae | 125 | break; |
dannyman939 | 0:c746ee34feae | 126 | case STARTSTREAM_MSG: |
dannyman939 | 0:c746ee34feae | 127 | case STOPSTREAM_MSG: |
dannyman939 | 0:c746ee34feae | 128 | streamPos = (m == STARTSTREAM_MSG); |
dannyman939 | 0:c746ee34feae | 129 | sendStreamPos = true; |
dannyman939 | 0:c746ee34feae | 130 | break; |
dannyman939 | 2:7301e63832ee | 131 | case STARTLOGINFO_MSG: |
dannyman939 | 2:7301e63832ee | 132 | case STOPLOGINFO_MSG: |
dannyman939 | 2:7301e63832ee | 133 | logMsgInfo = (m == STARTLOGINFO_MSG); |
dannyman939 | 2:7301e63832ee | 134 | sendLogMsgInfo = true; |
dannyman939 | 2:7301e63832ee | 135 | break; |
dannyman939 | 0:c746ee34feae | 136 | |
dannyman939 | 0:c746ee34feae | 137 | } |
dannyman939 | 0:c746ee34feae | 138 | } |
dannyman939 | 0:c746ee34feae | 139 | // message is still in progress, do not reset |
dannyman939 | 0:c746ee34feae | 140 | else |
dannyman939 | 0:c746ee34feae | 141 | { |
dannyman939 | 0:c746ee34feae | 142 | resetMessage = false; |
dannyman939 | 0:c746ee34feae | 143 | } |
dannyman939 | 0:c746ee34feae | 144 | } |
dannyman939 | 0:c746ee34feae | 145 | } |
dannyman939 | 0:c746ee34feae | 146 | // if message should be reset |
dannyman939 | 0:c746ee34feae | 147 | if (resetMessage) |
dannyman939 | 0:c746ee34feae | 148 | { |
dannyman939 | 0:c746ee34feae | 149 | // reset serial buffer character count |
dannyman939 | 0:c746ee34feae | 150 | serBufChars = 0; |
dannyman939 | 0:c746ee34feae | 151 | // if invalid message and most recent character is 'W' (first message character), |
dannyman939 | 0:c746ee34feae | 152 | // possible message collision |
dannyman939 | 0:c746ee34feae | 153 | if ((!validMessage) && (inChar == 'W')) |
dannyman939 | 0:c746ee34feae | 154 | { |
dannyman939 | 0:c746ee34feae | 155 | // start a new message |
dannyman939 | 0:c746ee34feae | 156 | serBuf[serBufChars++] = inChar; |
dannyman939 | 0:c746ee34feae | 157 | serBuf[serBufChars] == '\0'; |
dannyman939 | 0:c746ee34feae | 158 | } |
dannyman939 | 0:c746ee34feae | 159 | // Append end of string character |
dannyman939 | 0:c746ee34feae | 160 | serBuf[serBufChars] = '\0'; |
dannyman939 | 0:c746ee34feae | 161 | } |
dannyman939 | 0:c746ee34feae | 162 | } |
dannyman939 | 0:c746ee34feae | 163 | }; |
dannyman939 | 0:c746ee34feae | 164 | |
dannyman939 | 0:c746ee34feae | 165 | void sendASCII(char* ASCI_message, int numChars) |
dannyman939 | 0:c746ee34feae | 166 | { |
dannyman939 | 0:c746ee34feae | 167 | //char ASCI_message[] = "unlogall COM1"; |
dannyman939 | 0:c746ee34feae | 168 | int as = numChars - 1; |
dannyman939 | 0:c746ee34feae | 169 | unsigned char CR = 0x0d; //ASCII Carriage Return |
dannyman939 | 0:c746ee34feae | 170 | unsigned char LF = 0x0a; //ASCII Line Feed |
dannyman939 | 0:c746ee34feae | 171 | |
dannyman939 | 0:c746ee34feae | 172 | //printf("%s", ch); |
dannyman939 | 0:c746ee34feae | 173 | //printf("\n"); |
dannyman939 | 0:c746ee34feae | 174 | |
dannyman939 | 0:c746ee34feae | 175 | for (int i=0; i<as; i++) GPS_COM1.putc(ASCI_message[i]); |
dannyman939 | 0:c746ee34feae | 176 | GPS_COM1.putc(CR); //carriage return at end |
dannyman939 | 0:c746ee34feae | 177 | GPS_COM1.putc(LF); //line feed at end |
dannyman939 | 0:c746ee34feae | 178 | } |
dannyman939 | 0:c746ee34feae | 179 | |
dannyman939 | 0:c746ee34feae | 180 | //FILE* fp = NULL; |
dannyman939 | 0:c746ee34feae | 181 | void setupCOM(void) |
dannyman939 | 0:c746ee34feae | 182 | { |
dannyman939 | 0:c746ee34feae | 183 | //system starts with GPS in reset active |
dannyman939 | 0:c746ee34feae | 184 | //dis-engage the reset to get the GPS started |
dannyman939 | 0:c746ee34feae | 185 | GPS_Reset=1; wait_ms(1000); |
dannyman939 | 0:c746ee34feae | 186 | |
dannyman939 | 0:c746ee34feae | 187 | //establish 1PPS ISR |
dannyman939 | 0:c746ee34feae | 188 | PPSInt.rise(&detect1PPSISR); |
dannyman939 | 0:c746ee34feae | 189 | |
dannyman939 | 0:c746ee34feae | 190 | //set the USB serial data rate -- rate must be matched at the PC end |
dannyman939 | 0:c746ee34feae | 191 | //This the serial communication back to the the PC host |
dannyman939 | 0:c746ee34feae | 192 | //Launch the C++ serial port read program there to catch the ASCII characters |
dannyman939 | 0:c746ee34feae | 193 | //toPC.baud(9600); wait_ms(100); |
dannyman939 | 0:c746ee34feae | 194 | toPC.baud(8*115200); wait_ms(100); |
dannyman939 | 2:7301e63832ee | 195 | //toPC.baud(1*115200); wait_ms(100); |
dannyman939 | 0:c746ee34feae | 196 | toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n"); |
dannyman939 | 0:c746ee34feae | 197 | |
dannyman939 | 0:c746ee34feae | 198 | //just wait to lauinch the GPS receiver |
dannyman939 | 0:c746ee34feae | 199 | for (int i=0; i<5; i++) { toPC.printf(" to start: %3d \n", 4-i); wait(1); } |
dannyman939 | 0:c746ee34feae | 200 | |
dannyman939 | 0:c746ee34feae | 201 | |
dannyman939 | 0:c746ee34feae | 202 | mkdir("/sd/Data", 0777); |
dannyman939 | 0:c746ee34feae | 203 | |
dannyman939 | 0:c746ee34feae | 204 | /* |
dannyman939 | 0:c746ee34feae | 205 | fp = fopen("/sd/Data/IMUGPS.bin", "wb"); |
dannyman939 | 0:c746ee34feae | 206 | if (fp == NULL) |
dannyman939 | 0:c746ee34feae | 207 | { |
dannyman939 | 0:c746ee34feae | 208 | toPC.printf(" cannot open the IMUGPS data file \n"); |
dannyman939 | 0:c746ee34feae | 209 | } |
dannyman939 | 0:c746ee34feae | 210 | else |
dannyman939 | 0:c746ee34feae | 211 | toPC.printf(" opened the IMUGPS data file \n"); |
dannyman939 | 0:c746ee34feae | 212 | */ |
dannyman939 | 0:c746ee34feae | 213 | |
dannyman939 | 0:c746ee34feae | 214 | //this is the COM1 port from th GPS receiuver to the mbed |
dannyman939 | 0:c746ee34feae | 215 | //it should be always started at 9600 baud because thats the default for the GPS receiver |
dannyman939 | 0:c746ee34feae | 216 | GPS_COM1.baud(9600); wait_ms(100); |
dannyman939 | 0:c746ee34feae | 217 | |
dannyman939 | 0:c746ee34feae | 218 | // this ASCII command sets up the serial data from the GPS receiver on its COM1 |
dannyman939 | 0:c746ee34feae | 219 | char ch7[] = "serialconfig COM1 9600 n 8 1 n off"; |
dannyman939 | 0:c746ee34feae | 220 | // this is a software reset and has the same effect as a hardware reset (why do it??) |
dannyman939 | 0:c746ee34feae | 221 | char ch0[] = "RESET"; |
dannyman939 | 0:c746ee34feae | 222 | //this command stops all communication from the GPS receiver on COM1 |
dannyman939 | 0:c746ee34feae | 223 | //logs should still be presented on USB port so the CDU can be used in parallel |
dannyman939 | 0:c746ee34feae | 224 | char ch1[] = "unlogall COM1"; |
dannyman939 | 0:c746ee34feae | 225 | //set the final baud rate that we will use from here |
dannyman939 | 0:c746ee34feae | 226 | //allowable baud rate values: 9600 115200 230400 460800 921600 |
dannyman939 | 0:c746ee34feae | 227 | char ch2[] = "serialconfig COM1 921600 n 8 1 n off"; |
dannyman939 | 0:c746ee34feae | 228 | |
dannyman939 | 0:c746ee34feae | 229 | //the below commands request the POS, VEL, RANGE, and TIME messages |
dannyman939 | 0:c746ee34feae | 230 | char ch3[] = "log COM1 BESTPOSB ONTIME 1"; //messageID = 42 |
dannyman939 | 0:c746ee34feae | 231 | char ch4[] = "log COM1 BESTVelB ONTIME 1"; //messageID = 99 |
dannyman939 | 0:c746ee34feae | 232 | char ch5[] = "log COM1 RANGEB ONTIME 1"; //messageID = 43 |
dannyman939 | 0:c746ee34feae | 233 | char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101 |
dannyman939 | 0:c746ee34feae | 234 | |
dannyman939 | 0:c746ee34feae | 235 | //set up VARF to be 100Hz with 1X10^4 * 10^-8 = 10^-4 sec (10usec) pulse width |
dannyman939 | 0:c746ee34feae | 236 | char ch8[] = "FREQUENCYOUT enable 10000 1000000"; |
dannyman939 | 0:c746ee34feae | 237 | |
dannyman939 | 0:c746ee34feae | 238 | toPC.printf("set serial config \n"); |
dannyman939 | 0:c746ee34feae | 239 | sendASCII(ch7, sizeof(ch7)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 240 | //sendASCII(ch0, sizeof(ch0)); |
dannyman939 | 0:c746ee34feae | 241 | toPC.printf("unlog all messages \n"); |
dannyman939 | 0:c746ee34feae | 242 | sendASCII(ch1, sizeof(ch1)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 243 | toPC.printf("log BESTPOSB on COM1 \n"); |
dannyman939 | 0:c746ee34feae | 244 | sendASCII(ch3, sizeof(ch3)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 245 | toPC.printf("log BESTVELB on COM1\n"); |
dannyman939 | 0:c746ee34feae | 246 | sendASCII(ch4, sizeof(ch4)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 247 | toPC.printf("log RANGEB on COM1\n"); |
dannyman939 | 0:c746ee34feae | 248 | sendASCII(ch5, sizeof(ch5)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 249 | |
dannyman939 | 0:c746ee34feae | 250 | //toPC.printf("log TIMEB om COM1 \n"); |
dannyman939 | 0:c746ee34feae | 251 | //sendASCII(ch6, sizeof(ch6)); wait_ms(100); |
dannyman939 | 0:c746ee34feae | 252 | |
dannyman939 | 0:c746ee34feae | 253 | toPC.printf(" set up th VARF signal \n"); |
dannyman939 | 0:c746ee34feae | 254 | sendASCII(ch8, sizeof(ch8)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 255 | |
dannyman939 | 0:c746ee34feae | 256 | //set GPS output COM1 to the final high rate |
dannyman939 | 0:c746ee34feae | 257 | toPC.printf("set the COM ports to high rate\n"); |
dannyman939 | 0:c746ee34feae | 258 | sendASCII(ch2, sizeof(ch2)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 259 | |
dannyman939 | 0:c746ee34feae | 260 | //set the mbed COM port to match the GPS transmit rate |
dannyman939 | 0:c746ee34feae | 261 | //the below baud rate must match the COM1 rate coming from the GPS receiver |
dannyman939 | 0:c746ee34feae | 262 | GPS_COM1.baud(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL |
dannyman939 | 0:c746ee34feae | 263 | }; |
dannyman939 | 0:c746ee34feae | 264 | |
dannyman939 | 0:c746ee34feae | 265 | void setupTriggers() |
dannyman939 | 0:c746ee34feae | 266 | { |
dannyman939 | 1:cbb9104d826f | 267 | camera1Int.mode(PullUp); |
dannyman939 | 1:cbb9104d826f | 268 | camera2Pin = 1; |
dannyman939 | 0:c746ee34feae | 269 | //establish Trigger ISR |
dannyman939 | 1:cbb9104d826f | 270 | camera1Int.rise(&camera1ISR); |
dannyman939 | 0:c746ee34feae | 271 | |
dannyman939 | 0:c746ee34feae | 272 | } |
dannyman939 | 0:c746ee34feae | 273 | |
dannyman939 | 0:c746ee34feae | 274 | int test = 0; |
dannyman939 | 0:c746ee34feae | 275 | unsigned short messageCounter = 0; |
dannyman939 | 0:c746ee34feae | 276 | unsigned short savedMessageCounter = 0; |
dannyman939 | 2:7301e63832ee | 277 | unsigned char msgBuffer[1536]; |
dannyman939 | 1:cbb9104d826f | 278 | unsigned short messageLocation[6] = {0}; |
dannyman939 | 0:c746ee34feae | 279 | |
dannyman939 | 0:c746ee34feae | 280 | //see the mbed COOKBOOK for MODSERIAL |
dannyman939 | 0:c746ee34feae | 281 | //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output. |
dannyman939 | 0:c746ee34feae | 282 | void readSerialByte(MODSERIAL_IRQ_INFO *q) |
dannyman939 | 0:c746ee34feae | 283 | { |
dannyman939 | 0:c746ee34feae | 284 | MODSERIAL *serial = q->serial; |
dannyman939 | 0:c746ee34feae | 285 | unsigned char synch0 = serial->getc(); |
dannyman939 | 0:c746ee34feae | 286 | msgBuffer[byteCounter] = synch0; |
dannyman939 | 0:c746ee34feae | 287 | |
dannyman939 | 0:c746ee34feae | 288 | //we need to trap the GPS message header byte-string 0xAA44121C |
dannyman939 | 0:c746ee34feae | 289 | //generate a 4-byte sliding-window sequence from the input bytes |
dannyman939 | 0:c746ee34feae | 290 | //shift last 4-byte value left 8 bits & push recently read byte (synch0) into low-order byte |
dannyman939 | 0:c746ee34feae | 291 | test = (test<<8) | synch0; // |
dannyman939 | 0:c746ee34feae | 292 | |
dannyman939 | 0:c746ee34feae | 293 | if (test == 0xAA44121C) //test for the Receiver message header signature |
dannyman939 | 0:c746ee34feae | 294 | { |
dannyman939 | 1:cbb9104d826f | 295 | messageLocation[perSecMessageCounter] = byteCounter-3; //store the location of this message (with 4 synch words) |
dannyman939 | 0:c746ee34feae | 296 | perSecMessageCounter++; |
dannyman939 | 0:c746ee34feae | 297 | messageDetected = true; |
dannyman939 | 0:c746ee34feae | 298 | } |
dannyman939 | 0:c746ee34feae | 299 | byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected) |
dannyman939 | 2:7301e63832ee | 300 | |
dannyman939 | 0:c746ee34feae | 301 | }; |
dannyman939 | 0:c746ee34feae | 302 | |
dannyman939 | 0:c746ee34feae | 303 | int main() { |
dannyman939 | 0:c746ee34feae | 304 | |
dannyman939 | 0:c746ee34feae | 305 | //FILE *fpIMU = NULL; |
dannyman939 | 0:c746ee34feae | 306 | //FILE *fpGPS = NULL; |
dannyman939 | 0:c746ee34feae | 307 | FILE *fpNav = NULL; |
dannyman939 | 0:c746ee34feae | 308 | OEM615BESTPOS posMsg; |
dannyman939 | 0:c746ee34feae | 309 | OEM615BESTPOS curPos; |
dannyman939 | 0:c746ee34feae | 310 | OEM615BESTVEL velMsg; |
dannyman939 | 0:c746ee34feae | 311 | OEM615BESTVEL curVel; |
dannyman939 | 1:cbb9104d826f | 312 | int msgLen; |
dannyman939 | 1:cbb9104d826f | 313 | int msgEnd; |
dannyman939 | 0:c746ee34feae | 314 | |
dannyman939 | 2:7301e63832ee | 315 | //set up the GPS and mbed COM ports |
dannyman939 | 0:c746ee34feae | 316 | setupCOM(); |
dannyman939 | 0:c746ee34feae | 317 | |
dannyman939 | 0:c746ee34feae | 318 | //set up the ADIS16488 |
dannyman939 | 0:c746ee34feae | 319 | setupADIS(); |
dannyman939 | 0:c746ee34feae | 320 | |
dannyman939 | 0:c746ee34feae | 321 | //setup Hotshoe |
dannyman939 | 0:c746ee34feae | 322 | setupTriggers(); |
dannyman939 | 0:c746ee34feae | 323 | //attempt to use the mbed RTOS library |
dannyman939 | 0:c746ee34feae | 324 | //Thread thread(writeThread); |
dannyman939 | 0:c746ee34feae | 325 | |
dannyman939 | 0:c746ee34feae | 326 | toPC.printf("completed setting up COM ports \n"); |
dannyman939 | 0:c746ee34feae | 327 | |
dannyman939 | 0:c746ee34feae | 328 | //set up the interrupt to catch the serial byte availability |
dannyman939 | 0:c746ee34feae | 329 | GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq); |
dannyman939 | 0:c746ee34feae | 330 | |
dannyman939 | 0:c746ee34feae | 331 | timeFromPPS.start(); |
dannyman939 | 0:c746ee34feae | 332 | |
dannyman939 | 0:c746ee34feae | 333 | //while(PPSCounter < 1000) |
dannyman939 | 0:c746ee34feae | 334 | while(1) |
dannyman939 | 0:c746ee34feae | 335 | { |
dannyman939 | 0:c746ee34feae | 336 | //read the USB serial data from the PC to check for commands |
dannyman939 | 0:c746ee34feae | 337 | readFromPC(); |
dannyman939 | 0:c746ee34feae | 338 | |
dannyman939 | 0:c746ee34feae | 339 | if (sendPosVel) |
dannyman939 | 0:c746ee34feae | 340 | { |
dannyman939 | 0:c746ee34feae | 341 | sendPosVel=false; |
dannyman939 | 0:c746ee34feae | 342 | char solReady = 'N'; |
dannyman939 | 0:c746ee34feae | 343 | if (posMsg.solStatus == 0) |
dannyman939 | 0:c746ee34feae | 344 | { |
dannyman939 | 0:c746ee34feae | 345 | solReady = 'Y'; |
dannyman939 | 0:c746ee34feae | 346 | } |
dannyman939 | 0:c746ee34feae | 347 | double nVel = velMsg.horizontalSpeed*cos(velMsg.heading*DEGREES_TO_RADIANS); |
dannyman939 | 0:c746ee34feae | 348 | double eVel = velMsg.horizontalSpeed*sin(velMsg.heading*DEGREES_TO_RADIANS); |
dannyman939 | 0:c746ee34feae | 349 | // For the 1 second deltas with which we are dealing |
dannyman939 | 0:c746ee34feae | 350 | // this calculation should be close enough for now |
dannyman939 | 0:c746ee34feae | 351 | // approximately 1 nautical mile / minute latitude, 60 minutes/degree, 1852 meters/nautical mile |
dannyman939 | 0:c746ee34feae | 352 | double latMetersPerDeg = 60.0*1852.0; |
dannyman939 | 0:c746ee34feae | 353 | // longitude separation is approximately equal to latitude separation * cosine of latitude |
dannyman939 | 0:c746ee34feae | 354 | double lonMetersPerDeg = latMetersPerDeg*cos(posMsg.latitude*DEGREES_TO_RADIANS); |
dannyman939 | 0:c746ee34feae | 355 | |
dannyman939 | 0:c746ee34feae | 356 | // Elapsed time since last known GPS position |
dannyman939 | 0:c746ee34feae | 357 | double elTime = (double)PPSTimeOffset + timeFromPPS.read(); |
dannyman939 | 0:c746ee34feae | 358 | // Position time |
dannyman939 | 0:c746ee34feae | 359 | double posTime = GPSTime + elTime; |
dannyman939 | 0:c746ee34feae | 360 | |
dannyman939 | 0:c746ee34feae | 361 | // Estimated position based on previous position and velocity |
dannyman939 | 0:c746ee34feae | 362 | double latPos = posMsg.latitude + (nVel/latMetersPerDeg)*elTime; |
dannyman939 | 0:c746ee34feae | 363 | double lonPos = posMsg.longitude + (eVel/lonMetersPerDeg)*elTime; |
dannyman939 | 0:c746ee34feae | 364 | double htPos = posMsg.height + velMsg.verticalSpeed/(60*1852)*elTime; |
dannyman939 | 0:c746ee34feae | 365 | toPC.printf("WMsg POSVEL %5.3lf %d %c %8.5lf %9.5lf %4.3lf %4.3lf %4.3lf %4.3lf\n", |
dannyman939 | 0:c746ee34feae | 366 | posTime, |
dannyman939 | 0:c746ee34feae | 367 | posMsg.numSolSV, |
dannyman939 | 0:c746ee34feae | 368 | solReady, |
dannyman939 | 0:c746ee34feae | 369 | latPos, |
dannyman939 | 0:c746ee34feae | 370 | lonPos, |
dannyman939 | 0:c746ee34feae | 371 | htPos, |
dannyman939 | 0:c746ee34feae | 372 | nVel, |
dannyman939 | 0:c746ee34feae | 373 | eVel, |
dannyman939 | 0:c746ee34feae | 374 | velMsg.verticalSpeed |
dannyman939 | 0:c746ee34feae | 375 | ); |
dannyman939 | 0:c746ee34feae | 376 | } |
dannyman939 | 0:c746ee34feae | 377 | if (sendStatus) |
dannyman939 | 0:c746ee34feae | 378 | { |
dannyman939 | 0:c746ee34feae | 379 | sendStatus=false; |
dannyman939 | 0:c746ee34feae | 380 | char solReady = 'N'; |
dannyman939 | 0:c746ee34feae | 381 | if (posMsg.solStatus == 0) |
dannyman939 | 0:c746ee34feae | 382 | { |
dannyman939 | 0:c746ee34feae | 383 | solReady = 'Y'; |
dannyman939 | 0:c746ee34feae | 384 | } |
dannyman939 | 0:c746ee34feae | 385 | toPC.printf("WMsg STATUS %5.3lf %c\n", |
dannyman939 | 0:c746ee34feae | 386 | GPSTime, |
dannyman939 | 0:c746ee34feae | 387 | solReady |
dannyman939 | 0:c746ee34feae | 388 | ); |
dannyman939 | 0:c746ee34feae | 389 | } |
dannyman939 | 0:c746ee34feae | 390 | if (sendRecData) |
dannyman939 | 0:c746ee34feae | 391 | { |
dannyman939 | 0:c746ee34feae | 392 | sendRecData=false; |
dannyman939 | 0:c746ee34feae | 393 | char recChar = 'N'; |
dannyman939 | 0:c746ee34feae | 394 | if (recordData) |
dannyman939 | 0:c746ee34feae | 395 | { |
dannyman939 | 0:c746ee34feae | 396 | if ((fpNav == NULL)) |
dannyman939 | 0:c746ee34feae | 397 | { |
dannyman939 | 0:c746ee34feae | 398 | fpNav = fopen("/sd/Data/NAV.bin", "wb"); |
dannyman939 | 0:c746ee34feae | 399 | } |
dannyman939 | 0:c746ee34feae | 400 | if (fpNav != NULL) |
dannyman939 | 0:c746ee34feae | 401 | { |
dannyman939 | 0:c746ee34feae | 402 | recChar = 'Y'; |
dannyman939 | 0:c746ee34feae | 403 | } |
dannyman939 | 0:c746ee34feae | 404 | //recChar = 'Y'; |
dannyman939 | 0:c746ee34feae | 405 | } |
dannyman939 | 0:c746ee34feae | 406 | else |
dannyman939 | 0:c746ee34feae | 407 | { |
dannyman939 | 0:c746ee34feae | 408 | if (fpNav != NULL) |
dannyman939 | 0:c746ee34feae | 409 | { |
dannyman939 | 0:c746ee34feae | 410 | fclose(fpNav); |
dannyman939 | 0:c746ee34feae | 411 | fpNav = NULL; |
dannyman939 | 0:c746ee34feae | 412 | } |
dannyman939 | 0:c746ee34feae | 413 | /* |
dannyman939 | 0:c746ee34feae | 414 | if (fpIMU != NULL) |
dannyman939 | 0:c746ee34feae | 415 | { |
dannyman939 | 0:c746ee34feae | 416 | fclose(fpIMU); |
dannyman939 | 0:c746ee34feae | 417 | fpIMU = NULL; |
dannyman939 | 0:c746ee34feae | 418 | } |
dannyman939 | 0:c746ee34feae | 419 | if (fpGPS != NULL) |
dannyman939 | 0:c746ee34feae | 420 | { |
dannyman939 | 0:c746ee34feae | 421 | fclose(fpGPS); |
dannyman939 | 0:c746ee34feae | 422 | fpGPS = NULL; |
dannyman939 | 0:c746ee34feae | 423 | } |
dannyman939 | 0:c746ee34feae | 424 | */ |
dannyman939 | 0:c746ee34feae | 425 | } |
dannyman939 | 0:c746ee34feae | 426 | toPC.printf("WMsg RECORDDATA %c\n", |
dannyman939 | 0:c746ee34feae | 427 | recChar |
dannyman939 | 0:c746ee34feae | 428 | ); |
dannyman939 | 0:c746ee34feae | 429 | } |
dannyman939 | 0:c746ee34feae | 430 | recordDataled = recordData; |
dannyman939 | 0:c746ee34feae | 431 | if (sendStreamPos) |
dannyman939 | 0:c746ee34feae | 432 | { |
dannyman939 | 0:c746ee34feae | 433 | sendStreamPos=false; |
dannyman939 | 0:c746ee34feae | 434 | char streamChar = 'N'; |
dannyman939 | 0:c746ee34feae | 435 | if (streamPos) |
dannyman939 | 0:c746ee34feae | 436 | { |
dannyman939 | 0:c746ee34feae | 437 | streamChar = 'Y'; |
dannyman939 | 0:c746ee34feae | 438 | } |
dannyman939 | 0:c746ee34feae | 439 | toPC.printf("WMsg POSSTREAM %c\n", |
dannyman939 | 0:c746ee34feae | 440 | streamChar |
dannyman939 | 0:c746ee34feae | 441 | ); |
dannyman939 | 0:c746ee34feae | 442 | } |
dannyman939 | 2:7301e63832ee | 443 | if (sendLogMsgInfo) |
dannyman939 | 2:7301e63832ee | 444 | { |
dannyman939 | 2:7301e63832ee | 445 | sendLogMsgInfo=false; |
dannyman939 | 2:7301e63832ee | 446 | char logChar = 'N'; |
dannyman939 | 2:7301e63832ee | 447 | if (logMsgInfo) |
dannyman939 | 2:7301e63832ee | 448 | { |
dannyman939 | 2:7301e63832ee | 449 | logChar = 'Y'; |
dannyman939 | 2:7301e63832ee | 450 | } |
dannyman939 | 2:7301e63832ee | 451 | toPC.printf("WMsg LOGINFO %c\n", |
dannyman939 | 2:7301e63832ee | 452 | logChar |
dannyman939 | 2:7301e63832ee | 453 | ); |
dannyman939 | 2:7301e63832ee | 454 | } |
dannyman939 | 0:c746ee34feae | 455 | if (IMUDataReady) |
dannyman939 | 0:c746ee34feae | 456 | { |
dannyman939 | 0:c746ee34feae | 457 | //write the IMU data |
dannyman939 | 0:c746ee34feae | 458 | //if (recordData && (fpIMU != NULL)) |
dannyman939 | 0:c746ee34feae | 459 | if (recordData && (fpNav != NULL)) |
dannyman939 | 0:c746ee34feae | 460 | { |
dannyman939 | 0:c746ee34feae | 461 | fwrite(imuRec.dataWord, sizeof(IMUREC), 1, fpNav); |
dannyman939 | 0:c746ee34feae | 462 | } |
dannyman939 | 0:c746ee34feae | 463 | IMUDataReady = false; |
dannyman939 | 0:c746ee34feae | 464 | } |
dannyman939 | 2:7301e63832ee | 465 | //if (lookingForMessages && (timeFromPPS.read() > 0.100)) //it takes less than 20msec to receive all messages |
dannyman939 | 0:c746ee34feae | 466 | { |
dannyman939 | 0:c746ee34feae | 467 | |
dannyman939 | 0:c746ee34feae | 468 | //toPC.printf(" num messages = %3d time = %5d \n", perSecMessageCounter, timeFromPPS.read_us()); |
dannyman939 | 0:c746ee34feae | 469 | for (int i=0; i<perSecMessageCounter; i++) |
dannyman939 | 0:c746ee34feae | 470 | { |
dannyman939 | 2:7301e63832ee | 471 | // ensure at message header has arrived before processing |
dannyman939 | 2:7301e63832ee | 472 | if (byteCounter < (messageLocation[i] + sizeof(MESSAGEHEADER))) |
dannyman939 | 2:7301e63832ee | 473 | { |
dannyman939 | 2:7301e63832ee | 474 | // Complete message header has not been received. |
dannyman939 | 2:7301e63832ee | 475 | // Clear processed messages and break out of message processing loop |
dannyman939 | 2:7301e63832ee | 476 | for (int j = 0; j < i; j++) |
dannyman939 | 2:7301e63832ee | 477 | { |
dannyman939 | 2:7301e63832ee | 478 | messageLocation[j] = messageLocation[i+j]; |
dannyman939 | 2:7301e63832ee | 479 | perSecMessageCounter--; |
dannyman939 | 2:7301e63832ee | 480 | } |
dannyman939 | 2:7301e63832ee | 481 | break; |
dannyman939 | 2:7301e63832ee | 482 | } |
dannyman939 | 1:cbb9104d826f | 483 | msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]]; |
dannyman939 | 1:cbb9104d826f | 484 | // Ensure complete message has been received |
dannyman939 | 1:cbb9104d826f | 485 | // Message length is header length + message length + CRC |
dannyman939 | 2:7301e63832ee | 486 | msgLen = 28; |
dannyman939 | 2:7301e63832ee | 487 | switch (msgHeader[i]->messageID) |
dannyman939 | 2:7301e63832ee | 488 | { |
dannyman939 | 2:7301e63832ee | 489 | case 42: |
dannyman939 | 2:7301e63832ee | 490 | msgLen = 104; |
dannyman939 | 2:7301e63832ee | 491 | break; |
dannyman939 | 2:7301e63832ee | 492 | case 99: |
dannyman939 | 2:7301e63832ee | 493 | msgLen = 76; |
dannyman939 | 2:7301e63832ee | 494 | break; |
dannyman939 | 2:7301e63832ee | 495 | default: |
dannyman939 | 2:7301e63832ee | 496 | msgLen = msgHeader[i]->headerLength + msgHeader[i]->messageLength + sizeof(unsigned long); |
dannyman939 | 2:7301e63832ee | 497 | break; |
dannyman939 | 2:7301e63832ee | 498 | |
dannyman939 | 2:7301e63832ee | 499 | } |
dannyman939 | 1:cbb9104d826f | 500 | msgLen = msgHeader[i]->headerLength + msgHeader[i]->messageLength + sizeof(unsigned long); |
dannyman939 | 1:cbb9104d826f | 501 | // Find last byte position of message |
dannyman939 | 1:cbb9104d826f | 502 | msgEnd = messageLocation[i] + msgLen; |
dannyman939 | 1:cbb9104d826f | 503 | if (byteCounter < msgEnd) |
dannyman939 | 1:cbb9104d826f | 504 | { |
dannyman939 | 1:cbb9104d826f | 505 | // Complete message has not been received. |
dannyman939 | 1:cbb9104d826f | 506 | // Clear processed messages and break out of message processing loop |
dannyman939 | 1:cbb9104d826f | 507 | for (int j = 0; j < i; j++) |
dannyman939 | 1:cbb9104d826f | 508 | { |
dannyman939 | 1:cbb9104d826f | 509 | messageLocation[j] = messageLocation[i+j]; |
dannyman939 | 1:cbb9104d826f | 510 | perSecMessageCounter--; |
dannyman939 | 1:cbb9104d826f | 511 | } |
dannyman939 | 1:cbb9104d826f | 512 | break; |
dannyman939 | 1:cbb9104d826f | 513 | } |
dannyman939 | 1:cbb9104d826f | 514 | else if ((i < (perSecMessageCounter-1)) && |
dannyman939 | 2:7301e63832ee | 515 | (messageLocation[i+i] < msgEnd)) // ignore CRC for now |
dannyman939 | 1:cbb9104d826f | 516 | { |
dannyman939 | 1:cbb9104d826f | 517 | // Next message was started before this mesage was completely received. |
dannyman939 | 1:cbb9104d826f | 518 | // Ignore and continue on to the next message |
dannyman939 | 1:cbb9104d826f | 519 | continue; |
dannyman939 | 1:cbb9104d826f | 520 | } |
dannyman939 | 2:7301e63832ee | 521 | if (logMsgInfo) |
dannyman939 | 2:7301e63832ee | 522 | { |
dannyman939 | 2:7301e63832ee | 523 | toPC.printf("WMsg MESSAGEINFO %5d %5d %5d %5d %5d = %5d (%5d)\n", |
dannyman939 | 2:7301e63832ee | 524 | msgHeader[i]->messageID, |
dannyman939 | 2:7301e63832ee | 525 | messageLocation[i], |
dannyman939 | 2:7301e63832ee | 526 | msgHeader[i]->headerLength, |
dannyman939 | 2:7301e63832ee | 527 | msgHeader[i]->messageLength, |
dannyman939 | 2:7301e63832ee | 528 | sizeof(unsigned long), |
dannyman939 | 2:7301e63832ee | 529 | msgEnd, |
dannyman939 | 2:7301e63832ee | 530 | byteCounter); |
dannyman939 | 2:7301e63832ee | 531 | } |
dannyman939 | 0:c746ee34feae | 532 | //toPC.printf(" %5d ", msgHeader[i]->messageID); |
dannyman939 | 0:c746ee34feae | 533 | if ((msgHeader[i]->messageID == 42) || |
dannyman939 | 0:c746ee34feae | 534 | (msgHeader[i]->messageID == 99)) |
dannyman939 | 0:c746ee34feae | 535 | { |
dannyman939 | 0:c746ee34feae | 536 | if (msgHeader[i]->messageID == 42) |
dannyman939 | 0:c746ee34feae | 537 | { |
dannyman939 | 0:c746ee34feae | 538 | // Wait until velocity message has also been received before using as |
dannyman939 | 0:c746ee34feae | 539 | // base position |
dannyman939 | 1:cbb9104d826f | 540 | //memcpy(&curPos, &msgBuffer[messageLocation[i]], sizeof(OEM615BESTPOS)); |
dannyman939 | 1:cbb9104d826f | 541 | curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[i]]); |
dannyman939 | 0:c746ee34feae | 542 | if (streamPos) |
dannyman939 | 0:c746ee34feae | 543 | { |
dannyman939 | 0:c746ee34feae | 544 | 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 | 545 | curPos.msgHeader.GPSTime_msecs, |
dannyman939 | 0:c746ee34feae | 546 | curPos.solStatus, |
dannyman939 | 0:c746ee34feae | 547 | curPos.posType, |
dannyman939 | 0:c746ee34feae | 548 | curPos.latitude, |
dannyman939 | 0:c746ee34feae | 549 | curPos.longitude, |
dannyman939 | 0:c746ee34feae | 550 | curPos.height, |
dannyman939 | 0:c746ee34feae | 551 | curPos.undulation, |
dannyman939 | 0:c746ee34feae | 552 | curPos.latitudeSTD, |
dannyman939 | 0:c746ee34feae | 553 | curPos.longitudeSTD, |
dannyman939 | 0:c746ee34feae | 554 | curPos.heightSTD, |
dannyman939 | 0:c746ee34feae | 555 | curPos.diffAge, |
dannyman939 | 0:c746ee34feae | 556 | curPos.solutionAge, |
dannyman939 | 0:c746ee34feae | 557 | curPos.numSV, |
dannyman939 | 0:c746ee34feae | 558 | curPos.numSolSV, |
dannyman939 | 0:c746ee34feae | 559 | curPos.numGGL1, |
dannyman939 | 0:c746ee34feae | 560 | curPos.extSolStatus, |
dannyman939 | 0:c746ee34feae | 561 | curPos.sigMask); |
dannyman939 | 0:c746ee34feae | 562 | } |
dannyman939 | 0:c746ee34feae | 563 | } |
dannyman939 | 0:c746ee34feae | 564 | else if (msgHeader[i]->messageID == 99) |
dannyman939 | 0:c746ee34feae | 565 | { |
dannyman939 | 0:c746ee34feae | 566 | // Wait until position message has also been received before using as |
dannyman939 | 0:c746ee34feae | 567 | // base position |
dannyman939 | 1:cbb9104d826f | 568 | //memcpy(&curVel, &msgBuffer[messageLocation[i]], sizeof(OEM615BESTVEL)); |
dannyman939 | 1:cbb9104d826f | 569 | curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]]); |
dannyman939 | 0:c746ee34feae | 570 | } |
dannyman939 | 0:c746ee34feae | 571 | if ((curVel.msgHeader.GPSTime_msecs+250)/1000 == |
dannyman939 | 0:c746ee34feae | 572 | (curPos.msgHeader.GPSTime_msecs+250)/1000) |
dannyman939 | 0:c746ee34feae | 573 | { |
dannyman939 | 0:c746ee34feae | 574 | // update position and velocity used for calculation |
dannyman939 | 0:c746ee34feae | 575 | GPSTimemsecs = curPos.msgHeader.GPSTime_msecs; |
dannyman939 | 0:c746ee34feae | 576 | GPSTime = (double)GPSTimemsecs/1000.0; |
dannyman939 | 0:c746ee34feae | 577 | velMsg = curVel; |
dannyman939 | 0:c746ee34feae | 578 | posMsg = curPos; |
dannyman939 | 0:c746ee34feae | 579 | PPSTimeOffset = 0; |
dannyman939 | 0:c746ee34feae | 580 | } |
dannyman939 | 0:c746ee34feae | 581 | } |
dannyman939 | 2:7301e63832ee | 582 | if (i == (perSecMessageCounter-1)) |
dannyman939 | 2:7301e63832ee | 583 | { |
dannyman939 | 2:7301e63832ee | 584 | if (recordData && (fpNav != NULL)) |
dannyman939 | 2:7301e63832ee | 585 | { |
dannyman939 | 2:7301e63832ee | 586 | fwrite(msgBuffer, byteCounter, 1, fpNav); |
dannyman939 | 2:7301e63832ee | 587 | } |
dannyman939 | 2:7301e63832ee | 588 | byteCounter = 0; |
dannyman939 | 2:7301e63832ee | 589 | perSecMessageCounter = 0; |
dannyman939 | 2:7301e63832ee | 590 | } |
dannyman939 | 0:c746ee34feae | 591 | } |
dannyman939 | 0:c746ee34feae | 592 | //toPC.printf(" %3d %8d \n", msgHeader[0]->timeStatus, GPSTimemsecs); |
dannyman939 | 0:c746ee34feae | 593 | //if (recordData && (fpGPS != NULL)) |
dannyman939 | 2:7301e63832ee | 594 | |
dannyman939 | 2:7301e63832ee | 595 | /* |
dannyman939 | 0:c746ee34feae | 596 | if (recordData && (fpNav != NULL)) |
dannyman939 | 0:c746ee34feae | 597 | { |
dannyman939 | 0:c746ee34feae | 598 | fwrite(msgBuffer, byteCounter, 1, fpNav); |
dannyman939 | 0:c746ee34feae | 599 | } |
dannyman939 | 2:7301e63832ee | 600 | */ |
dannyman939 | 2:7301e63832ee | 601 | //lookingForMessages = false; |
dannyman939 | 0:c746ee34feae | 602 | } |
dannyman939 | 0:c746ee34feae | 603 | if (messageDetected) |
dannyman939 | 0:c746ee34feae | 604 | { |
dannyman939 | 0:c746ee34feae | 605 | //toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us()); |
dannyman939 | 0:c746ee34feae | 606 | messageDetected = false; |
dannyman939 | 0:c746ee34feae | 607 | } |
dannyman939 | 0:c746ee34feae | 608 | if (camera1EventDetected) |
dannyman939 | 0:c746ee34feae | 609 | { |
dannyman939 | 2:7301e63832ee | 610 | toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time); |
dannyman939 | 0:c746ee34feae | 611 | camera1EventDetected = false; |
dannyman939 | 0:c746ee34feae | 612 | } |
dannyman939 | 0:c746ee34feae | 613 | if (detectedGPS1PPS) |
dannyman939 | 0:c746ee34feae | 614 | { |
dannyman939 | 0:c746ee34feae | 615 | //toPC.printf(" PPSCounter = %4d byteCounter = %10d num Messages Received = %3d IMUClock = %4d\n", |
dannyman939 | 0:c746ee34feae | 616 | // PPSCounter, byteCounter, perSecMessageCounter, savedIMUClockCounter); |
dannyman939 | 2:7301e63832ee | 617 | if (recordData && (fpNav != NULL) && (byteCounter > 0)) |
dannyman939 | 2:7301e63832ee | 618 | { |
dannyman939 | 2:7301e63832ee | 619 | fwrite(msgBuffer, byteCounter, 1, fpNav); |
dannyman939 | 2:7301e63832ee | 620 | } |
dannyman939 | 0:c746ee34feae | 621 | byteCounter = 0; |
dannyman939 | 0:c746ee34feae | 622 | perSecMessageCounter=0; |
dannyman939 | 0:c746ee34feae | 623 | detectedGPS1PPS = false; |
dannyman939 | 0:c746ee34feae | 624 | } |
dannyman939 | 0:c746ee34feae | 625 | } |
dannyman939 | 0:c746ee34feae | 626 | toPC.printf(" normal termination \n"); |
dannyman939 | 0:c746ee34feae | 627 | } |