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