66 |
66 |
67 /* pointer fo the var which holds the mapping parameter of an mapping entry */ |
67 /* pointer fo the var which holds the mapping parameter of an mapping entry */ |
68 UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject; |
68 UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject; |
69 UNS16 index = (UNS16)((*pMappingParameter) >> 16); |
69 UNS16 index = (UNS16)((*pMappingParameter) >> 16); |
70 UNS8 Size = (UNS8)(*pMappingParameter); /* Size in bits */ |
70 UNS8 Size = (UNS8)(*pMappingParameter); /* Size in bits */ |
71 UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
|
72 UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
|
73 |
71 |
74 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); |
72 /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */ |
75 MSG_WAR(0x3050, " at index : ", TPDO_map->index); |
73 if(Size && ((offset + Size) <= 64)) { |
76 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); |
74 UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
77 |
75 UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
78 if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){ |
76 |
79 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); |
77 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); |
80 return 0xFF; |
78 MSG_WAR(0x3050, " at index : ", TPDO_map->index); |
|
79 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); |
|
80 |
|
81 if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){ |
|
82 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); |
|
83 return 0xFF; |
|
84 } |
|
85 /* copy bit per bit in little endian*/ |
|
86 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0); |
|
87 |
|
88 offset += Size ; |
81 } |
89 } |
82 /* copy bit per bit in little endian*/ |
|
83 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0); |
|
84 |
|
85 offset += Size ; |
|
86 prp_j++; |
90 prp_j++; |
87 }while( prp_j < *pMappingCount ); |
91 }while( prp_j < *pMappingCount ); |
88 |
92 |
89 pdo->len = 1 + ((offset - 1) >> 3); |
93 pdo->len = 1 + ((offset - 1) >> 3); |
90 |
94 |
223 contains the subindex, */ |
227 contains the subindex, */ |
224 /* and the lower 8 bits contains the size of the mapped |
228 /* and the lower 8 bits contains the size of the mapped |
225 variable. */ |
229 variable. */ |
226 |
230 |
227 Size = (UNS8)(*pMappingParameter); |
231 Size = (UNS8)(*pMappingParameter); |
228 |
232 |
229 /* copy bit per bit in little endian */ |
233 /* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */ |
230 CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); |
234 if(Size && ((offset + Size) <= (m->len << 3))) { |
231 |
235 /* copy bit per bit in little endian */ |
232 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => |
236 CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); |
233 2, ... */ |
237 |
234 |
238 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => |
235 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), |
239 2, ... */ |
236 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), |
240 |
237 tmp, &ByteSize, 0 ); |
241 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), |
238 |
242 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), |
239 if(objDict != OD_SUCCESSFUL) { |
243 tmp, &ByteSize, 0 ); |
240 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1); |
244 |
241 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16); |
245 if(objDict != OD_SUCCESSFUL) { |
242 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
246 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1); |
243 return 0xFF; |
247 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16); |
244 } |
248 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
245 |
249 return 0xFF; |
246 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); |
250 } |
247 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); |
251 |
248 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
252 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); |
249 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ |
253 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); |
250 offset += Size; |
254 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
|
255 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ |
|
256 offset += Size; |
|
257 } |
251 numMap++; |
258 numMap++; |
252 /*TODO : check that offset is not not greater that message size (in bit) */ |
|
253 } /* end loop while on mapped variables */ |
259 } /* end loop while on mapped variables */ |
254 |
260 |
255 offset=0x00; |
261 offset=0x00; |
256 numMap = 0; |
262 numMap = 0; |
257 return 0; |
263 return 0; |