ERT1 Communication Server: frame

00001 /* frame_comm.c --- 
00002  * 
00003  * Filename: frame_comm.c
00004  * Description: 
00005  * Author: Bryce Himebaugh
00006  * Maintainer: 
00007  * Created: Tue Oct 31 10:45:23 2006
00008  * Version: 
00009  * Last-Updated: 
00010  *           By: 
00011  *     Update #: 0
00012  * Keywords: 
00013  * Compatibility: 
00014  * 
00015  */
00016 
00017 /* Commentary: 
00018  * 
00019  * 
00020  * 
00021  */
00022 
00023 /* Change log:
00024  * 
00025  * 
00026  */
00027 
00028 /* This program is free software; you can redistribute it and/or modify
00029  * it under the terms of the GNU General Public License as published by
00030  * the Free Software Foundation; either version 2, or (at your option)
00031  * any later version.
00032  * 
00033  * This program is distributed in the hope that it will be useful,
00034  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00035  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00036  * GNU General Public License for more details.
00037  * 
00038  * You should have received a copy of the GNU General Public License
00039  * along with this program; see the file COPYING.  If not, write to the
00040  * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
00041  * Boston, MA 02111-1307, USA.
00042  */
00043 
00044 /* Code: */
00045 
00046 #include <stdio.h>
00047 #include <stdlib.h>
00048 #include <math.h>
00049 #include <string.h>
00050 #include <time.h>
00051 #include <sys/time.h>
00052 
00053 #include "frame_comm.h"
00054 
00055 #define PROTECT_STATUS  // status fields are protected from overwriting by messages
00056 
00057 void writeMessage(ControlFrame_t *cf,  char* outMsg) {
00058   int messageLength;
00059   int messageChecksum;
00060   
00061   char field[80];
00062   char field1[10];
00063   char msg[MAX_MSG_LENGTH]="";
00064     
00065   static struct timeval tv_base; 
00066   static int init=1;
00067   struct timeval tv;
00068  
00069   if (init) {
00070     gettimeofday(&tv,NULL);
00071     tv_base.tv_sec=tv.tv_sec;
00072     tv_base.tv_usec=tv.tv_usec;
00073     init=0;
00074   }
00075 
00076   // header
00077   // increment the message count upon each write
00078   (cf->msgCount)++;
00079   sprintf(field, "VehMsgCount= %9ld,", cf->msgCount );                                          
00080   strcat(msg, field);
00081 
00082   gettimeofday(&tv,NULL);
00083   if (tv.tv_usec<tv_base.tv_usec) {
00084     tv.tv_usec=(1000000-tv_base.tv_usec)+tv.tv_usec;
00085     tv.tv_sec--;
00086   }
00087   else {
00088     tv.tv_usec-=tv_base.tv_usec;
00089   }
00090   tv.tv_usec/=1000;            // Convert uS to mS. 
00091   tv.tv_sec-=tv_base.tv_sec;   // Subtract the seconds
00092   tv.tv_sec*=1000;             // Convert s to mS.
00093   tv.tv_sec+=tv.tv_usec;       // sum the uS and s portions after conversion to mS. 
00094   
00095   sprintf(field, "TimeStamp= %11ld,",tv.tv_sec);
00096   strcat(msg, field);
00097 
00098   // steering
00099   sprintf(field, "CmdTurnRadius= %6.2f,", cf->cmdAngle);        
00100   strcat(msg, field);
00101   sprintf(field, "StatusTurnRadius= %6.2f,", cf->curAngle);             
00102   strcat(msg, field);
00103   sprintf(field, "StatusSteeringPosLimitError= %6s,",cf->angleLimitErr);
00104   strcat(msg, field);
00105 
00106   // speed
00107   sprintf(field, "CmdVehSpeed= %6.2f,", cf->cmdSpeed );
00108   strcat(msg, field);
00109   sprintf(field, "StatusVehSpeed= %6.2f,", cf->curSpeed );
00110   strcat(msg, field);
00111   sprintf(field, "StatusPercentThrottle= %6.2f,", cf->pcntThrottle );
00112   strcat(msg, field);
00113   sprintf(field, "StatusVehSpeedLimitError= %6s,", cf->speedLimitErr ); 
00114   strcat(msg, field);
00115         
00116   // GPS
00117   sprintf(field, "GPSLat= %10.5f,", cf->GPSLat );
00118   strcat(msg, field);
00119   sprintf(field, "GPSLon= %10.5f,", cf->GPSLon );       
00120   strcat(msg, field);
00121   sprintf(field, "GPSSpdE= %8.3f,", cf->GPSVelEast );           
00122   strcat(msg, field);
00123   sprintf(field, "GPSSpdN= %8.3f,", cf->GPSVelNorth );          
00124   strcat(msg, field);
00125   sprintf(field, "GPSEstPositionError= %7.2f,", cf->GPSEpe );
00126   strcat(msg, field);
00127   sprintf(field, "GPSStatus= %6s,", cf->GPSStatus );
00128   strcat(msg, field);
00129   
00130   // status of the driver/cart connection
00131   sprintf(field, "StatusComm= %7s,", cf->connStatus );          
00132   strcat(msg, field);
00133   
00134   // command to change the system mode
00135   sprintf(field, "CmdSystemMode= %6s,", cf->cmdSystemMode );
00136   strcat(msg, field);
00137   
00138   // system mode
00139   sprintf(field, "StatusSystemMode= %6s,", cf->systemMode );
00140   strcat(msg, field);
00141   
00142   // error code
00143   sprintf(field, "StatusErrorCode= %3d,", cf->errorCode );                              
00144   strcat(msg, field);
00145     
00146   // message length (header and data) in bytes
00147   messageLength = strlen(msg) + 16;
00148   sprintf(field, "MsgLength= %4d,", messageLength );
00149   
00150   // checksum of header and data, mod 0xFF
00151   messageChecksum = ( strChecksum(field,0,strlen(field)-1) + strChecksum(msg,0,strlen(msg)-1) )%255;
00152   sprintf(field1, "%02x", messageChecksum );
00153   
00154   // assemble the outgoing message
00155   strcpy(outMsg, "{");          // sentinel
00156   strcat(outMsg, field);        // message length
00157   strcat(outMsg, msg);          // header and data
00158   strcat(outMsg, field1);       // message checksum
00159   strcat(outMsg, "}");          // end marker
00160   strcat(outMsg, "\n");         // end marker
00161 
00162   return;
00163 }
00164 
00165 
00166 
00167 // decode the message string into individual fields of the control frame
00168 //
00169 int readMessage(ControlFrame_t *cf,  char* inMsg) {
00170   int messageLength = 0;
00171 /*   int messageChecksum = 0; */
00172   
00173   char f[25][80];
00174   char fieldName[80];
00175   char fieldVal[80];
00176   int i;
00177   
00178   // read the comma-delimited fields of the message string
00179   //  int c = sscanf(inMsg, "{%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^,],%[^}]",f[0],f[1],f[2],f[3],f[4],f[5],f[6],f[7],f[8],f[9],f[10],f[11],f[12],f[13],f[14],f[15],f[16],f[17],f[18],f[19],f[20] );
00180 
00181   // identify the fields by the field names, place field values into the fields of the control frame
00182   for (i = 0; i < 20; i++) {    
00183     // parse one field into name and value, delimited by '='
00184     sscanf(f[i], " %[^=] = %s ", fieldName, fieldVal);
00185     // header
00186     if ( !strcmp(fieldName, "MsgLength") ) { sscanf(fieldVal, "%d", &(messageLength)); }
00187     // instruction fields
00188     else if ( !strcmp(fieldName, "CmdTurnRadius")) { sscanf(fieldVal, "%f", &(cf->cmdAngle)); }
00189     else if ( !strcmp(fieldName, "CmdVehSpeed")) { sscanf(fieldVal, "%f", &(cf->cmdSpeed)); }
00190     // desired system mode
00191    else if ( !strcmp(fieldName, "CmdSystemMode")) { sscanf(fieldVal, "%s", cf->cmdSystemMode); }
00192 
00193     // status fields, protected from overwriting by the received messages
00194 #ifndef PROTECT_STATUS
00195 
00196     // header
00197     else if ( !strcmp(fieldName, "VehMsgCount") ) { sscanf(fieldVal, "%ld", &(cf->msgCount)); }
00198     else if ( !strcmp(fieldName, "TimeStamp")) { sscanf(fieldVal, "%ld", &(cf->timeStamp)); }
00199     // steering
00200     else if ( !strcmp(fieldName, "StatusTurnRadius")) {sscanf(fieldVal, "%f", &(cf->curAngle)); }
00201     else if ( !strcmp(fieldName, "StatusSteeringPosLimitError")) {sscanf(fieldVal, "%s", &(cf->angleLimitErr)); }
00202     // speed
00203     else if ( !strcmp(fieldName, "StatusVehSpeed")) { sscanf(fieldVal, "%f", &(cf->curSpeed)); }
00204     else if ( !strcmp(fieldName, "StatusPercentThrottle")) { sscanf(fieldVal, "%f", &(cf->pcntThrottle)); }
00205     else if ( !strcmp(fieldName, "StatusVehSpeedLimitError")) { sscanf(fieldVal, "%s", &(cf->speedLimitErr)); }
00206     // GPS
00207     else if ( !strcmp(fieldName, "GPSLat")) { sscanf(fieldVal, "%f", &(cf->GPSLat)); }
00208     else if ( !strcmp(fieldName, "GPSLon")) { sscanf(fieldVal, "%f", &(cf->GPSLon)); }
00209     else if ( !strcmp(fieldName, "GPSSpdE")) { sscanf(fieldVal, "%f", &(cf->GPSVelEast)); }
00210     else if ( !strcmp(fieldName, "GPSSpdN")) { sscanf(fieldVal, "%f", &(cf->GPSVelNorth)); }
00211     else if ( !strcmp(fieldName, "GPSEstPositionError")) { sscanf(fieldVal, "%f", &(cf->GPSEpe)); }
00212     else if ( !strcmp(fieldName, "GPSStatus")) { sscanf(fieldVal, "%s", &(cf->GPSStatus)); }
00213     // connection status
00214     else if ( !strcmp(fieldName, "StatusComm")) { sscanf(fieldVal, "%s", &(cf->connStatus)); }
00215     // system mode
00216     else if ( !strcmp(fieldName, "StatusSystemMode")) { sscanf(fieldVal, "%s", &(cf->systemMode)); }
00217     // error code
00218     else if ( !strcmp(fieldName, "StatusErrorCode")) { sscanf(fieldVal, "%d", &(cf->errorCode)); }
00219 #endif  // PROTECT_STATUS
00220   }
00221   // value of messageLength, plus sentinel, checksum and endmarker length,
00222   // must equal the length of received string
00223 /*   if ( messageLength + 4 != (int)strlen(inMsg) ) return INVALID_MSG_LEN_IN_CMD; */
00224 /*   // checksum field must evaluate to the checksum (mod 0xFF) of header and data */
00225 /*   sscanf(f[20], "%x", &messageChecksum); */
00226 /*   if ( messageChecksum != strChecksum(inMsg, 1, strlen(inMsg)-4)%255 ) return INVALID_CHECKSUM_IN_CMD; */
00227 /*   return NO_CART_ERROR; */
00228   return (0);
00229 }
00230 
00231 
00232 
00233 // checksum of the string str, from position "start" to position "end"
00234 //
00235 int strChecksum(char *str, int start, int end) {
00236   int i;
00237   unsigned int cs = 0;
00238   for (i = start; i <= end; i++) cs += (unsigned int)str[i];
00239   return cs;
00240 }
00241 
00242 void initControlFrame(ControlFrame_t * cf) {
00243 
00244         cf->msgCount = 0;                       // count of messages sent by the cart
00245         cf->timeStamp = 0;                      // cart-maintained clock (ms since start-up)
00246 
00247         cf->cmdAngle = 0.0;                     // desired wheel (degrees from center)
00248         cf->curAngle = 0.0;                     // current wheel angle
00249         strcpy(cf->angleLimitErr, "none");
00250 
00251         cf->cmdSpeed = 0.0;                     // desired speed (m/s)
00252         cf->curSpeed = 0.0;                     // current speed
00253         cf->pcntThrottle = 0.0;                 // % of full throttle
00254         strcpy(cf->speedLimitErr, "none");
00255 
00256         cf->GPSLat = 0.0;                       // GPS latitude (degrees)
00257         cf->GPSLon = 0.0;                       // GPS longitude (degrees)
00258         cf->GPSVelEast = 0.0;                   // eastward velocity (m/s)
00259         cf->GPSVelNorth = 0.0;                  // northward velocity (m/s)
00260         cf->GPSEpe = 0.0;                       // GPS estimated position error (m)
00261         strcpy(cf->GPSStatus, "none");
00262 
00263         strcpy(cf->connStatus, "offline");
00264         strcpy(cf->cmdSystemMode, "current");
00265         strcpy(cf->systemMode, "manual");
00266 /*      cf->errorCode = NO_CART_ERROR; */
00267         cf->errorCode = 0;
00268         
00269 }
00270 
00271 int eval_frame_comm (ControlFrame_t *cf) {
00272   enum {ready,unready,offline};
00273   static int comm_mode=offline;
00274   switch (comm_mode) {
00275   case offline:
00276     break;
00277   case unready:
00278     break;
00279   case ready:
00280     break;
00281   }
00282   return(0);
00283 }
00284 
00285 /* frame_comm.c ends here */