nico@207: nico@207:
nico@207:00001 /* nico@207: 00002 This file is part of CanFestival, a library implementing CanOpen Stack. nico@207: 00003 nico@207: 00004 Copyright (C): Edouard TISSERANT and Francis DUPIN nico@207: 00005 nico@207: 00006 See COPYING file for copyrights details. nico@207: 00007 nico@207: 00008 This library is free software; you can redistribute it and/or nico@207: 00009 modify it under the terms of the GNU Lesser General Public nico@207: 00010 License as published by the Free Software Foundation; either nico@207: 00011 version 2.1 of the License, or (at your option) any later version. nico@207: 00012 nico@207: 00013 This library is distributed in the hope that it will be useful, nico@207: 00014 but WITHOUT ANY WARRANTY; without even the implied warranty of nico@207: 00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU nico@207: 00016 Lesser General Public License for more details. nico@207: 00017 nico@207: 00018 You should have received a copy of the GNU Lesser General Public nico@207: 00019 License along with this library; if not, write to the Free Software nico@207: 00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA nico@207: 00021 */ nico@207: 00022 #include "pdo.h" nico@207: 00023 #include "objacces.h" nico@207: 00024 #include "canfestival.h" nico@207: 00025 nico@207: 00026 /****************************************************************************/ nico@207: 00027 UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req) nico@207: 00028 { nico@207: 00029 UNS8 i; nico@207: 00030 if( d->nodeState == Operational ) { nico@207: 00031 Message m; nico@207: 00032 nico@207: 00033 /* Message copy for sending */ nico@207: 00034 m.cob_id.w = pdo.cobId & 0x7FF; /* Because the cobId is 11 bytes length */ nico@207: 00035 if ( req == NOT_A_REQUEST ) { nico@207: 00036 UNS8 i; nico@207: 00037 m.rtr = NOT_A_REQUEST; nico@207: 00038 m.len = pdo.len; nico@207: 00039 /* memcpy(&m.data, &pdo.data, m.len); */ nico@207: 00040 /* This Memcpy depends on packing structure. Avoid */ nico@207: 00041 for (i = 0 ; i < pdo.len ; i++) nico@207: 00042 m.data[i] = pdo.data[i]; nico@207: 00043 } nico@207: 00044 else { nico@207: 00045 m.rtr = REQUEST; nico@207: 00046 m.len = 0; nico@207: 00047 } nico@207: 00048 nico@207: 00049 MSG_WAR(0x3901, "sendPDO cobId :", m.cob_id.w); nico@207: 00050 MSG_WAR(0x3902, " Nb octets : ", m.len); nico@207: 00051 for (i = 0 ; i < m.len ; i++) { nico@207: 00052 MSG_WAR(0x3903," data : ", m.data[i]); nico@207: 00053 } nico@207: 00054 nico@207: 00055 return canSend(d->canHandle,&m); nico@207: 00056 } /* end if */ nico@207: 00057 return 0xFF; nico@207: 00058 } nico@207: 00059 nico@207: 00060 /***************************************************************************/ nico@207: 00061 UNS8 PDOmGR(CO_Data* d, UNS32 cobId) /* PDO Manager */ nico@207: 00062 { nico@207: 00063 UNS8 res; nico@207: 00064 UNS8 i; nico@207: 00065 s_PDO pdo; nico@207: 00066 nico@207: 00067 MSG_WAR(0x3905, "PDOmGR",0); nico@207: 00068 nico@207: 00069 /* if PDO is waiting for transmission, nico@207: 00070 preparation of the message to send */ nico@207: 00071 pdo.cobId = cobId; nico@207: 00072 pdo.len = d->process_var.count; nico@207: 00073 /* memcpy(&(pdo.data), &(process_var.data), pdo.len); */ nico@207: 00074 /* Ce memcpy devrait être portable */ nico@207: 00075 for ( i = 0 ; i < pdo.len ; i++) nico@207: 00076 pdo.data[i] = d->process_var.data[i]; nico@207: 00077 nico@207: 00078 res = sendPDO(d, pdo, NOT_A_REQUEST); nico@207: 00079 nico@207: 00080 return res; nico@207: 00081 } nico@207: 00082 nico@207: 00083 #if 0 nico@207: 00084 /*********************************************************************/ nico@207: 00085 /* TODO : implement bit mapping */ nico@207: 00086 /*********************************************************************/ nico@207: 00087 nico@207: 00088 UNS8 buildPDO(CO_Data* d, UNS16 index) nico@207: 00089 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */ nico@207: 00090 UNS16 ind; nico@207: 00091 UNS8 subInd; nico@207: 00092 nico@207: 00093 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ nico@207: 00094 /* pointer to the var which is mapped to a pdo */ nico@207: 00095 /* void * pMappedAppObject = NULL; */ nico@207: 00096 /* pointer fo the var which holds the mapping parameter of an mapping entry */ nico@207: 00097 UNS32 * pMappingParameter = NULL; nico@207: 00098 nico@207: 00099 UNS8 Size; nico@207: 00100 UNS8 dataType; nico@207: 00101 UNS8 offset; nico@207: 00102 UNS16 offsetObjdict; nico@207: 00103 UNS16 offsetObjdictPrm; nico@207: 00104 UNS32 objDict; nico@207: 00105 nico@207: 00106 subInd=(UNS8)0x00; nico@207: 00107 offset = 0x00; nico@207: 00108 ind = index - 0x1800; nico@207: 00109 nico@207: 00110 MSG_WAR(0x3910,"Prepare PDO to send index :", index); nico@207: 00111 nico@207: 00112 /* only operational state allows PDO transmission */ nico@207: 00113 if( d->nodeState != Operational ) { nico@207: 00114 MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index); nico@207: 00115 return 0xFF; nico@207: 00116 } nico@207: 00117 offsetObjdictPrm = d->firstIndex->PDO_TRS; nico@207: 00118 offsetObjdict = d->firstIndex->PDO_TRS_MAP; nico@207: 00119 nico@207: 00120 if (offsetObjdictPrm && offsetObjdict) nico@207: 00121 { nico@207: 00122 /* get mapped objects number to transmit with this PDO */ nico@207: 00123 pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject; nico@207: 00124 MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount); nico@207: 00125 MSG_WAR(0x3913, " at index : ", 0x1A00 + ind); nico@207: 00126 while (subInd < *pMappingCount) { /* Loop on mapped variables */ nico@207: 00127 /* get mapping parameters */ nico@207: 00128 pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject; nico@207: 00129 MSG_WAR(0x3914, "Get the mapping at index : ", (UNS16)0x1A00 + ind); nico@207: 00130 MSG_WAR(0x3915, " subIndex : ", subInd + 1); nico@207: 00131 MSG_WAR(0x3916, " value : ", *(UNS32 *)pMappingParameter); nico@207: 00132 /* Get the mapped variable */ nico@207: 00133 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); nico@207: 00134 objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16), nico@207: 00135 (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF), nico@207: 00136 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); nico@207: 00137 nico@207: 00138 if (objDict != OD_SUCCESSFUL) { nico@207: 00139 MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1); nico@207: 00140 MSG_WAR(0x2920, " Mapped at index : ", (*pMappingParameter) >> 16); nico@207: 00141 MSG_WAR(0x2921, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); nico@207: 00142 return 0xFF; nico@207: 00143 } nico@207: 00144 nico@207: 00145 offset += Size; nico@207: 00146 d->process_var.count = offset; nico@207: 00147 subInd++; nico@207: 00148 }/* end Loop on mapped variables */ nico@207: 00149 } nico@207: 00150 return 0; nico@207: 00151 } nico@207: 00152 #endif nico@207: 00153 /**************************************************************************/ nico@207: 00154 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId ) nico@207: 00155 { nico@207: 00156 UNS32 * pwCobId; nico@207: 00157 UNS16 offset; nico@207: 00158 UNS16 lastIndex; nico@207: 00159 UNS8 err; nico@207: 00160 nico@207: 00161 MSG_WAR(0x3930, "sendPDOrequest ",0); nico@207: 00162 /* Sending the request only if the cobid have been found on the PDO receive */ nico@207: 00163 /* part dictionary */ nico@207: 00164 offset = d->firstIndex->PDO_RCV; nico@207: 00165 lastIndex = d->lastIndex->PDO_RCV; nico@207: 00166 if (offset) nico@207: 00167 while (offset <= lastIndex) { nico@207: 00168 /*get the CobId*/ nico@207: 00169 pwCobId = d->objdict[offset].pSubindex[1].pObject; nico@207: 00170 nico@207: 00171 if ( *pwCobId == cobId ) { nico@207: 00172 s_PDO pdo; nico@207: 00173 pdo.cobId = *pwCobId; nico@207: 00174 pdo.len = 0; nico@207: 00175 err = sendPDO(d, pdo, REQUEST); nico@207: 00176 return err; nico@207: 00177 } nico@207: 00178 offset++; nico@207: 00179 } nico@207: 00180 MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId); nico@207: 00181 return 0xFF; nico@207: 00182 } nico@207: 00183 nico@207: 00184 nico@207: 00185 nico@207: 00186 /***********************************************************************/ nico@207: 00187 UNS8 proceedPDO(CO_Data* d, Message *m) nico@207: 00188 { nico@207: 00189 UNS8 numPdo; nico@207: 00190 UNS8 numMap; /* Number of the mapped varable */ nico@207: 00191 UNS8 i; nico@207: 00192 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ nico@207: 00193 /* pointer to the var which is mapped to a pdo... */ nico@207: 00194 /* void * pMappedAppObject = NULL; */ nico@207: 00195 /* pointer fo the var which holds the mapping parameter of an mapping entry */ nico@207: 00196 UNS32 * pMappingParameter = NULL; nico@207: 00197 UNS8 * pTransmissionType = NULL; /* pointer to the transmission type */ nico@207: 00198 UNS32 * pwCobId = NULL; nico@207: 00199 UNS8 Size; nico@207: 00200 UNS8 dataType; nico@207: 00201 UNS8 offset; nico@207: 00202 UNS8 status; nico@207: 00203 UNS32 objDict; nico@207: 00204 UNS16 offsetObjdict; nico@207: 00205 UNS16 lastIndex; nico@207: 00206 status = state1; nico@207: 00207 nico@207: 00208 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff)); nico@207: 00209 offset = 0x00; nico@207: 00210 numPdo = 0; nico@207: 00211 numMap = 0; nico@207: 00212 if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a request. */ nico@207: 00213 offsetObjdict = d->firstIndex->PDO_RCV; nico@207: 00214 lastIndex = d->lastIndex->PDO_RCV; nico@207: 00215 nico@207: 00216 /* study of all the PDO stored in the dictionary */ nico@207: 00217 if(offsetObjdict) nico@207: 00218 while (offsetObjdict <= lastIndex) { nico@207: 00219 nico@207: 00220 switch( status ) { nico@207: 00221 nico@207: 00222 case state1: /* data are stored in process_var array */ nico@207: 00223 /* memcpy(&(process_var.data), &m->data, (*m).len); */ nico@207: 00224 /* Ce memcpy devrait être portable. */ nico@207: 00225 for ( i = 0 ; i < m->len ; i++) nico@207: 00226 d->process_var.data[i] = m->data[i]; nico@207: 00227 d->process_var.count = (*m).len; nico@207: 00228 nico@207: 00229 status = state2; nico@207: 00230 break; nico@207: 00231 nico@207: 00232 case state2: nico@207: 00233 /* get CobId of the dictionary correspondant to the received PDO */ nico@207: 00234 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; nico@207: 00235 /* check the CobId coherance */ nico@207: 00236 /*pwCobId is the cobId read in the dictionary at the state 3 */ nico@207: 00237 if ( *pwCobId == (*m).cob_id.w ){ nico@207: 00238 /* The cobId is recognized */ nico@207: 00239 status = state4; nico@207: 00240 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); nico@207: 00241 break; nico@207: 00242 } nico@207: 00243 else { nico@207: 00244 /* cobId received does not match with those write in the dictionnary */ nico@207: 00245 numPdo++; nico@207: 00246 offsetObjdict++; nico@207: 00247 status = state2; nico@207: 00248 break; nico@207: 00249 } nico@207: 00250 nico@207: 00251 case state4: /* get mapped objects number */ nico@207: 00252 /* The cobId of the message received has been found in the dictionnary. */ nico@207: 00253 offsetObjdict = d->firstIndex->PDO_RCV_MAP; nico@207: 00254 lastIndex = d->lastIndex->PDO_RCV_MAP; nico@207: 00255 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; nico@207: 00256 numMap = 0; nico@207: 00257 while (numMap < *pMappingCount) { nico@207: 00258 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; nico@207: 00259 UNS8 ByteSize; nico@207: 00260 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; nico@207: 00261 if (pMappingParameter == NULL) { nico@207: 00262 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1); nico@207: 00263 return 0xFF; nico@207: 00264 } nico@207: 00265 /* Get the addresse of the mapped variable. */ nico@207: 00266 /* detail of *pMappingParameter : */ nico@207: 00267 /* The 16 hight bits contains the index, the medium 8 bits contains the subindex, */ nico@207: 00268 /* and the lower 8 bits contains the size of the mapped variable. */ nico@207: 00269 nico@207: 00270 Size = (UNS8)(*pMappingParameter); nico@207: 00271 nico@207: 00272 /* copy bit per bit in little endian */ nico@207: 00273 CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); nico@207: 00274 nico@207: 00275 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ nico@207: 00276 nico@207: 00277 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), nico@207: 00278 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), nico@207: 00279 tmp, &ByteSize, 0 ); nico@207: 00280 nico@207: 00281 if(objDict != OD_SUCCESSFUL) { nico@207: 00282 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1); nico@207: 00283 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16); nico@207: 00284 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); nico@207: 00285 return 0xFF; nico@207: 00286 } nico@207: 00287 nico@207: 00288 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); nico@207: 00289 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); nico@207: 00290 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); nico@207: 00291 /* MSG_WAR(0x3945, " data : ",*((UNS32 *)pMappedAppObject)); */ nico@207: 00292 offset += Size; nico@207: 00293 numMap++; nico@207: 00294 } /* end loop while on mapped variables */ nico@207: 00295 nico@207: 00296 offset=0x00; nico@207: 00297 numMap = 0; nico@207: 00298 return 0; nico@207: 00299 nico@207: 00300 }/* end switch status */ nico@207: 00301 }/* end while */ nico@207: 00302 }/* end if Donnees */ nico@207: 00303 nico@207: 00304 nico@207: 00305 else if ((*m).rtr == REQUEST ){ nico@207: 00306 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w); nico@207: 00307 status = state1; nico@207: 00308 offsetObjdict = d->firstIndex->PDO_TRS; nico@207: 00309 lastIndex = d->lastIndex->PDO_TRS; nico@207: 00310 if(offsetObjdict) while( offsetObjdict <= lastIndex ){ nico@207: 00311 /* study of all PDO stored in the objects dictionary */ nico@207: 00312 nico@207: 00313 switch( status ){ nico@207: 00314 nico@207: 00315 case state1: /* check the CobId */ nico@207: 00316 /* get CobId of the dictionary which match to the received PDO */ nico@207: 00317 pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject; nico@207: 00318 if ( *pwCobId == (*m).cob_id.w ) { nico@207: 00319 status = state4; nico@207: 00320 break; nico@207: 00321 } nico@207: 00322 else { nico@207: 00323 numPdo++; nico@207: 00324 offsetObjdict++; nico@207: 00325 } nico@207: 00326 status = state1; nico@207: 00327 break; nico@207: 00328 nico@207: 00329 nico@207: 00330 case state4: /* check transmission type (after request?) */ nico@207: 00331 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; nico@207: 00332 if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) { nico@207: 00333 status = state5; nico@207: 00334 break; nico@207: 00335 } nico@207: 00336 else { nico@207: 00337 /* The requested PDO is not to send on request. So, does nothing. */ nico@207: 00338 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w); nico@207: 00339 return 0xFF; nico@207: 00340 } nico@207: 00341 nico@207: 00342 case state5: /* get mapped objects number */ nico@207: 00343 offsetObjdict = d->firstIndex->PDO_TRS_MAP; nico@207: 00344 lastIndex = d->lastIndex->PDO_TRS_MAP; nico@207: 00345 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; nico@207: 00346 numMap = 0; nico@207: 00347 while (numMap < *pMappingCount) { nico@207: 00348 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; nico@207: 00349 /* Get the mapped variable */ nico@207: 00350 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); nico@207: 00351 objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16), nico@207: 00352 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF), nico@207: 00353 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); nico@207: 00354 if (objDict != OD_SUCCESSFUL) { nico@207: 00355 MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1); nico@207: 00356 MSG_WAR(0x2949, " Mapped at index : ", (*pMappingParameter) >> 16); nico@207: 00357 MSG_WAR(0x2950, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); nico@207: 00358 return 0xFF; nico@207: 00359 } nico@207: 00360 offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3); nico@207: 00361 d->process_var.count = offset; nico@207: 00362 numMap++; nico@207: 00363 nico@207: 00364 } /* end while */ nico@207: 00365 PDOmGR( d, *pwCobId ); /* Transmit the PDO */ nico@207: 00366 return 0; nico@207: 00367 nico@207: 00368 }/* end switch status */ nico@207: 00369 }/* end while */ nico@207: 00370 }/* end if Requete */ nico@207: 00371 nico@207: 00372 return 0; nico@207: 00373 } nico@207: 00374 nico@207: 00375 nico@207: 00376 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian) nico@207: 00377 { nico@207: 00378 //This loop copy as many bits that it can each time, crossing successively bytes nico@207: 00379 // boundaries from LSB to MSB. nico@207: 00380 while(NbBits > 0) nico@207: 00381 { nico@207: 00382 // Bit missalignement between src and dest nico@207: 00383 INTEGER8 Vect = DestBitIndex - SrcBitIndex; nico@207: 00384 nico@207: 00385 // We can now get src and align it to dest nico@207: 00386 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; nico@207: 00387 nico@207: 00388 // Compute the nb of bit we will be able to copy nico@207: 00389 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex ); nico@207: 00390 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit; nico@207: 00391 nico@207: 00392 // Create a mask that will serve in: nico@207: 00393 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex))); nico@207: 00394 nico@207: 00395 // - Filtering src nico@207: 00396 UNS8 Filtered = Aligned & ~Mask; nico@207: 00397 nico@207: 00398 // - and erase bits where we write, preserve where we don't nico@207: 00399 *DestByteIndex &= Mask; nico@207: 00400 nico@207: 00401 // Then write. nico@207: 00402 *DestByteIndex |= Filtered ; nico@207: 00403 nico@207: 00404 //Compute next time cursors for src nico@207: 00405 if((SrcBitIndex += BitsToCopy)>7) // cross boundary ? nico@207: 00406 { nico@207: 00407 SrcBitIndex = 0; // First bit nico@207: 00408 SrcByteIndex += (SrcBigEndian ? -1 : 1); // Next byte nico@207: 00409 } nico@207: 00410 nico@207: 00411 //Compute next time cursors for dest nico@207: 00412 if((DestBitIndex += BitsToCopy)>7) nico@207: 00413 { nico@207: 00414 DestBitIndex = 0; // First bit nico@207: 00415 DestByteIndex += (DestBigEndian ? -1 : 1);// Next byte nico@207: 00416 } nico@207: 00417 nico@207: 00418 //And decrement counter. nico@207: 00419 NbBits -= BitsToCopy; nico@207: 00420 } nico@207: 00421 nico@207: 00422 } nico@207: 00423 nico@207: 00424 #if 0 nico@207: 00425 nico@207: 00426 /*********************************************************************/ nico@207: 00427 /* TODO : reimplement this using CallBacks */ nico@207: 00428 /*********************************************************************/ nico@207: 00429 nico@207: 00430 UNS8 sendPDOevent( CO_Data* d, void * variable ) nico@207: 00431 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */ nico@207: 00432 UNS32 objDict = 0; nico@207: 00433 UNS8 ind, sub_ind; nico@207: 00434 UNS8 status; nico@207: 00435 UNS8 offset; nico@207: 00436 UNS8 * pMappingCount = NULL; nico@207: 00437 UNS32 * pMappingParameter = NULL; nico@207: 00438 void * pMappedAppObject = NULL; nico@207: 00439 UNS8 * pTransmissionType = NULL; /* pointer to the transmission type */ nico@207: 00440 UNS32 * pwCobId = NULL; nico@207: 00441 UNS8 * pSize; nico@207: 00442 UNS8 size; nico@207: 00443 UNS8 dataType; nico@207: 00444 UNS16 offsetObjdict; nico@207: 00445 UNS16 offsetObjdictPrm; nico@207: 00446 UNS16 lastIndex; nico@207: 00447 UNS8 numMap; nico@207: 00448 ind = 0x00; nico@207: 00449 sub_ind = 1; nico@207: 00450 offset = 0x00; nico@207: 00451 pSize = &size; nico@207: 00452 status = state1; nico@207: 00453 nico@207: 00454 /* look for the index and subindex where the variable is mapped */ nico@207: 00455 /* Then, send the pdo which contains the variable. */ nico@207: 00456 nico@207: 00457 MSG_WAR (0x3960, "sendPDOevent", 0); nico@207: 00458 offsetObjdictPrm = d->firstIndex->PDO_TRS; nico@207: 00459 nico@207: 00460 offsetObjdict = d->firstIndex->PDO_TRS_MAP; nico@207: 00461 lastIndex = d->lastIndex->PDO_TRS_MAP; nico@207: 00462 nico@207: 00463 if (offsetObjdictPrm && offsetObjdict) nico@207: 00464 /* Loop on PDO Transmit */ nico@207: 00465 while(offsetObjdict <= lastIndex){ nico@207: 00466 /* Check the transmission mode */ nico@207: 00467 pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject; nico@207: 00468 if (*pTransmissionType != TRANS_EVENT) { nico@207: 00469 ind++; nico@207: 00470 offsetObjdict++; nico@207: 00471 offsetObjdictPrm++; nico@207: 00472 continue; nico@207: 00473 } nico@207: 00474 pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject; nico@207: 00475 numMap = 1; /* mapped variable */ nico@207: 00476 while (numMap <= *pMappingCount) { nico@207: 00477 pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject; nico@207: 00478 /* Get the variable */ nico@207: 00479 objDict = getODentry( d, nico@207: 00480 (UNS16)((*pMappingParameter) >> 16), nico@207: 00481 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF), nico@207: 00482 (void * *)&pMappedAppObject, pSize, &dataType, 0 ); nico@207: 00483 if( objDict != OD_SUCCESSFUL ) { nico@207: 00484 MSG_WAR(0x2961, "Error in dict. at index : ", nico@207: 00485 (*pMappingParameter) >> (UNS8)16); nico@207: 00486 nico@207: 00487 MSG_WAR(0x2962, " subindex : ", nico@207: 00488 ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); nico@207: 00489 return 0xFF; nico@207: 00490 } nico@207: 00491 if (pMappedAppObject == variable) { // Variable found ! nico@207: 00492 MSG_WAR(0x3963, "Variable to send found at index : ", nico@207: 00493 (*pMappingParameter) >> 16); nico@207: 00494 MSG_WAR(0x3964, " subIndex : ", nico@207: 00495 ((*pMappingParameter) >> 8 ) & 0x000000FF); nico@207: 00496 buildPDO(d, 0x1800 + ind); nico@207: 00497 /* Get the cobId */ nico@207: 00498 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject; nico@207: 00499 PDOmGR( d, *pwCobId ); /* Send the PDO */ nico@207: 00500 return 0; nico@207: 00501 } nico@207: 00502 numMap++; nico@207: 00503 } /* End loop on mapped variable */ nico@207: 00504 ind++; nico@207: 00505 offsetObjdict++; nico@207: 00506 offsetObjdictPrm++; nico@207: 00507 } /* End loop while on PDO */ nico@207: 00508 nico@207: 00509 MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0); nico@207: 00510 return 0xFF; nico@207: 00511 nico@207: 00512 } nico@207: 00513 #endif nico@207: