james kain / Mbed 2 deprecated GPS_Incremental

JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013

Dependencies:   mbed

Fork of GPS_Incremental by Dan Matthews

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?

UserRevisionLine numberNew 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 }