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@0:c746ee34feae, 2013-03-19 (annotated)

Committer:
dannyman939
Date:
Tue Mar 19 02:17:40 2013 +0000
Revision:
0:c746ee34feae
Child:
1:cbb9104d826f
Basic functionality, Chris Version 0.0

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