/*
* Function for accessing _TPDO mapping parameter_ (index 0x1A00+) from SDO server.
*
* For more information see file CO_SDO.h.
*/
static CO_SDO_abortCode_t CO_ODF_TPDOmap(CO_ODF_arg_t *ODF_arg){
CO_TPDO_t *TPDO;
TPDO = (CO_TPDO_t*) ODF_arg->object;
/* Reading Object Dictionary variable */
if(ODF_arg->reading){
uint8_t *value = (uint8_t*) ODF_arg->data;
if(ODF_arg->subIndex == 0){
/* If there is error in mapping, dataLength is 0, so numberOfMappedObjects is 0. */
if(!TPDO->dataLength) *value = 0;
}
return CO_SDO_AB_NONE;
}
/* Writing Object Dictionary variable */
if(TPDO->restrictionFlags & 0x08)
return CO_SDO_AB_READONLY; /* Attempt to write a read only object. */
if(*TPDO->operatingState == CO_NMT_OPERATIONAL && (TPDO->restrictionFlags & 0x02))
return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */
if(TPDO->valid)
return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
/* numberOfMappedObjects */
if(ODF_arg->subIndex == 0){
uint8_t *value = (uint8_t*) ODF_arg->data;
if(*value > 8)
return CO_SDO_AB_VALUE_HIGH; /* Value of parameter written too high. */
/* configure mapping */
return CO_TPDOconfigMap(TPDO, *value);
}
/* mappedObject */
else{
uint32_t *value = (uint32_t*) ODF_arg->data;
uint8_t* pData;
uint8_t length = 0;
uint8_t dummy = 0;
uint8_t MBvar;
if(TPDO->dataLength)
return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
/* verify if mapping is correct */
return CO_PDOfindMap(
TPDO->SDO,
*value,
1,
&pData,
&length,
&dummy,
&MBvar);
}
return CO_SDO_AB_NONE;
}
/*
* Function for accessing _RPDO mapping parameter_ (index 0x1600+) from SDO server.
*
* For more information see file CO_SDO.h.
*/
static uint32_t CO_ODF_RPDOmap(CO_ODF_arg_t *ODF_arg){
CO_RPDO_t *RPDO;
RPDO = (CO_RPDO_t*) ODF_arg->object;
/* Reading Object Dictionary variable */
if(ODF_arg->reading){
uint8_t *value = (uint8_t*) ODF_arg->data;
if(ODF_arg->subIndex == 0){
/* If there is error in mapping, dataLength is 0, so numberOfMappedObjects is 0. */
if(!RPDO->dataLength) *value = 0;
}
return 0;
}
/* Writing Object Dictionary variable */
if(RPDO->restrictionFlags & 0x08)
return 0x06010002L; /* Attempt to write a read only object. */
if(*RPDO->operatingState == CO_NMT_OPERATIONAL && (RPDO->restrictionFlags & 0x02))
return 0x08000022L; /* Data cannot be transferred or stored to the application because of the present device state. */
if(RPDO->valid)
return 0x06090030L; /* Invalid value for parameter (download only). */
/* numberOfMappedObjects */
if(ODF_arg->subIndex == 0){
uint8_t *value = (uint8_t*) ODF_arg->data;
if(*value > 8)
return 0x06090031L; /* Value of parameter written too high. */
/* configure mapping */
return CO_RPDOconfigMap(RPDO, *value);
}
/* mappedObject */
else{
uint32_t *value = (uint32_t*) ODF_arg->data;
uint8_t* pData;
uint8_t length = 0;
uint8_t dummy = 0;
uint8_t MBvar;
if(RPDO->dataLength)
return 0x06090030L; /* Invalid value for parameter (download only). */
/* verify if mapping is correct */
return CO_PDOfindMap(
RPDO->SDO,
*value,
0,
&pData,
&length,
&dummy,
&MBvar);
}
return 0;
}
/*
* Configure TPDO Mapping parameter.
*
* Function is called from communication reset or when parameter changes.
*
* Function configures following variables from CO_TPDO_t: _dataLength_,
* _mapPointer_ and _sendIfCOSFlags_.
*
* @param TPDO TPDO object.
* @param noOfMappedObjects Number of mapped object (from OD).
*
* @return 0 on success, otherwise SDO abort code.
*/
static uint32_t CO_TPDOconfigMap(CO_TPDO_t* TPDO, uint8_t noOfMappedObjects){
int16_t i;
uint8_t length = 0;
uint32_t ret = 0;
const uint32_t* pMap = &TPDO->TPDOMapPar->mappedObject1;
TPDO->sendIfCOSFlags = 0;
for(i=noOfMappedObjects; i>0; i--){
int16_t j;
uint8_t* pData;
uint8_t prevLength = length;
uint8_t MBvar;
uint32_t map = *(pMap++);
/* function do much checking of errors in map */
ret = CO_PDOfindMap(
TPDO->SDO,
map,
1,
&pData,
&length,
&TPDO->sendIfCOSFlags,
&MBvar);
if(ret){
length = 0;
CO_errorReport(TPDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, map);
break;
}
/* write PDO data pointers */
#ifdef CO_BIG_ENDIAN
if(MBvar){
for(j=length-1; j>=prevLength; j--)
TPDO->mapPointer[j] = pData++;
}
else{
for(j=prevLength; j<length; j++)
TPDO->mapPointer[j] = pData++;
}
#else
for(j=prevLength; j<length; j++){
TPDO->mapPointer[j] = pData++;
}
#endif
}
TPDO->dataLength = length;
return ret;
}
/*
* Configure RPDO Mapping parameter.
*
* Function is called from communication reset or when parameter changes.
*
* Function configures following variables from CO_RPDO_t: _dataLength_ and
* _mapPointer_.
*
* @param RPDO RPDO object.
* @param noOfMappedObjects Number of mapped object (from OD).
*
* @return 0 on success, otherwise SDO abort code.
*/
static uint32_t CO_RPDOconfigMap(CO_RPDO_t* RPDO, uint8_t noOfMappedObjects){
int16_t i;
uint8_t length = 0;
uint32_t ret = 0;
const uint32_t* pMap = &RPDO->RPDOMapPar->mappedObject1;
for(i=noOfMappedObjects; i>0; i--){
int16_t j;
uint8_t* pData;
uint8_t dummy = 0;
uint8_t prevLength = length;
uint8_t MBvar;
uint32_t map = *(pMap++);
/* function do much checking of errors in map */
ret = CO_PDOfindMap(
RPDO->SDO,
map,
0,
&pData,
&length,
&dummy,
&MBvar);
if(ret){
length = 0;
CO_errorReport(RPDO->EM, ERROR_PDO_WRONG_MAPPING, map);
break;
}
/* write PDO data pointers */
#ifdef BIG_ENDIAN
if(MBvar){
for(j=length-1; j>=prevLength; j--)
RPDO->mapPointer[j] = pData++;
}
else{
for(j=prevLength; j<length; j++)
RPDO->mapPointer[j] = pData++;
}
#else
for(j=prevLength; j<length; j++){
RPDO->mapPointer[j] = pData++;
}
#endif
}
RPDO->dataLength = length;
return ret;
}
