Changes to allow hardware camera trigger
Fork of GPS_Incremental by
main.cpp@4:68268737ff89, 2013-03-29 (annotated)
- Committer:
- jekain314
- Date:
- Fri Mar 29 20:52:24 2013 +0000
- Revision:
- 4:68268737ff89
- Parent:
- 3:5913df46f94a
- Child:
- 6:2a8486283198
from Jim's site - March 29th,2013
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 | |
jekain314 | 4:68268737ff89 | 14 | //these are defines for the messages that are sent from the PC across the USB |
jekain314 | 4:68268737ff89 | 15 | //these messages produce reactions on the mbed |
dannyman939 | 0:c746ee34feae | 16 | #define STATUS_MSG 0 |
dannyman939 | 0:c746ee34feae | 17 | #define POSVEL_MSG 1 |
dannyman939 | 0:c746ee34feae | 18 | #define STARTDATA_MSG 2 |
dannyman939 | 0:c746ee34feae | 19 | #define STOPDATA_MSG 3 |
dannyman939 | 0:c746ee34feae | 20 | #define STARTSTREAM_MSG 4 |
dannyman939 | 0:c746ee34feae | 21 | #define STOPSTREAM_MSG 5 |
dannyman939 | 2:7301e63832ee | 22 | #define STARTLOGINFO_MSG 6 |
dannyman939 | 2:7301e63832ee | 23 | #define STOPLOGINFO_MSG 7 |
jekain314 | 4:68268737ff89 | 24 | |
dannyman939 | 0:c746ee34feae | 25 | #define DEGREES_TO_RADIANS (3.14519/180.0) |
dannyman939 | 0:c746ee34feae | 26 | |
jekain314 | 4:68268737ff89 | 27 | //general digital I/O specifications for this application |
dannyman939 | 0:c746ee34feae | 28 | //SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name); |
dannyman939 | 0:c746ee34feae | 29 | SDFileSystem sd(p11,p12,p13,p14,"sd"); |
dannyman939 | 0:c746ee34feae | 30 | //Serial debug(USBTX, USBRX); // tx, rx USB communication to the PC for debug purposes |
jekain314 | 4:68268737ff89 | 31 | DigitalOut ppsled(LED1); //blink an LED at the 1PPS |
jekain314 | 4:68268737ff89 | 32 | DigitalOut trig1led(LED2); //blink an LED at the camera trigger detection |
jekain314 | 4:68268737ff89 | 33 | DigitalOut recordDataled(LED4); //set the led when the record is on |
jekain314 | 4:68268737ff89 | 34 | InterruptIn camera1Int(p30); // camera interrupt in |
jekain314 | 4:68268737ff89 | 35 | DigitalOut camera2Pin(p29); // i dont believe we use the second camera interrupt |
dannyman939 | 0:c746ee34feae | 36 | //USB serial data stream back to the PC |
dannyman939 | 0:c746ee34feae | 37 | Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10 |
dannyman939 | 0:c746ee34feae | 38 | |
jekain314 | 4:68268737ff89 | 39 | bool detectedGPS1PPS = false; //flag set in the ISR and reset after processing the 1PPS event |
jekain314 | 4:68268737ff89 | 40 | bool recordData = false; //set to true when commanded from the PC |
jekain314 | 4:68268737ff89 | 41 | int PPSCounter = 0; //counts the 1PPS occurrences |
jekain314 | 4:68268737ff89 | 42 | int byteCounter = 0; //byte counter -- where used?? |
jekain314 | 4:68268737ff89 | 43 | unsigned short perSecMessageCounter=0; //counts the number of messages in a sec based on the header detection |
jekain314 | 4:68268737ff89 | 44 | bool lookingForMessages = true; //set in the PPS ISR and set false after the message processing in the main |
jekain314 | 4:68268737ff89 | 45 | bool messageDetected = false; //have detected a message header |
jekain314 | 4:68268737ff89 | 46 | int savedIMUClockCounter=0; |
jekain314 | 4:68268737ff89 | 47 | unsigned long IMUbytesWritten = 0; |
jekain314 | 4:68268737ff89 | 48 | int savedByteCounter = 0; |
jekain314 | 4:68268737ff89 | 49 | int savedPerSecMessageCounter=0; |
dannyman939 | 0:c746ee34feae | 50 | int IMUClockCounter = 0; |
dannyman939 | 0:c746ee34feae | 51 | bool camera1EventDetected = false; |
dannyman939 | 0:c746ee34feae | 52 | double camera1Time; |
dannyman939 | 2:7301e63832ee | 53 | char serBuf[128]; |
dannyman939 | 0:c746ee34feae | 54 | int serBufChars=0; |
jekain314 | 4:68268737ff89 | 55 | |
jekain314 | 4:68268737ff89 | 56 | //flags to control the PC command actions |
dannyman939 | 0:c746ee34feae | 57 | bool sendPosVel=false; |
dannyman939 | 0:c746ee34feae | 58 | bool sendStatus=false; |
dannyman939 | 0:c746ee34feae | 59 | bool sendRecData=false; |
dannyman939 | 0:c746ee34feae | 60 | bool streamPos=false; |
dannyman939 | 0:c746ee34feae | 61 | bool sendStreamPos=false; |
dannyman939 | 2:7301e63832ee | 62 | bool logMsgInfo=false; |
dannyman939 | 2:7301e63832ee | 63 | bool sendLogMsgInfo=false; |
dannyman939 | 0:c746ee34feae | 64 | |
dannyman939 | 0:c746ee34feae | 65 | //ISR for detection of the GPS 1PPS |
dannyman939 | 0:c746ee34feae | 66 | void detect1PPSISR(void) |
dannyman939 | 0:c746ee34feae | 67 | { |
jekain314 | 4:68268737ff89 | 68 | timeFromPPS.reset(); //reset the 1PPS timer upon 1PPS detection |
jekain314 | 4:68268737ff89 | 69 | savedIMUClockCounter = IMUClockCounter; //number of IMU clocks received since last 1PPS |
jekain314 | 4:68268737ff89 | 70 | savedByteCounter = byteCounter; |
jekain314 | 4:68268737ff89 | 71 | savedPerSecMessageCounter = perSecMessageCounter; |
jekain314 | 4:68268737ff89 | 72 | IMUClockCounter = 0; //counts number of IMU samples between 1PPS events |
jekain314 | 4:68268737ff89 | 73 | GPS_COM1.rxBufferFlush(); //flush the GPS serial buffer -- likely not needed but OK |
jekain314 | 4:68268737ff89 | 74 | byteCounter = 0; |
jekain314 | 4:68268737ff89 | 75 | perSecMessageCounter = 0; |
dannyman939 | 0:c746ee34feae | 76 | |
jekain314 | 4:68268737ff89 | 77 | detectedGPS1PPS = true; //reset in the main when 1PPS actions are complete |
jekain314 | 4:68268737ff89 | 78 | lookingForMessages = true; //means we should begin looking for new GPS messages |
jekain314 | 4:68268737ff89 | 79 | PPSCounter++; //count number of 1PPS epochs |
jekain314 | 4:68268737ff89 | 80 | PPSTimeOffset++; //counts the 1PPS events between occurrences when we have matching POS and VEL messages |
jekain314 | 4:68268737ff89 | 81 | ppsled = !ppsled; //blink an LED at the 1PPS |
dannyman939 | 0:c746ee34feae | 82 | }; |
dannyman939 | 0:c746ee34feae | 83 | |
dannyman939 | 0:c746ee34feae | 84 | //ISR for detection of the hotshoe trigger 1 |
dannyman939 | 1:cbb9104d826f | 85 | void camera1ISR(void) |
dannyman939 | 0:c746ee34feae | 86 | { |
jekain314 | 4:68268737ff89 | 87 | //PPSTimeOffset keeps track of missed |
dannyman939 | 0:c746ee34feae | 88 | camera1Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read(); |
jekain314 | 4:68268737ff89 | 89 | |
jekain314 | 4:68268737ff89 | 90 | trig1led = !trig1led; //blink an LEWD at the camera event detection |
jekain314 | 4:68268737ff89 | 91 | camera1EventDetected = true; //reset to false in main after processing the image detection |
dannyman939 | 0:c746ee34feae | 92 | }; |
dannyman939 | 0:c746ee34feae | 93 | |
dannyman939 | 0:c746ee34feae | 94 | void readFromPC() |
dannyman939 | 0:c746ee34feae | 95 | { |
jekain314 | 4:68268737ff89 | 96 | |
jekain314 | 4:68268737ff89 | 97 | //better solution |
jekain314 | 4:68268737ff89 | 98 | //start a timer when we get a char from the PC and set a bool: detectingPCMessage = true |
jekain314 | 4:68268737ff89 | 99 | //keep reading bytes until elapsed time from the first byte is : 0.01 secs of numByes > 16 |
jekain314 | 4:68268737ff89 | 100 | //the messages are from 10 to 16 bytes -- this will take from 10/921600 to 16/921600 secs at 921600 baud |
jekain314 | 4:68268737ff89 | 101 | //8*115200 = 921600 |
jekain314 | 4:68268737ff89 | 102 | //this is about 2e-5 or 20usec. |
jekain314 | 4:68268737ff89 | 103 | //so we could wait 50usec from the first char reeived and be certain we had all the chars for a message. |
jekain314 | 4:68268737ff89 | 104 | //the steps are below .... |
jekain314 | 4:68268737ff89 | 105 | //(1) convert this procedures to a ISR per byte |
jekain314 | 4:68268737ff89 | 106 | //(2) in the ISR, start the timer on the first byte received when !detectingPCMessage |
jekain314 | 4:68268737ff89 | 107 | //(3) set detectingPCMessage = true |
jekain314 | 4:68268737ff89 | 108 | //(4) in the main loop, test the timer for >50usec and parse the bytes to test fr a message |
jekain314 | 4:68268737ff89 | 109 | //(5) reset the timer to zero in the main loop after parsing the message |
jekain314 | 4:68268737ff89 | 110 | //(6) set detectingPCMessage = false; |
jekain314 | 4:68268737ff89 | 111 | |
jekain314 | 4:68268737ff89 | 112 | //The received commands obnly occur at the initialization stage -- time is not that critical there. |
jekain314 | 4:68268737ff89 | 113 | //during the real-time action, we will never pass the followong if test ( no characters received) |
dannyman939 | 0:c746ee34feae | 114 | if (toPC.readable()) //read a PC serial byte and test it for a command |
dannyman939 | 0:c746ee34feae | 115 | { |
dannyman939 | 0:c746ee34feae | 116 | // Read in next character |
dannyman939 | 0:c746ee34feae | 117 | char inChar = toPC.getc(); |
dannyman939 | 0:c746ee34feae | 118 | serBuf[serBufChars++] = inChar; |
jekain314 | 4:68268737ff89 | 119 | |
dannyman939 | 0:c746ee34feae | 120 | // Append end of string character |
jekain314 | 4:68268737ff89 | 121 | //why do this for every character we read? |
jekain314 | 4:68268737ff89 | 122 | //answer: we always assume we have a complete message and test for this below |
dannyman939 | 0:c746ee34feae | 123 | serBuf[serBufChars] = '\0'; |
jekain314 | 4:68268737ff89 | 124 | |
dannyman939 | 0:c746ee34feae | 125 | // Need to parse message to determine behavior |
dannyman939 | 0:c746ee34feae | 126 | // Need to clean this up |
jekain314 | 4:68268737ff89 | 127 | //need to do this outside the read byte --- |
dannyman939 | 2:7301e63832ee | 128 | char msgList[8][32]; |
dannyman939 | 0:c746ee34feae | 129 | sprintf(msgList[STATUS_MSG], "WMsg STATUS"); |
dannyman939 | 0:c746ee34feae | 130 | sprintf(msgList[POSVEL_MSG], "WMsg POSVEL"); |
dannyman939 | 0:c746ee34feae | 131 | sprintf(msgList[STARTDATA_MSG], "WMsg RECORDDATA Y"); |
dannyman939 | 0:c746ee34feae | 132 | sprintf(msgList[STOPDATA_MSG], "WMsg RECORDDATA N"); |
dannyman939 | 0:c746ee34feae | 133 | sprintf(msgList[STARTSTREAM_MSG], "WMsg POSSTREAM Y"); |
dannyman939 | 0:c746ee34feae | 134 | sprintf(msgList[STOPSTREAM_MSG], "WMsg POSSTREAM N"); |
dannyman939 | 2:7301e63832ee | 135 | sprintf(msgList[STARTLOGINFO_MSG], "WMsg LOGINFO Y"); |
dannyman939 | 2:7301e63832ee | 136 | sprintf(msgList[STOPLOGINFO_MSG], "WMsg LOGINFO N"); |
jekain314 | 4:68268737ff89 | 137 | //message length is from 10 to 16 chars |
jekain314 | 4:68268737ff89 | 138 | |
dannyman939 | 0:c746ee34feae | 139 | // assume an invalid message which needs to be reset |
dannyman939 | 0:c746ee34feae | 140 | bool validMessage = false; |
dannyman939 | 0:c746ee34feae | 141 | bool resetMessage = true; |
jekain314 | 4:68268737ff89 | 142 | |
dannyman939 | 0:c746ee34feae | 143 | // Check for valid message |
jekain314 | 4:68268737ff89 | 144 | for (int m = 0; m < 8 && !validMessage; m++) //check for all messages ... |
dannyman939 | 0:c746ee34feae | 145 | { |
dannyman939 | 0:c746ee34feae | 146 | if (strncmp(serBuf, msgList[m], serBufChars) == 0) |
dannyman939 | 0:c746ee34feae | 147 | { |
dannyman939 | 0:c746ee34feae | 148 | validMessage = true; |
jekain314 | 4:68268737ff89 | 149 | //test that chars in the serial buffer is same as message length |
dannyman939 | 0:c746ee34feae | 150 | if (serBufChars == strlen(msgList[m])) |
dannyman939 | 0:c746ee34feae | 151 | { |
dannyman939 | 0:c746ee34feae | 152 | switch(m) |
dannyman939 | 0:c746ee34feae | 153 | { |
dannyman939 | 0:c746ee34feae | 154 | case STATUS_MSG: |
dannyman939 | 0:c746ee34feae | 155 | sendStatus = true; |
dannyman939 | 0:c746ee34feae | 156 | break; |
dannyman939 | 0:c746ee34feae | 157 | case POSVEL_MSG: |
dannyman939 | 0:c746ee34feae | 158 | sendPosVel = true; |
dannyman939 | 0:c746ee34feae | 159 | break; |
dannyman939 | 0:c746ee34feae | 160 | case STARTDATA_MSG: |
dannyman939 | 0:c746ee34feae | 161 | case STOPDATA_MSG: |
dannyman939 | 0:c746ee34feae | 162 | recordData = (m == STARTDATA_MSG); |
dannyman939 | 0:c746ee34feae | 163 | sendRecData = true; |
dannyman939 | 0:c746ee34feae | 164 | break; |
dannyman939 | 0:c746ee34feae | 165 | case STARTSTREAM_MSG: |
dannyman939 | 0:c746ee34feae | 166 | case STOPSTREAM_MSG: |
dannyman939 | 0:c746ee34feae | 167 | streamPos = (m == STARTSTREAM_MSG); |
dannyman939 | 0:c746ee34feae | 168 | sendStreamPos = true; |
dannyman939 | 0:c746ee34feae | 169 | break; |
dannyman939 | 2:7301e63832ee | 170 | case STARTLOGINFO_MSG: |
dannyman939 | 2:7301e63832ee | 171 | case STOPLOGINFO_MSG: |
dannyman939 | 2:7301e63832ee | 172 | logMsgInfo = (m == STARTLOGINFO_MSG); |
dannyman939 | 2:7301e63832ee | 173 | sendLogMsgInfo = true; |
dannyman939 | 2:7301e63832ee | 174 | break; |
dannyman939 | 0:c746ee34feae | 175 | |
dannyman939 | 0:c746ee34feae | 176 | } |
dannyman939 | 0:c746ee34feae | 177 | } |
dannyman939 | 0:c746ee34feae | 178 | // message is still in progress, do not reset |
dannyman939 | 0:c746ee34feae | 179 | else |
dannyman939 | 0:c746ee34feae | 180 | { |
dannyman939 | 0:c746ee34feae | 181 | resetMessage = false; |
dannyman939 | 0:c746ee34feae | 182 | } |
dannyman939 | 0:c746ee34feae | 183 | } |
dannyman939 | 0:c746ee34feae | 184 | } |
jekain314 | 4:68268737ff89 | 185 | |
dannyman939 | 0:c746ee34feae | 186 | // if message should be reset |
dannyman939 | 0:c746ee34feae | 187 | if (resetMessage) |
dannyman939 | 0:c746ee34feae | 188 | { |
dannyman939 | 0:c746ee34feae | 189 | // reset serial buffer character count |
dannyman939 | 0:c746ee34feae | 190 | serBufChars = 0; |
dannyman939 | 0:c746ee34feae | 191 | // if invalid message and most recent character is 'W' (first message character), |
dannyman939 | 0:c746ee34feae | 192 | // possible message collision |
dannyman939 | 0:c746ee34feae | 193 | if ((!validMessage) && (inChar == 'W')) |
dannyman939 | 0:c746ee34feae | 194 | { |
dannyman939 | 0:c746ee34feae | 195 | // start a new message |
dannyman939 | 0:c746ee34feae | 196 | serBuf[serBufChars++] = inChar; |
jekain314 | 4:68268737ff89 | 197 | serBuf[serBufChars] = '\0'; |
dannyman939 | 0:c746ee34feae | 198 | } |
dannyman939 | 0:c746ee34feae | 199 | // Append end of string character |
dannyman939 | 0:c746ee34feae | 200 | serBuf[serBufChars] = '\0'; |
dannyman939 | 0:c746ee34feae | 201 | } |
dannyman939 | 0:c746ee34feae | 202 | } |
dannyman939 | 0:c746ee34feae | 203 | }; |
dannyman939 | 0:c746ee34feae | 204 | |
dannyman939 | 0:c746ee34feae | 205 | void sendASCII(char* ASCI_message, int numChars) |
dannyman939 | 0:c746ee34feae | 206 | { |
jekain314 | 4:68268737ff89 | 207 | ///////////////////////////////////////////////// |
jekain314 | 4:68268737ff89 | 208 | //send an ASCII command to the GPS receiver |
jekain314 | 4:68268737ff89 | 209 | ///////////////////////////////////////////////// |
jekain314 | 4:68268737ff89 | 210 | |
dannyman939 | 0:c746ee34feae | 211 | //char ASCI_message[] = "unlogall COM1"; |
dannyman939 | 0:c746ee34feae | 212 | int as = numChars - 1; |
dannyman939 | 0:c746ee34feae | 213 | unsigned char CR = 0x0d; //ASCII Carriage Return |
dannyman939 | 0:c746ee34feae | 214 | unsigned char LF = 0x0a; //ASCII Line Feed |
dannyman939 | 0:c746ee34feae | 215 | |
dannyman939 | 0:c746ee34feae | 216 | //printf("%s", ch); |
dannyman939 | 0:c746ee34feae | 217 | //printf("\n"); |
dannyman939 | 0:c746ee34feae | 218 | |
dannyman939 | 0:c746ee34feae | 219 | for (int i=0; i<as; i++) GPS_COM1.putc(ASCI_message[i]); |
dannyman939 | 0:c746ee34feae | 220 | GPS_COM1.putc(CR); //carriage return at end |
dannyman939 | 0:c746ee34feae | 221 | GPS_COM1.putc(LF); //line feed at end |
dannyman939 | 3:5913df46f94a | 222 | }; |
dannyman939 | 0:c746ee34feae | 223 | |
dannyman939 | 0:c746ee34feae | 224 | //FILE* fp = NULL; |
dannyman939 | 0:c746ee34feae | 225 | void setupCOM(void) |
dannyman939 | 0:c746ee34feae | 226 | { |
dannyman939 | 0:c746ee34feae | 227 | //system starts with GPS in reset active |
dannyman939 | 0:c746ee34feae | 228 | //dis-engage the reset to get the GPS started |
dannyman939 | 0:c746ee34feae | 229 | GPS_Reset=1; wait_ms(1000); |
dannyman939 | 0:c746ee34feae | 230 | |
dannyman939 | 0:c746ee34feae | 231 | //establish 1PPS ISR |
dannyman939 | 0:c746ee34feae | 232 | PPSInt.rise(&detect1PPSISR); |
dannyman939 | 0:c746ee34feae | 233 | |
dannyman939 | 0:c746ee34feae | 234 | //set the USB serial data rate -- rate must be matched at the PC end |
dannyman939 | 0:c746ee34feae | 235 | //This the serial communication back to the the PC host |
dannyman939 | 0:c746ee34feae | 236 | //Launch the C++ serial port read program there to catch the ASCII characters |
dannyman939 | 0:c746ee34feae | 237 | //toPC.baud(9600); wait_ms(100); |
dannyman939 | 0:c746ee34feae | 238 | toPC.baud(8*115200); wait_ms(100); |
dannyman939 | 2:7301e63832ee | 239 | //toPC.baud(1*115200); wait_ms(100); |
dannyman939 | 0:c746ee34feae | 240 | toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n"); |
dannyman939 | 0:c746ee34feae | 241 | |
jekain314 | 4:68268737ff89 | 242 | //just wait to launch the GPS receiver |
dannyman939 | 0:c746ee34feae | 243 | for (int i=0; i<5; i++) { toPC.printf(" to start: %3d \n", 4-i); wait(1); } |
dannyman939 | 0:c746ee34feae | 244 | |
dannyman939 | 0:c746ee34feae | 245 | |
dannyman939 | 0:c746ee34feae | 246 | mkdir("/sd/Data", 0777); |
dannyman939 | 0:c746ee34feae | 247 | |
dannyman939 | 0:c746ee34feae | 248 | /* |
dannyman939 | 0:c746ee34feae | 249 | fp = fopen("/sd/Data/IMUGPS.bin", "wb"); |
dannyman939 | 0:c746ee34feae | 250 | if (fp == NULL) |
dannyman939 | 0:c746ee34feae | 251 | { |
dannyman939 | 0:c746ee34feae | 252 | toPC.printf(" cannot open the IMUGPS data file \n"); |
dannyman939 | 0:c746ee34feae | 253 | } |
dannyman939 | 0:c746ee34feae | 254 | else |
dannyman939 | 0:c746ee34feae | 255 | toPC.printf(" opened the IMUGPS data file \n"); |
dannyman939 | 0:c746ee34feae | 256 | */ |
jekain314 | 4:68268737ff89 | 257 | |
jekain314 | 4:68268737ff89 | 258 | //NOTE: we do not assume that the GPS receiver has been pre-set up for the WALDO_FCS functionality |
jekain314 | 4:68268737ff89 | 259 | //we alwsys start with a reset and reprogram the receiver with our data out products |
jekain314 | 4:68268737ff89 | 260 | // this prevents failure because of a blown NVRAM as occurred for the older camera systems |
jekain314 | 4:68268737ff89 | 261 | |
dannyman939 | 0:c746ee34feae | 262 | //this is the COM1 port from th GPS receiuver to the mbed |
dannyman939 | 0:c746ee34feae | 263 | //it should be always started at 9600 baud because thats the default for the GPS receiver |
dannyman939 | 0:c746ee34feae | 264 | GPS_COM1.baud(9600); wait_ms(100); |
dannyman939 | 0:c746ee34feae | 265 | |
dannyman939 | 0:c746ee34feae | 266 | // this ASCII command sets up the serial data from the GPS receiver on its COM1 |
dannyman939 | 0:c746ee34feae | 267 | char ch7[] = "serialconfig COM1 9600 n 8 1 n off"; |
jekain314 | 4:68268737ff89 | 268 | // this is a software reset and has the same effect as a hardware reset (why do it?) |
jekain314 | 4:68268737ff89 | 269 | //char ch0[] = "RESET"; |
dannyman939 | 0:c746ee34feae | 270 | //this command stops all communication from the GPS receiver on COM1 |
jekain314 | 4:68268737ff89 | 271 | //logs should still be presented on USB port so the Novatel CDU application can be used on the PC in parallel |
dannyman939 | 0:c746ee34feae | 272 | char ch1[] = "unlogall COM1"; |
dannyman939 | 0:c746ee34feae | 273 | //set the final baud rate that we will use from here |
dannyman939 | 0:c746ee34feae | 274 | //allowable baud rate values: 9600 115200 230400 460800 921600 |
jekain314 | 4:68268737ff89 | 275 | //char ch2[] = "serialconfig COM1 921600 n 8 1 n off"; |
jekain314 | 4:68268737ff89 | 276 | char ch2[] = "serialconfig COM1 460800 n 8 1 n off"; |
dannyman939 | 0:c746ee34feae | 277 | |
dannyman939 | 0:c746ee34feae | 278 | //the below commands request the POS, VEL, RANGE, and TIME messages |
dannyman939 | 0:c746ee34feae | 279 | char ch3[] = "log COM1 BESTPOSB ONTIME 1"; //messageID = 42 |
dannyman939 | 0:c746ee34feae | 280 | char ch4[] = "log COM1 BESTVelB ONTIME 1"; //messageID = 99 |
dannyman939 | 0:c746ee34feae | 281 | char ch5[] = "log COM1 RANGEB ONTIME 1"; //messageID = 43 |
jekain314 | 4:68268737ff89 | 282 | //char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101 |
dannyman939 | 0:c746ee34feae | 283 | |
dannyman939 | 0:c746ee34feae | 284 | //set up VARF to be 100Hz with 1X10^4 * 10^-8 = 10^-4 sec (10usec) pulse width |
jekain314 | 4:68268737ff89 | 285 | //in fact, we do not use this output but it is available. |
jekain314 | 4:68268737ff89 | 286 | //originally planned to use this to command the IMU data |
dannyman939 | 0:c746ee34feae | 287 | char ch8[] = "FREQUENCYOUT enable 10000 1000000"; |
dannyman939 | 0:c746ee34feae | 288 | |
dannyman939 | 0:c746ee34feae | 289 | toPC.printf("set serial config \n"); |
dannyman939 | 0:c746ee34feae | 290 | sendASCII(ch7, sizeof(ch7)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 291 | //sendASCII(ch0, sizeof(ch0)); |
dannyman939 | 0:c746ee34feae | 292 | toPC.printf("unlog all messages \n"); |
dannyman939 | 0:c746ee34feae | 293 | sendASCII(ch1, sizeof(ch1)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 294 | toPC.printf("log BESTPOSB on COM1 \n"); |
dannyman939 | 0:c746ee34feae | 295 | sendASCII(ch3, sizeof(ch3)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 296 | toPC.printf("log BESTVELB on COM1\n"); |
dannyman939 | 0:c746ee34feae | 297 | sendASCII(ch4, sizeof(ch4)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 298 | toPC.printf("log RANGEB on COM1\n"); |
dannyman939 | 0:c746ee34feae | 299 | sendASCII(ch5, sizeof(ch5)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 300 | |
dannyman939 | 0:c746ee34feae | 301 | //toPC.printf("log TIMEB om COM1 \n"); |
dannyman939 | 0:c746ee34feae | 302 | //sendASCII(ch6, sizeof(ch6)); wait_ms(100); |
dannyman939 | 0:c746ee34feae | 303 | |
dannyman939 | 0:c746ee34feae | 304 | toPC.printf(" set up th VARF signal \n"); |
dannyman939 | 0:c746ee34feae | 305 | sendASCII(ch8, sizeof(ch8)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 306 | |
dannyman939 | 0:c746ee34feae | 307 | //set GPS output COM1 to the final high rate |
dannyman939 | 0:c746ee34feae | 308 | toPC.printf("set the COM ports to high rate\n"); |
dannyman939 | 0:c746ee34feae | 309 | sendASCII(ch2, sizeof(ch2)); wait_ms(500); |
dannyman939 | 0:c746ee34feae | 310 | |
dannyman939 | 0:c746ee34feae | 311 | //set the mbed COM port to match the GPS transmit rate |
dannyman939 | 0:c746ee34feae | 312 | //the below baud rate must match the COM1 rate coming from the GPS receiver |
jekain314 | 4:68268737ff89 | 313 | GPS_COM1.baud(460800); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL |
jekain314 | 4:68268737ff89 | 314 | //GPS_COM1.baud(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL |
dannyman939 | 0:c746ee34feae | 315 | }; |
dannyman939 | 0:c746ee34feae | 316 | |
dannyman939 | 0:c746ee34feae | 317 | void setupTriggers() |
dannyman939 | 0:c746ee34feae | 318 | { |
dannyman939 | 1:cbb9104d826f | 319 | camera1Int.mode(PullUp); |
dannyman939 | 1:cbb9104d826f | 320 | camera2Pin = 1; |
dannyman939 | 0:c746ee34feae | 321 | //establish Trigger ISR |
dannyman939 | 1:cbb9104d826f | 322 | camera1Int.rise(&camera1ISR); |
dannyman939 | 0:c746ee34feae | 323 | |
dannyman939 | 3:5913df46f94a | 324 | }; |
dannyman939 | 0:c746ee34feae | 325 | |
dannyman939 | 0:c746ee34feae | 326 | int test = 0; |
dannyman939 | 0:c746ee34feae | 327 | unsigned short messageCounter = 0; |
dannyman939 | 0:c746ee34feae | 328 | unsigned short savedMessageCounter = 0; |
jekain314 | 4:68268737ff89 | 329 | unsigned char msgBuffer[1536]; //array to contain one full second of GPS bytes |
jekain314 | 4:68268737ff89 | 330 | unsigned short messageLocation[6] = {0}; //stores the message location start within the message buffer |
dannyman939 | 0:c746ee34feae | 331 | |
dannyman939 | 0:c746ee34feae | 332 | //see the mbed COOKBOOK for MODSERIAL |
dannyman939 | 0:c746ee34feae | 333 | //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output. |
dannyman939 | 0:c746ee34feae | 334 | void readSerialByte(MODSERIAL_IRQ_INFO *q) |
dannyman939 | 0:c746ee34feae | 335 | { |
dannyman939 | 0:c746ee34feae | 336 | MODSERIAL *serial = q->serial; |
dannyman939 | 0:c746ee34feae | 337 | unsigned char synch0 = serial->getc(); |
dannyman939 | 0:c746ee34feae | 338 | msgBuffer[byteCounter] = synch0; |
dannyman939 | 0:c746ee34feae | 339 | |
dannyman939 | 0:c746ee34feae | 340 | //we need to trap the GPS message header byte-string 0xAA44121C |
dannyman939 | 0:c746ee34feae | 341 | //generate a 4-byte sliding-window sequence from the input bytes |
dannyman939 | 0:c746ee34feae | 342 | //shift last 4-byte value left 8 bits & push recently read byte (synch0) into low-order byte |
dannyman939 | 0:c746ee34feae | 343 | test = (test<<8) | synch0; // |
dannyman939 | 0:c746ee34feae | 344 | |
dannyman939 | 0:c746ee34feae | 345 | if (test == 0xAA44121C) //test for the Receiver message header signature |
dannyman939 | 0:c746ee34feae | 346 | { |
dannyman939 | 1:cbb9104d826f | 347 | messageLocation[perSecMessageCounter] = byteCounter-3; //store the location of this message (with 4 synch words) |
dannyman939 | 0:c746ee34feae | 348 | perSecMessageCounter++; |
dannyman939 | 0:c746ee34feae | 349 | messageDetected = true; |
dannyman939 | 0:c746ee34feae | 350 | } |
jekain314 | 4:68268737ff89 | 351 | //byteCounter reset to zero in main after the 1PPS is detected -- its NOT reset in the 1PPS ISR |
dannyman939 | 0:c746ee34feae | 352 | byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected) |
dannyman939 | 2:7301e63832ee | 353 | |
dannyman939 | 0:c746ee34feae | 354 | }; |
dannyman939 | 0:c746ee34feae | 355 | |
jekain314 | 4:68268737ff89 | 356 | void earthCoefficients(double latitudeRad, double longitudeRad, double height, double &latRateFac, double &lonRateFac) |
jekain314 | 4:68268737ff89 | 357 | { |
jekain314 | 4:68268737ff89 | 358 | //compute the lat and lon factors for use in the interpolation of the lat and lon between 1 sec epochs |
jekain314 | 4:68268737ff89 | 359 | //see this document (page 32) www.fas.org/spp/military/program/nav/basicnav.pdf |
jekain314 | 4:68268737ff89 | 360 | double eccen = 0.0818191908426; //WGS84 earth eccentricity |
jekain314 | 4:68268737ff89 | 361 | double earthRadius = 6378137; //WGS84 earthRadius in meters |
jekain314 | 4:68268737ff89 | 362 | double eccenSinLat = eccen * sin(latitudeRad); |
jekain314 | 4:68268737ff89 | 363 | double temp1 = 1.0 - eccenSinLat*eccenSinLat; |
jekain314 | 4:68268737ff89 | 364 | double temp2 = sqrt(temp1); |
jekain314 | 4:68268737ff89 | 365 | double r_meridian = earthRadius * ( 1.0 - eccen*eccen)/ (temp1 * temp2); |
jekain314 | 4:68268737ff89 | 366 | double r_normal = earthRadius / temp2; |
jekain314 | 4:68268737ff89 | 367 | |
jekain314 | 4:68268737ff89 | 368 | //multiply latRateFac times V-north to get the latitude rate in radians [er sec |
jekain314 | 4:68268737ff89 | 369 | latRateFac = 1.0 / (r_meridian + height); |
jekain314 | 4:68268737ff89 | 370 | |
jekain314 | 4:68268737ff89 | 371 | //multiply lonRatFac by VEast to get the longitude rate in radians per sec |
jekain314 | 4:68268737ff89 | 372 | lonRateFac = 1.0 / ( (r_normal + height) * cos(latitudeRad) ); |
jekain314 | 4:68268737ff89 | 373 | } |
jekain314 | 4:68268737ff89 | 374 | |
dannyman939 | 0:c746ee34feae | 375 | int main() { |
dannyman939 | 0:c746ee34feae | 376 | |
dannyman939 | 0:c746ee34feae | 377 | //FILE *fpIMU = NULL; |
dannyman939 | 0:c746ee34feae | 378 | //FILE *fpGPS = NULL; |
dannyman939 | 0:c746ee34feae | 379 | FILE *fpNav = NULL; |
jekain314 | 4:68268737ff89 | 380 | |
jekain314 | 4:68268737ff89 | 381 | OEM615BESTPOS posMsg; //BESTPOS structure in OEMV615.h |
jekain314 | 4:68268737ff89 | 382 | OEM615BESTPOS curPos; //BESTPOS structure in OEMV615.h |
jekain314 | 4:68268737ff89 | 383 | OEM615BESTVEL velMsg; //BESTVEL structure in OEMV615.h |
jekain314 | 4:68268737ff89 | 384 | OEM615BESTVEL curVel; //BESTVEL structure in OEMV615.h |
jekain314 | 4:68268737ff89 | 385 | |
dannyman939 | 1:cbb9104d826f | 386 | int msgLen; |
dannyman939 | 1:cbb9104d826f | 387 | int msgEnd; |
dannyman939 | 0:c746ee34feae | 388 | |
dannyman939 | 2:7301e63832ee | 389 | //set up the GPS and mbed COM ports |
dannyman939 | 0:c746ee34feae | 390 | setupCOM(); |
dannyman939 | 0:c746ee34feae | 391 | |
dannyman939 | 0:c746ee34feae | 392 | //set up the ADIS16488 |
dannyman939 | 0:c746ee34feae | 393 | setupADIS(); |
dannyman939 | 0:c746ee34feae | 394 | |
dannyman939 | 0:c746ee34feae | 395 | //setup Hotshoe |
dannyman939 | 0:c746ee34feae | 396 | setupTriggers(); |
jekain314 | 4:68268737ff89 | 397 | |
dannyman939 | 0:c746ee34feae | 398 | //attempt to use the mbed RTOS library |
dannyman939 | 0:c746ee34feae | 399 | //Thread thread(writeThread); |
dannyman939 | 0:c746ee34feae | 400 | |
jekain314 | 4:68268737ff89 | 401 | //this doesnt show up on the PC Rich Text Box because we dont start the init til after this occurs |
dannyman939 | 0:c746ee34feae | 402 | toPC.printf("completed setting up COM ports \n"); |
dannyman939 | 0:c746ee34feae | 403 | |
jekain314 | 4:68268737ff89 | 404 | //set up the interrupt to catch the GPS receiver serial bytes as they are presented |
dannyman939 | 0:c746ee34feae | 405 | GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq); |
dannyman939 | 0:c746ee34feae | 406 | |
jekain314 | 4:68268737ff89 | 407 | timeFromPPS.start(); //start the time for measuring time from 1PPS events |
dannyman939 | 0:c746ee34feae | 408 | |
jekain314 | 4:68268737ff89 | 409 | while(PPSCounter < 100) |
jekain314 | 4:68268737ff89 | 410 | /////////////////////////////////////////////////////////////////////////// |
jekain314 | 4:68268737ff89 | 411 | // top of the while loop |
jekain314 | 4:68268737ff89 | 412 | /////////////////////////////////////////////////////////////////////////// |
jekain314 | 4:68268737ff89 | 413 | //while(1) |
dannyman939 | 0:c746ee34feae | 414 | { |
dannyman939 | 0:c746ee34feae | 415 | //read the USB serial data from the PC to check for commands |
jekain314 | 4:68268737ff89 | 416 | //in the primary real-time portion, there are no bytes from the PC so this has no impact |
dannyman939 | 0:c746ee34feae | 417 | readFromPC(); |
dannyman939 | 0:c746ee34feae | 418 | |
jekain314 | 4:68268737ff89 | 419 | //we should put the below stuff into the readPC() procedure. |
jekain314 | 4:68268737ff89 | 420 | //only do these actions in response to a command so no need for the tests w/o an inoput byte from the PC |
jekain314 | 4:68268737ff89 | 421 | //perform the activities as a response to the commands |
jekain314 | 4:68268737ff89 | 422 | if (sendPosVel) //true if we want to return a position solution |
dannyman939 | 0:c746ee34feae | 423 | { |
jekain314 | 4:68268737ff89 | 424 | sendPosVel=false; //set to true if a POSVEL is requested from the PC |
dannyman939 | 0:c746ee34feae | 425 | char solReady = 'N'; |
jekain314 | 4:68268737ff89 | 426 | if (posMsg.solStatus == 0) //how is this set?? |
dannyman939 | 0:c746ee34feae | 427 | { |
dannyman939 | 0:c746ee34feae | 428 | solReady = 'Y'; |
dannyman939 | 0:c746ee34feae | 429 | } |
jekain314 | 4:68268737ff89 | 430 | |
jekain314 | 4:68268737ff89 | 431 | //north and east velocity from the horizontal speed and heading |
jekain314 | 4:68268737ff89 | 432 | //velMsg may not be the "current" message --- but is the one also associated with a position message |
dannyman939 | 0:c746ee34feae | 433 | double nVel = velMsg.horizontalSpeed*cos(velMsg.heading*DEGREES_TO_RADIANS); |
dannyman939 | 0:c746ee34feae | 434 | double eVel = velMsg.horizontalSpeed*sin(velMsg.heading*DEGREES_TO_RADIANS); |
jekain314 | 4:68268737ff89 | 435 | |
dannyman939 | 0:c746ee34feae | 436 | // For the 1 second deltas with which we are dealing |
jekain314 | 4:68268737ff89 | 437 | // This calculation should be close enough for now |
jekain314 | 4:68268737ff89 | 438 | // Approximately 1 nautical mile / minute latitude, 60 minutes/degree, 1852 meters/nautical mile |
dannyman939 | 0:c746ee34feae | 439 | double latMetersPerDeg = 60.0*1852.0; |
dannyman939 | 0:c746ee34feae | 440 | // longitude separation is approximately equal to latitude separation * cosine of latitude |
dannyman939 | 0:c746ee34feae | 441 | double lonMetersPerDeg = latMetersPerDeg*cos(posMsg.latitude*DEGREES_TO_RADIANS); |
dannyman939 | 0:c746ee34feae | 442 | |
dannyman939 | 0:c746ee34feae | 443 | // Elapsed time since last known GPS position |
jekain314 | 4:68268737ff89 | 444 | //PPSTimeOffset is a result of possibly missing a prior GPS position message |
jekain314 | 4:68268737ff89 | 445 | // timeFromPPS.read() is always the time from the moset recent 1PPS |
dannyman939 | 0:c746ee34feae | 446 | double elTime = (double)PPSTimeOffset + timeFromPPS.read(); |
jekain314 | 4:68268737ff89 | 447 | |
jekain314 | 4:68268737ff89 | 448 | // Position time -- GPSTime is the time of the last valid GPS position message |
dannyman939 | 0:c746ee34feae | 449 | double posTime = GPSTime + elTime; |
dannyman939 | 0:c746ee34feae | 450 | |
dannyman939 | 0:c746ee34feae | 451 | // Estimated position based on previous position and velocity |
jekain314 | 4:68268737ff89 | 452 | // posMsg is the last time when the BESTVEL and BESTPOS messages had identical times |
dannyman939 | 0:c746ee34feae | 453 | double latPos = posMsg.latitude + (nVel/latMetersPerDeg)*elTime; |
dannyman939 | 0:c746ee34feae | 454 | double lonPos = posMsg.longitude + (eVel/lonMetersPerDeg)*elTime; |
dannyman939 | 0:c746ee34feae | 455 | double htPos = posMsg.height + velMsg.verticalSpeed/(60*1852)*elTime; |
dannyman939 | 0:c746ee34feae | 456 | toPC.printf("WMsg POSVEL %5.3lf %d %c %8.5lf %9.5lf %4.3lf %4.3lf %4.3lf %4.3lf\n", |
dannyman939 | 0:c746ee34feae | 457 | posTime, |
dannyman939 | 0:c746ee34feae | 458 | posMsg.numSolSV, |
dannyman939 | 0:c746ee34feae | 459 | solReady, |
dannyman939 | 0:c746ee34feae | 460 | latPos, |
dannyman939 | 0:c746ee34feae | 461 | lonPos, |
dannyman939 | 0:c746ee34feae | 462 | htPos, |
dannyman939 | 0:c746ee34feae | 463 | nVel, |
dannyman939 | 0:c746ee34feae | 464 | eVel, |
dannyman939 | 0:c746ee34feae | 465 | velMsg.verticalSpeed |
dannyman939 | 0:c746ee34feae | 466 | ); |
dannyman939 | 0:c746ee34feae | 467 | } |
jekain314 | 4:68268737ff89 | 468 | |
jekain314 | 4:68268737ff89 | 469 | //all this does is assess the GPS convergence -- really available in the above |
jekain314 | 4:68268737ff89 | 470 | if (sendStatus) //send the status message to the PC |
dannyman939 | 0:c746ee34feae | 471 | { |
dannyman939 | 0:c746ee34feae | 472 | sendStatus=false; |
dannyman939 | 0:c746ee34feae | 473 | char solReady = 'N'; |
dannyman939 | 0:c746ee34feae | 474 | if (posMsg.solStatus == 0) |
dannyman939 | 0:c746ee34feae | 475 | { |
dannyman939 | 0:c746ee34feae | 476 | solReady = 'Y'; |
dannyman939 | 0:c746ee34feae | 477 | } |
dannyman939 | 0:c746ee34feae | 478 | toPC.printf("WMsg STATUS %5.3lf %c\n", |
dannyman939 | 0:c746ee34feae | 479 | GPSTime, |
dannyman939 | 0:c746ee34feae | 480 | solReady |
dannyman939 | 0:c746ee34feae | 481 | ); |
dannyman939 | 0:c746ee34feae | 482 | } |
jekain314 | 4:68268737ff89 | 483 | |
jekain314 | 4:68268737ff89 | 484 | //should just record ALL the data -- can pck over it in the post-processing |
jekain314 | 4:68268737ff89 | 485 | if (sendRecData) //begin to (or stop) record the serial data |
dannyman939 | 0:c746ee34feae | 486 | { |
dannyman939 | 0:c746ee34feae | 487 | sendRecData=false; |
dannyman939 | 0:c746ee34feae | 488 | char recChar = 'N'; |
dannyman939 | 0:c746ee34feae | 489 | if (recordData) |
dannyman939 | 0:c746ee34feae | 490 | { |
dannyman939 | 0:c746ee34feae | 491 | if ((fpNav == NULL)) |
dannyman939 | 0:c746ee34feae | 492 | { |
dannyman939 | 0:c746ee34feae | 493 | fpNav = fopen("/sd/Data/NAV.bin", "wb"); |
dannyman939 | 0:c746ee34feae | 494 | } |
dannyman939 | 0:c746ee34feae | 495 | if (fpNav != NULL) |
dannyman939 | 0:c746ee34feae | 496 | { |
dannyman939 | 0:c746ee34feae | 497 | recChar = 'Y'; |
dannyman939 | 0:c746ee34feae | 498 | } |
dannyman939 | 0:c746ee34feae | 499 | //recChar = 'Y'; |
dannyman939 | 0:c746ee34feae | 500 | } |
dannyman939 | 0:c746ee34feae | 501 | else |
dannyman939 | 0:c746ee34feae | 502 | { |
dannyman939 | 0:c746ee34feae | 503 | if (fpNav != NULL) |
dannyman939 | 0:c746ee34feae | 504 | { |
dannyman939 | 0:c746ee34feae | 505 | fclose(fpNav); |
dannyman939 | 0:c746ee34feae | 506 | fpNav = NULL; |
dannyman939 | 0:c746ee34feae | 507 | } |
dannyman939 | 0:c746ee34feae | 508 | /* |
dannyman939 | 0:c746ee34feae | 509 | if (fpIMU != NULL) |
dannyman939 | 0:c746ee34feae | 510 | { |
dannyman939 | 0:c746ee34feae | 511 | fclose(fpIMU); |
dannyman939 | 0:c746ee34feae | 512 | fpIMU = NULL; |
dannyman939 | 0:c746ee34feae | 513 | } |
dannyman939 | 0:c746ee34feae | 514 | if (fpGPS != NULL) |
dannyman939 | 0:c746ee34feae | 515 | { |
dannyman939 | 0:c746ee34feae | 516 | fclose(fpGPS); |
dannyman939 | 0:c746ee34feae | 517 | fpGPS = NULL; |
dannyman939 | 0:c746ee34feae | 518 | } |
dannyman939 | 0:c746ee34feae | 519 | */ |
dannyman939 | 0:c746ee34feae | 520 | } |
dannyman939 | 0:c746ee34feae | 521 | toPC.printf("WMsg RECORDDATA %c\n", |
dannyman939 | 0:c746ee34feae | 522 | recChar |
dannyman939 | 0:c746ee34feae | 523 | ); |
dannyman939 | 0:c746ee34feae | 524 | } |
jekain314 | 4:68268737ff89 | 525 | |
jekain314 | 4:68268737ff89 | 526 | //this is called everytime through the loop -- wasteful |
jekain314 | 4:68268737ff89 | 527 | //recordDataled = recordData; |
jekain314 | 4:68268737ff89 | 528 | |
jekain314 | 4:68268737ff89 | 529 | if (sendStreamPos) //stream the position data to the PC |
dannyman939 | 0:c746ee34feae | 530 | { |
dannyman939 | 0:c746ee34feae | 531 | sendStreamPos=false; |
dannyman939 | 0:c746ee34feae | 532 | char streamChar = 'N'; |
dannyman939 | 0:c746ee34feae | 533 | if (streamPos) |
dannyman939 | 0:c746ee34feae | 534 | { |
dannyman939 | 0:c746ee34feae | 535 | streamChar = 'Y'; |
dannyman939 | 0:c746ee34feae | 536 | } |
dannyman939 | 0:c746ee34feae | 537 | toPC.printf("WMsg POSSTREAM %c\n", |
dannyman939 | 0:c746ee34feae | 538 | streamChar |
dannyman939 | 0:c746ee34feae | 539 | ); |
dannyman939 | 0:c746ee34feae | 540 | } |
jekain314 | 4:68268737ff89 | 541 | |
jekain314 | 4:68268737ff89 | 542 | //not sure this is ever used .. |
jekain314 | 4:68268737ff89 | 543 | if (sendLogMsgInfo) //send log info to the PC |
dannyman939 | 2:7301e63832ee | 544 | { |
dannyman939 | 2:7301e63832ee | 545 | sendLogMsgInfo=false; |
dannyman939 | 2:7301e63832ee | 546 | char logChar = 'N'; |
dannyman939 | 2:7301e63832ee | 547 | if (logMsgInfo) |
dannyman939 | 2:7301e63832ee | 548 | { |
dannyman939 | 2:7301e63832ee | 549 | logChar = 'Y'; |
dannyman939 | 2:7301e63832ee | 550 | } |
dannyman939 | 2:7301e63832ee | 551 | toPC.printf("WMsg LOGINFO %c\n", |
dannyman939 | 2:7301e63832ee | 552 | logChar |
dannyman939 | 2:7301e63832ee | 553 | ); |
dannyman939 | 2:7301e63832ee | 554 | } |
jekain314 | 4:68268737ff89 | 555 | |
jekain314 | 4:68268737ff89 | 556 | |
jekain314 | 4:68268737ff89 | 557 | //////////////////////////////////////////////////////////////////////////// |
jekain314 | 4:68268737ff89 | 558 | //below is where we process the complete stored GPS message for the second |
jekain314 | 4:68268737ff89 | 559 | //The !IMUDataReady test prevents the IMU and GPS data from being written |
jekain314 | 4:68268737ff89 | 560 | //to disk on the same pass through thi loop |
jekain314 | 4:68268737ff89 | 561 | ///////////////////////////////////////////////////////////////////////////// |
jekain314 | 4:68268737ff89 | 562 | if (!IMUDataReady && lookingForMessages && (timeFromPPS.read_us() > 20000)) //it takes less than 20msec to receive all messages |
jekain314 | 4:68268737ff89 | 563 | { |
jekain314 | 4:68268737ff89 | 564 | toPC.printf(" num messages = %3d time = %5d \n", perSecMessageCounter, timeFromPPS.read_us()); |
jekain314 | 4:68268737ff89 | 565 | |
jekain314 | 4:68268737ff89 | 566 | //cycle through all the bytes stored this sec (after the 1PPS as set) |
jekain314 | 4:68268737ff89 | 567 | // perSecMessageCounter is incremented whenever we detect a new message headet 0xAA44121C sequence |
jekain314 | 4:68268737ff89 | 568 | for (int i=0; i<perSecMessageCounter; i++) |
dannyman939 | 0:c746ee34feae | 569 | { |
jekain314 | 4:68268737ff89 | 570 | msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]]; |
jekain314 | 4:68268737ff89 | 571 | toPC.printf("WMsg MESSAGEINFO %5d %5d \n", |
jekain314 | 4:68268737ff89 | 572 | msgHeader[i]->messageID, |
jekain314 | 4:68268737ff89 | 573 | messageLocation[i]); |
jekain314 | 4:68268737ff89 | 574 | //test for a message 42 (BESTPOS) |
jekain314 | 4:68268737ff89 | 575 | if (msgHeader[i]->messageID == 42) |
dannyman939 | 2:7301e63832ee | 576 | { |
jekain314 | 4:68268737ff89 | 577 | curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[i]]); |
dannyman939 | 2:7301e63832ee | 578 | |
jekain314 | 4:68268737ff89 | 579 | if (streamPos) |
dannyman939 | 1:cbb9104d826f | 580 | { |
dannyman939 | 0:c746ee34feae | 581 | 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 | 582 | curPos.msgHeader.GPSTime_msecs, |
dannyman939 | 0:c746ee34feae | 583 | curPos.solStatus, |
dannyman939 | 0:c746ee34feae | 584 | curPos.posType, |
dannyman939 | 0:c746ee34feae | 585 | curPos.latitude, |
dannyman939 | 0:c746ee34feae | 586 | curPos.longitude, |
dannyman939 | 0:c746ee34feae | 587 | curPos.height, |
dannyman939 | 0:c746ee34feae | 588 | curPos.undulation, |
dannyman939 | 0:c746ee34feae | 589 | curPos.latitudeSTD, |
dannyman939 | 0:c746ee34feae | 590 | curPos.longitudeSTD, |
dannyman939 | 0:c746ee34feae | 591 | curPos.heightSTD, |
dannyman939 | 0:c746ee34feae | 592 | curPos.diffAge, |
dannyman939 | 0:c746ee34feae | 593 | curPos.solutionAge, |
dannyman939 | 0:c746ee34feae | 594 | curPos.numSV, |
dannyman939 | 0:c746ee34feae | 595 | curPos.numSolSV, |
dannyman939 | 0:c746ee34feae | 596 | curPos.numGGL1, |
dannyman939 | 0:c746ee34feae | 597 | curPos.extSolStatus, |
dannyman939 | 0:c746ee34feae | 598 | curPos.sigMask); |
dannyman939 | 0:c746ee34feae | 599 | } |
dannyman939 | 0:c746ee34feae | 600 | } |
jekain314 | 4:68268737ff89 | 601 | |
jekain314 | 4:68268737ff89 | 602 | //check for a message 99 (BESTVEL) -- and cast it into its message structure |
jekain314 | 4:68268737ff89 | 603 | else if (msgHeader[i]->messageID == 99) |
jekain314 | 4:68268737ff89 | 604 | { |
jekain314 | 4:68268737ff89 | 605 | curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]]); |
jekain314 | 4:68268737ff89 | 606 | } |
jekain314 | 4:68268737ff89 | 607 | |
jekain314 | 4:68268737ff89 | 608 | //the below test ensures that the positin and veocity are matched in time |
jekain314 | 4:68268737ff89 | 609 | //not sure the reason for the "250" below |
jekain314 | 4:68268737ff89 | 610 | if ((curVel.msgHeader.GPSTime_msecs+250)/1000 == |
jekain314 | 4:68268737ff89 | 611 | (curPos.msgHeader.GPSTime_msecs+250)/1000) |
dannyman939 | 2:7301e63832ee | 612 | { |
jekain314 | 4:68268737ff89 | 613 | // update position and velocity used for calculation |
jekain314 | 4:68268737ff89 | 614 | GPSTimemsecs = curPos.msgHeader.GPSTime_msecs; |
jekain314 | 4:68268737ff89 | 615 | GPSTime = (double)GPSTimemsecs/1000.0; |
jekain314 | 4:68268737ff89 | 616 | velMsg = curVel; // |
jekain314 | 4:68268737ff89 | 617 | posMsg = curPos; |
jekain314 | 4:68268737ff89 | 618 | |
jekain314 | 4:68268737ff89 | 619 | ///////////////////////////////////////////////////////////////////////////////////////// |
jekain314 | 4:68268737ff89 | 620 | //IMPORTANT: we reset the PPSTimeOffset when we have a matching position and velocity |
jekain314 | 4:68268737ff89 | 621 | PPSTimeOffset = 0; |
jekain314 | 4:68268737ff89 | 622 | ///////////////////////////////////////////////////////////////////////////////////////// |
dannyman939 | 2:7301e63832ee | 623 | } |
jekain314 | 4:68268737ff89 | 624 | } |
jekain314 | 4:68268737ff89 | 625 | lookingForMessages = false; |
jekain314 | 4:68268737ff89 | 626 | |
jekain314 | 4:68268737ff89 | 627 | } //end of the GPS message processing |
jekain314 | 4:68268737ff89 | 628 | |
jekain314 | 4:68268737ff89 | 629 | //the IMU data record is read from the SPI in the ISR and the IMUDataReady is set true |
jekain314 | 4:68268737ff89 | 630 | //we write the IMU data here |
jekain314 | 4:68268737ff89 | 631 | if (IMUDataReady) //IMUDataReady is true if we have a recent IMU data record |
jekain314 | 4:68268737ff89 | 632 | { |
jekain314 | 4:68268737ff89 | 633 | imuRec.GPSTime = GPSTimemsecs + timeFromPPS.read_us()/1000000.0; |
jekain314 | 4:68268737ff89 | 634 | spi.write((int) HIGH_REGISTER[0]); // next read will return results from HIGH_REGITER[0] |
jekain314 | 4:68268737ff89 | 635 | for (int i=0; i<6; i++) //read the 6 rate and accel variables |
jekain314 | 4:68268737ff89 | 636 | { |
jekain314 | 4:68268737ff89 | 637 | wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]) ; |
jekain314 | 4:68268737ff89 | 638 | if (i<5) // dont this on the last because this was pre-called |
jekain314 | 4:68268737ff89 | 639 | { wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]); } |
jekain314 | 4:68268737ff89 | 640 | imuRec.dataWord[i] = wd.dataWord; //data word is a signed long |
jekain314 | 4:68268737ff89 | 641 | |
jekain314 | 4:68268737ff89 | 642 | } |
jekain314 | 4:68268737ff89 | 643 | IMURecordCounter++; |
jekain314 | 4:68268737ff89 | 644 | //write the IMU data |
jekain314 | 4:68268737ff89 | 645 | //if (recordData && (fpIMU != NULL)) |
dannyman939 | 0:c746ee34feae | 646 | if (recordData && (fpNav != NULL)) |
dannyman939 | 0:c746ee34feae | 647 | { |
jekain314 | 4:68268737ff89 | 648 | IMUbytesWritten += fwrite(imuRec.dataWord, sizeof(IMUREC), 1, fpNav); |
dannyman939 | 0:c746ee34feae | 649 | } |
jekain314 | 4:68268737ff89 | 650 | IMUClockCounter++; |
jekain314 | 4:68268737ff89 | 651 | IMUDataReady = false; |
dannyman939 | 0:c746ee34feae | 652 | } |
jekain314 | 4:68268737ff89 | 653 | |
jekain314 | 4:68268737ff89 | 654 | if (messageDetected) //some GPS message header has been detected |
dannyman939 | 0:c746ee34feae | 655 | { |
jekain314 | 4:68268737ff89 | 656 | toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us()); |
dannyman939 | 0:c746ee34feae | 657 | messageDetected = false; |
dannyman939 | 0:c746ee34feae | 658 | } |
jekain314 | 4:68268737ff89 | 659 | |
jekain314 | 4:68268737ff89 | 660 | if (camera1EventDetected) //we have detected a camera trigger event |
dannyman939 | 0:c746ee34feae | 661 | { |
dannyman939 | 2:7301e63832ee | 662 | toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time); |
dannyman939 | 0:c746ee34feae | 663 | camera1EventDetected = false; |
dannyman939 | 0:c746ee34feae | 664 | } |
jekain314 | 4:68268737ff89 | 665 | |
jekain314 | 4:68268737ff89 | 666 | if (detectedGPS1PPS) //true if we are exactly at a 1PPS event detection |
dannyman939 | 0:c746ee34feae | 667 | { |
jekain314 | 4:68268737ff89 | 668 | toPC.printf(" PPSCounter=%4d byteCounter=%10d Msgs Received=%3d IMUClock=%4d bytesWritten=%8d\n", |
jekain314 | 4:68268737ff89 | 669 | PPSCounter, savedByteCounter, savedPerSecMessageCounter, savedIMUClockCounter, IMUbytesWritten); |
dannyman939 | 2:7301e63832ee | 670 | if (recordData && (fpNav != NULL) && (byteCounter > 0)) |
dannyman939 | 2:7301e63832ee | 671 | { |
jekain314 | 4:68268737ff89 | 672 | // we know that we are not reading a GPS message at exactly the 1PPS occurrence |
jekain314 | 4:68268737ff89 | 673 | fwrite(msgBuffer, byteCounter, 1, fpNav); // this writes out a complete set of messages for ths sec |
dannyman939 | 2:7301e63832ee | 674 | } |
dannyman939 | 0:c746ee34feae | 675 | detectedGPS1PPS = false; |
dannyman939 | 0:c746ee34feae | 676 | } |
dannyman939 | 0:c746ee34feae | 677 | } |
jekain314 | 4:68268737ff89 | 678 | |
jekain314 | 4:68268737ff89 | 679 | fclose(fpNav); |
dannyman939 | 0:c746ee34feae | 680 | toPC.printf(" normal termination \n"); |
dannyman939 | 0:c746ee34feae | 681 | } |