james kain / Mbed 2 deprecated Waldo_Embed_V2

this version has all of Jim's fixes for reading the GPS and IMU data synchronously

Dependencies:   MODSERIAL SDFileSystem mbed SDShell CRC CommHandler FP LinkedList LogUtil

PCMessaging.h@0:432b860b6ff7, 2013-04-22 (annotated)

Committer:
jekain314
Date:
Mon Apr 22 21:26:04 2013 +0000
Revision:
0:432b860b6ff7
Child:
1:8e24e633f8d8
does not work yet with mbed_test. Correctly collects GPS and IMU data.

Who changed what in which revision?

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