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@3:5913df46f94a, 2013-03-22 (annotated)

Committer:
dannyman939
Date:
Fri Mar 22 01:33:24 2013 +0000
Revision:
3:5913df46f94a
Parent:
2:7301e63832ee
Child:
4:68268737ff89
version that shows the trigger times in the mbed test application

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