00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "pdo.h"
00025 #include "objacces.h"
00026 #include "canfestival.h"
00027
00037 UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req)
00038 {
00039 UNS8 i;
00040 if( d->nodeState == Operational ) {
00041 Message m;
00042
00044 m.cob_id.w = pdo.cobId & 0x7FF;
00046 if ( req == NOT_A_REQUEST ) {
00047 UNS8 i;
00048 m.rtr = NOT_A_REQUEST;
00049 m.len = pdo.len;
00052 for (i = 0 ; i < pdo.len ; i++)
00053 m.data[i] = pdo.data[i];
00054 }
00055 else {
00056 m.rtr = REQUEST;
00057 m.len = 0;
00058 }
00059
00060 MSG_WAR(0x3901, "sendPDO cobId :", m.cob_id.w);
00061 MSG_WAR(0x3902, " Nb octets : ", m.len);
00062 for (i = 0 ; i < m.len ; i++) {
00063 MSG_WAR(0x3903," data : ", m.data[i]);
00064 }
00065
00066 return canSend(d->canHandle,&m);
00067 }
00068 return 0xFF;
00069 }
00070
00079 UNS8 PDOmGR(CO_Data* d, UNS32 cobId)
00080 {
00081 UNS8 res;
00082 UNS8 i;
00083 s_PDO pdo;
00084
00085 MSG_WAR(0x3905, "PDOmGR",0);
00086
00089 pdo.cobId = cobId;
00090 pdo.len = d->process_var.count;
00092
00093 for ( i = 0 ; i < pdo.len ; i++)
00094 pdo.data[i] = d->process_var.data[i];
00095
00096 res = sendPDO(d, pdo, NOT_A_REQUEST);
00097
00098 return res;
00099 }
00100
00101 #if 0
00102
00103
00104
00105
00106 UNS8 buildPDO(CO_Data* d, UNS16 index)
00107 {
00108 UNS16 ind;
00109 UNS8 subInd;
00110
00111 UNS8 * pMappingCount = NULL;
00112
00113
00114
00115
00116 UNS32 * pMappingParameter = NULL;
00117
00118 UNS8 Size;
00119 UNS8 dataType;
00120 UNS8 offset;
00121 UNS16 offsetObjdict;
00122 UNS16 offsetObjdictPrm;
00123 UNS32 objDict;
00124
00125 subInd=(UNS8)0x00;
00126 offset = 0x00;
00127 ind = index - 0x1800;
00128
00129 MSG_WAR(0x3910,"Prepare PDO to send index :", index);
00130
00132 if( d->nodeState != Operational ) {
00133 MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);
00134 return 0xFF;
00135 }
00136 offsetObjdictPrm = d->firstIndex->PDO_TRS;
00137 offsetObjdict = d->firstIndex->PDO_TRS_MAP;
00138
00139 if (offsetObjdictPrm && offsetObjdict)
00140 {
00142 pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;
00143 MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);
00144 MSG_WAR(0x3913, " at index : ", 0x1A00 + ind);
00145 while (subInd < *pMappingCount) {
00147 pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;
00148 MSG_WAR(0x3914, "Get the mapping at index : ", (UNS16)0x1A00 + ind);
00149 MSG_WAR(0x3915, " subIndex : ", subInd + 1);
00150 MSG_WAR(0x3916, " value : ", *(UNS32 *)pMappingParameter);
00152 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
00153 objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),
00154 (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
00155 (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
00156
00157 if (objDict != OD_SUCCESSFUL) {
00158 MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1);
00159 MSG_WAR(0x2920, " Mapped at index : ", (*pMappingParameter) >> 16);
00160 MSG_WAR(0x2921, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00161 return 0xFF;
00162 }
00163
00164 offset += Size;
00165 d->process_var.count = offset;
00166 subInd++;
00167 }
00168 }
00169 return 0;
00170 }
00171 #endif
00172
00181 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId )
00182 {
00183 UNS32 * pwCobId;
00184 UNS16 offset;
00185 UNS16 lastIndex;
00186 UNS8 err;
00187
00188 MSG_WAR(0x3930, "sendPDOrequest ",0);
00192 offset = d->firstIndex->PDO_RCV;
00193 lastIndex = d->lastIndex->PDO_RCV;
00194 if (offset)
00195 while (offset <= lastIndex) {
00197 pwCobId = d->objdict[offset].pSubindex[1].pObject;
00198
00199 if ( *pwCobId == cobId ) {
00200 s_PDO pdo;
00201 pdo.cobId = *pwCobId;
00202 pdo.len = 0;
00203 err = sendPDO(d, pdo, REQUEST);
00204 return err;
00205 }
00206 offset++;
00207 }
00208 MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId);
00209 return 0xFF;
00210 }
00211
00212
00221 UNS8 proceedPDO(CO_Data* d, Message *m)
00222 {
00223 UNS8 numPdo;
00224 UNS8 numMap;
00225 UNS8 i;
00226 UNS8 * pMappingCount = NULL;
00231 UNS32 * pMappingParameter = NULL;
00232 UNS8 * pTransmissionType = NULL;
00234 UNS32 * pwCobId = NULL;
00235 UNS8 Size;
00236 UNS8 dataType;
00237 UNS8 offset;
00238 UNS8 status;
00239 UNS32 objDict;
00240 UNS16 offsetObjdict;
00241 UNS16 lastIndex;
00242 status = state1;
00243
00244 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
00245 offset = 0x00;
00246 numPdo = 0;
00247 numMap = 0;
00248 if((*m).rtr == NOT_A_REQUEST ) {
00250 offsetObjdict = d->firstIndex->PDO_RCV;
00251 lastIndex = d->lastIndex->PDO_RCV;
00252
00254 if(offsetObjdict)
00255 while (offsetObjdict <= lastIndex) {
00256
00257 switch( status ) {
00258
00259 case state1:
00262 for ( i = 0 ; i < m->len ; i++)
00263 d->process_var.data[i] = m->data[i];
00264 d->process_var.count = (*m).len;
00265
00266 status = state2;
00267 break;
00268
00269 case state2:
00272 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
00276 if ( *pwCobId == (*m).cob_id.w ){
00278 status = state4;
00279 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
00280 break;
00281 }
00282 else {
00285 numPdo++;
00286 offsetObjdict++;
00287 status = state2;
00288 break;
00289 }
00290
00291 case state4:
00294 offsetObjdict = d->firstIndex->PDO_RCV_MAP;
00295 lastIndex = d->lastIndex->PDO_RCV_MAP;
00296 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
00297 numMap = 0;
00298 while (numMap < *pMappingCount) {
00299 UNS8 tmp[]= {0,0,0,0,0,0,0,0};
00300 UNS8 ByteSize;
00301 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
00302 if (pMappingParameter == NULL) {
00303 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
00304 return 0xFF;
00305 }
00313 Size = (UNS8)(*pMappingParameter);
00314
00316 CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
00317
00318 ByteSize = 1 + ((Size - 1) >> 3);
00319
00320
00321 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
00322 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
00323 tmp, &ByteSize, 0 );
00324
00325 if(objDict != OD_SUCCESSFUL) {
00326 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
00327 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16);
00328 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00329 return 0xFF;
00330 }
00331
00332 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
00333 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16);
00334 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00336 offset += Size;
00337 numMap++;
00338 }
00340 offset=0x00;
00341 numMap = 0;
00342 return 0;
00343
00344 }
00345 }
00346 }
00347 else if ((*m).rtr == REQUEST ){
00348 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
00349 status = state1;
00350 offsetObjdict = d->firstIndex->PDO_TRS;
00351 lastIndex = d->lastIndex->PDO_TRS;
00352 if(offsetObjdict) while( offsetObjdict <= lastIndex ){
00355 switch( status ){
00356
00357 case state1:
00360 pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;
00361 if ( *pwCobId == (*m).cob_id.w ) {
00362 status = state4;
00363 break;
00364 }
00365 else {
00366 numPdo++;
00367 offsetObjdict++;
00368 }
00369 status = state1;
00370 break;
00371
00372
00373 case state4:
00374 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
00375 if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {
00376 status = state5;
00377 break;
00378 }
00379 else {
00382 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
00383 return 0xFF;
00384 }
00385
00386 case state5:
00387 offsetObjdict = d->firstIndex->PDO_TRS_MAP;
00388 lastIndex = d->lastIndex->PDO_TRS_MAP;
00389 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
00390 numMap = 0;
00391 while (numMap < *pMappingCount) {
00392 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
00394 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
00395 objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),
00396 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),
00397 (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
00398 if (objDict != OD_SUCCESSFUL) {
00399 MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1);
00400 MSG_WAR(0x2949, " Mapped at index : ", (*pMappingParameter) >> 16);
00401 MSG_WAR(0x2950, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00402 return 0xFF;
00403 }
00404 offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);
00405 d->process_var.count = offset;
00406 numMap++;
00407
00408 }
00409 PDOmGR( d, *pwCobId );
00410 return 0;
00411
00412 }
00413 }
00414 }
00416 return 0;
00417 }
00418
00430 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
00431 {
00434
00435 while(NbBits > 0)
00436 {
00438 INTEGER8 Vect = DestBitIndex - SrcBitIndex;
00439
00441 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
00442
00444 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex );
00445 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
00446
00448 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
00449
00451 UNS8 Filtered = Aligned & ~Mask;
00452
00454 *DestByteIndex &= Mask;
00455
00457 *DestByteIndex |= Filtered ;
00458
00460 if((SrcBitIndex += BitsToCopy)>7)
00461 {
00462 SrcBitIndex = 0;
00463 SrcByteIndex += (SrcBigEndian ? -1 : 1);
00464 }
00465
00466
00468 if((DestBitIndex += BitsToCopy)>7)
00469 {
00470 DestBitIndex = 0;
00471 DestByteIndex += (DestBigEndian ? -1 : 1);
00472 }
00473
00475 NbBits -= BitsToCopy;
00476 }
00477
00478 }
00479
00480 #if 0
00481
00482
00483
00484
00485
00486
00495 UNS8 sendPDOevent( CO_Data* d, void * variable )
00496 {
00498 UNS32 objDict = 0;
00499 UNS8 ind, sub_ind;
00500 UNS8 status;
00501 UNS8 offset;
00502 UNS8 * pMappingCount = NULL;
00503 UNS32 * pMappingParameter = NULL;
00504 void * pMappedAppObject = NULL;
00505 UNS8 * pTransmissionType = NULL;
00507 UNS32 * pwCobId = NULL;
00508 UNS8 * pSize;
00509 UNS8 size;
00510 UNS8 dataType;
00511 UNS16 offsetObjdict;
00512 UNS16 offsetObjdictPrm;
00513 UNS16 lastIndex;
00514 UNS8 numMap;
00515 ind = 0x00;
00516 sub_ind = 1;
00517 offset = 0x00;
00518 pSize = &size;
00519 status = state1;
00520
00521
00525 MSG_WAR (0x3960, "sendPDOevent", 0);
00526 offsetObjdictPrm = d->firstIndex->PDO_TRS;
00527
00528 offsetObjdict = d->firstIndex->PDO_TRS_MAP;
00529 lastIndex = d->lastIndex->PDO_TRS_MAP;
00530
00531 if (offsetObjdictPrm && offsetObjdict)
00533 while(offsetObjdict <= lastIndex){
00535 pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject;
00536 if (*pTransmissionType != TRANS_EVENT) {
00537 ind++;
00538 offsetObjdict++;
00539 offsetObjdictPrm++;
00540 continue;
00541 }
00542 pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject;
00543 numMap = 1;
00544 while (numMap <= *pMappingCount) {
00545 pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject;
00547 objDict = getODentry( d,
00548 (UNS16)((*pMappingParameter) >> 16),
00549 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF),
00550 (void * *)&pMappedAppObject, pSize, &dataType, 0 );
00551 if( objDict != OD_SUCCESSFUL ) {
00552 MSG_WAR(0x2961, "Error in dict. at index : ",
00553 (*pMappingParameter) >> (UNS8)16);
00554
00555 MSG_WAR(0x2962, " subindex : ",
00556 ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
00557 return 0xFF;
00558 }
00559 if (pMappedAppObject == variable) {
00560 MSG_WAR(0x3963, "Variable to send found at index : ",
00561 (*pMappingParameter) >> 16);
00562 MSG_WAR(0x3964, " subIndex : ",
00563 ((*pMappingParameter) >> 8 ) & 0x000000FF);
00564 buildPDO(d, 0x1800 + ind);
00566 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject;
00567 PDOmGR( d, *pwCobId );
00568 return 0;
00569 }
00570 numMap++;
00571 }
00572 ind++;
00573 offsetObjdict++;
00574 offsetObjdictPrm++;
00575 }
00577 MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0);
00578 return 0xFF;
00579
00580 }
00581 #endif
00582
00583
00584