Dan Matthews / Mbed 2 deprecated GPS_Incremental-V1_0

Changes to allow hardware camera trigger

Dependencies:   mbed

Fork of GPS_Incremental by james kain

PCMessaging.h@10:078891935385, 2013-04-18 (annotated)

Committer:
dannyman939
Date:
Thu Apr 18 00:59:49 2013 +0000
Revision:
10:078891935385
Parent:
9:13724ed3f825 
Hardware trigger camera using the DigitalOut of Pin 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jekain314 6:2a8486283198 1 //these are defines for the messages that are sent from the PC across the USB
jekain314 6:2a8486283198 2 //these messages produce reactions on the mbed
jekain314 8:2e20e4cf53e6 3 const unsigned char STATUS_MSG =0;
jekain314 8:2e20e4cf53e6 4 const unsigned char POSVEL_MSG =1;
jekain314 6:2a8486283198 5 const unsigned char STARTDATA_MSG =2;
jekain314 6:2a8486283198 6 const unsigned char STOPDATA_MSG =3;
jekain314 6:2a8486283198 7 const unsigned char STARTSTREAM_MSG =4;
jekain314 6:2a8486283198 8 const unsigned char STOPSTREAM_MSG =5;
jekain314 6:2a8486283198 9 const unsigned char STARTLOGINFO_MSG =6;
jekain314 6:2a8486283198 10 const unsigned char STOPLOGINFO_MSG =7;
dannyman939 10:078891935385 11 const unsigned char SEND_TRIG =8;
jekain314 6:2a8486283198 12
jekain314 6:2a8486283198 13 const double DEGREES_TO_RADIANS = acos(-1.0)/180.0;
jekain314 6:2a8486283198 14 const double eccen = 0.0818191908426; //WGS84 earth eccentricity
jekain314 6:2a8486283198 15 const double earthRadius = 6378137; //WGS84 earthRadius in meters
jekain314 6:2a8486283198 16
jekain314 6:2a8486283198 17 char serBuf[128];
jekain314 6:2a8486283198 18 int serBufChars=0;
jekain314 6:2a8486283198 19
jekain314 6:2a8486283198 20 //flags to control the PC command actions
jekain314 6:2a8486283198 21 bool sendPosVel =false;
jekain314 6:2a8486283198 22 bool sendStatus =false;
jekain314 6:2a8486283198 23 bool sendRecData =false;
jekain314 6:2a8486283198 24 bool streamPos =false;
jekain314 6:2a8486283198 25 bool sendStreamPos =false;
jekain314 6:2a8486283198 26 bool logMsgInfo =false;
jekain314 6:2a8486283198 27 bool sendLogMsgInfo =false;
jekain314 6:2a8486283198 28 bool recordData =false;
dannyman939 10:078891935385 29 bool camtrg =false;
jekain314 6:2a8486283198 30
jekain314 6:2a8486283198 31
dannyman939 10:078891935385 32 const unsigned char numMessages = 9; //number of potential messages
jekain314 6:2a8486283198 33 char msgList[numMessages][32]; //text array storing the command messages from the PC
jekain314 8:2e20e4cf53e6 34 char minMessageSize = 11; //minimum size of a text message
jekain314 6:2a8486283198 35 unsigned char CR = 0x0d; //ASCII Carriage Return
jekain314 6:2a8486283198 36 unsigned char LF = 0x0a; //ASCII Line Feed
jekain314 6:2a8486283198 37
jekain314 6:2a8486283198 38 void setUpMessages(void)
jekain314 6:2a8486283198 39 {
jekain314 6:2a8486283198 40 //set up the ASCII text records that are candidates to be passed from the PC
jekain314 6:2a8486283198 41 sprintf(msgList[STATUS_MSG], "WMsg STATUS");
jekain314 6:2a8486283198 42 sprintf(msgList[POSVEL_MSG], "WMsg POSVEL");
jekain314 6:2a8486283198 43 sprintf(msgList[STARTDATA_MSG], "WMsg RECORDDATA Y");
jekain314 6:2a8486283198 44 sprintf(msgList[STOPDATA_MSG], "WMsg RECORDDATA N");
jekain314 6:2a8486283198 45 sprintf(msgList[STARTSTREAM_MSG], "WMsg POSSTREAM Y");
jekain314 6:2a8486283198 46 sprintf(msgList[STOPSTREAM_MSG], "WMsg POSSTREAM N");
jekain314 6:2a8486283198 47 sprintf(msgList[STARTLOGINFO_MSG], "WMsg LOGINFO Y");
jekain314 6:2a8486283198 48 sprintf(msgList[STOPLOGINFO_MSG], "WMsg LOGINFO N");
dannyman939 10:078891935385 49 sprintf(msgList[SEND_TRIG], "WMsg SEND_TRIG");
jekain314 6:2a8486283198 50 //message length is from 10 to 16 chars
jekain314 6:2a8486283198 51
jekain314 6:2a8486283198 52 toPC.printf(" finished setting up messages \n");
jekain314 6:2a8486283198 53 }
jekain314 6:2a8486283198 54
jekain314 6:2a8486283198 55 void readFromPC()
jekain314 6:2a8486283198 56 {
jekain314 6:2a8486283198 57 //The received commands only occur at the initialization stage -- time is not that critical there.
jekain314 6:2a8486283198 58 //during the real-time action, we will never pass the followong if test ( no characters received)
jekain314 6:2a8486283198 59 if (toPC.readable()) //read a PC serial byte and test it for a command
jekain314 6:2a8486283198 60 {
jekain314 6:2a8486283198 61
jekain314 6:2a8486283198 62 // Read in next character
jekain314 6:2a8486283198 63 char inChar = toPC.getc(); //read char from the USB serial link to the PC
jekain314 8:2e20e4cf53e6 64 //toPC.printf("%02x ",inChar);
jekain314 6:2a8486283198 65
jekain314 6:2a8486283198 66 //incoming messages will end witb a CR / LF -- disregard these chars
jekain314 8:2e20e4cf53e6 67 if (inChar == CR || inChar == LF) return; //CR is a 0x0a
jekain314 6:2a8486283198 68
jekain314 6:2a8486283198 69 serBuf[serBufChars++] = inChar; //set this char in a char array
jekain314 6:2a8486283198 70
jekain314 6:2a8486283198 71 //no need to continue if numChars are less than the shortest candidate message
jekain314 8:2e20e4cf53e6 72 //if (serBufChars < minMessageSize) return;
jekain314 8:2e20e4cf53e6 73
jekain314 6:2a8486283198 74 // Append end of string
jekain314 6:2a8486283198 75 //We always assume we have a complete message string and test for this below
jekain314 6:2a8486283198 76 serBuf[serBufChars] = '\0';
jekain314 6:2a8486283198 77
jekain314 6:2a8486283198 78 bool validMessage = false;
jekain314 6:2a8486283198 79
jekain314 8:2e20e4cf53e6 80
jekain314 6:2a8486283198 81 // Check for valid message -- there are numMessages possible messages
jekain314 6:2a8486283198 82 for (int m = 0; m < numMessages && !validMessage; m++) //check for all messages ...
jekain314 6:2a8486283198 83 {
jekain314 6:2a8486283198 84 //toPC.printf(" \n\n found chars to test %3d %3d %s \n\n\n ", serBufChars, strlen(msgList[m]), serBuf );
jekain314 6:2a8486283198 85
jekain314 6:2a8486283198 86 //check the current partial message against ALL possible messages
jekain314 6:2a8486283198 87 //messages must match in both strength length and text
jekain314 6:2a8486283198 88 if (serBufChars == strlen(msgList[m]) && strncmp(serBuf, msgList[m], serBufChars) == 0 )
jekain314 6:2a8486283198 89 {
jekain314 6:2a8486283198 90
jekain314 8:2e20e4cf53e6 91 //toPC.printf( "\n found valid message %s \n\n", serBuf);
jekain314 6:2a8486283198 92
jekain314 6:2a8486283198 93 validMessage = true;
jekain314 6:2a8486283198 94 serBufChars = 0; //reset the character count to reset for next message
jekain314 6:2a8486283198 95
jekain314 6:2a8486283198 96 //set programmatic action flags based on the message
jekain314 6:2a8486283198 97 switch(m)
jekain314 6:2a8486283198 98 {
jekain314 6:2a8486283198 99 case STATUS_MSG:
jekain314 6:2a8486283198 100 sendStatus = true; //send a status message back to PC
jekain314 6:2a8486283198 101 break;
jekain314 6:2a8486283198 102 case POSVEL_MSG:
jekain314 6:2a8486283198 103 sendPosVel = true; //send a posvel message back to PC
jekain314 6:2a8486283198 104 break;
jekain314 6:2a8486283198 105 case STARTDATA_MSG: //start the data recording to the SD card
jekain314 6:2a8486283198 106 recordData = true;
jekain314 6:2a8486283198 107 sendRecData = true;
jekain314 6:2a8486283198 108 break;
jekain314 6:2a8486283198 109 case STOPDATA_MSG: //stop the data recording to the SD card
jekain314 6:2a8486283198 110 recordData = false;
jekain314 6:2a8486283198 111 sendRecData = true;
jekain314 6:2a8486283198 112 break;
jekain314 6:2a8486283198 113 case STARTSTREAM_MSG:
jekain314 6:2a8486283198 114 case STOPSTREAM_MSG:
jekain314 6:2a8486283198 115 streamPos = (m == STARTSTREAM_MSG);
jekain314 6:2a8486283198 116 sendStreamPos = true;
jekain314 6:2a8486283198 117 break;
jekain314 6:2a8486283198 118 case STARTLOGINFO_MSG:
jekain314 6:2a8486283198 119 case STOPLOGINFO_MSG:
jekain314 6:2a8486283198 120 logMsgInfo = (m == STARTLOGINFO_MSG);
jekain314 6:2a8486283198 121 sendLogMsgInfo = true;
dannyman939 10:078891935385 122 break;
dannyman939 10:078891935385 123 case SEND_TRIG:
dannyman939 10:078891935385 124 camtrg = true; //send a posvel message back to PC
dannyman939 10:078891935385 125 break;
jekain314 6:2a8486283198 126 } //end Switch statement
jekain314 6:2a8486283198 127 break;
jekain314 6:2a8486283198 128 } //end test for a valid message
jekain314 6:2a8486283198 129 } //end message text loop
jekain314 6:2a8486283198 130 } //end pc.readable
jekain314 6:2a8486283198 131 };
jekain314 6:2a8486283198 132
jekain314 6:2a8486283198 133 void earthCoefficients(double latitudeRad, double longitudeRad, double height, double &latRateFac, double &lonRateFac)
jekain314 6:2a8486283198 134 {
jekain314 6:2a8486283198 135 //compute the lat and lon factors for use in the interpolation of the lat and lon between 1 sec epochs
jekain314 6:2a8486283198 136 //latRateFac & lonRateFac multiplied by Vnorth or VEast to get latRate and lonRate
jekain314 6:2a8486283198 137 //see this document (page 32) www.fas.org/spp/military/program/nav/basicnav.pdf
jekain314 6:2a8486283198 138
jekain314 6:2a8486283198 139 double eccenSinLat = eccen * sin(latitudeRad);
jekain314 6:2a8486283198 140 double temp1 = 1.0 - eccenSinLat*eccenSinLat;
jekain314 6:2a8486283198 141 double temp2 = sqrt(temp1);
jekain314 6:2a8486283198 142 double r_meridian = earthRadius * ( 1.0 - eccen*eccen)/ (temp1 * temp2);
jekain314 6:2a8486283198 143 double r_normal = earthRadius / temp2;
jekain314 6:2a8486283198 144
jekain314 6:2a8486283198 145 //divide Vnorth by latRateFac to get the latitude rate in deg per sec
jekain314 6:2a8486283198 146 latRateFac = (r_meridian + height)* DEGREES_TO_RADIANS;
jekain314 6:2a8486283198 147
jekain314 6:2a8486283198 148 //divide VEast by lonRateFac to get the longitude rate in deg per sec
jekain314 6:2a8486283198 149 lonRateFac = (r_normal + height) * cos(latitudeRad)* DEGREES_TO_RADIANS;
jekain314 6:2a8486283198 150 }
jekain314 6:2a8486283198 151
jekain314 6:2a8486283198 152 void sendPosVelMessageToPC(OEM615BESTPOS posMsg, OEM615BESTVEL velMsg)
jekain314 6:2a8486283198 153 {
jekain314 6:2a8486283198 154 //north and east velocity from the horizontal speed and heading
jekain314 6:2a8486283198 155 //velMsg may not be the "current" message --- but is the one also associated with a position message
jekain314 6:2a8486283198 156 double nVel = velMsg.horizontalSpeed*cos(velMsg.heading*DEGREES_TO_RADIANS);
jekain314 6:2a8486283198 157 double eVel = velMsg.horizontalSpeed*sin(velMsg.heading*DEGREES_TO_RADIANS);
jekain314 6:2a8486283198 158
jekain314 6:2a8486283198 159 double latRateFac;
jekain314 6:2a8486283198 160 double lonRateFac;
jekain314 6:2a8486283198 161
jekain314 6:2a8486283198 162 earthCoefficients( posMsg.latitude*DEGREES_TO_RADIANS,
jekain314 6:2a8486283198 163 posMsg.longitude*DEGREES_TO_RADIANS,
jekain314 6:2a8486283198 164 posMsg.height,
jekain314 6:2a8486283198 165 latRateFac, lonRateFac);
jekain314 6:2a8486283198 166
jekain314 6:2a8486283198 167 //commented calculations are for a spherical earth (Chris's original computation)
jekain314 6:2a8486283198 168 // For the 1 second deltas with which we are dealing
jekain314 6:2a8486283198 169 // This calculation should be close enough for now
jekain314 6:2a8486283198 170 // Approximately 1 nautical mile / minute latitude, 60 minutes/degree, 1852 meters/nautical mile
jekain314 6:2a8486283198 171 //double latMetersPerDeg = 60.0*1852.0;
jekain314 6:2a8486283198 172 // longitude separation is approximately equal to latitude separation * cosine of latitude
jekain314 6:2a8486283198 173 //double lonMetersPerDeg = latMetersPerDeg*cos(posMsg.latitude*DEGREES_TO_RADIANS);
jekain314 6:2a8486283198 174
jekain314 6:2a8486283198 175 // Elapsed time since last known GPS position
jekain314 6:2a8486283198 176 //PPSTimeOffset is a result of possibly missing a prior GPS position message
jekain314 6:2a8486283198 177 // timeFromPPS.read() is always the time from the moset recent 1PPS
jekain314 6:2a8486283198 178 double elTime = (double)PPSTimeOffset + timeFromPPS.read();
jekain314 6:2a8486283198 179
jekain314 6:2a8486283198 180 // Position time -- GPSTime is the time of the last valid GPS position message
jekain314 6:2a8486283198 181 double posTime = GPSTime + elTime;
jekain314 6:2a8486283198 182
jekain314 6:2a8486283198 183 //toPC.printf(" elTime = %6.3f PPSimeOffset = %6.3f \n", elTime, PPSTimeOffset);
jekain314 6:2a8486283198 184 //toPC.printf(" latRateFac = %10.3f lonRateFac = %10.3f \n", latRateFac, lonRateFac);
jekain314 6:2a8486283198 185 //toPC.printf(" latRateFac = %10.3f lonRateFac = %10.3f \n", latMetersPerDeg, lonMetersPerDeg);
jekain314 6:2a8486283198 186
jekain314 6:2a8486283198 187 // Estimated position based on previous position and velocity
jekain314 6:2a8486283198 188 // posMsg is the last time when the BESTVEL and BESTPOS messages had identical times
jekain314 6:2a8486283198 189 //double latPos = posMsg.latitude + (nVel/latMetersPerDeg)*elTime;
jekain314 6:2a8486283198 190 //double lonPos = posMsg.longitude + (eVel/lonMetersPerDeg)*elTime;
jekain314 6:2a8486283198 191
jekain314 6:2a8486283198 192 double latPos = posMsg.latitude + (nVel/latRateFac)*elTime;
jekain314 6:2a8486283198 193 double lonPos = posMsg.longitude + (eVel/lonRateFac)*elTime;
jekain314 6:2a8486283198 194 double htPos = posMsg.height + velMsg.verticalSpeed/(60*1852)*elTime;
jekain314 6:2a8486283198 195
jekain314 8:2e20e4cf53e6 196 char solReady = 'N';
jekain314 8:2e20e4cf53e6 197 //solStatus
jekain314 8:2e20e4cf53e6 198 if (posMsg.solStatus == 0) //see description of solution status in OEMV615.h
jekain314 8:2e20e4cf53e6 199 {
jekain314 8:2e20e4cf53e6 200 solReady = 'Y';
jekain314 8:2e20e4cf53e6 201 }
jekain314 8:2e20e4cf53e6 202
jekain314 8:2e20e4cf53e6 203 toPC.printf("WMsg POSVEL %5.3lf %1d %c %8.5lf %9.5lf %4.3lf %4.3lf %4.3lf %4.3lf\n",
jekain314 6:2a8486283198 204 posTime,
jekain314 6:2a8486283198 205 posMsg.numSolSV,
jekain314 8:2e20e4cf53e6 206 solReady,
jekain314 6:2a8486283198 207 latPos,
jekain314 6:2a8486283198 208 lonPos,
jekain314 6:2a8486283198 209 htPos,
jekain314 6:2a8486283198 210 nVel,
jekain314 6:2a8486283198 211 eVel,
jekain314 6:2a8486283198 212 velMsg.verticalSpeed
jekain314 6:2a8486283198 213 );
jekain314 6:2a8486283198 214 }
jekain314 6:2a8486283198 215
jekain314 6:2a8486283198 216 void processPCmessages(FILE* &fpNav, OEM615BESTPOS posMsg, OEM615BESTVEL velMsg)
jekain314 6:2a8486283198 217 {
jekain314 6:2a8486283198 218
jekain314 6:2a8486283198 219 //we should put the below stuff into the readPC() procedure.
jekain314 6:2a8486283198 220 //only do these actions in response to a command so no need for the tests w/o an inoput byte from the PC
jekain314 6:2a8486283198 221 //perform the activities as a response to the commands
dannyman939 10:078891935385 222
dannyman939 10:078891935385 223 if (camtrg) //true if camera trigger command sent from PC
dannyman939 10:078891935385 224 {
dannyman939 10:078891935385 225 camera1EventDetected=true;
dannyman939 10:078891935385 226
dannyman939 10:078891935385 227 }
jekain314 6:2a8486283198 228 if (sendPosVel) //true if we want to return a position solution
jekain314 6:2a8486283198 229 {
jekain314 6:2a8486283198 230 sendPosVel=false; //set to true if a POSVEL is requested from the PC
jekain314 6:2a8486283198 231 sendPosVelMessageToPC(posMsg, velMsg);
jekain314 6:2a8486283198 232 }
jekain314 6:2a8486283198 233
jekain314 6:2a8486283198 234 //all this does is assess the GPS convergence -- really available in the above
jekain314 6:2a8486283198 235 if (sendStatus) //send the status message to the PC
jekain314 6:2a8486283198 236 {
jekain314 6:2a8486283198 237 sendStatus=false;
jekain314 6:2a8486283198 238 char solReady = 'N';
jekain314 6:2a8486283198 239 //solStatus
jekain314 6:2a8486283198 240 if (posMsg.solStatus == 0) //see description of solution status in OEMV615.h
jekain314 6:2a8486283198 241 {
jekain314 6:2a8486283198 242 solReady = 'Y';
jekain314 6:2a8486283198 243 }
jekain314 6:2a8486283198 244 toPC.printf("WMsg STATUS %5.3lf %c\n",
jekain314 6:2a8486283198 245 GPSTime,
jekain314 6:2a8486283198 246 solReady
jekain314 6:2a8486283198 247 );
jekain314 6:2a8486283198 248 }
jekain314 6:2a8486283198 249
jekain314 6:2a8486283198 250 //should just record ALL the data -- can pick over it in the post-processing
jekain314 6:2a8486283198 251 if (sendRecData) //begin to (or stop) record the serial data
jekain314 6:2a8486283198 252 {
jekain314 6:2a8486283198 253 sendRecData=false;
jekain314 6:2a8486283198 254 char recChar = 'N';
jekain314 6:2a8486283198 255 if (recordData)
jekain314 6:2a8486283198 256 {
jekain314 6:2a8486283198 257 if ((fpNav == NULL))
jekain314 6:2a8486283198 258 {
jekain314 6:2a8486283198 259 fpNav = fopen("/sd/Data/NAV.bin", "wb");
jekain314 6:2a8486283198 260 //toPC.printf("\n opened the SD card file recordData=%10d \n\n", fpNav);
jekain314 6:2a8486283198 261 }
jekain314 6:2a8486283198 262 if (fpNav != NULL)
jekain314 6:2a8486283198 263 {
jekain314 6:2a8486283198 264 recChar = 'Y';
jekain314 6:2a8486283198 265 }
jekain314 6:2a8486283198 266 else
jekain314 6:2a8486283198 267 {
jekain314 6:2a8486283198 268 toPC.printf(" Could not open the SD card \n\n");
jekain314 6:2a8486283198 269 }
jekain314 6:2a8486283198 270 }
jekain314 6:2a8486283198 271 else
jekain314 6:2a8486283198 272 {
jekain314 6:2a8486283198 273 if (fpNav != NULL)
jekain314 6:2a8486283198 274 {
jekain314 6:2a8486283198 275 toPC.printf(" closing the SD card file \n\n");
jekain314 6:2a8486283198 276 fclose(fpNav);
jekain314 9:13724ed3f825 277 wait(1.0);
jekain314 9:13724ed3f825 278 //toPC.printf("\n after closing the SD card file \n\n");
jekain314 6:2a8486283198 279 recordData = false;
jekain314 6:2a8486283198 280 fpNav = NULL;
jekain314 6:2a8486283198 281 }
jekain314 6:2a8486283198 282 }
jekain314 6:2a8486283198 283 toPC.printf("WMsg RECORDDATA %c\n",
jekain314 6:2a8486283198 284 recChar
jekain314 6:2a8486283198 285 );
jekain314 6:2a8486283198 286 }
jekain314 6:2a8486283198 287
jekain314 6:2a8486283198 288 if (sendStreamPos) //stream the position data to the PC
jekain314 6:2a8486283198 289 {
jekain314 6:2a8486283198 290 sendStreamPos=false;
jekain314 6:2a8486283198 291 char streamChar = 'N';
jekain314 6:2a8486283198 292 if (streamPos)
jekain314 6:2a8486283198 293 {
jekain314 6:2a8486283198 294 streamChar = 'Y';
jekain314 6:2a8486283198 295 }
jekain314 6:2a8486283198 296 toPC.printf("WMsg POSSTREAM %c\n",
jekain314 6:2a8486283198 297 streamChar
jekain314 6:2a8486283198 298 );
jekain314 6:2a8486283198 299 }
jekain314 6:2a8486283198 300
jekain314 6:2a8486283198 301 //not sure this is ever used ..
jekain314 6:2a8486283198 302 if (sendLogMsgInfo) //send log info to the PC
jekain314 6:2a8486283198 303 {
jekain314 6:2a8486283198 304 sendLogMsgInfo=false;
jekain314 6:2a8486283198 305 char logChar = 'N';
jekain314 6:2a8486283198 306 if (logMsgInfo)
jekain314 6:2a8486283198 307 {
jekain314 6:2a8486283198 308 logChar = 'Y';
jekain314 6:2a8486283198 309 }
jekain314 6:2a8486283198 310 toPC.printf("WMsg LOGINFO %c\n",
jekain314 6:2a8486283198 311 logChar
jekain314 6:2a8486283198 312 );
jekain314 6:2a8486283198 313 }
jekain314 6:2a8486283198 314
jekain314 6:2a8486283198 315
jekain314 6:2a8486283198 316 }