diff -r 003cc3c63855 -r f49e5a6b7804 doc/doxygen/html/pdo_8c-source.html --- a/doc/doxygen/html/pdo_8c-source.html Fri Jun 08 09:02:43 2007 +0200 +++ b/doc/doxygen/html/pdo_8c-source.html Fri Jun 08 09:23:56 2007 +0200 @@ -1,6 +1,6 @@
-00001 /* + +pdo.c
Go to the documentation of this file.00001 /* 00002 This file is part of CanFestival, a library implementing CanOpen 00003 Stack. 00004 @@ -38,55 +42,62 @@ 00021 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 00022 USA 00023 */ -00024 #include "pdo.h" -00025 #include "objacces.h" -00026 #include "canfestival.h" +00024 #include "pdo.h" +00025 #include "objacces.h" +00026 #include "canfestival.h" 00027 -00037 UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req) +00037 UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req) 00038 { -00039 UNS8 i; -00040 if( d->nodeState == Operational ) { -00041 Message m; +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]; +00043 /* Message copy for sending */ +00044 m.cob_id.w = pdo.cobId & 0x7FF; /* Because the cobId is 11 bytes +00045 length */ +00046 if ( req == NOT_A_REQUEST ) { +00047 UNS8 i; +00048 m.rtr = NOT_A_REQUEST; +00049 m.len = pdo.len; +00050 /* memcpy(&m.data, &pdo.data, m.len); */ +00051 /* This Memcpy depends on packing structure. Avoid */ +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; +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]); +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 } +00066 return canSend(d->canHandle,&m); +00067 } /* end if */ 00068 return 0xFF; 00069 } 00070 -00079 UNS8 PDOmGR(CO_Data* d, UNS32 cobId) +00079 UNS8 PDOmGR(CO_Data* d, UNS32 cobId) 00080 { -00081 UNS8 res; -00082 UNS8 i; -00083 s_PDO pdo; +00081 UNS8 res; +00082 UNS8 i; +00083 s_PDO pdo; 00084 -00085 MSG_WAR(0x3905, "PDOmGR",0); +00085 MSG_WAR(0x3905, "PDOmGR",0); 00086 -00089 pdo.cobId = cobId; -00090 pdo.len = d->process_var.count; +00087 /* if PDO is waiting for transmission, +00088 preparation of the message to send */ +00089 pdo.cobId = cobId; +00090 pdo.len = d->process_var.count; +00091 /* memcpy(&(pdo.data), &(process_var.data), pdo.len); */ 00092 /* Ce memcpy devrait tre portable */ -00093 for ( i = 0 ; i < pdo.len ; i++) -00094 pdo.data[i] = d->process_var.data[i]; +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); +00096 res = sendPDO(d, pdo, NOT_A_REQUEST); 00097 00098 return res; 00099 } @@ -96,291 +107,348 @@ 00103 /* TODO : implement bit mapping */ 00104 /*********************************************************************/ 00105 -00106 UNS8 buildPDO(CO_Data* d, UNS16 index) +00106 UNS8 buildPDO(CO_Data* d, UNS16 index) 00107 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */ -00108 UNS16 ind; -00109 UNS8 subInd; +00108 UNS16 ind; +00109 UNS8 subInd; 00110 -00111 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ +00111 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ 00112 /* pointer to the var which is mapped to a pdo */ 00113 /* void * pMappedAppObject = NULL; */ 00114 /* pointer fo the var which holds the mapping parameter of an mapping entry */ 00115 -00116 UNS32 * pMappingParameter = NULL; +00116 UNS32 * pMappingParameter = NULL; 00117 -00118 UNS8 Size; -00119 UNS8 dataType; -00120 UNS8 offset; -00121 UNS16 offsetObjdict; -00122 UNS16 offsetObjdictPrm; -00123 UNS32 objDict; +00118 UNS8 Size; +00119 UNS8 dataType; +00120 UNS8 offset; +00121 UNS16 offsetObjdict; +00122 UNS16 offsetObjdictPrm; +00123 UNS32 objDict; 00124 -00125 subInd=(UNS8)0x00; +00125 subInd=(UNS8)0x00; 00126 offset = 0x00; 00127 ind = index - 0x1800; 00128 -00129 MSG_WAR(0x3910,"Prepare PDO to send index :", index); +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); +00131 /* only operational state allows PDO transmission */ +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; +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 ); +00141 /* get mapped objects number to transmit with this PDO */ +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) { /* Loop on mapped variables */ +00146 /* get mapping parameters */ +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); +00151 /* Get the mapped variable */ +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); +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; +00165 d->process_var.count = offset; 00166 subInd++; -00167 } +00167 }/* end Loop on mapped variables */ 00168 } 00169 return 0; 00170 } 00171 #endif 00172 -00181 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId ) +00181 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId ) 00182 { -00183 UNS32 * pwCobId; -00184 UNS16 offset; -00185 UNS16 lastIndex; -00186 UNS8 err; +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; +00188 MSG_WAR(0x3930, "sendPDOrequest ",0); +00189 /* Sending the request only if the cobid have been found on the PDO +00190 receive */ +00191 /* part dictionary */ +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; +00196 /* get the CobId*/ +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); +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); +00208 MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId); 00209 return 0xFF; 00210 } 00211 00212 -00221 UNS8 proceedPDO(CO_Data* d, Message *m) +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; +00223 UNS8 numPdo; +00224 UNS8 numMap; /* Number of the mapped varable */ +00225 UNS8 i; +00226 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ +00227 /* pointer to the var which is mapped to a pdo... */ +00228 /* void * pMappedAppObject = NULL; */ +00229 /* pointer fo the var which holds the mapping parameter of an +00230 mapping entry */ +00231 UNS32 * pMappingParameter = NULL; +00232 UNS8 * pTransmissionType = NULL; /* pointer to the transmission +00233 type */ +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)); +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; +00248 if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a +00249 request. */ +00250 offsetObjdict = d->firstIndex->PDO_RCV; +00251 lastIndex = d->lastIndex->PDO_RCV; 00252 +00253 /* study of all the PDO stored in the dictionary */ 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; +00259 case state1:/* data are stored in process_var array */ +00260 /* memcpy(&(process_var.data), &m->data, (*m).len); */ +00261 /* Ce memcpy devrait etre portable */ +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; +00266 status = state2; 00267 break; 00268 -00269 case state2: -00272 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; +00269 case state2: +00270 /* get CobId of the dictionary correspondant to the received +00271 PDO */ +00272 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; +00273 /* check the CobId coherance */ +00274 /*pwCobId is the cobId read in the dictionary at the state 3 +00275 */ 00276 if ( *pwCobId == (*m).cob_id.w ){ -00278 status = state4; -00279 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); +00277 /* The cobId is recognized */ +00278 status = state4; +00279 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); 00280 break; 00281 } 00282 else { +00283 /* cobId received does not match with those write in the +00284 dictionnary */ 00285 numPdo++; 00286 offsetObjdict++; -00287 status = state2; +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; +00291 case state4:/* Get Mapped Objects Number */ +00292 /* The cobId of the message received has been found in the +00293 dictionnary. */ +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; +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); +00303 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1); 00304 return 0xFF; 00305 } -00313 Size = (UNS8)(*pMappingParameter); +00306 /* Get the addresse of the mapped variable. */ +00307 /* detail of *pMappingParameter : */ +00308 /* The 16 hight bits contains the index, the medium 8 bits +00309 contains the subindex, */ +00310 /* and the lower 8 bits contains the size of the mapped +00311 variable. */ +00312 +00313 Size = (UNS8)(*pMappingParameter); 00314 -00316 CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); +00315 /* copy bit per bit in little endian */ +00316 CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); 00317 00318 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 00319 2, ... */ 00320 -00321 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), -00322 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), +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); +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); +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); +00335 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ 00336 offset += Size; 00337 numMap++; -00338 } +00338 } /* end loop while on mapped variables */ +00339 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; +00344 }/* end switch status*/ +00345 }/* end while*/ +00346 }/* end if Donnees */ +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 ){ +00353 /* study of all PDO stored in the objects dictionary */ +00354 00355 switch( status ){ 00356 -00357 case state1: -00360 pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject; +00357 case state1:/* check the CobId */ +00358 /* get CobId of the dictionary which match to the received PDO +00359 */ +00360 pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject; 00361 if ( *pwCobId == (*m).cob_id.w ) { -00362 status = state4; +00362 status = state4; 00363 break; 00364 } 00365 else { 00366 numPdo++; 00367 offsetObjdict++; 00368 } -00369 status = state1; +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; +00373 case state4:/* check transmission type (after request?) */ +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); +00380 /* The requested PDO is not to send on request. So, does +00381 nothing. */ +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; +00386 case state5:/* get mapped objects number */ +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); +00392 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; +00393 /* Get the mapped variable */ +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; +00404 offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3); +00405 d->process_var.count = offset; 00406 numMap++; 00407 -00408 } -00409 PDOmGR( d, *pwCobId ); +00408 } /* end while */ +00409 PDOmGR( d, *pwCobId ); /* Transmit the PDO */ 00410 return 0; 00411 -00412 } -00413 } -00414 } +00412 }/* end switch status */ +00413 }/* end while */ +00414 }/* end if Requete */ +00415 00416 return 0; 00417 } 00418 -00430 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian) +00430 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian) 00431 { +00432 /* This loop copy as many bits that it can each time, crossing*/ +00433 /* successively bytes*/ 00434 // boundaries from LSB to MSB. 00435 while(NbBits > 0) 00436 { -00438 INTEGER8 Vect = DestBitIndex - SrcBitIndex; +00437 /* Bit missalignement between src and dest*/ +00438 INTEGER8 Vect = DestBitIndex - SrcBitIndex; 00439 -00441 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; +00440 /* We can now get src and align it to dest*/ +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; +00443 /* Compute the nb of bit we will be able to copy*/ +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))); +00447 /* Create a mask that will serve in:*/ +00448 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex))); 00449 -00451 UNS8 Filtered = Aligned & ~Mask; +00450 /* - Filtering src*/ +00451 UNS8 Filtered = Aligned & ~Mask; 00452 +00453 /* - and erase bits where we write, preserve where we don't*/ 00454 *DestByteIndex &= Mask; 00455 +00456 /* Then write.*/ 00457 *DestByteIndex |= Filtered ; 00458 -00460 if((SrcBitIndex += BitsToCopy)>7) +00459 /*Compute next time cursors for src*/ +00460 if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/ 00461 { -00462 SrcBitIndex = 0; -00463 SrcByteIndex += (SrcBigEndian ? -1 : 1); +00462 SrcBitIndex = 0;/* First bit*/ +00463 SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/ 00464 } 00465 00466 +00467 /*Compute next time cursors for dest*/ 00468 if((DestBitIndex += BitsToCopy)>7) 00469 { -00470 DestBitIndex = 0; -00471 DestByteIndex += (DestBigEndian ? -1 : 1); +00470 DestBitIndex = 0;/* First bit*/ +00471 DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/ 00472 } 00473 +00474 /*And decrement counter.*/ 00475 NbBits -= BitsToCopy; 00476 } 00477 @@ -393,79 +461,89 @@ 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; +00495 UNS8 sendPDOevent( CO_Data* d, void * variable ) +00496 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite +00497 loop if it fails.*/ +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; +00505 UNS8 * pTransmissionType = NULL; /* pointer to the transmission +00506 type */ +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; +00519 status = state1; 00520 00521 -00525 MSG_WAR (0x3960, "sendPDOevent", 0); -00526 offsetObjdictPrm = d->firstIndex->PDO_TRS; +00522 /* look for the index and subindex where the variable is mapped */ +00523 /* Then, send the pdo which contains the variable. */ +00524 +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; +00528 offsetObjdict = d->firstIndex->PDO_TRS_MAP; +00529 lastIndex = d->lastIndex->PDO_TRS_MAP; 00530 00531 if (offsetObjdictPrm && offsetObjdict) +00532 /* Loop on PDO Transmit */ 00533 while(offsetObjdict <= lastIndex){ -00535 pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject; -00536 if (*pTransmissionType != TRANS_EVENT) { +00534 /* Check the transmission mode */ +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; +00542 pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject; +00543 numMap = 1; /* mapped variable */ 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), +00545 pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject; +00546 /* Get the variable */ +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); +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); +00555 MSG_WAR(0x2962, " subindex : ", +00556 ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); 00557 return 0xFF; 00558 } 00559 if (pMappedAppObject == variable) { // Variable found ! -00560 MSG_WAR(0x3963, "Variable to send found at index : ", +00560 MSG_WAR(0x3963, "Variable to send found at index : ", 00561 (*pMappingParameter) >> 16); -00562 MSG_WAR(0x3964, " subIndex : ", +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 ); +00564 buildPDO(d, 0x1800 + ind); +00565 /* Get the cobId */ +00566 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject; +00567 PDOmGR( d, *pwCobId ); /* Send the PDO */ 00568 return 0; 00569 } 00570 numMap++; -00571 } +00571 } /* End loop on mapped variable */ 00572 ind++; 00573 offsetObjdict++; 00574 offsetObjdictPrm++; -00575 } -00577 MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0); +00575 } /* End loop while on PDO */ +00576 +00577 MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0); 00578 return 0xFF; 00579 00580 } @@ -473,7 +551,7 @@ 00582 00583 00584 -
Generated on Tue Jun 5 18:32:05 2007 for CanFestival by +