Go to the documentation of this file.00001 #include "nfv2.h"
00002
00003 uint8_t crcTable[256];
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 uint8_t NF_Interpreter( NF_STRUCT_ComBuf *NFComBuf,
00015 uint8_t *rxBuf,
00016 uint8_t *rxPt,
00017 uint8_t *commandArray,
00018 uint8_t *commandCnt){
00019 static uint8_t n;
00020 uint8_t rxAddress, rxParamsCnt;
00021 uint8_t rxBufIter, combufDataIter, rxDataIter, dataBytesIter;
00022 uint8_t *dataPt, *u8TempPt;
00023
00024 if((*rxPt) == 0 && rxBuf[0]!='#'){
00025 return 0;
00026 }
00027 else if((*rxPt) == 3){
00028 if(( (rxBuf[1] & 0xff) == ((~rxBuf[2]) & 0xff) ) && (rxBuf[1] > 3)){
00029 n = rxBuf[1];
00030 (*rxPt)++;
00031 }
00032 else{
00033 (*rxPt) = 0;
00034 }
00035 return 0;
00036 }
00037 else if((*rxPt) > 3 && (*rxPt) >= n){
00038 (*rxPt) = 0;
00039
00040 if(NFv2_CrcFast(((const uint8_t*)rxBuf) + 3, n-3) != rxBuf[n]){
00041 return 0;
00042 }
00043
00044
00045
00046
00047
00048 }
00049 else{
00050 (*rxPt)++;
00051 return 0;
00052 }
00053
00054
00055 rxAddress = rxBuf[3];
00056 rxBufIter = 4;
00057 *commandCnt = 0;
00058 while(rxBufIter < n){
00059 dataPt = rxBuf + rxBufIter+2;
00060
00061
00062
00063
00064
00065
00066
00067 #ifdef NF_BUFSZ_SetDrivesMode
00068 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMode){
00069 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00070 combufDataIter = 0;
00071 rxDataIter = 0;
00072 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMode;
00073 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMode)) {
00074 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMode.data[combufDataIter]);
00075 dataBytesIter = 0;
00076 while(dataBytesIter < NF_DATABYTES_SetDrivesMode){
00077 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMode + dataBytesIter);
00078 dataBytesIter++;
00079 }
00080
00081 combufDataIter++;
00082 rxDataIter++;
00083 }
00084 NFComBuf->SetDrivesMode.updated = 1;
00085 }
00086 }
00087 else
00088 #endif
00089
00090 #ifdef NF_BUFSZ_SetDrivesSpeed
00091 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesSpeed){
00092 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00093 combufDataIter = 0;
00094 rxDataIter = 0;
00095 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesSpeed;
00096 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesSpeed)) {
00097 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesSpeed.data[combufDataIter]);
00098 dataBytesIter = 0;
00099 while(dataBytesIter < NF_DATABYTES_SetDrivesSpeed){
00100 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesSpeed + dataBytesIter);
00101 dataBytesIter++;
00102 }
00103
00104 combufDataIter++;
00105 rxDataIter++;
00106 }
00107 NFComBuf->SetDrivesSpeed.updated = 1;
00108 }
00109 }
00110 else
00111 #endif
00112
00113 #ifdef NF_BUFSZ_SetDrivesCurrent
00114 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesCurrent){
00115 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00116 combufDataIter = 0;
00117 rxDataIter = 0;
00118 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesCurrent;
00119 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesCurrent)) {
00120 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesCurrent.data[combufDataIter]);
00121 dataBytesIter = 0;
00122 while(dataBytesIter < NF_DATABYTES_SetDrivesCurrent){
00123 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesCurrent + dataBytesIter);
00124 dataBytesIter++;
00125 }
00126
00127 combufDataIter++;
00128 rxDataIter++;
00129 }
00130 NFComBuf->SetDrivesCurrent.updated = 1;
00131 }
00132 }
00133 else
00134 #endif
00135
00136 #ifdef NF_BUFSZ_SetDrivesPosition
00137 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesPosition){
00138 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00139 combufDataIter = 0;
00140 rxDataIter = 0;
00141 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesPosition;
00142 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesPosition)) {
00143 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesPosition.data[combufDataIter]);
00144 dataBytesIter = 0;
00145 while(dataBytesIter < NF_DATABYTES_SetDrivesPosition){
00146 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesPosition + dataBytesIter);
00147 dataBytesIter++;
00148 }
00149
00150 combufDataIter++;
00151 rxDataIter++;
00152 }
00153 NFComBuf->SetDrivesPosition.updated = 1;
00154 }
00155 }
00156 else
00157 #endif
00158
00159 #ifdef NF_BUFSZ_SetDrivesPWM
00160 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesPWM){
00161 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00162 combufDataIter = 0;
00163 rxDataIter = 0;
00164 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesPWM;
00165 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesPWM)) {
00166 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesPWM.data[combufDataIter]);
00167 dataBytesIter = 0;
00168 while(dataBytesIter < NF_DATABYTES_SetDrivesPWM){
00169 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesPWM + dataBytesIter);
00170 dataBytesIter++;
00171 }
00172
00173 combufDataIter++;
00174 rxDataIter++;
00175 }
00176 NFComBuf->SetDrivesPWM.updated = 1;
00177 }
00178 }
00179 else
00180 #endif
00181
00182 #ifdef NF_BUFSZ_SetDrivesMaxCurrent
00183 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMaxCurrent){
00184 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00185 combufDataIter = 0;
00186 rxDataIter = 0;
00187 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMaxCurrent;
00188 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMaxCurrent)) {
00189 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxCurrent.data[combufDataIter]);
00190 dataBytesIter = 0;
00191 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxCurrent){
00192 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMaxCurrent + dataBytesIter);
00193 dataBytesIter++;
00194 }
00195
00196 combufDataIter++;
00197 rxDataIter++;
00198 }
00199 NFComBuf->SetDrivesMaxCurrent.updated = 1;
00200 }
00201 }
00202 else
00203 #endif
00204
00205 #ifdef NF_BUFSZ_SetDrivesMaxSpeed
00206 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMaxSpeed){
00207 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00208 combufDataIter = 0;
00209 rxDataIter = 0;
00210 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMaxSpeed;
00211 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMaxSpeed)) {
00212 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxSpeed.data[combufDataIter]);
00213 dataBytesIter = 0;
00214 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxSpeed){
00215 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMaxSpeed + dataBytesIter);
00216 dataBytesIter++;
00217 }
00218
00219 combufDataIter++;
00220 rxDataIter++;
00221 }
00222 NFComBuf->SetDrivesMaxSpeed.updated = 1;
00223 }
00224 }
00225 else
00226 #endif
00227
00228 #ifdef NF_BUFSZ_SetDrivesMinPosition
00229 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMinPosition){
00230 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00231 combufDataIter = 0;
00232 rxDataIter = 0;
00233 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMinPosition;
00234 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMinPosition)) {
00235 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMinPosition.data[combufDataIter]);
00236 dataBytesIter = 0;
00237 while(dataBytesIter < NF_DATABYTES_SetDrivesMinPosition){
00238 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMinPosition + dataBytesIter);
00239 dataBytesIter++;
00240 }
00241
00242 combufDataIter++;
00243 rxDataIter++;
00244 }
00245 NFComBuf->SetDrivesMinPosition.updated = 1;
00246 }
00247 }
00248 else
00249 #endif
00250
00251 #ifdef NF_BUFSZ_SetDrivesMaxPosition
00252 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMaxPosition){
00253 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00254 combufDataIter = 0;
00255 rxDataIter = 0;
00256 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMaxPosition;
00257 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMaxPosition)) {
00258 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxPosition.data[combufDataIter]);
00259 dataBytesIter = 0;
00260 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxPosition){
00261 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMaxPosition + dataBytesIter);
00262 dataBytesIter++;
00263 }
00264
00265 combufDataIter++;
00266 rxDataIter++;
00267 }
00268 NFComBuf->SetDrivesMaxPosition.updated = 1;
00269 }
00270 }
00271 else
00272 #endif
00273
00274 #ifdef NF_BUFSZ_SetDrivesMisc
00275 if(rxBuf[rxBufIter] == NF_COMMAND_SetDrivesMisc){
00276 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00277 combufDataIter = 0;
00278 rxDataIter = 0;
00279 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDrivesMisc;
00280 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDrivesMisc)) {
00281 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMisc.data[combufDataIter]);
00282 dataBytesIter = 0;
00283 while(dataBytesIter < NF_DATABYTES_SetDrivesMisc){
00284 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDrivesMisc + dataBytesIter);
00285 dataBytesIter++;
00286 }
00287
00288 combufDataIter++;
00289 rxDataIter++;
00290 }
00291 NFComBuf->SetDrivesMisc.updated = 1;
00292 }
00293 }
00294 else
00295 #endif
00296
00297
00298
00299 #ifdef NF_BUFSZ_SetCurrentRegulator
00300 if(rxBuf[rxBufIter] == NF_COMMAND_SetCurrentRegulator){
00301 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00302 combufDataIter = 0;
00303 rxDataIter = 0;
00304 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetCurrentRegulator;
00305 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetCurrentRegulator)) {
00306 u8TempPt = (uint8_t*) &(NFComBuf->SetCurrentRegulator.data[combufDataIter]);
00307 dataBytesIter = 0;
00308 while(dataBytesIter < NF_DATABYTES_SetCurrentRegulator){
00309 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetCurrentRegulator + dataBytesIter);
00310 dataBytesIter++;
00311 }
00312
00313 combufDataIter++;
00314 rxDataIter++;
00315 }
00316 NFComBuf->SetCurrentRegulator.updated = 1;
00317 }
00318 }
00319 else
00320 #endif
00321
00322 #ifdef NF_BUFSZ_SetSpeedRegulator
00323 if(rxBuf[rxBufIter] == NF_COMMAND_SetSpeedRegulator){
00324 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00325 combufDataIter = 0;
00326 rxDataIter = 0;
00327 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetSpeedRegulator;
00328 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetSpeedRegulator)) {
00329 u8TempPt = (uint8_t*) &(NFComBuf->SetSpeedRegulator.data[combufDataIter]);
00330 dataBytesIter = 0;
00331 while(dataBytesIter < NF_DATABYTES_SetSpeedRegulator){
00332 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetSpeedRegulator + dataBytesIter);
00333 dataBytesIter++;
00334 }
00335
00336 combufDataIter++;
00337 rxDataIter++;
00338 }
00339 NFComBuf->SetSpeedRegulator.updated = 1;
00340 }
00341 }
00342 else
00343 #endif
00344
00345 #ifdef NF_BUFSZ_SetPositionRegulator
00346 if(rxBuf[rxBufIter] == NF_COMMAND_SetPositionRegulator){
00347 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00348 combufDataIter = 0;
00349 rxDataIter = 0;
00350 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetPositionRegulator;
00351 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetPositionRegulator)) {
00352 u8TempPt = (uint8_t*) &(NFComBuf->SetPositionRegulator.data[combufDataIter]);
00353 dataBytesIter = 0;
00354 while(dataBytesIter < NF_DATABYTES_SetPositionRegulator){
00355 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetPositionRegulator + dataBytesIter);
00356 dataBytesIter++;
00357 }
00358
00359 combufDataIter++;
00360 rxDataIter++;
00361 }
00362 NFComBuf->SetPositionRegulator.updated = 1;
00363 }
00364 }
00365 else
00366 #endif
00367
00368
00369
00370 #ifdef NF_BUFSZ_SetServosMode
00371 if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMode){
00372 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00373 combufDataIter = 0;
00374 rxDataIter = 0;
00375 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMode;
00376 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMode)) {
00377 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMode.data[combufDataIter]);
00378 dataBytesIter = 0;
00379 while(dataBytesIter < NF_DATABYTES_SetServosMode){
00380 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMode + dataBytesIter);
00381 dataBytesIter++;
00382 }
00383
00384 combufDataIter++;
00385 rxDataIter++;
00386 }
00387 NFComBuf->SetServosMode.updated = 1;
00388 }
00389 }
00390 else
00391 #endif
00392
00393 #ifdef NF_BUFSZ_SetServosPosition
00394 if(rxBuf[rxBufIter] == NF_COMMAND_SetServosPosition){
00395 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00396 combufDataIter = 0;
00397 rxDataIter = 0;
00398 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosPosition;
00399 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosPosition)) {
00400 u8TempPt = (uint8_t*) &(NFComBuf->SetServosPosition.data[combufDataIter]);
00401 dataBytesIter = 0;
00402 while(dataBytesIter < NF_DATABYTES_SetServosPosition){
00403 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosPosition + dataBytesIter);
00404 dataBytesIter++;
00405 }
00406
00407 combufDataIter++;
00408 rxDataIter++;
00409 }
00410 NFComBuf->SetServosPosition.updated = 1;
00411 }
00412 }
00413 else
00414 #endif
00415
00416 #ifdef NF_BUFSZ_SetServosSpeed
00417 if(rxBuf[rxBufIter] == NF_COMMAND_SetServosSpeed){
00418 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00419 combufDataIter = 0;
00420 rxDataIter = 0;
00421 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosSpeed;
00422 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosSpeed)) {
00423 u8TempPt = (uint8_t*) &(NFComBuf->SetServosSpeed.data[combufDataIter]);
00424 dataBytesIter = 0;
00425 while(dataBytesIter < NF_DATABYTES_SetServosSpeed){
00426 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosSpeed + dataBytesIter);
00427 dataBytesIter++;
00428 }
00429
00430 combufDataIter++;
00431 rxDataIter++;
00432 }
00433 NFComBuf->SetServosSpeed.updated = 1;
00434 }
00435 }
00436 else
00437 #endif
00438
00439 #ifdef NF_BUFSZ_SetServosMaxSpeed
00440 if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMaxSpeed){
00441 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00442 combufDataIter = 0;
00443 rxDataIter = 0;
00444 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMaxSpeed;
00445 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMaxSpeed)) {
00446 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMaxSpeed.data[combufDataIter]);
00447 dataBytesIter = 0;
00448 while(dataBytesIter < NF_DATABYTES_SetServosMaxSpeed){
00449 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMaxSpeed + dataBytesIter);
00450 dataBytesIter++;
00451 }
00452
00453 combufDataIter++;
00454 rxDataIter++;
00455 }
00456 NFComBuf->SetServosMaxSpeed.updated = 1;
00457 }
00458 }
00459 else
00460 #endif
00461
00462 #ifdef NF_BUFSZ_SetServosMinPosition
00463 if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMinPosition){
00464 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00465 combufDataIter = 0;
00466 rxDataIter = 0;
00467 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMinPosition;
00468 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMinPosition)) {
00469 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMinPosition.data[combufDataIter]);
00470 dataBytesIter = 0;
00471 while(dataBytesIter < NF_DATABYTES_SetServosMinPosition){
00472 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMinPosition + dataBytesIter);
00473 dataBytesIter++;
00474 }
00475
00476 combufDataIter++;
00477 rxDataIter++;
00478 }
00479 NFComBuf->SetServosMinPosition.updated = 1;
00480 }
00481 }
00482 else
00483 #endif
00484
00485 #ifdef NF_BUFSZ_SetServosMaxPosition
00486 if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMaxPosition){
00487 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00488 combufDataIter = 0;
00489 rxDataIter = 0;
00490 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMaxPosition;
00491 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMaxPosition)) {
00492 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMaxPosition.data[combufDataIter]);
00493 dataBytesIter = 0;
00494 while(dataBytesIter < NF_DATABYTES_SetServosMaxPosition){
00495 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMaxPosition + dataBytesIter);
00496 dataBytesIter++;
00497 }
00498
00499 combufDataIter++;
00500 rxDataIter++;
00501 }
00502 NFComBuf->SetServosMaxPosition.updated = 1;
00503 }
00504 }
00505 else
00506 #endif
00507
00508 #ifdef NF_BUFSZ_SetServosPositionAdj
00509 if(rxBuf[rxBufIter] == NF_COMMAND_SetServosPositionAdj){
00510 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00511 combufDataIter = 0;
00512 rxDataIter = 0;
00513 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosPositionAdj;
00514 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosPositionAdj)) {
00515 u8TempPt = (uint8_t*) &(NFComBuf->SetServosPositionAdj.data[combufDataIter]);
00516 dataBytesIter = 0;
00517 while(dataBytesIter < NF_DATABYTES_SetServosPositionAdj){
00518 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosPositionAdj + dataBytesIter);
00519 dataBytesIter++;
00520 }
00521
00522 combufDataIter++;
00523 rxDataIter++;
00524 }
00525 NFComBuf->SetServosPositionAdj.updated = 1;
00526 }
00527 }
00528 else
00529 #endif
00530
00531 #ifdef NF_BUFSZ_SetServosMisc
00532 if(rxBuf[rxBufIter] == NF_COMMAND_SetServosMisc){
00533 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00534 combufDataIter = 0;
00535 rxDataIter = 0;
00536 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetServosMisc;
00537 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetServosMisc)) {
00538 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMisc.data[combufDataIter]);
00539 dataBytesIter = 0;
00540 while(dataBytesIter < NF_DATABYTES_SetServosMisc){
00541 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetServosMisc + dataBytesIter);
00542 dataBytesIter++;
00543 }
00544
00545 combufDataIter++;
00546 rxDataIter++;
00547 }
00548 NFComBuf->SetServosMisc.updated = 1;
00549 }
00550 }
00551 else
00552 #endif
00553
00554
00555
00556 #ifdef NF_BUFSZ_SetDigitalOutputs
00557 if(rxBuf[rxBufIter] == NF_COMMAND_SetDigitalOutputs){
00558 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00559 combufDataIter = 0;
00560 rxDataIter = 0;
00561 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDigitalOutputs;
00562 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDigitalOutputs)) {
00563 u8TempPt = (uint8_t*) &(NFComBuf->SetDigitalOutputs.data[combufDataIter]);
00564 dataBytesIter = 0;
00565 while(dataBytesIter < NF_DATABYTES_SetDigitalOutputs){
00566 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDigitalOutputs + dataBytesIter);
00567 dataBytesIter++;
00568 }
00569
00570 combufDataIter++;
00571 rxDataIter++;
00572 }
00573 NFComBuf->SetDigitalOutputs.updated = 1;
00574 }
00575 }
00576 else
00577 #endif
00578
00579
00580
00581 #ifdef NF_BUFSZ_SetDisplayMode
00582 if(rxBuf[rxBufIter] == NF_COMMAND_SetDisplayMode){
00583 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00584 combufDataIter = 0;
00585 rxDataIter = 0;
00586 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDisplayMode;
00587 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDisplayMode)) {
00588 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayMode.data[combufDataIter]);
00589 dataBytesIter = 0;
00590 while(dataBytesIter < NF_DATABYTES_SetDisplayMode){
00591 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDisplayMode + dataBytesIter);
00592 dataBytesIter++;
00593 }
00594
00595 combufDataIter++;
00596 rxDataIter++;
00597 }
00598 NFComBuf->SetDisplayMode.updated = 1;
00599 }
00600 }
00601 else
00602 #endif
00603
00604 #ifdef NF_BUFSZ_SetDisplayText
00605 if(rxBuf[rxBufIter] == NF_COMMAND_SetDisplayText){
00606 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00607 combufDataIter = 0;
00608 rxDataIter = 0;
00609 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDisplayText;
00610 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDisplayText)) {
00611 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayText.data[combufDataIter]);
00612 dataBytesIter = 0;
00613 while(dataBytesIter < NF_DATABYTES_SetDisplayText){
00614 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDisplayText + dataBytesIter);
00615 dataBytesIter++;
00616 }
00617
00618 combufDataIter++;
00619 rxDataIter++;
00620 }
00621 NFComBuf->SetDisplayText.updated = 1;
00622 }
00623 }
00624 else
00625 #endif
00626
00627 #ifdef NF_BUFSZ_SetDisplayBacklight
00628 if(rxBuf[rxBufIter] == NF_COMMAND_SetDisplayBacklight){
00629 if(rxAddress == NFComBuf->myAddress || rxAddress == NF_BroadcastAddress) {
00630 combufDataIter = 0;
00631 rxDataIter = 0;
00632 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_SetDisplayBacklight;
00633 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_SetDisplayBacklight)) {
00634 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayBacklight.data[combufDataIter]);
00635 dataBytesIter = 0;
00636 while(dataBytesIter < NF_DATABYTES_SetDisplayBacklight){
00637 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_SetDisplayBacklight + dataBytesIter);
00638 dataBytesIter++;
00639 }
00640
00641 combufDataIter++;
00642 rxDataIter++;
00643 }
00644 NFComBuf->SetDisplayBacklight.updated = 1;
00645 }
00646 }
00647 else
00648 #endif
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658 #ifdef NF_BUFSZ_ReadDrivesCurrent
00659 if(rxBuf[rxBufIter] == NF_COMMAND_ReadDrivesCurrent){
00660 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDrivesCurrent;
00661
00662 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00663 commandArray[*commandCnt] = NF_COMMAND_ReadDrivesCurrent;
00664 (*commandCnt) ++;
00665 }
00666
00667 else if(rxParamsCnt > 0){
00668 combufDataIter = 0;
00669 rxDataIter = 0;
00670 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDrivesCurrent)) {
00671 if(NFComBuf->ReadDrivesCurrent.addr[combufDataIter] == rxAddress) {
00672 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesCurrent.data[combufDataIter]);
00673 dataBytesIter = 0;
00674 while(dataBytesIter < NF_DATABYTES_ReadDrivesCurrent){
00675 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDrivesCurrent + dataBytesIter);
00676 dataBytesIter++;
00677 }
00678
00679 rxDataIter++;
00680 }
00681 combufDataIter++;
00682 }
00683 }
00684 }
00685 else
00686 #endif
00687
00688 #ifdef NF_BUFSZ_ReadDrivesSpeed
00689 if(rxBuf[rxBufIter] == NF_COMMAND_ReadDrivesSpeed){
00690 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDrivesSpeed;
00691
00692 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00693 commandArray[*commandCnt] = NF_COMMAND_ReadDrivesSpeed;
00694 (*commandCnt) ++;
00695 }
00696
00697 else if(rxParamsCnt > 0){
00698 combufDataIter = 0;
00699 rxDataIter = 0;
00700 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDrivesSpeed)) {
00701 if(NFComBuf->ReadDrivesSpeed.addr[combufDataIter] == rxAddress) {
00702 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesSpeed.data[combufDataIter]);
00703 dataBytesIter = 0;
00704 while(dataBytesIter < NF_DATABYTES_ReadDrivesSpeed){
00705 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDrivesSpeed + dataBytesIter);
00706 dataBytesIter++;
00707 }
00708
00709 rxDataIter++;
00710 }
00711 combufDataIter++;
00712 }
00713 }
00714 }
00715 else
00716 #endif
00717
00718 #ifdef NF_BUFSZ_ReadDrivesPosition
00719 if(rxBuf[rxBufIter] == NF_COMMAND_ReadDrivesPosition){
00720 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDrivesPosition;
00721
00722 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00723 commandArray[*commandCnt] = NF_COMMAND_ReadDrivesPosition;
00724 (*commandCnt) ++;
00725 }
00726
00727 else if(rxParamsCnt > 0){
00728 combufDataIter = 0;
00729 rxDataIter = 0;
00730 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDrivesPosition)) {
00731 if(NFComBuf->ReadDrivesPosition.addr[combufDataIter] == rxAddress) {
00732 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesPosition.data[combufDataIter]);
00733 dataBytesIter = 0;
00734 while(dataBytesIter < NF_DATABYTES_ReadDrivesPosition){
00735 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDrivesPosition + dataBytesIter);
00736 dataBytesIter++;
00737 }
00738
00739 rxDataIter++;
00740 }
00741 combufDataIter++;
00742 }
00743 }
00744 }
00745 else
00746 #endif
00747
00748 #ifdef NF_BUFSZ_ReadDrivesStatus
00749 if(rxBuf[rxBufIter] == NF_COMMAND_ReadDrivesStatus){
00750 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDrivesStatus;
00751
00752 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00753 commandArray[*commandCnt] = NF_COMMAND_ReadDrivesStatus;
00754 (*commandCnt) ++;
00755 }
00756
00757 else if(rxParamsCnt > 0){
00758 combufDataIter = 0;
00759 rxDataIter = 0;
00760 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDrivesStatus)) {
00761 if(NFComBuf->ReadDrivesStatus.addr[combufDataIter] == rxAddress) {
00762 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesStatus.data[combufDataIter]);
00763 dataBytesIter = 0;
00764 while(dataBytesIter < NF_DATABYTES_ReadDrivesStatus){
00765 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDrivesStatus + dataBytesIter);
00766 dataBytesIter++;
00767 }
00768
00769 rxDataIter++;
00770 }
00771 combufDataIter++;
00772 }
00773 }
00774 }
00775 else
00776 #endif
00777
00778
00779
00780 #ifdef NF_BUFSZ_ReadServosCurrent
00781 if(rxBuf[rxBufIter] == NF_COMMAND_ReadServosCurrent){
00782 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadServosCurrent;
00783
00784 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00785 commandArray[*commandCnt] = NF_COMMAND_ReadServosCurrent;
00786 (*commandCnt) ++;
00787 }
00788
00789 else if(rxParamsCnt > 0){
00790 combufDataIter = 0;
00791 rxDataIter = 0;
00792 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadServosCurrent)) {
00793 if(NFComBuf->ReadServosCurrent.addr[combufDataIter] == rxAddress) {
00794 u8TempPt = (uint8_t*) &(NFComBuf->ReadServosCurrent.data[combufDataIter]);
00795 dataBytesIter = 0;
00796 while(dataBytesIter < NF_DATABYTES_ReadServosCurrent){
00797 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadServosCurrent + dataBytesIter);
00798 dataBytesIter++;
00799 }
00800
00801 rxDataIter++;
00802 }
00803 combufDataIter++;
00804 }
00805 }
00806 }
00807 else
00808 #endif
00809
00810 #ifdef NF_BUFSZ_ReadServosPosition
00811 if(rxBuf[rxBufIter] == NF_COMMAND_ReadServosPosition){
00812 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadServosPosition;
00813
00814 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00815 commandArray[*commandCnt] = NF_COMMAND_ReadServosPosition;
00816 (*commandCnt) ++;
00817 }
00818
00819 else if(rxParamsCnt > 0){
00820 combufDataIter = 0;
00821 rxDataIter = 0;
00822 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadServosPosition)) {
00823 if(NFComBuf->ReadServosPosition.addr[combufDataIter] == rxAddress) {
00824 u8TempPt = (uint8_t*) &(NFComBuf->ReadServosPosition.data[combufDataIter]);
00825 dataBytesIter = 0;
00826 while(dataBytesIter < NF_DATABYTES_ReadServosPosition){
00827 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadServosPosition + dataBytesIter);
00828 dataBytesIter++;
00829 }
00830
00831 rxDataIter++;
00832 }
00833 combufDataIter++;
00834 }
00835 }
00836 }
00837 else
00838 #endif
00839
00840 #ifdef NF_BUFSZ_ReadServosStatus
00841 if(rxBuf[rxBufIter] == NF_COMMAND_ReadServosStatus){
00842 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadServosStatus;
00843
00844 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00845 commandArray[*commandCnt] = NF_COMMAND_ReadServosStatus;
00846 (*commandCnt) ++;
00847 }
00848
00849 else if(rxParamsCnt > 0){
00850 combufDataIter = 0;
00851 rxDataIter = 0;
00852 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadServosStatus)) {
00853 if(NFComBuf->ReadServosStatus.addr[combufDataIter] == rxAddress) {
00854 u8TempPt = (uint8_t*) &(NFComBuf->ReadServosStatus.data[combufDataIter]);
00855 dataBytesIter = 0;
00856 while(dataBytesIter < NF_DATABYTES_ReadServosStatus){
00857 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadServosStatus + dataBytesIter);
00858 dataBytesIter++;
00859 }
00860
00861 rxDataIter++;
00862 }
00863 combufDataIter++;
00864 }
00865 }
00866 }
00867 else
00868 #endif
00869
00870
00871
00872 #ifdef NF_BUFSZ_ReadDigitalInputs
00873 if(rxBuf[rxBufIter] == NF_COMMAND_ReadDigitalInputs){
00874 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDigitalInputs;
00875
00876 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00877 commandArray[*commandCnt] = NF_COMMAND_ReadDigitalInputs;
00878 (*commandCnt) ++;
00879 }
00880
00881 else if(rxParamsCnt > 0){
00882 combufDataIter = 0;
00883 rxDataIter = 0;
00884 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDigitalInputs)) {
00885 if(NFComBuf->ReadDigitalInputs.addr[combufDataIter] == rxAddress) {
00886 u8TempPt = (uint8_t*) &(NFComBuf->ReadDigitalInputs.data[combufDataIter]);
00887 dataBytesIter = 0;
00888 while(dataBytesIter < NF_DATABYTES_ReadDigitalInputs){
00889 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDigitalInputs + dataBytesIter);
00890 dataBytesIter++;
00891 }
00892
00893 rxDataIter++;
00894 }
00895 combufDataIter++;
00896 }
00897 }
00898 }
00899 else
00900 #endif
00901
00902
00903
00904 #ifdef NF_BUFSZ_ReadAnalogInputs
00905 if(rxBuf[rxBufIter] == NF_COMMAND_ReadAnalogInputs){
00906 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadAnalogInputs;
00907
00908 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00909 commandArray[*commandCnt] = NF_COMMAND_ReadAnalogInputs;
00910 (*commandCnt) ++;
00911 }
00912
00913 else if(rxParamsCnt > 0){
00914 combufDataIter = 0;
00915 rxDataIter = 0;
00916 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadAnalogInputs)) {
00917 if(NFComBuf->ReadAnalogInputs.addr[combufDataIter] == rxAddress) {
00918 u8TempPt = (uint8_t*) &(NFComBuf->ReadAnalogInputs.data[combufDataIter]);
00919 dataBytesIter = 0;
00920 while(dataBytesIter < NF_DATABYTES_ReadAnalogInputs){
00921 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadAnalogInputs + dataBytesIter);
00922 dataBytesIter++;
00923 }
00924
00925 rxDataIter++;
00926 }
00927 combufDataIter++;
00928 }
00929 }
00930 }
00931 else
00932 #endif
00933
00934
00935
00936 #ifdef NF_BUFSZ_ReadDistance
00937 if(rxBuf[rxBufIter] == NF_COMMAND_ReadDistance){
00938 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDistance;
00939
00940 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00941 commandArray[*commandCnt] = NF_COMMAND_ReadDistance;
00942 (*commandCnt) ++;
00943 }
00944
00945 else if(rxParamsCnt > 0){
00946 combufDataIter = 0;
00947 rxDataIter = 0;
00948 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDistance)) {
00949 if(NFComBuf->ReadDistance.addr[combufDataIter] == rxAddress) {
00950 u8TempPt = (uint8_t*) &(NFComBuf->ReadDistance.data[combufDataIter]);
00951 dataBytesIter = 0;
00952 while(dataBytesIter < NF_DATABYTES_ReadDistance){
00953 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDistance + dataBytesIter);
00954 dataBytesIter++;
00955 }
00956
00957 rxDataIter++;
00958 }
00959 combufDataIter++;
00960 }
00961 }
00962 }
00963 else
00964 #endif
00965
00966
00967
00968 #ifdef NF_BUFSZ_ReadDeviceStatus
00969 if(rxBuf[rxBufIter] == NF_COMMAND_ReadDeviceStatus){
00970 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDeviceStatus;
00971
00972 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
00973 commandArray[*commandCnt] = NF_COMMAND_ReadDeviceStatus;
00974 (*commandCnt) ++;
00975 }
00976
00977 else if(rxParamsCnt > 0){
00978 combufDataIter = 0;
00979 rxDataIter = 0;
00980 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDeviceStatus)) {
00981 if(NFComBuf->ReadDeviceStatus.addr[combufDataIter] == rxAddress) {
00982 u8TempPt = (uint8_t*) &(NFComBuf->ReadDeviceStatus.data[combufDataIter]);
00983 dataBytesIter = 0;
00984 while(dataBytesIter < NF_DATABYTES_ReadDeviceStatus){
00985 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDeviceStatus + dataBytesIter);
00986 dataBytesIter++;
00987 }
00988
00989 rxDataIter++;
00990 }
00991 combufDataIter++;
00992 }
00993 }
00994 }
00995 else
00996 #endif
00997
00998 #ifdef NF_BUFSZ_ReadDeviceVitals
00999 if(rxBuf[rxBufIter] == NF_COMMAND_ReadDeviceVitals){
01000 rxParamsCnt = rxBuf[rxBufIter+1] / NF_DATABYTES_ReadDeviceVitals;
01001
01002 if((rxParamsCnt == 0) && (rxAddress == NFComBuf->myAddress)){
01003 commandArray[*commandCnt] = NF_COMMAND_ReadDeviceVitals;
01004 (*commandCnt) ++;
01005 }
01006
01007 else if(rxParamsCnt > 0){
01008 combufDataIter = 0;
01009 rxDataIter = 0;
01010 while((rxDataIter < rxParamsCnt) && (combufDataIter < NF_BUFSZ_ReadDeviceVitals)) {
01011 if(NFComBuf->ReadDeviceVitals.addr[combufDataIter] == rxAddress) {
01012 u8TempPt = (uint8_t*) &(NFComBuf->ReadDeviceVitals.data[combufDataIter]);
01013 dataBytesIter = 0;
01014 while(dataBytesIter < NF_DATABYTES_ReadDeviceVitals){
01015 *(u8TempPt + dataBytesIter) = *(dataPt + rxDataIter*NF_DATABYTES_ReadDeviceVitals + dataBytesIter);
01016 dataBytesIter++;
01017 }
01018
01019 rxDataIter++;
01020 }
01021 combufDataIter++;
01022 }
01023 }
01024 }
01025 else
01026 #endif
01027 NFComBuf->unknownCommandReceived = 1;
01028
01029
01030 rxBufIter += rxBuf[rxBufIter+1]+2;
01031 }
01032
01033 return rxBufIter;
01034 }
01035
01036
01037
01038
01039
01040
01041
01042
01043
01044 uint8_t NF_MakeCommandFrame(NF_STRUCT_ComBuf *NFComBuf,
01045 uint8_t *txBuf,
01046 const uint8_t *commandArray,
01047 uint8_t commandCnt,
01048 uint8_t txAddress){
01049 uint8_t txBufIter, commandIter, txDataIter, combufDataIter, dataBytesIter;
01050
01051 uint8_t *dataPt;
01052 uint8_t *u8TempPt;
01053
01054 txBuf[0] = '#';
01055 txBuf[3] = txAddress;
01056
01057 txBufIter=4;
01058 commandIter=0;
01059
01060 while(commandIter < commandCnt){
01061
01062 dataPt = txBuf+txBufIter+2;
01063
01064
01065
01066 #ifdef NF_BUFSZ_SetDrivesMode
01067 if(commandArray[commandIter] == NF_COMMAND_SetDrivesMode){
01068 combufDataIter = 0;
01069 txDataIter = 0;
01070 while(combufDataIter < NF_BUFSZ_SetDrivesMode) {
01071 if(NFComBuf->SetDrivesMode.addr[combufDataIter] == txAddress) {
01072 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMode.data[combufDataIter]);
01073 dataBytesIter = 0;
01074 while(dataBytesIter < NF_DATABYTES_SetDrivesMode){
01075 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMode + dataBytesIter) = *(u8TempPt + dataBytesIter);
01076 dataBytesIter++;
01077 }
01078
01079 txDataIter++;
01080 }
01081 combufDataIter++;
01082 }
01083 if(txDataIter > 0){
01084 txBuf[txBufIter] = NF_COMMAND_SetDrivesMode;
01085 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMode;
01086 txBufIter += txBuf[txBufIter+1]+2;
01087 }
01088 }
01089 else
01090 #endif
01091
01092 #ifdef NF_BUFSZ_SetDrivesSpeed
01093 if(commandArray[commandIter] == NF_COMMAND_SetDrivesSpeed){
01094 combufDataIter = 0;
01095 txDataIter = 0;
01096 while(combufDataIter < NF_BUFSZ_SetDrivesSpeed) {
01097 if(NFComBuf->SetDrivesSpeed.addr[combufDataIter] == txAddress) {
01098 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesSpeed.data[combufDataIter]);
01099 dataBytesIter = 0;
01100 while(dataBytesIter < NF_DATABYTES_SetDrivesSpeed){
01101 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01102 dataBytesIter++;
01103 }
01104
01105 txDataIter++;
01106 }
01107 combufDataIter++;
01108 }
01109 if(txDataIter > 0){
01110 txBuf[txBufIter] = NF_COMMAND_SetDrivesSpeed;
01111 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesSpeed;
01112 txBufIter += txBuf[txBufIter+1]+2;
01113 }
01114 }
01115 else
01116 #endif
01117
01118 #ifdef NF_BUFSZ_SetDrivesCurrent
01119 if(commandArray[commandIter] == NF_COMMAND_SetDrivesCurrent){
01120 combufDataIter = 0;
01121 txDataIter = 0;
01122 while(combufDataIter < NF_BUFSZ_SetDrivesCurrent) {
01123 if(NFComBuf->SetDrivesCurrent.addr[combufDataIter] == txAddress) {
01124 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesCurrent.data[combufDataIter]);
01125 dataBytesIter = 0;
01126 while(dataBytesIter < NF_DATABYTES_SetDrivesCurrent){
01127 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesCurrent + dataBytesIter) = *(u8TempPt + dataBytesIter);
01128 dataBytesIter++;
01129 }
01130
01131 txDataIter++;
01132 }
01133 combufDataIter++;
01134 }
01135 if(txDataIter > 0){
01136 txBuf[txBufIter] = NF_COMMAND_SetDrivesCurrent;
01137 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesCurrent;
01138 txBufIter += txBuf[txBufIter+1]+2;
01139 }
01140 }
01141 else
01142 #endif
01143
01144 #ifdef NF_BUFSZ_SetDrivesPosition
01145 if(commandArray[commandIter] == NF_COMMAND_SetDrivesPosition){
01146 combufDataIter = 0;
01147 txDataIter = 0;
01148 while(combufDataIter < NF_BUFSZ_SetDrivesPosition) {
01149 if(NFComBuf->SetDrivesPosition.addr[combufDataIter] == txAddress) {
01150 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesPosition.data[combufDataIter]);
01151 dataBytesIter = 0;
01152 while(dataBytesIter < NF_DATABYTES_SetDrivesPosition){
01153 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01154 dataBytesIter++;
01155 }
01156
01157 txDataIter++;
01158 }
01159 combufDataIter++;
01160 }
01161 if(txDataIter > 0){
01162 txBuf[txBufIter] = NF_COMMAND_SetDrivesPosition;
01163 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesPosition;
01164 txBufIter += txBuf[txBufIter+1]+2;
01165 }
01166 }
01167 else
01168 #endif
01169
01170 #ifdef NF_BUFSZ_SetDrivesPWM
01171 if(commandArray[commandIter] == NF_COMMAND_SetDrivesPWM){
01172 combufDataIter = 0;
01173 txDataIter = 0;
01174 while(combufDataIter < NF_BUFSZ_SetDrivesPWM) {
01175 if(NFComBuf->SetDrivesPWM.addr[combufDataIter] == txAddress) {
01176 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesPWM.data[combufDataIter]);
01177 dataBytesIter = 0;
01178 while(dataBytesIter < NF_DATABYTES_SetDrivesPWM){
01179 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesPWM + dataBytesIter) = *(u8TempPt + dataBytesIter);
01180 dataBytesIter++;
01181 }
01182
01183 txDataIter++;
01184 }
01185 combufDataIter++;
01186 }
01187 if(txDataIter > 0){
01188 txBuf[txBufIter] = NF_COMMAND_SetDrivesPWM;
01189 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesPWM;
01190 txBufIter += txBuf[txBufIter+1]+2;
01191 }
01192 }
01193 else
01194 #endif
01195
01196 #ifdef NF_BUFSZ_SetDrivesMaxCurrent
01197 if(commandArray[commandIter] == NF_COMMAND_SetDrivesMaxCurrent){
01198 combufDataIter = 0;
01199 txDataIter = 0;
01200 while(combufDataIter < NF_BUFSZ_SetDrivesMaxCurrent) {
01201 if(NFComBuf->SetDrivesMaxCurrent.addr[combufDataIter] == txAddress) {
01202 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxCurrent.data[combufDataIter]);
01203 dataBytesIter = 0;
01204 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxCurrent){
01205 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMaxCurrent + dataBytesIter) = *(u8TempPt + dataBytesIter);
01206 dataBytesIter++;
01207 }
01208
01209 txDataIter++;
01210 }
01211 combufDataIter++;
01212 }
01213 if(txDataIter > 0){
01214 txBuf[txBufIter] = NF_COMMAND_SetDrivesMaxCurrent;
01215 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMaxCurrent;
01216 txBufIter += txBuf[txBufIter+1]+2;
01217 }
01218 }
01219 else
01220 #endif
01221
01222 #ifdef NF_BUFSZ_SetDrivesMaxSpeed
01223 if(commandArray[commandIter] == NF_COMMAND_SetDrivesMaxSpeed){
01224 combufDataIter = 0;
01225 txDataIter = 0;
01226 while(combufDataIter < NF_BUFSZ_SetDrivesMaxSpeed) {
01227 if(NFComBuf->SetDrivesMaxSpeed.addr[combufDataIter] == txAddress) {
01228 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxSpeed.data[combufDataIter]);
01229 dataBytesIter = 0;
01230 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxSpeed){
01231 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMaxSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01232 dataBytesIter++;
01233 }
01234
01235 txDataIter++;
01236 }
01237 combufDataIter++;
01238 }
01239 if(txDataIter > 0){
01240 txBuf[txBufIter] = NF_COMMAND_SetDrivesMaxSpeed;
01241 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMaxSpeed;
01242 txBufIter += txBuf[txBufIter+1]+2;
01243 }
01244 }
01245 else
01246 #endif
01247
01248 #ifdef NF_BUFSZ_SetDrivesMinPosition
01249 if(commandArray[commandIter] == NF_COMMAND_SetDrivesMinPosition){
01250 combufDataIter = 0;
01251 txDataIter = 0;
01252 while(combufDataIter < NF_BUFSZ_SetDrivesMinPosition) {
01253 if(NFComBuf->SetDrivesMinPosition.addr[combufDataIter] == txAddress) {
01254 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMinPosition.data[combufDataIter]);
01255 dataBytesIter = 0;
01256 while(dataBytesIter < NF_DATABYTES_SetDrivesMinPosition){
01257 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMinPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01258 dataBytesIter++;
01259 }
01260
01261 txDataIter++;
01262 }
01263 combufDataIter++;
01264 }
01265 if(txDataIter > 0){
01266 txBuf[txBufIter] = NF_COMMAND_SetDrivesMinPosition;
01267 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMinPosition;
01268 txBufIter += txBuf[txBufIter+1]+2;
01269 }
01270 }
01271 else
01272 #endif
01273
01274 #ifdef NF_BUFSZ_SetDrivesMaxPosition
01275 if(commandArray[commandIter] == NF_COMMAND_SetDrivesMaxPosition){
01276 combufDataIter = 0;
01277 txDataIter = 0;
01278 while(combufDataIter < NF_BUFSZ_SetDrivesMaxPosition) {
01279 if(NFComBuf->SetDrivesMaxPosition.addr[combufDataIter] == txAddress) {
01280 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMaxPosition.data[combufDataIter]);
01281 dataBytesIter = 0;
01282 while(dataBytesIter < NF_DATABYTES_SetDrivesMaxPosition){
01283 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMaxPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01284 dataBytesIter++;
01285 }
01286
01287 txDataIter++;
01288 }
01289 combufDataIter++;
01290 }
01291 if(txDataIter > 0){
01292 txBuf[txBufIter] = NF_COMMAND_SetDrivesMaxPosition;
01293 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMaxPosition;
01294 txBufIter += txBuf[txBufIter+1]+2;
01295 }
01296 }
01297 else
01298 #endif
01299
01300 #ifdef NF_BUFSZ_SetDrivesMisc
01301 if(commandArray[commandIter] == NF_COMMAND_SetDrivesMisc){
01302 combufDataIter = 0;
01303 txDataIter = 0;
01304 while(combufDataIter < NF_BUFSZ_SetDrivesMisc) {
01305 if(NFComBuf->SetDrivesMisc.addr[combufDataIter] == txAddress) {
01306 u8TempPt = (uint8_t*) &(NFComBuf->SetDrivesMisc.data[combufDataIter]);
01307 dataBytesIter = 0;
01308 while(dataBytesIter < NF_DATABYTES_SetDrivesMisc){
01309 *(dataPt + txDataIter*NF_DATABYTES_SetDrivesMisc + dataBytesIter) = *(u8TempPt + dataBytesIter);
01310 dataBytesIter++;
01311 }
01312
01313 txDataIter++;
01314 }
01315 combufDataIter++;
01316 }
01317 if(txDataIter > 0){
01318 txBuf[txBufIter] = NF_COMMAND_SetDrivesMisc;
01319 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDrivesMisc;
01320 txBufIter += txBuf[txBufIter+1]+2;
01321 }
01322 }
01323 else
01324 #endif
01325
01326
01327
01328 #ifdef NF_BUFSZ_SetCurrentRegulator
01329 if(commandArray[commandIter] == NF_COMMAND_SetCurrentRegulator){
01330 combufDataIter = 0;
01331 txDataIter = 0;
01332 while(combufDataIter < NF_BUFSZ_SetCurrentRegulator) {
01333 if(NFComBuf->SetCurrentRegulator.addr[combufDataIter] == txAddress) {
01334 u8TempPt = (uint8_t*) &(NFComBuf->SetCurrentRegulator.data[combufDataIter]);
01335 dataBytesIter = 0;
01336 while(dataBytesIter < NF_DATABYTES_SetCurrentRegulator){
01337 *(dataPt + txDataIter*NF_DATABYTES_SetCurrentRegulator + dataBytesIter) = *(u8TempPt + dataBytesIter);
01338 dataBytesIter++;
01339 }
01340
01341 txDataIter++;
01342 }
01343 combufDataIter++;
01344 }
01345 if(txDataIter > 0){
01346 txBuf[txBufIter] = NF_COMMAND_SetCurrentRegulator;
01347 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetCurrentRegulator;
01348 txBufIter += txBuf[txBufIter+1]+2;
01349 }
01350 }
01351 else
01352 #endif
01353
01354 #ifdef NF_BUFSZ_SetSpeedRegulator
01355 if(commandArray[commandIter] == NF_COMMAND_SetSpeedRegulator){
01356 combufDataIter = 0;
01357 txDataIter = 0;
01358 while(combufDataIter < NF_BUFSZ_SetSpeedRegulator) {
01359 if(NFComBuf->SetSpeedRegulator.addr[combufDataIter] == txAddress) {
01360 u8TempPt = (uint8_t*) &(NFComBuf->SetSpeedRegulator.data[combufDataIter]);
01361 dataBytesIter = 0;
01362 while(dataBytesIter < NF_DATABYTES_SetSpeedRegulator){
01363 *(dataPt + txDataIter*NF_DATABYTES_SetSpeedRegulator + dataBytesIter) = *(u8TempPt + dataBytesIter);
01364 dataBytesIter++;
01365 }
01366
01367 txDataIter++;
01368 }
01369 combufDataIter++;
01370 }
01371 if(txDataIter > 0){
01372 txBuf[txBufIter] = NF_COMMAND_SetSpeedRegulator;
01373 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetSpeedRegulator;
01374 txBufIter += txBuf[txBufIter+1]+2;
01375 }
01376 }
01377 else
01378 #endif
01379
01380 #ifdef NF_BUFSZ_SetPositionRegulator
01381 if(commandArray[commandIter] == NF_COMMAND_SetPositionRegulator){
01382 combufDataIter = 0;
01383 txDataIter = 0;
01384 while(combufDataIter < NF_BUFSZ_SetPositionRegulator) {
01385 if(NFComBuf->SetPositionRegulator.addr[combufDataIter] == txAddress) {
01386 u8TempPt = (uint8_t*) &(NFComBuf->SetPositionRegulator.data[combufDataIter]);
01387 dataBytesIter = 0;
01388 while(dataBytesIter < NF_DATABYTES_SetPositionRegulator){
01389 *(dataPt + txDataIter*NF_DATABYTES_SetPositionRegulator + dataBytesIter) = *(u8TempPt + dataBytesIter);
01390 dataBytesIter++;
01391 }
01392
01393 txDataIter++;
01394 }
01395 combufDataIter++;
01396 }
01397 if(txDataIter > 0){
01398 txBuf[txBufIter] = NF_COMMAND_SetPositionRegulator;
01399 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetPositionRegulator;
01400 txBufIter += txBuf[txBufIter+1]+2;
01401 }
01402 }
01403 else
01404 #endif
01405
01406
01407
01408 #ifdef NF_BUFSZ_SetServosMode
01409 if(commandArray[commandIter] == NF_COMMAND_SetServosMode){
01410 combufDataIter = 0;
01411 txDataIter = 0;
01412 while(combufDataIter < NF_BUFSZ_SetServosMode) {
01413 if(NFComBuf->SetServosMode.addr[combufDataIter] == txAddress) {
01414 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMode.data[combufDataIter]);
01415 dataBytesIter = 0;
01416 while(dataBytesIter < NF_DATABYTES_SetServosMode){
01417 *(dataPt + txDataIter*NF_DATABYTES_SetServosMode + dataBytesIter) = *(u8TempPt + dataBytesIter);
01418 dataBytesIter++;
01419 }
01420
01421 txDataIter++;
01422 }
01423 combufDataIter++;
01424 }
01425 if(txDataIter > 0){
01426 txBuf[txBufIter] = NF_COMMAND_SetServosMode;
01427 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMode;
01428 txBufIter += txBuf[txBufIter+1]+2;
01429 }
01430 }
01431 else
01432 #endif
01433
01434 #ifdef NF_BUFSZ_SetServosPosition
01435 if(commandArray[commandIter] == NF_COMMAND_SetServosPosition){
01436 combufDataIter = 0;
01437 txDataIter = 0;
01438 while(combufDataIter < NF_BUFSZ_SetServosPosition) {
01439 if(NFComBuf->SetServosPosition.addr[combufDataIter] == txAddress) {
01440 u8TempPt = (uint8_t*) &(NFComBuf->SetServosPosition.data[combufDataIter]);
01441 dataBytesIter = 0;
01442 while(dataBytesIter < NF_DATABYTES_SetServosPosition){
01443 *(dataPt + txDataIter*NF_DATABYTES_SetServosPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01444 dataBytesIter++;
01445 }
01446
01447 txDataIter++;
01448 }
01449 combufDataIter++;
01450 }
01451 if(txDataIter > 0){
01452 txBuf[txBufIter] = NF_COMMAND_SetServosPosition;
01453 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosPosition;
01454 txBufIter += txBuf[txBufIter+1]+2;
01455 }
01456 }
01457 else
01458 #endif
01459
01460 #ifdef NF_BUFSZ_SetServosSpeed
01461 if(commandArray[commandIter] == NF_COMMAND_SetServosSpeed){
01462 combufDataIter = 0;
01463 txDataIter = 0;
01464 while(combufDataIter < NF_BUFSZ_SetServosSpeed) {
01465 if(NFComBuf->SetServosSpeed.addr[combufDataIter] == txAddress) {
01466 u8TempPt = (uint8_t*) &(NFComBuf->SetServosSpeed.data[combufDataIter]);
01467 dataBytesIter = 0;
01468 while(dataBytesIter < NF_DATABYTES_SetServosSpeed){
01469 *(dataPt + txDataIter*NF_DATABYTES_SetServosSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01470 dataBytesIter++;
01471 }
01472
01473 txDataIter++;
01474 }
01475 combufDataIter++;
01476 }
01477 if(txDataIter > 0){
01478 txBuf[txBufIter] = NF_COMMAND_SetServosSpeed;
01479 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosSpeed;
01480 txBufIter += txBuf[txBufIter+1]+2;
01481 }
01482 }
01483 else
01484 #endif
01485
01486 #ifdef NF_BUFSZ_SetServosMaxSpeed
01487 if(commandArray[commandIter] == NF_COMMAND_SetServosMaxSpeed){
01488 combufDataIter = 0;
01489 txDataIter = 0;
01490 while(combufDataIter < NF_BUFSZ_SetServosMaxSpeed) {
01491 if(NFComBuf->SetServosMaxSpeed.addr[combufDataIter] == txAddress) {
01492 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMaxSpeed.data[combufDataIter]);
01493 dataBytesIter = 0;
01494 while(dataBytesIter < NF_DATABYTES_SetServosMaxSpeed){
01495 *(dataPt + txDataIter*NF_DATABYTES_SetServosMaxSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01496 dataBytesIter++;
01497 }
01498
01499 txDataIter++;
01500 }
01501 combufDataIter++;
01502 }
01503 if(txDataIter > 0){
01504 txBuf[txBufIter] = NF_COMMAND_SetServosMaxSpeed;
01505 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMaxSpeed;
01506 txBufIter += txBuf[txBufIter+1]+2;
01507 }
01508 }
01509 else
01510 #endif
01511
01512 #ifdef NF_BUFSZ_SetServosMinPosition
01513 if(commandArray[commandIter] == NF_COMMAND_SetServosMinPosition){
01514 combufDataIter = 0;
01515 txDataIter = 0;
01516 while(combufDataIter < NF_BUFSZ_SetServosMinPosition) {
01517 if(NFComBuf->SetServosMinPosition.addr[combufDataIter] == txAddress) {
01518 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMinPosition.data[combufDataIter]);
01519 dataBytesIter = 0;
01520 while(dataBytesIter < NF_DATABYTES_SetServosMinPosition){
01521 *(dataPt + txDataIter*NF_DATABYTES_SetServosMinPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01522 dataBytesIter++;
01523 }
01524
01525 txDataIter++;
01526 }
01527 combufDataIter++;
01528 }
01529 if(txDataIter > 0){
01530 txBuf[txBufIter] = NF_COMMAND_SetServosMinPosition;
01531 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMinPosition;
01532 txBufIter += txBuf[txBufIter+1]+2;
01533 }
01534 }
01535 else
01536 #endif
01537
01538 #ifdef NF_BUFSZ_SetServosMaxPosition
01539 if(commandArray[commandIter] == NF_COMMAND_SetServosMaxPosition){
01540 combufDataIter = 0;
01541 txDataIter = 0;
01542 while(combufDataIter < NF_BUFSZ_SetServosMaxPosition) {
01543 if(NFComBuf->SetServosMaxPosition.addr[combufDataIter] == txAddress) {
01544 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMaxPosition.data[combufDataIter]);
01545 dataBytesIter = 0;
01546 while(dataBytesIter < NF_DATABYTES_SetServosMaxPosition){
01547 *(dataPt + txDataIter*NF_DATABYTES_SetServosMaxPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01548 dataBytesIter++;
01549 }
01550
01551 txDataIter++;
01552 }
01553 combufDataIter++;
01554 }
01555 if(txDataIter > 0){
01556 txBuf[txBufIter] = NF_COMMAND_SetServosMaxPosition;
01557 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMaxPosition;
01558 txBufIter += txBuf[txBufIter+1]+2;
01559 }
01560 }
01561 else
01562 #endif
01563
01564 #ifdef NF_BUFSZ_SetServosMisc
01565 if(commandArray[commandIter] == NF_COMMAND_SetServosMisc){
01566 combufDataIter = 0;
01567 txDataIter = 0;
01568 while(combufDataIter < NF_BUFSZ_SetServosMisc) {
01569 if(NFComBuf->SetServosMisc.addr[combufDataIter] == txAddress) {
01570 u8TempPt = (uint8_t*) &(NFComBuf->SetServosMisc.data[combufDataIter]);
01571 dataBytesIter = 0;
01572 while(dataBytesIter < NF_DATABYTES_SetServosMisc){
01573 *(dataPt + txDataIter*NF_DATABYTES_SetServosMisc + dataBytesIter) = *(u8TempPt + dataBytesIter);
01574 dataBytesIter++;
01575 }
01576
01577 txDataIter++;
01578 }
01579 combufDataIter++;
01580 }
01581 if(txDataIter > 0){
01582 txBuf[txBufIter] = NF_COMMAND_SetServosMisc;
01583 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetServosMisc;
01584 txBufIter += txBuf[txBufIter+1]+2;
01585 }
01586 }
01587 else
01588 #endif
01589
01590
01591
01592 #ifdef NF_BUFSZ_SetDigitalOutputs
01593 if(commandArray[commandIter] == NF_COMMAND_SetDigitalOutputs){
01594 combufDataIter = 0;
01595 txDataIter = 0;
01596 while(combufDataIter < NF_BUFSZ_SetDigitalOutputs) {
01597 if(NFComBuf->SetDigitalOutputs.addr[combufDataIter] == txAddress) {
01598 u8TempPt = (uint8_t*) &(NFComBuf->SetDigitalOutputs.data[combufDataIter]);
01599 dataBytesIter = 0;
01600 while(dataBytesIter < NF_DATABYTES_SetDigitalOutputs){
01601 *(dataPt + txDataIter*NF_DATABYTES_SetDigitalOutputs + dataBytesIter) = *(u8TempPt + dataBytesIter);
01602 dataBytesIter++;
01603 }
01604
01605 txDataIter++;
01606 }
01607 combufDataIter++;
01608 }
01609 if(txDataIter > 0){
01610 txBuf[txBufIter] = NF_COMMAND_SetDigitalOutputs;
01611 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDigitalOutputs;
01612 txBufIter += txBuf[txBufIter+1]+2;
01613 }
01614 }
01615 else
01616 #endif
01617
01618
01619
01620 #ifdef NF_BUFSZ_SetDisplayMode
01621 if(commandArray[commandIter] == NF_COMMAND_SetDisplayMode){
01622 combufDataIter = 0;
01623 txDataIter = 0;
01624 while(combufDataIter < NF_BUFSZ_SetDisplayMode) {
01625 if(NFComBuf->SetDisplayMode.addr[combufDataIter] == txAddress) {
01626 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayMode.data[combufDataIter]);
01627 dataBytesIter = 0;
01628 while(dataBytesIter < NF_DATABYTES_SetDisplayMode){
01629 *(dataPt + txDataIter*NF_DATABYTES_SetDisplayMode + dataBytesIter) = *(u8TempPt + dataBytesIter);
01630 dataBytesIter++;
01631 }
01632
01633 txDataIter++;
01634 }
01635 combufDataIter++;
01636 }
01637 if(txDataIter > 0){
01638 txBuf[txBufIter] = NF_COMMAND_SetDisplayMode;
01639 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDisplayMode;
01640 txBufIter += txBuf[txBufIter+1]+2;
01641 }
01642 }
01643 else
01644 #endif
01645
01646 #ifdef NF_BUFSZ_SetDisplayText
01647 if(commandArray[commandIter] == NF_COMMAND_SetDisplayText){
01648 combufDataIter = 0;
01649 txDataIter = 0;
01650 while(combufDataIter < NF_BUFSZ_SetDisplayText) {
01651 if(NFComBuf->SetDisplayText.addr[combufDataIter] == txAddress) {
01652 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayText.data[combufDataIter]);
01653 dataBytesIter = 0;
01654 while(dataBytesIter < NF_DATABYTES_SetDisplayText){
01655 *(dataPt + txDataIter*NF_DATABYTES_SetDisplayText + dataBytesIter) = *(u8TempPt + dataBytesIter);
01656 dataBytesIter++;
01657 }
01658
01659 txDataIter++;
01660 }
01661 combufDataIter++;
01662 }
01663 if(txDataIter > 0){
01664 txBuf[txBufIter] = NF_COMMAND_SetDisplayText;
01665 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDisplayText;
01666 txBufIter += txBuf[txBufIter+1]+2;
01667 }
01668 }
01669 else
01670 #endif
01671
01672 #ifdef NF_BUFSZ_SetDisplayBacklight
01673 if(commandArray[commandIter] == NF_COMMAND_SetDisplayBacklight){
01674 combufDataIter = 0;
01675 txDataIter = 0;
01676 while(combufDataIter < NF_BUFSZ_SetDisplayBacklight) {
01677 if(NFComBuf->SetDisplayBacklight.addr[combufDataIter] == txAddress) {
01678 u8TempPt = (uint8_t*) &(NFComBuf->SetDisplayBacklight.data[combufDataIter]);
01679 dataBytesIter = 0;
01680 while(dataBytesIter < NF_DATABYTES_SetDisplayBacklight){
01681 *(dataPt + txDataIter*NF_DATABYTES_SetDisplayBacklight + dataBytesIter) = *(u8TempPt + dataBytesIter);
01682 dataBytesIter++;
01683 }
01684
01685 txDataIter++;
01686 }
01687 combufDataIter++;
01688 }
01689 if(txDataIter > 0){
01690 txBuf[txBufIter] = NF_COMMAND_SetDisplayBacklight;
01691 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_SetDisplayBacklight;
01692 txBufIter += txBuf[txBufIter+1]+2;
01693 }
01694 }
01695 else
01696 #endif
01697
01698
01699
01700
01701
01702
01703
01704
01705
01706 #ifdef NF_BUFSZ_ReadDeviceStatus
01707 if(commandArray[commandIter] == NF_COMMAND_ReadDeviceStatus){
01708
01709 if(txAddress == NFComBuf->myAddress){
01710 combufDataIter = 0;
01711 txDataIter = 0;
01712 while(combufDataIter < NF_BUFSZ_ReadDeviceStatus) {
01713 u8TempPt = (uint8_t*) &(NFComBuf->ReadDeviceStatus.data[combufDataIter]);
01714 dataBytesIter = 0;
01715 while(dataBytesIter < NF_DATABYTES_ReadDeviceStatus){
01716 *(dataPt + txDataIter*NF_DATABYTES_ReadDeviceStatus + dataBytesIter) = *(u8TempPt + dataBytesIter);
01717 dataBytesIter++;
01718 }
01719 txDataIter++;
01720 combufDataIter++;
01721 }
01722 txBuf[txBufIter] = NF_COMMAND_ReadDeviceStatus;
01723 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDeviceStatus;
01724 txBufIter += txBuf[txBufIter+1]+2;
01725 }
01726
01727 else{
01728 txBuf[txBufIter] = NF_COMMAND_ReadDeviceStatus;
01729 txBuf[txBufIter+1] = 0;
01730 txBufIter += 2;
01731
01732 }
01733 }
01734 else
01735 #endif
01736
01737 #ifdef NF_BUFSZ_ReadDeviceVitals
01738 if(commandArray[commandIter] == NF_COMMAND_ReadDeviceVitals){
01739
01740 if(txAddress == NFComBuf->myAddress){
01741 combufDataIter = 0;
01742 txDataIter = 0;
01743 while(combufDataIter < NF_BUFSZ_ReadDeviceVitals) {
01744 u8TempPt = (uint8_t*) &(NFComBuf->ReadDeviceVitals.data[combufDataIter]);
01745 dataBytesIter = 0;
01746 while(dataBytesIter < NF_DATABYTES_ReadDeviceVitals){
01747 *(dataPt + txDataIter*NF_DATABYTES_ReadDeviceVitals + dataBytesIter) = *(u8TempPt + dataBytesIter);
01748 dataBytesIter++;
01749 }
01750 txDataIter++;
01751 combufDataIter++;
01752 }
01753 txBuf[txBufIter] = NF_COMMAND_ReadDeviceVitals;
01754 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDeviceVitals;
01755 txBufIter += txBuf[txBufIter+1]+2;
01756 }
01757
01758 else{
01759 txBuf[txBufIter] = NF_COMMAND_ReadDeviceVitals;
01760 txBuf[txBufIter+1] = 0;
01761 txBufIter += 2;
01762
01763 }
01764 }
01765 else
01766 #endif
01767
01768
01769
01770 #ifdef NF_BUFSZ_ReadDrivesPosition
01771 if(commandArray[commandIter] == NF_COMMAND_ReadDrivesPosition){
01772
01773 if(txAddress == NFComBuf->myAddress){
01774 combufDataIter = 0;
01775 txDataIter = 0;
01776 while(combufDataIter < NF_BUFSZ_ReadDrivesPosition) {
01777 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesPosition.data[combufDataIter]);
01778 dataBytesIter = 0;
01779 while(dataBytesIter < NF_DATABYTES_ReadDrivesPosition){
01780 *(dataPt + txDataIter*NF_DATABYTES_ReadDrivesPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01781 dataBytesIter++;
01782 }
01783 txDataIter++;
01784 combufDataIter++;
01785 }
01786 txBuf[txBufIter] = NF_COMMAND_ReadDrivesPosition;
01787 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDrivesPosition;
01788 txBufIter += txBuf[txBufIter+1]+2;
01789 }
01790
01791 else{
01792 txBuf[txBufIter] = NF_COMMAND_ReadDrivesPosition;
01793 txBuf[txBufIter+1] = 0;
01794 txBufIter += 2;
01795
01796 }
01797 }
01798 else
01799 #endif
01800
01801 #ifdef NF_BUFSZ_ReadDrivesCurrent
01802 if(commandArray[commandIter] == NF_COMMAND_ReadDrivesCurrent){
01803
01804 if(txAddress == NFComBuf->myAddress){
01805 combufDataIter = 0;
01806 txDataIter = 0;
01807 while(combufDataIter < NF_BUFSZ_ReadDrivesCurrent) {
01808 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesCurrent.data[combufDataIter]);
01809 dataBytesIter = 0;
01810 while(dataBytesIter < NF_DATABYTES_ReadDrivesCurrent){
01811 *(dataPt + txDataIter*NF_DATABYTES_ReadDrivesCurrent + dataBytesIter) = *(u8TempPt + dataBytesIter);
01812 dataBytesIter++;
01813 }
01814 txDataIter++;
01815 combufDataIter++;
01816 }
01817 txBuf[txBufIter] = NF_COMMAND_ReadDrivesCurrent;
01818 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDrivesCurrent;
01819 txBufIter += txBuf[txBufIter+1]+2;
01820 }
01821
01822 else{
01823 txBuf[txBufIter] = NF_COMMAND_ReadDrivesCurrent;
01824 txBuf[txBufIter+1] = 0;
01825 txBufIter += 2;
01826
01827 }
01828 }
01829 else
01830 #endif
01831
01832 #ifdef NF_BUFSZ_ReadDrivesSpeed
01833 if(commandArray[commandIter] == NF_COMMAND_ReadDrivesSpeed){
01834
01835 if(txAddress == NFComBuf->myAddress){
01836 combufDataIter = 0;
01837 txDataIter = 0;
01838 while(combufDataIter < NF_BUFSZ_ReadDrivesSpeed) {
01839 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesSpeed.data[combufDataIter]);
01840 dataBytesIter = 0;
01841 while(dataBytesIter < NF_DATABYTES_ReadDrivesSpeed){
01842 *(dataPt + txDataIter*NF_DATABYTES_ReadDrivesSpeed + dataBytesIter) = *(u8TempPt + dataBytesIter);
01843 dataBytesIter++;
01844 }
01845 txDataIter++;
01846 combufDataIter++;
01847 }
01848 txBuf[txBufIter] = NF_COMMAND_ReadDrivesSpeed;
01849 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDrivesSpeed;
01850 txBufIter += txBuf[txBufIter+1]+2;
01851 }
01852
01853 else{
01854 txBuf[txBufIter] = NF_COMMAND_ReadDrivesSpeed;
01855 txBuf[txBufIter+1] = 0;
01856 txBufIter += 2;
01857
01858 }
01859 }
01860 else
01861 #endif
01862
01863 #ifdef NF_BUFSZ_ReadDrivesStatus
01864 if(commandArray[commandIter] == NF_COMMAND_ReadDrivesStatus){
01865
01866 if(txAddress == NFComBuf->myAddress){
01867 combufDataIter = 0;
01868 txDataIter = 0;
01869 while(combufDataIter < NF_BUFSZ_ReadDrivesStatus) {
01870 u8TempPt = (uint8_t*) &(NFComBuf->ReadDrivesStatus.data[combufDataIter]);
01871 dataBytesIter = 0;
01872 while(dataBytesIter < NF_DATABYTES_ReadDrivesStatus){
01873 *(dataPt + txDataIter*NF_DATABYTES_ReadDrivesStatus + dataBytesIter) = *(u8TempPt + dataBytesIter);
01874 dataBytesIter++;
01875 }
01876 txDataIter++;
01877 combufDataIter++;
01878 }
01879 txBuf[txBufIter] = NF_COMMAND_ReadDrivesStatus;
01880 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDrivesStatus;
01881 txBufIter += txBuf[txBufIter+1]+2;
01882 }
01883
01884 else{
01885 txBuf[txBufIter] = NF_COMMAND_ReadDrivesStatus;
01886 txBuf[txBufIter+1] = 0;
01887 txBufIter += 2;
01888
01889 }
01890 }
01891 else
01892 #endif
01893
01894
01895
01896 #ifdef NF_BUFSZ_ReadServosPosition
01897 if(commandArray[commandIter] == NF_COMMAND_ReadServosPosition){
01898
01899 if(txAddress == NFComBuf->myAddress){
01900 combufDataIter = 0;
01901 txDataIter = 0;
01902 while(combufDataIter < NF_BUFSZ_ReadServosPosition) {
01903 u8TempPt = (uint8_t*) &(NFComBuf->ReadServosPosition.data[combufDataIter]);
01904 dataBytesIter = 0;
01905 while(dataBytesIter < NF_DATABYTES_ReadServosPosition){
01906 *(dataPt + txDataIter*NF_DATABYTES_ReadServosPosition + dataBytesIter) = *(u8TempPt + dataBytesIter);
01907 dataBytesIter++;
01908 }
01909 txDataIter++;
01910 combufDataIter++;
01911 }
01912 txBuf[txBufIter] = NF_COMMAND_ReadServosPosition;
01913 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadServosPosition;
01914 txBufIter += txBuf[txBufIter+1]+2;
01915 }
01916
01917 else{
01918 txBuf[txBufIter] = NF_COMMAND_ReadServosPosition;
01919 txBuf[txBufIter+1] = 0;
01920 txBufIter += 2;
01921
01922 }
01923 }
01924 else
01925 #endif
01926
01927 #ifdef NF_BUFSZ_ReadServosCurrent
01928 if(commandArray[commandIter] == NF_COMMAND_ReadServosCurrent){
01929
01930 if(txAddress == NFComBuf->myAddress){
01931 combufDataIter = 0;
01932 txDataIter = 0;
01933 while(combufDataIter < NF_BUFSZ_ReadServosCurrent) {
01934 u8TempPt = (uint8_t*) &(NFComBuf->ReadServosCurrent.data[combufDataIter]);
01935 dataBytesIter = 0;
01936 while(dataBytesIter < NF_DATABYTES_ReadServosCurrent){
01937 *(dataPt + txDataIter*NF_DATABYTES_ReadServosCurrent + dataBytesIter) = *(u8TempPt + dataBytesIter);
01938 dataBytesIter++;
01939 }
01940 txDataIter++;
01941 combufDataIter++;
01942 }
01943 txBuf[txBufIter] = NF_COMMAND_ReadServosCurrent;
01944 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadServosCurrent;
01945 txBufIter += txBuf[txBufIter+1]+2;
01946 }
01947
01948 else{
01949 txBuf[txBufIter] = NF_COMMAND_ReadServosCurrent;
01950 txBuf[txBufIter+1] = 0;
01951 txBufIter += 2;
01952
01953 }
01954 }
01955 else
01956 #endif
01957
01958 #ifdef NF_BUFSZ_ReadServosStatus
01959 if(commandArray[commandIter] == NF_COMMAND_ReadServosStatus){
01960
01961 if(txAddress == NFComBuf->myAddress){
01962 combufDataIter = 0;
01963 txDataIter = 0;
01964 while(combufDataIter < NF_BUFSZ_ReadServosStatus) {
01965 u8TempPt = (uint8_t*) &(NFComBuf->ReadServosStatus.data[combufDataIter]);
01966 dataBytesIter = 0;
01967 while(dataBytesIter < NF_DATABYTES_ReadServosStatus){
01968 *(dataPt + txDataIter*NF_DATABYTES_ReadServosStatus + dataBytesIter) = *(u8TempPt + dataBytesIter);
01969 dataBytesIter++;
01970 }
01971 txDataIter++;
01972 combufDataIter++;
01973 }
01974 txBuf[txBufIter] = NF_COMMAND_ReadServosStatus;
01975 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadServosStatus;
01976 txBufIter += txBuf[txBufIter+1]+2;
01977 }
01978
01979 else{
01980 txBuf[txBufIter] = NF_COMMAND_ReadServosStatus;
01981 txBuf[txBufIter+1] = 0;
01982 txBufIter += 2;
01983
01984 }
01985 }
01986 else
01987 #endif
01988
01989
01990
01991 #ifdef NF_BUFSZ_ReadAnalogInputs
01992 if(commandArray[commandIter] == NF_COMMAND_ReadAnalogInputs){
01993
01994 if(txAddress == NFComBuf->myAddress){
01995 combufDataIter = 0;
01996 txDataIter = 0;
01997 while(combufDataIter < NF_BUFSZ_ReadAnalogInputs) {
01998 u8TempPt = (uint8_t*) &(NFComBuf->ReadAnalogInputs.data[combufDataIter]);
01999 dataBytesIter = 0;
02000 while(dataBytesIter < NF_DATABYTES_ReadAnalogInputs){
02001 *(dataPt + txDataIter*NF_DATABYTES_ReadAnalogInputs + dataBytesIter) = *(u8TempPt + dataBytesIter);
02002 dataBytesIter++;
02003 }
02004 txDataIter++;
02005 combufDataIter++;
02006 }
02007 txBuf[txBufIter] = NF_COMMAND_ReadAnalogInputs;
02008 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadAnalogInputs;
02009 txBufIter += txBuf[txBufIter+1]+2;
02010 }
02011
02012 else{
02013 txBuf[txBufIter] = NF_COMMAND_ReadAnalogInputs;
02014 txBuf[txBufIter+1] = 0;
02015 txBufIter += 2;
02016
02017 }
02018 }
02019 else
02020 #endif
02021
02022
02023
02024 #ifdef NF_BUFSZ_ReadDistance
02025 if(commandArray[commandIter] == NF_COMMAND_ReadDistance){
02026
02027 if(txAddress == NFComBuf->myAddress){
02028 combufDataIter = 0;
02029 txDataIter = 0;
02030 while(combufDataIter < NF_BUFSZ_ReadDistance) {
02031 u8TempPt = (uint8_t*) &(NFComBuf->ReadDistance.data[combufDataIter]);
02032 dataBytesIter = 0;
02033 while(dataBytesIter < NF_DATABYTES_ReadDistance){
02034 *(dataPt + txDataIter*NF_DATABYTES_ReadDistance + dataBytesIter) = *(u8TempPt + dataBytesIter);
02035 dataBytesIter++;
02036 }
02037 txDataIter++;
02038 combufDataIter++;
02039 }
02040 txBuf[txBufIter] = NF_COMMAND_ReadDistance;
02041 txBuf[txBufIter+1] = txDataIter * NF_DATABYTES_ReadDistance;
02042 txBufIter += txBuf[txBufIter+1]+2;
02043 }
02044
02045 else{
02046 txBuf[txBufIter] = NF_COMMAND_ReadDistance;
02047 txBuf[txBufIter+1] = 0;
02048 txBufIter += 2;
02049
02050 }
02051 }
02052 else
02053 #endif
02054
02055 {
02056 NFComBuf->unknownCommandRequested = 1;
02057 commandIter++;
02058 continue;
02059 }
02060 commandIter++;
02061 }
02062 txBuf[txBufIter] = NFv2_CrcFast(((const uint8_t*)txBuf) + 3, txBufIter-3);
02063 txBuf[1] = txBufIter;
02064 txBuf[2] = ~txBufIter;
02065
02066
02067 return txBufIter+1;
02068 }
02069
02070 void NF_ComBufReset(NF_STRUCT_ComBuf *NFComBuf){
02071 uint16_t combufDataIter;
02072
02073
02074 #ifdef NF_BUFSZ_ReadDeviceStatus
02075 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDeviceStatus; combufDataIter++){
02076 NFComBuf->ReadDeviceStatus.addr[combufDataIter] = NF_BroadcastAddress;
02077 NFComBuf->ReadDeviceStatus.data[combufDataIter] = 0;
02078 }
02079 #endif
02080
02081 #ifdef NF_BUFSZ_ReadDeviceVitals
02082 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDeviceVitals; combufDataIter++){
02083 NFComBuf->ReadDeviceVitals.addr[combufDataIter] = NF_BroadcastAddress;
02084 NFComBuf->ReadDeviceVitals.data[combufDataIter] = 0;
02085 }
02086 #endif
02087
02088
02089
02090 #ifdef NF_BUFSZ_SetDrivesMode
02091 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMode; combufDataIter++){
02092 NFComBuf->SetDrivesMode.addr[combufDataIter] = NF_BroadcastAddress;
02093 NFComBuf->SetDrivesMode.data[combufDataIter] = 0;
02094 }
02095 #endif
02096
02097 #ifdef NF_BUFSZ_SetDrivesSpeed
02098 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesSpeed; combufDataIter++){
02099 NFComBuf->SetDrivesSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02100 NFComBuf->SetDrivesSpeed.data[combufDataIter] = 0;
02101 }
02102 #endif
02103
02104 #ifdef NF_BUFSZ_SetDrivesCurrent
02105 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesCurrent; combufDataIter++){
02106 NFComBuf->SetDrivesCurrent.addr[combufDataIter] = NF_BroadcastAddress;
02107 NFComBuf->SetDrivesCurrent.data[combufDataIter] = 0;
02108 }
02109 #endif
02110
02111 #ifdef NF_BUFSZ_SetDrivesPosition
02112 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesPosition; combufDataIter++){
02113 NFComBuf->SetDrivesPosition.addr[combufDataIter] = NF_BroadcastAddress;
02114 NFComBuf->SetDrivesPosition.data[combufDataIter] = 0;
02115 }
02116 #endif
02117
02118 #ifdef NF_BUFSZ_SetDrivesPWM
02119 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesPWM; combufDataIter++){
02120 NFComBuf->SetDrivesPWM.addr[combufDataIter] = NF_BroadcastAddress;
02121 NFComBuf->SetDrivesPWM.data[combufDataIter] = 0;
02122 }
02123 #endif
02124
02125 #ifdef NF_BUFSZ_SetDrivesMaxCurrent
02126 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMaxCurrent; combufDataIter++){
02127 NFComBuf->SetDrivesMaxCurrent.addr[combufDataIter] = NF_BroadcastAddress;
02128 NFComBuf->SetDrivesMaxCurrent.data[combufDataIter] = 0;
02129 }
02130 #endif
02131
02132 #ifdef NF_BUFSZ_SetDrivesMaxSpeed
02133 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMaxSpeed; combufDataIter++){
02134 NFComBuf->SetDrivesMaxSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02135 NFComBuf->SetDrivesMaxSpeed.data[combufDataIter] = 0;
02136 }
02137 #endif
02138
02139 #ifdef NF_BUFSZ_SetDrivesMinPosition
02140 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMinPosition; combufDataIter++){
02141 NFComBuf->SetDrivesMinPosition.addr[combufDataIter] = NF_BroadcastAddress;
02142 NFComBuf->SetDrivesMinPosition.data[combufDataIter] = 0;
02143 }
02144 #endif
02145
02146 #ifdef NF_BUFSZ_SetDrivesMaxPosition
02147 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMaxPosition; combufDataIter++){
02148 NFComBuf->SetDrivesMaxPosition.addr[combufDataIter] = NF_BroadcastAddress;
02149 NFComBuf->SetDrivesMaxPosition.data[combufDataIter] = 0;
02150 }
02151 #endif
02152
02153 #ifdef NF_BUFSZ_ReadDrivesCurrent
02154 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDrivesCurrent; combufDataIter++){
02155 NFComBuf->ReadDrivesCurrent.addr[combufDataIter] = NF_BroadcastAddress;
02156 NFComBuf->ReadDrivesCurrent.data[combufDataIter] = 0;
02157 }
02158 #endif
02159
02160 #ifdef NF_BUFSZ_ReadDrivesSpeed
02161 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDrivesSpeed; combufDataIter++){
02162 NFComBuf->ReadDrivesSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02163 NFComBuf->ReadDrivesSpeed.data[combufDataIter] = 0;
02164 }
02165 #endif
02166
02167 #ifdef NF_BUFSZ_ReadDrivesPosition
02168 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDrivesPosition; combufDataIter++){
02169 NFComBuf->ReadDrivesPosition.addr[combufDataIter] = NF_BroadcastAddress;
02170 NFComBuf->ReadDrivesPosition.data[combufDataIter] = 0;
02171 }
02172 #endif
02173
02174 #ifdef NF_BUFSZ_SetDrivesMisc
02175 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDrivesMisc; combufDataIter++){
02176 NFComBuf->SetDrivesMisc.addr[combufDataIter] = NF_BroadcastAddress;
02177 NFComBuf->SetDrivesMisc.data[combufDataIter] = 0;
02178 }
02179 #endif
02180
02181 #ifdef NF_BUFSZ_ReadDrivesStatus
02182 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDrivesStatus; combufDataIter++){
02183 NFComBuf->ReadDrivesStatus.addr[combufDataIter] = NF_BroadcastAddress;
02184 NFComBuf->ReadDrivesStatus.data[combufDataIter] = 0;
02185 }
02186 #endif
02187
02188
02189 #ifdef NF_BUFSZ_SetServosMode
02190 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMode; combufDataIter++){
02191 NFComBuf->SetServosMode.addr[combufDataIter] = NF_BroadcastAddress;
02192 NFComBuf->SetServosMode.data[combufDataIter] = 0;
02193 }
02194 #endif
02195
02196 #ifdef NF_BUFSZ_SetServosPosition
02197 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosPosition; combufDataIter++){
02198 NFComBuf->SetServosPosition.addr[combufDataIter] = NF_BroadcastAddress;
02199 NFComBuf->SetServosPosition.data[combufDataIter] = 0;
02200 }
02201 #endif
02202
02203 #ifdef NF_BUFSZ_SetServosSpeed
02204 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosSpeed; combufDataIter++){
02205 NFComBuf->SetServosSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02206 NFComBuf->SetServosSpeed.data[combufDataIter] = 0;
02207 }
02208 #endif
02209
02210 #ifdef NF_BUFSZ_SetServosMaxSpeed
02211 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMaxSpeed; combufDataIter++){
02212 NFComBuf->SetServosMaxSpeed.addr[combufDataIter] = NF_BroadcastAddress;
02213 NFComBuf->SetServosMaxSpeed.data[combufDataIter] = 0;
02214 }
02215 #endif
02216
02217 #ifdef NF_BUFSZ_SetServosMinPosition
02218 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMinPosition; combufDataIter++){
02219 NFComBuf->SetServosMinPosition.addr[combufDataIter] = NF_BroadcastAddress;
02220 NFComBuf->SetServosMinPosition.data[combufDataIter] = 0;
02221 }
02222 #endif
02223
02224 #ifdef NF_BUFSZ_SetServosMaxPosition
02225 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMaxPosition; combufDataIter++){
02226 NFComBuf->SetServosMaxPosition.addr[combufDataIter] = NF_BroadcastAddress;
02227 NFComBuf->SetServosMaxPosition.data[combufDataIter] = 0;
02228 }
02229 #endif
02230
02231 #ifdef NF_BUFSZ_ReadServosCurrent
02232 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadServosCurrent; combufDataIter++){
02233 NFComBuf->ReadServosCurrent.addr[combufDataIter] = NF_BroadcastAddress;
02234 NFComBuf->ReadServosCurrent.data[combufDataIter] = 0;
02235 }
02236 #endif
02237
02238 #ifdef NF_BUFSZ_ReadServosPosition
02239 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadServosPosition; combufDataIter++){
02240 NFComBuf->ReadServosPosition.addr[combufDataIter] = NF_BroadcastAddress;
02241 NFComBuf->ReadServosPosition.data[combufDataIter] = 0;
02242 }
02243 #endif
02244
02245 #ifdef NF_BUFSZ_SetServosMisc
02246 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetServosMisc; combufDataIter++){
02247 NFComBuf->SetServosMisc.addr[combufDataIter] = NF_BroadcastAddress;
02248 NFComBuf->SetServosMisc.data[combufDataIter] = 0;
02249 }
02250 #endif
02251
02252 #ifdef NF_BUFSZ_ReadServosStatus
02253 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadServosStatus; combufDataIter++){
02254 NFComBuf->ReadServosStatus.addr[combufDataIter] = NF_BroadcastAddress;
02255 NFComBuf->ReadServosStatus.data[combufDataIter] = 0;
02256 }
02257 #endif
02258
02259
02260
02261 #ifdef NF_BUFSZ_ReadDigitalInputs
02262 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDigitalInputs; combufDataIter++){
02263 NFComBuf->ReadDigitalInputs.addr[combufDataIter] = NF_BroadcastAddress;
02264 NFComBuf->ReadDigitalInputs.data[combufDataIter] = 0;
02265 }
02266 #endif
02267
02268 #ifdef NF_BUFSZ_SetDigitalOutputs
02269 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDigitalOutputs; combufDataIter++){
02270 NFComBuf->SetDigitalOutputs.addr[combufDataIter] = NF_BroadcastAddress;
02271 NFComBuf->SetDigitalOutputs.data[combufDataIter] = 0;
02272 }
02273 #endif
02274
02275
02276
02277 #ifdef NF_BUFSZ_ReadAnalogInputs
02278 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadAnalogInputs; combufDataIter++){
02279 NFComBuf->ReadAnalogInputs.addr[combufDataIter] = NF_BroadcastAddress;
02280 NFComBuf->ReadAnalogInputs.data[combufDataIter] = 0;
02281 }
02282 #endif
02283
02284
02285
02286 #ifdef NF_BUFSZ_ReadDistance
02287 for(combufDataIter=0; combufDataIter<NF_BUFSZ_ReadDistance; combufDataIter++){
02288 NFComBuf->ReadDistance.addr[combufDataIter] = NF_BroadcastAddress;
02289 NFComBuf->ReadDistance.data[combufDataIter] = 0;
02290 }
02291 #endif
02292
02293
02294
02295 #ifdef NF_BUFSZ_SetDisplayMode
02296 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDisplayMode; combufDataIter++){
02297 NFComBuf->SetDisplayMode.addr[combufDataIter] = NF_BroadcastAddress;
02298 NFComBuf->SetDisplayMode.data[combufDataIter] = 0;
02299 }
02300 #endif
02301
02302 #ifdef NF_BUFSZ_SetDisplayText
02303 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDisplayText; combufDataIter++){
02304 NFComBuf->SetDisplayText.addr[combufDataIter] = NF_BroadcastAddress;
02305 NFComBuf->SetDisplayText.data[combufDataIter] = 0;
02306 }
02307 #endif
02308
02309 #ifdef NF_BUFSZ_SetDisplayBacklight
02310 for(combufDataIter=0; combufDataIter<NF_BUFSZ_SetDisplayBacklight; combufDataIter++){
02311 NFComBuf->SetDisplayBacklight.addr[combufDataIter] = NF_BroadcastAddress;
02312 NFComBuf->SetDisplayBacklight.data[combufDataIter] = 0;
02313 }
02314 #endif
02315 }
02316
02317 void NFv2_CrcInit(void)
02318 {
02319 uint8_t remainder;
02320 uint8_t b;
02321 uint16_t dividend;
02322
02323
02324
02325
02326 for (dividend = 0; dividend < 256; ++dividend)
02327 {
02328
02329
02330
02331 remainder = dividend << (WIDTH - 8);
02332
02333
02334
02335
02336 for (b = 8; b > 0; --b)
02337 {
02338
02339
02340
02341 if (remainder & TOPBIT)
02342 {
02343 remainder = (remainder << 1) ^ POLYNOMIAL;
02344 }
02345 else
02346 {
02347 remainder = (remainder << 1);
02348 }
02349 }
02350
02351
02352
02353
02354 crcTable[dividend] = remainder;
02355 }
02356
02357 }
02358
02359 uint8_t NFv2_CrcFast(const uint8_t message[], uint8_t nBytes)
02360 {
02361 uint8_t mydata;
02362 uint8_t byte;
02363 uint8_t remainder = 0;
02364
02365
02366
02367
02368
02369 for (byte = 0; byte < nBytes; ++byte)
02370 {
02371 mydata = message[byte] ^ (remainder >> (WIDTH - 8));
02372 remainder = crcTable[mydata] ^ (remainder << 8);
02373 }
02374
02375
02376
02377
02378 return (remainder);
02379
02380 }