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