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
00048 UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo)
00049 {
00050 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;
00051 const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
00052
00053 UNS8 prp_j = 0x00;
00054 UNS8 offset = 0x00;
00055 const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject;
00056
00057 pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF;
00058 pdo->rtr = NOT_A_REQUEST;
00059
00060 MSG_WAR(0x3009, " PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject);
00061 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount );
00062
00063 do{
00064 UNS8 dataType;
00065 UNS8 tmp[]= {0,0,0,0,0,0,0,0};
00066
00067
00068 UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject;
00069 UNS16 index = (UNS16)((*pMappingParameter) >> 16);
00070 UNS8 Size = (UNS8)(*pMappingParameter);
00071 UNS8 ByteSize = 1 + ((Size - 1) >> 3);
00072 UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
00073
00074 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter);
00075 MSG_WAR(0x3050, " at index : ", TPDO_map->index);
00076 MSG_WAR(0x3051, " sub-index : ", prp_j + 1);
00077
00078 if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){
00079 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
00080 return 0xFF;
00081 }
00082
00083 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0);
00084
00085 offset += Size ;
00086 prp_j++;
00087 }while( prp_j < *pMappingCount );
00088
00089 pdo->len = 1 + ((offset - 1) >> 3);
00090
00091 MSG_WAR(0x3015, " End scan mapped variable", 0);
00092
00093 return 0;
00094 }
00095
00104 UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex )
00105 {
00106 UNS32 * pwCobId;
00107 UNS16 offset = d->firstIndex->PDO_RCV;
00108 UNS16 lastIndex = d->lastIndex->PDO_RCV;
00109
00110
00111
00112
00113
00114 MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex);
00115
00116 if (offset && RPDOIndex >= 0x1400){
00117 offset += RPDOIndex - 0x1400;
00118 if (offset <= lastIndex) {
00119
00120 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
00121
00122 MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId);
00123
00124 Message pdo = {*pwCobId, REQUEST, 0};
00125 return canSend(d->canHandle,&pdo);
00126 }
00127 }
00128 MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
00129 return 0xFF;
00130 }
00131
00132
00141 UNS8 proceedPDO(CO_Data* d, Message *m)
00142 {
00143 UNS8 numPdo;
00144 UNS8 numMap;
00145 UNS8 i;
00146 UNS8 * pMappingCount = NULL;
00147
00148
00149
00150
00151 UNS32 * pMappingParameter = NULL;
00152 UNS8 * pTransmissionType = NULL;
00153
00154 UNS32 * pwCobId = NULL;
00155 UNS8 Size;
00156 UNS8 dataType;
00157 UNS8 offset;
00158 UNS8 status;
00159 UNS32 objDict;
00160 UNS16 offsetObjdict;
00161 UNS16 lastIndex;
00162
00163 status = state2;
00164
00165 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
00166 offset = 0x00;
00167 numPdo = 0;
00168 numMap = 0;
00169 if((*m).rtr == NOT_A_REQUEST ) {
00170
00171
00172 offsetObjdict = d->firstIndex->PDO_RCV;
00173 lastIndex = d->lastIndex->PDO_RCV;
00174
00175
00176 if(offsetObjdict)
00177 while (offsetObjdict <= lastIndex) {
00178
00179 switch( status ) {
00180
00181 case state2:
00182
00183
00184 pwCobId = (UNS32*) d->objdict[offsetObjdict].pSubindex[1].pObject;
00185
00186
00187
00188 if ( *pwCobId == (*m).cob_id.w ){
00189
00190 status = state4;
00191 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
00192 break;
00193 }
00194 else {
00195
00196
00197 numPdo++;
00198 offsetObjdict++;
00199 status = state2;
00200 break;
00201 }
00202
00203 case state4:
00204
00205
00206 offsetObjdict = d->firstIndex->PDO_RCV_MAP;
00207 lastIndex = d->lastIndex->PDO_RCV_MAP;
00208 pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
00209 numMap = 0;
00210 while (numMap < *pMappingCount) {
00211 UNS8 tmp[]= {0,0,0,0,0,0,0,0};
00212 UNS8 ByteSize;
00213 pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
00214 if (pMappingParameter == NULL) {
00215 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
00216 return 0xFF;
00217 }
00218
00219
00220
00221
00222
00223
00224
00225 Size = (UNS8)(*pMappingParameter);
00226
00227
00228 CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
00229
00230 ByteSize = 1 + ((Size - 1) >> 3);
00231
00232
00233 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
00234 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
00235 tmp, &ByteSize, 0 );
00236
00237 if(objDict != OD_SUCCESSFUL) {
00238 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
00239 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16);
00240 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00241 return 0xFF;
00242 }
00243
00244 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
00245 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16);
00246 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00247
00248 offset += Size;
00249 numMap++;
00250
00251 }
00252
00253 offset=0x00;
00254 numMap = 0;
00255 return 0;
00256
00257 }
00258 }
00259 }
00260 else if ((*m).rtr == REQUEST ){
00261 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
00262 status = state1;
00263 offsetObjdict = d->firstIndex->PDO_TRS;
00264 lastIndex = d->lastIndex->PDO_TRS;
00265 if(offsetObjdict) while( offsetObjdict <= lastIndex ){
00266
00267
00268 switch( status ){
00269
00270 case state1:
00271
00272
00273 pwCobId = (UNS32*) (d->objdict + offsetObjdict)->pSubindex[1].pObject;
00274 if ( *pwCobId == (*m).cob_id.w ) {
00275 status = state4;
00276 break;
00277 }
00278 else {
00279 numPdo++;
00280 offsetObjdict++;
00281 }
00282 status = state1;
00283 break;
00284
00285
00286 case state4:
00287 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;
00288
00289 if ( (*pTransmissionType == TRANS_RTR)) {
00290 status = state5;
00291 break;
00292
00293 }else if ((*pTransmissionType == TRANS_RTR_SYNC )) {
00294 if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){
00295
00296 canSend(d->canHandle,&d->PDO_status[numPdo].last_message);
00297 return 0;
00298 }else{
00299
00300
00301 MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id.w);
00302 status = state5;
00303 }
00304 break;
00305 }else if(
00306 (*pTransmissionType == TRANS_EVENT_PROFILE) ||
00307 (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {
00308
00309 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
00310 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
00311 d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED;
00312
00313 PDOEventTimerAlarm(d, numPdo);
00314 return 0;
00315 }else {
00316
00317
00318 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
00319 return 0xFF;
00320 }
00321
00322 case state5:
00323 {
00324 Message pdo;
00325 if( buildPDO(d, numPdo, &pdo))
00326 {
00327 MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo);
00328 return 0xFF;
00329 }
00330 canSend(d->canHandle,&pdo);
00331 return 0;
00332 }
00333 }
00334 }
00335 }
00336
00337 return 0;
00338 }
00339
00351 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
00352 {
00353
00354
00355
00356 while(NbBits > 0)
00357 {
00358
00359 INTEGER8 Vect = DestBitIndex - SrcBitIndex;
00360
00361
00362 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
00363
00364
00365 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex );
00366 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
00367
00368
00369 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
00370
00371
00372 UNS8 Filtered = Aligned & ~Mask;
00373
00374
00375 *DestByteIndex &= Mask;
00376
00377
00378 *DestByteIndex |= Filtered ;
00379
00380
00381 if((SrcBitIndex += BitsToCopy)>7)
00382 {
00383 SrcBitIndex = 0;
00384 SrcByteIndex += (SrcBigEndian ? -1 : 1);
00385 }
00386
00387
00388
00389 if((DestBitIndex += BitsToCopy)>7)
00390 {
00391 DestBitIndex = 0;
00392 DestByteIndex += (DestBigEndian ? -1 : 1);
00393 }
00394
00395
00396 NbBits -= BitsToCopy;
00397 }
00398
00399 }
00408 UNS8 sendPDOevent( CO_Data* d)
00409 {
00410
00411 return _sendPDOevent(d, 0);
00412 }
00413
00414
00415 void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)
00416 {
00417
00418 d->PDO_status[pdoNum].event_timer = TIMER_NONE;
00419
00420 d->PDO_status[pdoNum].last_message.cob_id.w = 0;
00421 _sendPDOevent( d, 0 );
00422 }
00423
00424 void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)
00425 {
00426
00427 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
00428
00429 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
00430 _sendPDOevent( d, 0 );
00431 }
00432
00442 UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )
00443 {
00444 UNS8 pdoNum = 0x00;
00445 UNS8* pTransmissionType = NULL;
00446 UNS8 status = state3;
00447 UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
00448 UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
00449 UNS16 lastIndex = d->lastIndex->PDO_TRS;
00450
00451
00452 if(offsetObjdict){
00453 Message pdo = Message_Initializer;
00454 while( offsetObjdict <= lastIndex) {
00455 switch( status ) {
00456 case state3:
00457 if (
00458
00459 *(UNS32*)d->objdict[offsetObjdict].pSubindex[0].pObject & 0x8000) {
00460 MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
00461
00462 status = state11;
00463 break;
00464 }
00465
00466 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;
00467 MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
00468
00469
00470
00471 if( isSyncEvent &&
00472 (*pTransmissionType >= TRANS_SYNC_MIN) &&
00473 (*pTransmissionType <= TRANS_SYNC_MAX) &&
00474 (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) {
00475
00476 d->PDO_status[pdoNum].transmit_type_parameter = 0;
00477 MSG_WAR(0x3964, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
00478 pdo = (Message)Message_Initializer;
00479 if(buildPDO(d, pdoNum, &pdo))
00480 {
00481 MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum);
00482 status = state11;
00483 break;
00484 }
00485 status = state5;
00486
00487 }else if( isSyncEvent &&
00488 (*pTransmissionType == TRANS_RTR_SYNC)) {
00489 if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message))
00490 {
00491 MSG_ERR(0x1966, " Couldn't build TPDO number : ", pdoNum);
00492 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_RTR_SYNC_READY;
00493 }else{
00494 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_RTR_SYNC_READY;
00495 }
00496 status = state11;
00497 break;
00498
00499 }else if((*pTransmissionType == TRANS_EVENT_PROFILE ||
00500 *pTransmissionType == TRANS_EVENT_SPECIFIC )&&
00501 !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) {
00502 MSG_WAR(0x3968, " PDO is on EVENT. Trans type : ", *pTransmissionType);
00503 pdo = (Message)Message_Initializer;
00504 if(buildPDO(d, pdoNum, &pdo))
00505 {
00506 MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum);
00507 status = state11;
00508 break;
00509 }
00510
00511
00512 if(d->PDO_status[pdoNum].last_message.cob_id.w == pdo.cob_id.w &&
00513 d->PDO_status[pdoNum].last_message.len == pdo.len &&
00514 *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){
00515
00516 status = state11;
00517 }else{
00518 MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum);
00519
00520 status = state5;
00521
00522
00523 DelAlarm(d->PDO_status[pdoNum].event_timer);
00524 d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject), 0);
00525
00526 DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
00527 d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject * 100), 0);
00528
00529
00530 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED;
00531 }
00532 }else{
00533 MSG_WAR(0x306C, " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType);
00534 status = state11;
00535 }
00536 break;
00537 case state5:
00538
00539 d->PDO_status[pdoNum].last_message = pdo;
00540 MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id.w);
00541 MSG_WAR(0x396E, " Nb octets : ", pdo.len);
00542
00543 canSend(d->canHandle,&pdo);
00544 status = state11;
00545 break;
00546 case state11:
00547 pdoNum++;
00548 offsetObjdict++;
00549 offsetObjdictMap++;
00550 MSG_WAR(0x3970, "next pdo index : ", pdoNum);
00551 status = state3;
00552 break;
00553
00554 default:
00555 MSG_ERR(0x1972,"Unknown state has been reached : %d",status);
00556 return 0xFF;
00557 }
00558
00559 }
00560 }
00561 return 0;
00562 }
00563
00573 UNS32 TPDO_Communication_Parameter_Callback(CO_Data* d, const indextable * OD_entry, UNS8 bSubindex)
00574 {
00575
00576 if(d->CurrentCommunicationState.csPDO) switch(bSubindex)
00577 {
00578 case 2:
00579 case 3:
00580 case 5:
00581 {
00582 UNS8 pTransmissionType = *(UNS8*) OD_entry->pSubindex[2].pObject;
00583 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
00584 UNS8 numPdo = OD_entry - TPDO_com;
00585
00586
00587 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
00588 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
00589 d->PDO_status[numPdo].transmit_type_parameter = 0;
00590
00591 PDOEventTimerAlarm(d, numPdo);
00592 return 0;
00593 }
00594
00595 default:
00596 break;
00597 }
00598 return 0;
00599 }
00600
00601 void PDOInit(CO_Data* d)
00602 {
00603
00604 UNS16 pdoIndex = 0x1800;
00605
00606 UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
00607 UNS16 lastIndex = d->lastIndex->PDO_TRS;
00608 if(offsetObjdict) while( offsetObjdict <= lastIndex) {
00609
00610 UNS32 errorCode;
00611 ODCallback_t *CallbackList;
00612
00613 scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
00614 if(errorCode == OD_SUCCESSFUL && CallbackList)
00615 {
00616
00617
00618 CallbackList[2] = &TPDO_Communication_Parameter_Callback;
00619
00620 CallbackList[3] = &TPDO_Communication_Parameter_Callback;
00621
00622 CallbackList[5] = &TPDO_Communication_Parameter_Callback;
00623 }
00624 pdoIndex++;
00625 offsetObjdict++;
00626 }
00627
00628
00629 _sendPDOevent( d, 0 );
00630 }
00631
00632 void PDOStop(CO_Data* d)
00633 {
00634
00635 UNS8 pdoNum = 0x00;
00636 UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
00637 UNS16 lastIndex = d->lastIndex->PDO_TRS;
00638 if(offsetObjdict) while( offsetObjdict <= lastIndex) {
00639
00640 d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer);
00641 d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
00642
00643 d->PDO_status[pdoNum].transmit_type_parameter = 0;
00644 d->PDO_status[pdoNum].last_message.cob_id.w = 0;
00645 pdoNum++;
00646 offsetObjdict++;
00647 }
00648 }