nico@207: nico@207:
nico@215:00001 /* nico@210: 00002 This file is part of CanFestival, a library implementing CanOpen nico@210: 00003 Stack. nico@210: 00004 nico@210: 00005 Copyright (C): Edouard TISSERANT and Francis DUPIN nico@210: 00006 nico@210: 00007 See COPYING file for copyrights details. nico@210: 00008 nico@210: 00009 This library is free software; you can redistribute it and/or nico@210: 00010 modify it under the terms of the GNU Lesser General Public nico@210: 00011 License as published by the Free Software Foundation; either nico@210: 00012 version 2.1 of the License, or (at your option) any later version. nico@210: 00013 nico@210: 00014 This library is distributed in the hope that it will be useful, nico@210: 00015 but WITHOUT ANY WARRANTY; without even the implied warranty of nico@210: 00016 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU nico@210: 00017 Lesser General Public License for more details. nico@210: 00018 nico@210: 00019 You should have received a copy of the GNU Lesser General Public nico@210: 00020 License along with this library; if not, write to the Free Software nico@210: 00021 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 nico@210: 00022 USA nico@210: 00023 */ nico@215: 00024 #include "pdo.h" nico@215: 00025 #include "objacces.h" nico@215: 00026 #include "canfestival.h" nico@210: 00027 etisserant@240: 00048 UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo) etisserant@240: 00049 { etisserant@240: 00050 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; etisserant@240: 00051 const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo; etisserant@240: 00052 etisserant@240: 00053 UNS8 prp_j = 0x00; etisserant@240: 00054 UNS8 offset = 0x00; etisserant@240: 00055 const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject; etisserant@240: 00056 etisserant@240: 00057 pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF; etisserant@240: 00058 pdo->rtr = NOT_A_REQUEST; nico@207: 00059 etisserant@240: 00060 MSG_WAR(0x3009, " PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject); etisserant@240: 00061 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount ); etisserant@240: 00062 etisserant@240: 00063 do{ etisserant@240: 00064 UNS8 dataType; /* Unused */ etisserant@240: 00065 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; /* temporary space to hold bits */ etisserant@240: 00066 etisserant@240: 00067 /* pointer fo the var which holds the mapping parameter of an mapping entry */ etisserant@240: 00068 UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject; etisserant@240: 00069 UNS16 index = (UNS16)((*pMappingParameter) >> 16); etisserant@240: 00070 UNS8 Size = (UNS8)(*pMappingParameter); /* Size in bits */ etisserant@240: 00071 UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ etisserant@240: 00072 UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); etisserant@240: 00073 etisserant@240: 00074 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); etisserant@240: 00075 MSG_WAR(0x3050, " at index : ", TPDO_map->index); etisserant@240: 00076 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); etisserant@240: 00077 etisserant@240: 00078 if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){ etisserant@240: 00079 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); etisserant@240: 00080 return 0xFF; etisserant@240: 00081 } etisserant@240: 00082 /* copy bit per bit in little endian*/ etisserant@240: 00083 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0); nico@210: 00084 etisserant@240: 00085 offset += Size ; etisserant@240: 00086 prp_j++; etisserant@240: 00087 }while( prp_j < *pMappingCount ); etisserant@240: 00088 etisserant@240: 00089 pdo->len = 1 + ((offset - 1) >> 3); etisserant@240: 00090 etisserant@240: 00091 MSG_WAR(0x3015, " End scan mapped variable", 0); etisserant@240: 00092 etisserant@240: 00093 return 0; etisserant@240: 00094 } nico@210: 00095 etisserant@240: 00104 UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex ) etisserant@240: 00105 { etisserant@240: 00106 UNS32 * pwCobId; etisserant@240: 00107 UNS16 offset = d->firstIndex->PDO_RCV; etisserant@240: 00108 UNS16 lastIndex = d->lastIndex->PDO_RCV; etisserant@240: 00109 etisserant@240: 00110 /* Sending the request only if the cobid have been found on the PDO etisserant@240: 00111 receive */ etisserant@240: 00112 /* part dictionary */ etisserant@240: 00113 etisserant@240: 00114 MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex); etisserant@240: 00115 etisserant@240: 00116 if (offset && RPDOIndex >= 0x1400){ etisserant@240: 00117 offset += RPDOIndex - 0x1400; etisserant@240: 00118 if (offset <= lastIndex) { etisserant@240: 00119 /* get the CobId*/ etisserant@240: 00120 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; etisserant@240: 00121 etisserant@240: 00122 MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId); etisserant@240: 00123 etisserant@240: 00124 Message pdo = {*pwCobId, REQUEST, 0}; etisserant@240: 00125 return canSend(d->canHandle,&pdo); etisserant@240: 00126 } etisserant@240: 00127 } etisserant@240: 00128 MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex); etisserant@240: 00129 return 0xFF; etisserant@240: 00130 } etisserant@240: 00131 etisserant@240: 00132 etisserant@240: 00141 UNS8 proceedPDO(CO_Data* d, Message *m) etisserant@240: 00142 { etisserant@240: 00143 UNS8 numPdo; etisserant@240: 00144 UNS8 numMap; /* Number of the mapped varable */ etisserant@240: 00145 UNS8 i; etisserant@240: 00146 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ etisserant@240: 00147 /* pointer to the var which is mapped to a pdo... */ etisserant@240: 00148 /* void * pMappedAppObject = NULL; */ etisserant@240: 00149 /* pointer fo the var which holds the mapping parameter of an etisserant@240: 00150 mapping entry */ etisserant@240: 00151 UNS32 * pMappingParameter = NULL; etisserant@240: 00152 UNS8 * pTransmissionType = NULL; /* pointer to the transmission etisserant@240: 00153 type */ etisserant@240: 00154 UNS32 * pwCobId = NULL; etisserant@240: 00155 UNS8 Size; etisserant@240: 00156 UNS8 dataType; etisserant@240: 00157 UNS8 offset; etisserant@240: 00158 UNS8 status; etisserant@240: 00159 UNS32 objDict; etisserant@240: 00160 UNS16 offsetObjdict; etisserant@240: 00161 UNS16 lastIndex; etisserant@240: 00162 etisserant@240: 00163 status = state2; etisserant@240: 00164 etisserant@240: 00165 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff)); etisserant@240: 00166 offset = 0x00; etisserant@240: 00167 numPdo = 0; etisserant@240: 00168 numMap = 0; etisserant@240: 00169 if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a etisserant@240: 00170 request. */ etisserant@240: 00171 etisserant@240: 00172 offsetObjdict = d->firstIndex->PDO_RCV; etisserant@240: 00173 lastIndex = d->lastIndex->PDO_RCV; etisserant@240: 00174 etisserant@240: 00175 /* study of all the PDO stored in the dictionary */ etisserant@240: 00176 if(offsetObjdict) etisserant@240: 00177 while (offsetObjdict <= lastIndex) { etisserant@240: 00178 etisserant@240: 00179 switch( status ) { etisserant@240: 00180 etisserant@240: 00181 case state2: etisserant@240: 00182 /* get CobId of the dictionary correspondant to the received etisserant@240: 00183 PDO */ etisserant@240: 00184 pwCobId = (UNS32*) d->objdict[offsetObjdict].pSubindex[1].pObject; etisserant@240: 00185 /* check the CobId coherance */ etisserant@240: 00186 /*pwCobId is the cobId read in the dictionary at the state 3 etisserant@240: 00187 */ etisserant@240: 00188 if ( *pwCobId == (*m).cob_id.w ){ etisserant@240: 00189 /* The cobId is recognized */ etisserant@240: 00190 status = state4; etisserant@240: 00191 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); etisserant@240: 00192 break; etisserant@240: 00193 } etisserant@240: 00194 else { etisserant@240: 00195 /* cobId received does not match with those write in the etisserant@240: 00196 dictionnary */ etisserant@240: 00197 numPdo++; etisserant@240: 00198 offsetObjdict++; etisserant@240: 00199 status = state2; etisserant@240: 00200 break; etisserant@240: 00201 } etisserant@240: 00202 etisserant@240: 00203 case state4:/* Get Mapped Objects Number */ etisserant@240: 00204 /* The cobId of the message received has been found in the etisserant@240: 00205 dictionnary. */ etisserant@240: 00206 offsetObjdict = d->firstIndex->PDO_RCV_MAP; etisserant@240: 00207 lastIndex = d->lastIndex->PDO_RCV_MAP; etisserant@240: 00208 pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; etisserant@240: 00209 numMap = 0; etisserant@240: 00210 while (numMap < *pMappingCount) { etisserant@240: 00211 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; etisserant@240: 00212 UNS8 ByteSize; etisserant@240: 00213 pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; etisserant@240: 00214 if (pMappingParameter == NULL) { etisserant@240: 00215 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1); etisserant@240: 00216 return 0xFF; etisserant@240: 00217 } etisserant@240: 00218 /* Get the addresse of the mapped variable. */ etisserant@240: 00219 /* detail of *pMappingParameter : */ etisserant@240: 00220 /* The 16 hight bits contains the index, the medium 8 bits etisserant@240: 00221 contains the subindex, */ etisserant@240: 00222 /* and the lower 8 bits contains the size of the mapped etisserant@240: 00223 variable. */ etisserant@240: 00224 etisserant@240: 00225 Size = (UNS8)(*pMappingParameter); etisserant@240: 00226 etisserant@240: 00227 /* copy bit per bit in little endian */ etisserant@240: 00228 CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); etisserant@240: 00229 etisserant@240: 00230 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => etisserant@240: 00231 2, ... */ etisserant@240: 00232 etisserant@240: 00233 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), etisserant@240: 00234 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), etisserant@240: 00235 tmp, &ByteSize, 0 ); etisserant@240: 00236 etisserant@240: 00237 if(objDict != OD_SUCCESSFUL) { etisserant@240: 00238 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1); etisserant@240: 00239 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16); etisserant@240: 00240 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); etisserant@240: 00241 return 0xFF; etisserant@240: 00242 } nico@210: 00243 etisserant@240: 00244 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); etisserant@240: 00245 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); etisserant@240: 00246 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); etisserant@240: 00247 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ etisserant@240: 00248 offset += Size; etisserant@240: 00249 numMap++; etisserant@240: 00250 /*TODO : check that offset is not not greater that message size (in bit) */ etisserant@240: 00251 } /* end loop while on mapped variables */ nico@210: 00252 etisserant@240: 00253 offset=0x00; etisserant@240: 00254 numMap = 0; etisserant@240: 00255 return 0; nico@210: 00256 etisserant@240: 00257 }/* end switch status*/ etisserant@240: 00258 }/* end while*/ etisserant@240: 00259 }/* end if Donnees */ etisserant@240: 00260 else if ((*m).rtr == REQUEST ){ etisserant@240: 00261 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w); etisserant@240: 00262 status = state1; etisserant@240: 00263 offsetObjdict = d->firstIndex->PDO_TRS; etisserant@240: 00264 lastIndex = d->lastIndex->PDO_TRS; etisserant@240: 00265 if(offsetObjdict) while( offsetObjdict <= lastIndex ){ etisserant@240: 00266 /* study of all PDO stored in the objects dictionary */ etisserant@240: 00267 etisserant@240: 00268 switch( status ){ etisserant@240: 00269 etisserant@240: 00270 case state1:/* check the CobId */ etisserant@240: 00271 /* get CobId of the dictionary which match to the received PDO etisserant@240: 00272 */ etisserant@240: 00273 pwCobId = (UNS32*) (d->objdict + offsetObjdict)->pSubindex[1].pObject; etisserant@240: 00274 if ( *pwCobId == (*m).cob_id.w ) { etisserant@240: 00275 status = state4; etisserant@240: 00276 break; etisserant@240: 00277 } etisserant@240: 00278 else { etisserant@240: 00279 numPdo++; etisserant@240: 00280 offsetObjdict++; etisserant@240: 00281 } etisserant@240: 00282 status = state1; etisserant@240: 00283 break; etisserant@240: 00284 etisserant@240: 00285 etisserant@240: 00286 case state4:/* check transmission type */ etisserant@240: 00287 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; etisserant@240: 00288 /* If PDO is to be sampled and send on RTR, do it*/ etisserant@240: 00289 if ( (*pTransmissionType == TRANS_RTR)) { etisserant@240: 00290 status = state5; etisserant@240: 00291 break; etisserant@240: 00292 /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */ etisserant@240: 00293 }else if ((*pTransmissionType == TRANS_RTR_SYNC )) { etisserant@240: 00294 if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){ etisserant@240: 00295 /*Data ready, just send*/ etisserant@240: 00296 canSend(d->canHandle,&d->PDO_status[numPdo].last_message); etisserant@240: 00297 return 0; etisserant@240: 00298 }else{ etisserant@240: 00299 /* if SYNC did never occur, force emission with current data */ etisserant@240: 00300 /* DS301 do not tell what to do in such a case...*/ etisserant@240: 00301 MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id.w); etisserant@240: 00302 status = state5; etisserant@240: 00303 } etisserant@240: 00304 break; etisserant@240: 00305 }else if( etisserant@240: 00306 (*pTransmissionType == TRANS_EVENT_PROFILE) || etisserant@240: 00307 (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) { etisserant@240: 00308 /* Zap all timers and inhibit flag */ etisserant@240: 00309 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer); etisserant@240: 00310 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer); etisserant@240: 00311 d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED; etisserant@240: 00312 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ etisserant@240: 00313 PDOEventTimerAlarm(d, numPdo); etisserant@240: 00314 return 0; etisserant@240: 00315 }else { etisserant@240: 00316 /* The requested PDO is not to send on request. So, does etisserant@240: 00317 nothing. */ etisserant@240: 00318 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w); etisserant@240: 00319 return 0xFF; etisserant@240: 00320 } etisserant@240: 00321 etisserant@240: 00322 case state5:/* build and send requested PDO */ etisserant@240: 00323 { etisserant@240: 00324 Message pdo; etisserant@240: 00325 if( buildPDO(d, numPdo, &pdo)) etisserant@240: 00326 { etisserant@240: 00327 MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo); etisserant@240: 00328 return 0xFF; etisserant@240: 00329 } etisserant@240: 00330 canSend(d->canHandle,&pdo); etisserant@240: 00331 return 0; etisserant@240: 00332 } etisserant@240: 00333 }/* end switch status */ etisserant@240: 00334 }/* end while */ etisserant@240: 00335 }/* end if Requete */ etisserant@240: 00336 etisserant@240: 00337 return 0; etisserant@240: 00338 } nico@215: 00339 etisserant@240: 00351 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian) etisserant@240: 00352 { etisserant@240: 00353 /* This loop copy as many bits that it can each time, crossing*/ etisserant@240: 00354 /* successively bytes*/ etisserant@240: 00355 // boundaries from LSB to MSB. etisserant@240: 00356 while(NbBits > 0) etisserant@240: 00357 { etisserant@240: 00358 /* Bit missalignement between src and dest*/ etisserant@240: 00359 INTEGER8 Vect = DestBitIndex - SrcBitIndex; etisserant@240: 00360 etisserant@240: 00361 /* We can now get src and align it to dest*/ etisserant@240: 00362 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; etisserant@240: 00363 etisserant@240: 00364 /* Compute the nb of bit we will be able to copy*/ etisserant@240: 00365 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex ); etisserant@240: 00366 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit; etisserant@240: 00367 etisserant@240: 00368 /* Create a mask that will serve in:*/ etisserant@240: 00369 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex))); etisserant@240: 00370 etisserant@240: 00371 /* - Filtering src*/ etisserant@240: 00372 UNS8 Filtered = Aligned & ~Mask; etisserant@240: 00373 etisserant@240: 00374 /* - and erase bits where we write, preserve where we don't*/ etisserant@240: 00375 *DestByteIndex &= Mask; etisserant@240: 00376 etisserant@240: 00377 /* Then write.*/ etisserant@240: 00378 *DestByteIndex |= Filtered ; etisserant@240: 00379 etisserant@240: 00380 /*Compute next time cursors for src*/ etisserant@240: 00381 if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/ etisserant@240: 00382 { etisserant@240: 00383 SrcBitIndex = 0;/* First bit*/ etisserant@240: 00384 SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/ etisserant@240: 00385 } etisserant@240: 00386 etisserant@240: 00387 etisserant@240: 00388 /*Compute next time cursors for dest*/ etisserant@240: 00389 if((DestBitIndex += BitsToCopy)>7) etisserant@240: 00390 { etisserant@240: 00391 DestBitIndex = 0;/* First bit*/ etisserant@240: 00392 DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/ etisserant@240: 00393 } etisserant@240: 00394 etisserant@240: 00395 /*And decrement counter.*/ etisserant@240: 00396 NbBits -= BitsToCopy; etisserant@240: 00397 } etisserant@240: 00398 etisserant@240: 00399 } etisserant@240: 00408 UNS8 sendPDOevent( CO_Data* d) etisserant@240: 00409 { etisserant@240: 00410 /* Calls _sendPDOevent specifying it is not a sync event */ etisserant@240: 00411 return _sendPDOevent(d, 0); etisserant@240: 00412 } etisserant@240: 00413 etisserant@240: 00414 etisserant@240: 00415 void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum) etisserant@240: 00416 { etisserant@240: 00417 /* This is needed to avoid deletion of re-attribuated timer */ etisserant@240: 00418 d->PDO_status[pdoNum].event_timer = TIMER_NONE; etisserant@240: 00419 /* force emission of PDO by artificially changing last emitted*/ etisserant@240: 00420 d->PDO_status[pdoNum].last_message.cob_id.w = 0; etisserant@240: 00421 _sendPDOevent( d, 0 ); /* not a Sync Event*/ etisserant@240: 00422 } etisserant@240: 00423 etisserant@240: 00424 void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum) etisserant@240: 00425 { etisserant@240: 00426 /* This is needed to avoid deletion of re-attribuated timer */ etisserant@240: 00427 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; etisserant@240: 00428 /* Remove inhibit flag */ etisserant@240: 00429 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED; etisserant@240: 00430 _sendPDOevent( d, 0 ); /* not a Sync Event*/ etisserant@240: 00431 } etisserant@240: 00432 etisserant@240: 00442 UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent ) etisserant@240: 00443 { etisserant@240: 00444 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ etisserant@240: 00445 UNS8* pTransmissionType = NULL; etisserant@240: 00446 UNS8 status = state3; etisserant@240: 00447 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; etisserant@240: 00448 UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; etisserant@240: 00449 UNS16 lastIndex = d->lastIndex->PDO_TRS; etisserant@240: 00450 etisserant@240: 00451 /* study all PDO stored in the objects dictionary */ etisserant@240: 00452 if(offsetObjdict){ etisserant@240: 00453 Message pdo = Message_Initializer; etisserant@240: 00454 while( offsetObjdict <= lastIndex) { etisserant@240: 00455 switch( status ) { etisserant@240: 00456 case state3: etisserant@240: 00457 if (/*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5)*/ etisserant@240: 00458 /* check if TPDO is not valid */ etisserant@240: 00459 *(UNS32*)d->objdict[offsetObjdict].pSubindex[0].pObject & 0x8000) { etisserant@240: 00460 MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum); etisserant@240: 00461 /*Go next TPDO*/ etisserant@240: 00462 status = state11; etisserant@240: 00463 break; nico@210: 00464 } etisserant@240: 00465 /* get the PDO transmission type */ etisserant@240: 00466 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; etisserant@240: 00467 MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum); etisserant@240: 00468 etisserant@240: 00469 /* check if transmission type is SYNCRONOUS */ etisserant@240: 00470 /* The message is transmited every n SYNC with n=TransmissionType */ etisserant@240: 00471 if( isSyncEvent && etisserant@240: 00472 (*pTransmissionType >= TRANS_SYNC_MIN) && etisserant@240: 00473 (*pTransmissionType <= TRANS_SYNC_MAX) && etisserant@240: 00474 (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) { etisserant@240: 00475 /*Reset count of SYNC*/ etisserant@240: 00476 d->PDO_status[pdoNum].transmit_type_parameter = 0; etisserant@240: 00477 MSG_WAR(0x3964, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); etisserant@240: 00478 pdo = (Message)Message_Initializer; etisserant@240: 00479 if(buildPDO(d, pdoNum, &pdo)) etisserant@240: 00480 { etisserant@240: 00481 MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum); etisserant@240: 00482 status = state11; etisserant@240: 00483 break; etisserant@240: 00484 } etisserant@240: 00485 status = state5; etisserant@240: 00486 /* If transmission RTR, with data sampled on SYNC */ etisserant@240: 00487 }else if( isSyncEvent && etisserant@240: 00488 (*pTransmissionType == TRANS_RTR_SYNC)) { etisserant@240: 00489 if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message)) etisserant@240: 00490 { etisserant@240: 00491 MSG_ERR(0x1966, " Couldn't build TPDO number : ", pdoNum); etisserant@240: 00492 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_RTR_SYNC_READY; etisserant@240: 00493 }else{ etisserant@240: 00494 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_RTR_SYNC_READY; etisserant@240: 00495 } etisserant@240: 00496 status = state11; etisserant@240: 00497 break; etisserant@240: 00498 /* If transmission on Event and not inhibited, check for changes */ etisserant@240: 00499 }else if((*pTransmissionType == TRANS_EVENT_PROFILE || etisserant@240: 00500 *pTransmissionType == TRANS_EVENT_SPECIFIC )&& etisserant@240: 00501 !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) { etisserant@240: 00502 MSG_WAR(0x3968, " PDO is on EVENT. Trans type : ", *pTransmissionType); etisserant@240: 00503 pdo = (Message)Message_Initializer; etisserant@240: 00504 if(buildPDO(d, pdoNum, &pdo)) etisserant@240: 00505 { etisserant@240: 00506 MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum); etisserant@240: 00507 status = state11; etisserant@240: 00508 break; etisserant@240: 00509 } etisserant@240: 00510 etisserant@240: 00511 /*Compare new and old PDO*/ etisserant@240: 00512 if(d->PDO_status[pdoNum].last_message.cob_id.w == pdo.cob_id.w && etisserant@240: 00513 d->PDO_status[pdoNum].last_message.len == pdo.len && etisserant@240: 00514 *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){ etisserant@240: 00515 /* No changes -> go to next pdo*/ etisserant@240: 00516 status = state11; etisserant@240: 00517 }else{ etisserant@240: 00518 MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum); etisserant@240: 00519 /* Changes detected -> transmit message */ etisserant@240: 00520 status = state5; etisserant@240: 00521 etisserant@240: 00522 /* Start both event_timer and inhibit_timer*/ etisserant@240: 00523 DelAlarm(d->PDO_status[pdoNum].event_timer); etisserant@240: 00524 d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject), 0); etisserant@240: 00525 etisserant@240: 00526 DelAlarm(d->PDO_status[pdoNum].inhibit_timer); etisserant@240: 00527 d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject * 100), 0); etisserant@240: 00528 etisserant@240: 00529 /* and inhibit TPDO */ etisserant@240: 00530 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED; etisserant@240: 00531 } etisserant@240: 00532 }else{ etisserant@240: 00533 MSG_WAR(0x306C, " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType); etisserant@240: 00534 status = state11; etisserant@240: 00535 } etisserant@240: 00536 break; etisserant@240: 00537 case state5: /*Send the pdo*/ etisserant@240: 00538 /*store_as_last_message*/ etisserant@240: 00539 d->PDO_status[pdoNum].last_message = pdo; etisserant@240: 00540 MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id.w); etisserant@240: 00541 MSG_WAR(0x396E, " Nb octets : ", pdo.len); etisserant@240: 00542 etisserant@240: 00543 canSend(d->canHandle,&pdo); etisserant@240: 00544 status = state11; etisserant@240: 00545 break; etisserant@240: 00546 case state11: /*Go to next TPDO*/ etisserant@240: 00547 pdoNum++; etisserant@240: 00548 offsetObjdict++; etisserant@240: 00549 offsetObjdictMap++; etisserant@240: 00550 MSG_WAR(0x3970, "next pdo index : ", pdoNum); etisserant@240: 00551 status = state3; etisserant@240: 00552 break; etisserant@240: 00553 etisserant@240: 00554 default: etisserant@240: 00555 MSG_ERR(0x1972,"Unknown state has been reached : %d",status); etisserant@240: 00556 return 0xFF; etisserant@240: 00557 }/* end switch case */ etisserant@240: 00558 etisserant@240: 00559 }/* end while */ etisserant@240: 00560 } etisserant@240: 00561 return 0; etisserant@240: 00562 } etisserant@240: 00563 etisserant@240: 00573 UNS32 TPDO_Communication_Parameter_Callback(CO_Data* d, const indextable * OD_entry, UNS8 bSubindex) etisserant@240: 00574 { etisserant@240: 00575 /* If PDO are actives */ etisserant@240: 00576 if(d->CurrentCommunicationState.csPDO) switch(bSubindex) etisserant@240: 00577 { etisserant@240: 00578 case 2: /* Changed transmition type */ etisserant@240: 00579 case 3: /* Changed inhibit time */ etisserant@240: 00580 case 5: /* Changed event time */ etisserant@240: 00581 { etisserant@240: 00582 UNS8 pTransmissionType = *(UNS8*) OD_entry->pSubindex[2].pObject; etisserant@240: 00583 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS; etisserant@240: 00584 UNS8 numPdo = OD_entry - TPDO_com; /* number of the actual processed pdo-nr. */ etisserant@240: 00585 etisserant@240: 00586 /* Zap all timers and inhibit flag */ etisserant@240: 00587 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer); etisserant@240: 00588 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer); etisserant@240: 00589 d->PDO_status[numPdo].transmit_type_parameter = 0; etisserant@240: 00590 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ etisserant@240: 00591 PDOEventTimerAlarm(d, numPdo); etisserant@240: 00592 return 0; etisserant@240: 00593 } etisserant@240: 00594 etisserant@240: 00595 default: /* other subindex are ignored*/ etisserant@240: 00596 break; etisserant@240: 00597 } etisserant@240: 00598 return 0; etisserant@240: 00599 } etisserant@240: 00600 etisserant@240: 00601 void PDOInit(CO_Data* d) etisserant@240: 00602 { etisserant@240: 00603 /* For each TPDO mapping parameters */ etisserant@240: 00604 UNS16 pdoIndex = 0x1800; /* OD index of TDPO */ etisserant@240: 00605 etisserant@240: 00606 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; etisserant@240: 00607 UNS16 lastIndex = d->lastIndex->PDO_TRS; etisserant@240: 00608 if(offsetObjdict) while( offsetObjdict <= lastIndex) { etisserant@240: 00609 /* Assign callbacks to sensible TPDO mapping subindexes */ etisserant@240: 00610 UNS32 errorCode; etisserant@240: 00611 ODCallback_t *CallbackList; etisserant@240: 00612 /* Find callback list */ etisserant@240: 00613 scanIndexOD (d, pdoIndex, &errorCode, &CallbackList); etisserant@240: 00614 if(errorCode == OD_SUCCESSFUL && CallbackList) etisserant@240: 00615 { etisserant@240: 00616 /*Assign callbacks to corresponding subindex*/ etisserant@240: 00617 /* Transmission type */ etisserant@240: 00618 CallbackList[2] = &TPDO_Communication_Parameter_Callback; etisserant@240: 00619 /* Inhibit time */ etisserant@240: 00620 CallbackList[3] = &TPDO_Communication_Parameter_Callback; etisserant@240: 00621 /* Event timer */ etisserant@240: 00622 CallbackList[5] = &TPDO_Communication_Parameter_Callback; etisserant@240: 00623 } etisserant@240: 00624 pdoIndex++; etisserant@240: 00625 offsetObjdict++; etisserant@240: 00626 } etisserant@240: 00627 etisserant@240: 00628 /* Trigger a non-sync event */ etisserant@240: 00629 _sendPDOevent( d, 0 ); etisserant@240: 00630 } etisserant@240: 00631 etisserant@240: 00632 void PDOStop(CO_Data* d) etisserant@240: 00633 { etisserant@240: 00634 /* For each TPDO mapping parameters */ etisserant@240: 00635 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ etisserant@240: 00636 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; etisserant@240: 00637 UNS16 lastIndex = d->lastIndex->PDO_TRS; etisserant@240: 00638 if(offsetObjdict) while( offsetObjdict <= lastIndex) { etisserant@240: 00639 /* Delete TPDO timers */ etisserant@240: 00640 d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer); etisserant@240: 00641 d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer); etisserant@240: 00642 /* Reset transmit type parameter */ etisserant@240: 00643 d->PDO_status[pdoNum].transmit_type_parameter = 0; etisserant@240: 00644 d->PDO_status[pdoNum].last_message.cob_id.w = 0; etisserant@240: 00645 pdoNum++; etisserant@240: 00646 offsetObjdict++; etisserant@240: 00647 } etisserant@240: 00648 } etisserant@240: