#include <stdio.h>#include <stdlib.h>#include <math.h>#include <string.h>#include <time.h>#include <sys/time.h>#include "frame_comm.h"Include dependency graph for frame_comm.c:
Go to the source code of this file.
Defines | |
| #define | PROTECT_STATUS |
Functions | |
| void | writeMessage (ControlFrame_t *cf, char *outMsg) |
| int | readMessage (ControlFrame_t *cf, char *inMsg) |
| int | strChecksum (char *str, int start, int end) |
| void | initControlFrame (ControlFrame_t *cf) |
| int | eval_frame_comm (ControlFrame_t *cf) |
|
|
Definition at line 55 of file frame_comm.c. |
|
|
Definition at line 271 of file frame_comm.c. References ControlFrame_t. 00271 {
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 }
|
|
|
Definition at line 242 of file frame_comm.c. References ControlFrame::angleLimitErr, ControlFrame::cmdAngle, ControlFrame::cmdSpeed, ControlFrame::cmdSystemMode, ControlFrame::connStatus, ControlFrame_t, ControlFrame::curAngle, ControlFrame::curSpeed, ControlFrame::errorCode, ControlFrame::GPSEpe, ControlFrame::GPSLat, ControlFrame::GPSLon, ControlFrame::GPSStatus, ControlFrame::GPSVelEast, ControlFrame::GPSVelNorth, ControlFrame::msgCount, ControlFrame::pcntThrottle, ControlFrame::speedLimitErr, ControlFrame::systemMode, and ControlFrame::timeStamp. Referenced by main(). 00242 {
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 }
|
|
||||||||||||
|
Definition at line 169 of file frame_comm.c. References ControlFrame::angleLimitErr, ControlFrame::cmdAngle, ControlFrame::cmdSpeed, ControlFrame::cmdSystemMode, ControlFrame::connStatus, ControlFrame_t, ControlFrame::curAngle, ControlFrame::curSpeed, ControlFrame::errorCode, ControlFrame::GPSEpe, ControlFrame::GPSLat, ControlFrame::GPSLon, ControlFrame::GPSStatus, ControlFrame::GPSVelEast, ControlFrame::GPSVelNorth, ControlFrame::msgCount, ControlFrame::pcntThrottle, ControlFrame::speedLimitErr, ControlFrame::systemMode, and ControlFrame::timeStamp. 00169 {
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 }
|
|
||||||||||||||||
|
Definition at line 235 of file frame_comm.c. Referenced by writeMessage(). 00235 {
00236 int i;
00237 unsigned int cs = 0;
00238 for (i = start; i <= end; i++) cs += (unsigned int)str[i];
00239 return cs;
00240 }
|
|
||||||||||||
|
Definition at line 57 of file frame_comm.c. References ControlFrame::angleLimitErr, ControlFrame::cmdAngle, ControlFrame::cmdSpeed, ControlFrame::cmdSystemMode, ControlFrame::connStatus, ControlFrame_t, ControlFrame::curAngle, ControlFrame::curSpeed, ControlFrame::errorCode, ControlFrame::GPSEpe, ControlFrame::GPSLat, ControlFrame::GPSLon, ControlFrame::GPSStatus, ControlFrame::GPSVelEast, ControlFrame::GPSVelNorth, msg, ControlFrame::msgCount, ControlFrame::pcntThrottle, ControlFrame::speedLimitErr, strChecksum(), and ControlFrame::systemMode. Referenced by socket_comm(). 00057 {
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 }
|
Here is the call graph for this function:
1.3.9.1