/* * CANopen Process Data Object. * * @file CO_PDO.c * @ingroup CO_PDO * @author Janez Paternoster * @copyright 2004 - 2020 Janez Paternoster * * This file is part of CANopenNode, an opensource CANopen Stack. * Project home page is . * For more information on CANopen see . * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "CO_driver.h" #include "CO_SDO.h" #include "CO_Emergency.h" #include "CO_NMT_Heartbeat.h" #include "CO_SYNC.h" #include "CO_PDO.h" /* * Read received message from CAN module. * * Function will be called (by CAN receive interrupt) every time, when CAN * message with correct identifier will be received. For more information and * description of parameters see file CO_driver.h. * If new message arrives and previous message wasn't processed yet, then * previous message will be lost and overwritten by new message. That's OK with PDOs. */ static void CO_PDO_receive(void *object, const CO_CANrxMsg_t *msg){ CO_RPDO_t *RPDO; RPDO = (CO_RPDO_t*)object; /* this is the correct pointer type of the first argument */ if( (RPDO->valid) && (*RPDO->operatingState == CO_NMT_OPERATIONAL) && (msg->DLC >= RPDO->dataLength)) { if(RPDO->SYNC && RPDO->synchronous && RPDO->SYNC->CANrxToggle) { /* copy data into second buffer and set 'new message' flag */ RPDO->CANrxData[1][0] = msg->data[0]; RPDO->CANrxData[1][1] = msg->data[1]; RPDO->CANrxData[1][2] = msg->data[2]; RPDO->CANrxData[1][3] = msg->data[3]; RPDO->CANrxData[1][4] = msg->data[4]; RPDO->CANrxData[1][5] = msg->data[5]; RPDO->CANrxData[1][6] = msg->data[6]; RPDO->CANrxData[1][7] = msg->data[7]; SET_CANrxNew(RPDO->CANrxNew[1]); } else { /* copy data into default buffer and set 'new message' flag */ RPDO->CANrxData[0][0] = msg->data[0]; RPDO->CANrxData[0][1] = msg->data[1]; RPDO->CANrxData[0][2] = msg->data[2]; RPDO->CANrxData[0][3] = msg->data[3]; RPDO->CANrxData[0][4] = msg->data[4]; RPDO->CANrxData[0][5] = msg->data[5]; RPDO->CANrxData[0][6] = msg->data[6]; RPDO->CANrxData[0][7] = msg->data[7]; SET_CANrxNew(RPDO->CANrxNew[0]); } } } /* * Configure RPDO Communication parameter. * * Function is called from commuincation reset or when parameter changes. * * Function configures following variable from CO_RPDO_t: _valid_. It also * configures CAN rx buffer. If configuration fails, emergency message is send * and device is not able to enter NMT operational. * * @param RPDO RPDO object. * @param COB_IDUsedByRPDO _RPDO communication parameter_, _COB-ID for PDO_ variable * from Object dictionary (index 0x1400+, subindex 1). */ static void CO_RPDOconfigCom(CO_RPDO_t* RPDO, uint32_t COB_IDUsedByRPDO){ uint16_t ID; CO_ReturnError_t r; ID = (uint16_t)COB_IDUsedByRPDO; /* is RPDO used? */ if((COB_IDUsedByRPDO & 0xBFFFF800L) == 0 && RPDO->dataLength && ID){ /* is used default COB-ID? */ if(ID == RPDO->defaultCOB_ID) ID += RPDO->nodeId; RPDO->valid = true; RPDO->synchronous = (RPDO->RPDOCommPar->transmissionType <= 240) ? true : false; } else{ ID = 0; RPDO->valid = false; CLEAR_CANrxNew(RPDO->CANrxNew[0]); CLEAR_CANrxNew(RPDO->CANrxNew[1]); } r = CO_CANrxBufferInit( RPDO->CANdevRx, /* CAN device */ RPDO->CANdevRxIdx, /* rx buffer index */ ID, /* CAN identifier */ 0x7FF, /* mask */ 0, /* rtr */ (void*)RPDO, /* object passed to receive function */ CO_PDO_receive); /* this function will process received message */ if(r != CO_ERROR_NO){ RPDO->valid = false; CLEAR_CANrxNew(RPDO->CANrxNew[0]); CLEAR_CANrxNew(RPDO->CANrxNew[1]); } } /* * Configure TPDO Communication parameter. * * Function is called from commuincation reset or when parameter changes. * * Function configures following variable from CO_TPDO_t: _valid_. It also * configures CAN tx buffer. If configuration fails, emergency message is send * and device is not able to enter NMT operational. * * @param TPDO TPDO object. * @param COB_IDUsedByTPDO _TPDO communication parameter_, _COB-ID for PDO_ variable * from Object dictionary (index 0x1400+, subindex 1). * @param syncFlag Indicate, if TPDO is synchronous. */ static void CO_TPDOconfigCom(CO_TPDO_t* TPDO, uint32_t COB_IDUsedByTPDO, uint8_t syncFlag){ uint16_t ID; ID = (uint16_t)COB_IDUsedByTPDO; /* is TPDO used? */ if((COB_IDUsedByTPDO & 0xBFFFF800L) == 0 && TPDO->dataLength && ID){ /* is used default COB-ID? */ if(ID == TPDO->defaultCOB_ID) ID += TPDO->nodeId; TPDO->valid = true; } else{ ID = 0; TPDO->valid = false; } TPDO->CANtxBuff = CO_CANtxBufferInit( TPDO->CANdevTx, /* CAN device */ TPDO->CANdevTxIdx, /* index of specific buffer inside CAN module */ ID, /* CAN identifier */ 0, /* rtr */ TPDO->dataLength, /* number of data bytes */ syncFlag); /* synchronous message flag bit */ if(TPDO->CANtxBuff == 0){ TPDO->valid = false; } } /* * Find mapped variable in Object Dictionary. * * Function is called from CO_R(T)PDOconfigMap or when mapping parameter changes. * * @param SDO SDO object. * @param map PDO mapping parameter. * @param R_T 0 for RPDO map, 1 for TPDO map. * @param ppData Pointer to returning parameter: pointer to data of mapped variable. * @param pLength Pointer to returning parameter: *add* length of mapped variable. * @param pSendIfCOSFlags Pointer to returning parameter: sendIfCOSFlags variable. * @param pIsMultibyteVar Pointer to returning parameter: true for multibyte variable. * * @return 0 on success, otherwise SDO abort code. */ static uint32_t CO_PDOfindMap( CO_SDO_t *SDO, uint32_t map, uint8_t R_T, uint8_t **ppData, uint8_t *pLength, uint8_t *pSendIfCOSFlags, uint8_t *pIsMultibyteVar) { uint16_t entryNo; uint16_t index; uint8_t subIndex; uint8_t dataLen; uint8_t objectLen; uint8_t attr; index = (uint16_t)(map>>16); subIndex = (uint8_t)(map>>8); dataLen = (uint8_t) map; /* data length in bits */ /* data length must be byte aligned */ if(dataLen&0x07) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */ dataLen >>= 3; /* new data length is in bytes */ *pLength += dataLen; /* total PDO length can not be more than 8 bytes */ if(*pLength > 8) return CO_SDO_AB_MAP_LEN; /* The number and length of the objects to be mapped would exceed PDO length. */ /* is there a reference to dummy entries */ if(index <=7 && subIndex == 0){ static uint32_t dummyTX = 0; static uint32_t dummyRX; uint8_t dummySize = 4; if(index<2) dummySize = 0; else if(index==2 || index==5) dummySize = 1; else if(index==3 || index==6) dummySize = 2; /* is size of variable big enough for map */ if(dummySize < dataLen) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */ /* Data and ODE pointer */ if(R_T == 0) *ppData = (uint8_t*) &dummyRX; else *ppData = (uint8_t*) &dummyTX; return 0; } /* find object in Object Dictionary */ entryNo = CO_OD_find(SDO, index); /* Does object exist in OD? */ if(entryNo == 0xFFFF || subIndex > SDO->OD[entryNo].maxSubIndex) return CO_SDO_AB_NOT_EXIST; /* Object does not exist in the object dictionary. */ attr = CO_OD_getAttribute(SDO, entryNo, subIndex); /* Is object Mappable for RPDO? */ if(R_T==0 && !((attr&CO_ODA_RPDO_MAPABLE) && (attr&CO_ODA_WRITEABLE))) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */ /* Is object Mappable for TPDO? */ if(R_T!=0 && !((attr&CO_ODA_TPDO_MAPABLE) && (attr&CO_ODA_READABLE))) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */ /* is size of variable big enough for map */ objectLen = CO_OD_getLength(SDO, entryNo, subIndex); if(objectLen < dataLen) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */ /* mark multibyte variable */ *pIsMultibyteVar = (attr&CO_ODA_MB_VALUE) ? 1 : 0; /* pointer to data */ *ppData = (uint8_t*) CO_OD_getDataPointer(SDO, entryNo, subIndex); #ifdef CO_BIG_ENDIAN /* skip unused MSB bytes */ if(*pIsMultibyteVar){ *ppData += objectLen - dataLen; } #endif /* setup change of state flags */ if(attr&CO_ODA_TPDO_DETECT_COS){ int16_t i; for(i=*pLength-dataLen; i<*pLength; i++){ *pSendIfCOSFlags |= 1<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, 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--) RPDO->mapPointer[j] = pData++; } else{ for(j=prevLength; jmapPointer[j] = pData++; } #else for(j=prevLength; jmapPointer[j] = pData++; } #endif } RPDO->dataLength = length; return ret; } /* * 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; jmapPointer[j] = pData++; } #else for(j=prevLength; jmapPointer[j] = pData++; } #endif } TPDO->dataLength = length; return ret; } /* * Function for accessing _RPDO communication parameter_ (index 0x1400+) from SDO server. * * For more information see file CO_SDO.h. */ static CO_SDO_abortCode_t CO_ODF_RPDOcom(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){ if(ODF_arg->subIndex == 1){ uint32_t value = CO_getUint32(ODF_arg->data); /* if default COB ID is used, write default value here */ if(((value)&0xFFFF) == RPDO->defaultCOB_ID && RPDO->defaultCOB_ID) value += RPDO->nodeId; /* If PDO is not valid, set bit 31 */ if(!RPDO->valid) value |= 0x80000000L; CO_setUint32(ODF_arg->data, value); } return CO_SDO_AB_NONE; } /* Writing Object Dictionary variable */ if(RPDO->restrictionFlags & 0x04) return CO_SDO_AB_READONLY; /* Attempt to write a read only object. */ if(*RPDO->operatingState == CO_NMT_OPERATIONAL && (RPDO->restrictionFlags & 0x01)) return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */ if(ODF_arg->subIndex == 1){ /* COB_ID */ uint32_t value = CO_getUint32(ODF_arg->data); /* bits 11...29 must be zero */ if(value & 0x3FFF8000L) return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */ /* if default COB-ID is being written, write defaultCOB_ID without nodeId */ if(((value)&0xFFFF) == (RPDO->defaultCOB_ID + RPDO->nodeId)){ value &= 0xC0000000L; value += RPDO->defaultCOB_ID; CO_setUint32(ODF_arg->data, value); } /* if PDO is valid, bits 0..29 can not be changed */ if(RPDO->valid && ((value ^ RPDO->RPDOCommPar->COB_IDUsedByRPDO) & 0x3FFFFFFFL)) return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */ /* configure RPDO */ CO_RPDOconfigCom(RPDO, value); } else if(ODF_arg->subIndex == 2){ /* Transmission_type */ uint8_t *value = (uint8_t*) ODF_arg->data; bool_t synchronousPrev = RPDO->synchronous; /* values from 241...253 are not valid */ if(*value >= 241 && *value <= 253) return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */ RPDO->synchronous = (*value <= 240) ? true : false; /* Remove old message from second buffer. */ if(RPDO->synchronous != synchronousPrev) { CLEAR_CANrxNew(RPDO->CANrxNew[1]); } } return CO_SDO_AB_NONE; } /* * Function for accessing _TPDO communication parameter_ (index 0x1800+) from SDO server. * * For more information see file CO_SDO.h. */ static CO_SDO_abortCode_t CO_ODF_TPDOcom(CO_ODF_arg_t *ODF_arg){ CO_TPDO_t *TPDO; TPDO = (CO_TPDO_t*) ODF_arg->object; if(ODF_arg->subIndex == 4) return CO_SDO_AB_SUB_UNKNOWN; /* Sub-index does not exist. */ /* Reading Object Dictionary variable */ if(ODF_arg->reading){ if(ODF_arg->subIndex == 1){ /* COB_ID */ uint32_t value = CO_getUint32(ODF_arg->data); /* if default COB ID is used, write default value here */ if(((value)&0xFFFF) == TPDO->defaultCOB_ID && TPDO->defaultCOB_ID) value += TPDO->nodeId; /* If PDO is not valid, set bit 31 */ if(!TPDO->valid) value |= 0x80000000L; CO_setUint32(ODF_arg->data, value); } return CO_SDO_AB_NONE; } /* Writing Object Dictionary variable */ if(TPDO->restrictionFlags & 0x04) return CO_SDO_AB_READONLY; /* Attempt to write a read only object. */ if(*TPDO->operatingState == CO_NMT_OPERATIONAL && (TPDO->restrictionFlags & 0x01)) return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */ if(ODF_arg->subIndex == 1){ /* COB_ID */ uint32_t value = CO_getUint32(ODF_arg->data); /* bits 11...29 must be zero */ if(value & 0x3FFF8000L) return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */ /* if default COB-ID is being written, write defaultCOB_ID without nodeId */ if(((value)&0xFFFF) == (TPDO->defaultCOB_ID + TPDO->nodeId)){ value &= 0xC0000000L; value += TPDO->defaultCOB_ID; CO_setUint32(ODF_arg->data, value); } /* if PDO is valid, bits 0..29 can not be changed */ if(TPDO->valid && ((value ^ TPDO->TPDOCommPar->COB_IDUsedByTPDO) & 0x3FFFFFFFL)) return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */ /* configure TPDO */ CO_TPDOconfigCom(TPDO, value, TPDO->CANtxBuff->syncFlag); TPDO->syncCounter = 255; } else if(ODF_arg->subIndex == 2){ /* Transmission_type */ uint8_t *value = (uint8_t*) ODF_arg->data; /* values from 241...253 are not valid */ if(*value >= 241 && *value <= 253) return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */ TPDO->CANtxBuff->syncFlag = (*value <= 240) ? 1 : 0; TPDO->syncCounter = 255; } else if(ODF_arg->subIndex == 3){ /* Inhibit_Time */ /* if PDO is valid, value can not be changed */ if(TPDO->valid) return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */ TPDO->inhibitTimer = 0; } else if(ODF_arg->subIndex == 5){ /* Event_Timer */ uint16_t value = CO_getUint16(ODF_arg->data); TPDO->eventTimer = ((uint32_t) value) * 1000; } else if(ODF_arg->subIndex == 6){ /* SYNC start value */ uint8_t *value = (uint8_t*) ODF_arg->data; /* if PDO is valid, value can not be changed */ if(TPDO->valid) return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */ /* values from 240...255 are not valid */ if(*value > 240) return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */ } 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 CO_SDO_abortCode_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 CO_SDO_AB_NONE; } /* Writing Object Dictionary variable */ if(RPDO->restrictionFlags & 0x08) return CO_SDO_AB_READONLY; /* Attempt to write a read only object. */ if(*RPDO->operatingState == CO_NMT_OPERATIONAL && (RPDO->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(RPDO->valid) return CO_SDO_AB_UNSUPPORTED_ACCESS; /* Unsupported access to an object. */ /* numberOfMappedObjects */ if(ODF_arg->subIndex == 0){ uint8_t *value = (uint8_t*) ODF_arg->data; if(*value > 8) return CO_SDO_AB_MAP_LEN; /* Number and length of object to be mapped exceeds PDO length. */ /* configure mapping */ return CO_RPDOconfigMap(RPDO, *value); } /* mappedObject */ else{ uint32_t value = CO_getUint32(ODF_arg->data); uint8_t* pData; uint8_t length = 0; uint8_t dummy = 0; uint8_t MBvar; if(RPDO->dataLength) return CO_SDO_AB_UNSUPPORTED_ACCESS; /* Unsupported access to an object. */ /* verify if mapping is correct */ return CO_PDOfindMap( RPDO->SDO, value, 0, &pData, &length, &dummy, &MBvar); } //return CO_SDO_AB_NONE; } /* * 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_UNSUPPORTED_ACCESS; /* Unsupported access to an object. */ /* numberOfMappedObjects */ if(ODF_arg->subIndex == 0){ uint8_t *value = (uint8_t*) ODF_arg->data; if(*value > 8) return CO_SDO_AB_MAP_LEN; /* Number and length of object to be mapped exceeds PDO length. */ /* configure mapping */ return CO_TPDOconfigMap(TPDO, *value); } /* mappedObject */ else{ uint32_t value = CO_getUint32(ODF_arg->data); uint8_t* pData; uint8_t length = 0; uint8_t dummy = 0; uint8_t MBvar; if(TPDO->dataLength) return CO_SDO_AB_UNSUPPORTED_ACCESS; /* Unsupported access to an object. */ /* verify if mapping is correct */ return CO_PDOfindMap( TPDO->SDO, value, 1, &pData, &length, &dummy, &MBvar); } //return CO_SDO_AB_NONE; } /******************************************************************************/ CO_ReturnError_t CO_RPDO_init( CO_RPDO_t *RPDO, CO_EM_t *em, CO_SDO_t *SDO, CO_SYNC_t *SYNC, uint8_t *operatingState, uint8_t nodeId, uint16_t defaultCOB_ID, uint8_t restrictionFlags, const CO_RPDOCommPar_t *RPDOCommPar, const CO_RPDOMapPar_t *RPDOMapPar, uint16_t idx_RPDOCommPar, uint16_t idx_RPDOMapPar, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx) { /* verify arguments */ if(RPDO==NULL || em==NULL || SDO==NULL || operatingState==NULL || RPDOCommPar==NULL || RPDOMapPar==NULL || CANdevRx==NULL){ return CO_ERROR_ILLEGAL_ARGUMENT; } /* Configure object variables */ RPDO->em = em; RPDO->SDO = SDO; RPDO->SYNC = SYNC; RPDO->RPDOCommPar = RPDOCommPar; RPDO->RPDOMapPar = RPDOMapPar; RPDO->operatingState = operatingState; RPDO->nodeId = nodeId; RPDO->defaultCOB_ID = defaultCOB_ID; RPDO->restrictionFlags = restrictionFlags; /* Configure Object dictionary entry at index 0x1400+ and 0x1600+ */ CO_OD_configure(SDO, idx_RPDOCommPar, CO_ODF_RPDOcom, (void*)RPDO, 0, 0); CO_OD_configure(SDO, idx_RPDOMapPar, CO_ODF_RPDOmap, (void*)RPDO, 0, 0); /* configure communication and mapping */ CLEAR_CANrxNew(RPDO->CANrxNew[0]); CLEAR_CANrxNew(RPDO->CANrxNew[1]); RPDO->CANdevRx = CANdevRx; RPDO->CANdevRxIdx = CANdevRxIdx; CO_RPDOconfigMap(RPDO, RPDOMapPar->numberOfMappedObjects); CO_RPDOconfigCom(RPDO, RPDOCommPar->COB_IDUsedByRPDO); return CO_ERROR_NO; } /******************************************************************************/ CO_ReturnError_t CO_TPDO_init( CO_TPDO_t *TPDO, CO_EM_t *em, CO_SDO_t *SDO, CO_SYNC_t *SYNC, uint8_t *operatingState, uint8_t nodeId, uint16_t defaultCOB_ID, uint8_t restrictionFlags, const CO_TPDOCommPar_t *TPDOCommPar, const CO_TPDOMapPar_t *TPDOMapPar, uint16_t idx_TPDOCommPar, uint16_t idx_TPDOMapPar, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx) { /* verify arguments */ if(TPDO==NULL || em==NULL || SDO==NULL || operatingState==NULL || TPDOCommPar==NULL || TPDOMapPar==NULL || CANdevTx==NULL){ return CO_ERROR_ILLEGAL_ARGUMENT; } /* Configure object variables */ TPDO->em = em; TPDO->SDO = SDO; TPDO->SYNC = SYNC; TPDO->TPDOCommPar = TPDOCommPar; TPDO->TPDOMapPar = TPDOMapPar; TPDO->operatingState = operatingState; TPDO->nodeId = nodeId; TPDO->defaultCOB_ID = defaultCOB_ID; TPDO->restrictionFlags = restrictionFlags; /* Configure Object dictionary entry at index 0x1800+ and 0x1A00+ */ CO_OD_configure(SDO, idx_TPDOCommPar, CO_ODF_TPDOcom, (void*)TPDO, 0, 0); CO_OD_configure(SDO, idx_TPDOMapPar, CO_ODF_TPDOmap, (void*)TPDO, 0, 0); /* configure communication and mapping */ TPDO->CANdevTx = CANdevTx; TPDO->CANdevTxIdx = CANdevTxIdx; TPDO->syncCounter = 255; TPDO->inhibitTimer = 0; TPDO->eventTimer = ((uint32_t) TPDOCommPar->eventTimer) * 1000; if(TPDOCommPar->transmissionType>=254) TPDO->sendRequest = 1; CO_TPDOconfigMap(TPDO, TPDOMapPar->numberOfMappedObjects); CO_TPDOconfigCom(TPDO, TPDOCommPar->COB_IDUsedByTPDO, ((TPDOCommPar->transmissionType<=240) ? 1 : 0)); if((TPDOCommPar->transmissionType>240 && TPDOCommPar->transmissionType<254) || TPDOCommPar->SYNCStartValue>240){ TPDO->valid = false; } return CO_ERROR_NO; } /******************************************************************************/ uint8_t CO_TPDOisCOS(CO_TPDO_t *TPDO){ /* Prepare TPDO data automatically from Object Dictionary variables */ uint8_t* pPDOdataByte; uint8_t** ppODdataByte; pPDOdataByte = &TPDO->CANtxBuff->data[TPDO->dataLength]; ppODdataByte = &TPDO->mapPointer[TPDO->dataLength]; switch(TPDO->dataLength){ case 8: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x80)) return 1; // fallthrough case 7: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x40)) return 1; // fallthrough case 6: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x20)) return 1; // fallthrough case 5: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x10)) return 1; // fallthrough case 4: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x08)) return 1; // fallthrough case 3: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x04)) return 1; // fallthrough case 2: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x02)) return 1; // fallthrough case 1: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x01)) return 1; // fallthrough } return 0; } //#define TPDO_CALLS_EXTENSION /******************************************************************************/ int16_t CO_TPDOsend(CO_TPDO_t *TPDO){ int16_t i; uint8_t* pPDOdataByte; uint8_t** ppODdataByte; #ifdef TPDO_CALLS_EXTENSION if(TPDO->SDO->ODExtensions){ /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */ const uint32_t* pMap = &TPDO->TPDOMapPar->mappedObject1; CO_SDO_t *pSDO = TPDO->SDO; for(i=TPDO->TPDOMapPar->numberOfMappedObjects; i>0; i--){ uint32_t map = *(pMap++); uint16_t index = (uint16_t)(map>>16); uint8_t subIndex = (uint8_t)(map>>8); uint16_t entryNo = CO_OD_find(pSDO, index); if ( entryNo == 0xFFFF ) continue; CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo]; if( ext->pODFunc == NULL) continue; CO_ODF_arg_t ODF_arg; memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t)); ODF_arg.reading = true; ODF_arg.index = index; ODF_arg.subIndex = subIndex; ODF_arg.object = ext->object; ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex); ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex); ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); //https://github.com/CANopenNode/CANopenNode/issues/100 ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex); ext->pODFunc(&ODF_arg); } } #endif i = TPDO->dataLength; pPDOdataByte = &TPDO->CANtxBuff->data[0]; ppODdataByte = &TPDO->mapPointer[0]; /* Copy data from Object dictionary. */ for(; i>0; i--) { *(pPDOdataByte++) = **(ppODdataByte++); } TPDO->sendRequest = 0; return CO_CANsend(TPDO->CANdevTx, TPDO->CANtxBuff); } //#define RPDO_CALLS_EXTENSION /******************************************************************************/ void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas){ if(!RPDO->valid || !(*RPDO->operatingState == CO_NMT_OPERATIONAL)) { CLEAR_CANrxNew(RPDO->CANrxNew[0]); CLEAR_CANrxNew(RPDO->CANrxNew[1]); } else if(!RPDO->synchronous || syncWas) { #if defined(RPDO_CALLS_EXTENSION) bool_t update = false; #endif /* defined(RPDO_CALLS_EXTENSION) */ uint8_t bufNo = 0; /* Determine, which of the two rx buffers, contains relevant message. */ if(RPDO->SYNC && RPDO->synchronous && !RPDO->SYNC->CANrxToggle) { bufNo = 1; } while(IS_CANrxNew(RPDO->CANrxNew[bufNo])){ int16_t i; uint8_t* pPDOdataByte; uint8_t** ppODdataByte; i = RPDO->dataLength; pPDOdataByte = &RPDO->CANrxData[bufNo][0]; ppODdataByte = &RPDO->mapPointer[0]; /* Copy data to Object dictionary. If between the copy operation CANrxNew * is set to true by receive thread, then copy the latest data again. */ CLEAR_CANrxNew(RPDO->CANrxNew[bufNo]); for(; i>0; i--) { **(ppODdataByte++) = *(pPDOdataByte++); } #if defined(RPDO_CALLS_EXTENSION) update = true; #endif /* defined(RPDO_CALLS_EXTENSION) */ } #ifdef RPDO_CALLS_EXTENSION if(update && RPDO->SDO->ODExtensions){ int16_t i; /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */ const uint32_t* pMap = &RPDO->RPDOMapPar->mappedObject1; CO_SDO_t *pSDO = RPDO->SDO; for(i=RPDO->RPDOMapPar->numberOfMappedObjects; i>0; i--){ uint32_t map = *(pMap++); uint16_t index = (uint16_t)(map>>16); uint8_t subIndex = (uint8_t)(map>>8); uint16_t entryNo = CO_OD_find(pSDO, index); if ( entryNo == 0xFFFF ) continue; CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo]; if( ext->pODFunc == NULL) continue; CO_ODF_arg_t ODF_arg; memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t)); ODF_arg.reading = false; ODF_arg.index = index; ODF_arg.subIndex = subIndex; ODF_arg.object = ext->object; ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex); ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex); ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); //https://github.com/CANopenNode/CANopenNode/issues/100 ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex); ext->pODFunc(&ODF_arg); } } #endif } } /******************************************************************************/ void CO_TPDO_process( CO_TPDO_t *TPDO, bool_t syncWas, uint32_t timeDifference_us) { if(TPDO->valid && *TPDO->operatingState == CO_NMT_OPERATIONAL){ /* Send PDO by application request or by Event timer */ if(TPDO->TPDOCommPar->transmissionType >= 253){ if(TPDO->inhibitTimer == 0 && (TPDO->sendRequest || (TPDO->TPDOCommPar->eventTimer && TPDO->eventTimer == 0))){ if(CO_TPDOsend(TPDO) == CO_ERROR_NO){ /* successfully sent */ TPDO->inhibitTimer = ((uint32_t) TPDO->TPDOCommPar->inhibitTime) * 100; TPDO->eventTimer = ((uint32_t) TPDO->TPDOCommPar->eventTimer) * 1000; } } } /* Synchronous PDOs */ else if(TPDO->SYNC && syncWas){ /* send synchronous acyclic PDO */ if(TPDO->TPDOCommPar->transmissionType == 0){ if(TPDO->sendRequest) CO_TPDOsend(TPDO); } /* send synchronous cyclic PDO */ else{ /* is the start of synchronous TPDO transmission */ if(TPDO->syncCounter == 255){ if(TPDO->SYNC->counterOverflowValue && TPDO->TPDOCommPar->SYNCStartValue) TPDO->syncCounter = 254; /* SYNCStartValue is in use */ else TPDO->syncCounter = TPDO->TPDOCommPar->transmissionType; } /* if the SYNCStartValue is in use, start first TPDO after SYNC with matched SYNCStartValue. */ if(TPDO->syncCounter == 254){ if(TPDO->SYNC->counter == TPDO->TPDOCommPar->SYNCStartValue){ TPDO->syncCounter = TPDO->TPDOCommPar->transmissionType; CO_TPDOsend(TPDO); } } /* Send PDO after every N-th Sync */ else if(--TPDO->syncCounter == 0){ TPDO->syncCounter = TPDO->TPDOCommPar->transmissionType; CO_TPDOsend(TPDO); } } } } else{ /* Not operational or valid. Force TPDO first send after operational or valid. */ if(TPDO->TPDOCommPar->transmissionType>=254) TPDO->sendRequest = 1; else TPDO->sendRequest = 0; } /* update timers */ TPDO->inhibitTimer = (TPDO->inhibitTimer > timeDifference_us) ? (TPDO->inhibitTimer - timeDifference_us) : 0; TPDO->eventTimer = (TPDO->eventTimer > timeDifference_us) ? (TPDO->eventTimer - timeDifference_us) : 0; }