nikon/CANopenNode
1
0
Fork 0
Code Activity Actions
CANopenNode/304/CO_SRDO.c
Janez a528eb1669 Better organised configuration macros (CO_config.h): 
- rearranged and better documented
 - default values moved from CO_driver.h into appropriate files
 - rearranged also in CO_driver_target.h files
 - parts of the stack or whole objects can be disabled.
 - configuration is verified for depencies

Additional:
 - renamed some members of CO_Default_CAN_ID_t
 - moved helpers CO_setUint32() etc from CO_SDOserver.h into CO_driver.h.
 - change wrong CO_ERROR_PARAMETERS to CO_ERROR_ILLEGAL_ARGUMENT.
 - renamed CO_ERROR_PARAMETERS to CO_ERROR_OD_PARAMETERS
2020-09-14 15:50:21 +02:00

847 lines
31 KiB
C
Raw Blame History

/**
* CANopen Safety Related Data Object protocol.
*
* @file CO_SRDO.c
* @ingroup CO_SRDO
* @author Robert Grüning
* @copyright 2020 - 2020 Robert Grüning
*
* This file is part of CANopenNode, an opensource CANopen Stack.
* Project home page is <https://github.com/CANopenNode/CANopenNode>.
* For more information on CANopen see <http://www.can-cia.org/>.
*
* 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 "304/CO_SRDO.h"
#if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE
#include "301/crc16-ccitt.h"
/* verify configuration */
#if !((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE)
#error CO_CONFIG_CRC16_ENABLE must be enabled.
#endif
#define CO_SRDO_INVALID (0U)
#define CO_SRDO_TX (1U)
#define CO_SRDO_RX (2U)
#define CO_SRDO_VALID_MAGIC (0xA5)
static void CO_SRDO_receive_normal(void *object, void *msg){
CO_SRDO_t *SRDO;
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
uint8_t *data = CO_CANrxMsg_readData(msg);
SRDO = (CO_SRDO_t*)object; /* this is the correct pointer type of the first argument */
if( (SRDO->valid == CO_SRDO_RX) &&
(DLC >= SRDO->dataLength) && !CO_FLAG_READ(SRDO->CANrxNew[1]))
{
/* copy data into appropriate buffer and set 'new message' flag */
memcpy(SRDO->CANrxData[0], data, sizeof(SRDO->CANrxData[0]));
CO_FLAG_SET(SRDO->CANrxNew[0]);
#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE
/* Optional signal to RTOS, which can resume task, which handles SRDO. */
if(SRDO->pFunctSignalPre != NULL) {
SRDO->pFunctSignalPre(SRDO->functSignalObjectPre);
}
#endif
}
}
static void CO_SRDO_receive_inverted(void *object, void *msg){
CO_SRDO_t *SRDO;
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
uint8_t *data = CO_CANrxMsg_readData(msg);
SRDO = (CO_SRDO_t*)object; /* this is the correct pointer type of the first argument */
if( (SRDO->valid == CO_SRDO_RX) &&
(DLC >= SRDO->dataLength) && CO_FLAG_READ(SRDO->CANrxNew[0]))
{
/* copy data into appropriate buffer and set 'new message' flag */
memcpy(SRDO->CANrxData[1], data, sizeof(SRDO->CANrxData[1]));
CO_FLAG_SET(SRDO->CANrxNew[1]);
#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE
/* Optional signal to RTOS, which can resume task, which handles SRDO. */
if(SRDO->pFunctSignalPre != NULL) {
SRDO->pFunctSignalPre(SRDO->functSignalObjectPre);
}
#endif
}
}
static void CO_SRDOconfigCom(CO_SRDO_t* SRDO, uint32_t COB_IDnormal, uint32_t COB_IDinverted){
uint16_t IDs[2][2] = {0};
uint16_t* ID;
uint16_t successCount = 0;
int16_t i;
uint32_t COB_ID[2];
COB_ID[0] = COB_IDnormal;
COB_ID[1] = COB_IDinverted;
SRDO->valid = CO_SRDO_INVALID;
/* is SRDO used? */
if(*SRDO->SRDOGuard->configurationValid == CO_SRDO_VALID_MAGIC && (SRDO->SRDOCommPar->informationDirection == CO_SRDO_TX || SRDO->SRDOCommPar->informationDirection == CO_SRDO_RX) &&
SRDO->dataLength){
ID = &IDs[SRDO->SRDOCommPar->informationDirection - 1][0];
/* is used default COB-ID? */
for(i = 0; i < 2; i++){
if(!(COB_ID[i] & 0xBFFFF800L)){
ID[i] = (uint16_t)COB_ID[i] & 0x7FF;
if(ID[i] == SRDO->defaultCOB_ID[i] && SRDO->nodeId <= 64){
ID[i] += 2*SRDO->nodeId;
}
if(0x101 <= ID[i] && ID[i] <= 0x180 && ((ID[i] & 1) != i )){
successCount++;
}
}
}
}
/* all ids are ok*/
if(successCount == 2){
SRDO->valid = SRDO->SRDOCommPar->informationDirection;
if (SRDO->valid == CO_SRDO_TX){
SRDO->timer = 500 * SRDO->nodeId; /* 0.5ms * node-ID delay*/
}
else if (SRDO->valid == CO_SRDO_RX){
SRDO->timer = SRDO->SRDOCommPar->safetyCycleTime * 1000U;
}
}
else{
memset(IDs, 0, sizeof(IDs));
CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
SRDO->valid = CO_SRDO_INVALID;
}
for(i = 0; i < 2; i++){
CO_ReturnError_t r;
SRDO->CANtxBuff[i] = CO_CANtxBufferInit(
SRDO->CANdevTx, /* CAN device */
SRDO->CANdevTxIdx[i], /* index of specific buffer inside CAN module */
IDs[0][i], /* CAN identifier */
0, /* rtr */
SRDO->dataLength, /* number of data bytes */
0); /* synchronous message flag bit */
if(SRDO->CANtxBuff[i] == 0){
SRDO->valid = CO_SRDO_INVALID;
}
r = CO_CANrxBufferInit(
SRDO->CANdevRx, /* CAN device */
SRDO->CANdevRxIdx[i], /* rx buffer index */
IDs[1][i], /* CAN identifier */
0x7FF, /* mask */
0, /* rtr */
(void*)SRDO, /* object passed to receive function */
i ? CO_SRDO_receive_inverted : CO_SRDO_receive_normal); /* this function will process received message */
if(r != CO_ERROR_NO){
SRDO->valid = CO_SRDO_INVALID;
CO_FLAG_CLEAR(SRDO->CANrxNew[i]);
}
}
}
static uint32_t CO_SRDOfindMap(
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<<i;
}
}
return 0;
}
static uint32_t CO_SRDOconfigMap(CO_SRDO_t* SRDO, uint8_t noOfMappedObjects){
int16_t i;
uint8_t lengths[2] = {0};
uint32_t ret = 0;
const uint32_t* pMap = &SRDO->SRDOMapPar->mappedObjects[0];
for(i=noOfMappedObjects; i>0; i--){
int16_t j;
uint8_t* pData;
uint8_t dummy = 0;
uint8_t* length = &lengths[i%2];
uint8_t** mapPointer = SRDO->mapPointer[i%2];
uint8_t prevLength = *length;
uint8_t MBvar;
uint32_t map = *(pMap++);
/* function do much checking of errors in map */
ret = CO_SRDOfindMap(
SRDO->SDO,
map,
SRDO->SRDOCommPar->informationDirection == CO_SRDO_TX,
&pData,
length,
&dummy,
&MBvar);
if(ret){
*length = 0;
CO_errorReport(SRDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, map);
break;
}
/* write SRDO data pointers */
#ifdef CO_BIG_ENDIAN
if(MBvar){
for(j=*length-1; j>=prevLength; j--)
mapPointer[j] = pData++;
}
else{
for(j=prevLength; j<*length; j++)
mapPointer[j] = pData++;
}
#else
for(j=prevLength; j<*length; j++){
mapPointer[j] = pData++;
}
#endif
}
if(lengths[0] == lengths[1])
SRDO->dataLength = lengths[0];
else{
SRDO->dataLength = 0;
CO_errorReport(SRDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, 0);
ret = CO_SDO_AB_MAP_LEN;
}
return ret;
}
static uint16_t CO_SRDOcalcCrc(const CO_SRDO_t *SRDO){
uint16_t i;
uint16_t result = 0x0000;
uint8_t buffer[4];
uint32_t cob;
const CO_SRDOCommPar_t *com = SRDO->SRDOCommPar;
const CO_SRDOMapPar_t *map = SRDO->SRDOMapPar;
result = crc16_ccitt(&com->informationDirection, 1, result);
CO_memcpySwap2(&buffer[0], &com->safetyCycleTime);
result = crc16_ccitt(&buffer[0], 2, result);
result = crc16_ccitt(&com->safetyRelatedValidationTime, 1, result);
/* adjust COB-ID if the default is used
Caution: if the node id changes and you are using the default COB-ID you have to recalculate the checksum
This behaviour is controversial and could be changed or made optional.
*/
cob = com->COB_ID1_normal;
if(((cob)&0x7FF) == SRDO->defaultCOB_ID[0] && SRDO->nodeId <= 64)
cob += SRDO->nodeId;
CO_memcpySwap4(&buffer[0], &cob);
result = crc16_ccitt(&buffer[0], 4, result);
cob = com->COB_ID2_inverted;
if(((cob)&0x7FF) == SRDO->defaultCOB_ID[1] && SRDO->nodeId <= 64)
cob += SRDO->nodeId;
CO_memcpySwap4(&buffer[0], &cob);
result = crc16_ccitt(&buffer[0], 4, result);
result = crc16_ccitt(&map->numberOfMappedObjects, 1, result);
for(i = 0; i < map->numberOfMappedObjects;){
uint8_t subindex = i + 1;
result = crc16_ccitt(&subindex, 1, result);
CO_memcpySwap4(&buffer[0], &map->mappedObjects[i]);
result = crc16_ccitt(&buffer[0], 4, result);
i = subindex;
}
return result;
}
static CO_SDO_abortCode_t CO_ODF_SRDOcom(CO_ODF_arg_t *ODF_arg){
CO_SRDO_t *SRDO;
SRDO = (CO_SRDO_t*) ODF_arg->object;
/* Reading Object Dictionary variable */
if(ODF_arg->reading){
if(ODF_arg->subIndex == 5 || ODF_arg->subIndex == 6){
uint32_t value = CO_getUint32(ODF_arg->data);
uint16_t index = ODF_arg->subIndex - 5;
/* if default COB ID is used, write default value here */
if(((value)&0x7FF) == SRDO->defaultCOB_ID[index] && SRDO->nodeId <= 64)
value += SRDO->nodeId;
/* If SRDO is not valid, set bit 31. but I do not think this applies to SRDO ?! */
/* if(!SRDO->valid) value |= 0x80000000L; */
CO_setUint32(ODF_arg->data, value);
}
return CO_SDO_AB_NONE;
}
/* Writing Object Dictionary variable */
if(*SRDO->SRDOGuard->operatingState == CO_NMT_OPERATIONAL)
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){
uint8_t value = ODF_arg->data[0];
if (value > 2)
return CO_SDO_AB_INVALID_VALUE;
}
else if(ODF_arg->subIndex == 2){
uint16_t value = CO_getUint16(ODF_arg->data);
if (value == 0)
return CO_SDO_AB_INVALID_VALUE;
}
else if(ODF_arg->subIndex == 3){
uint8_t value = ODF_arg->data[0];
if (value == 0)
return CO_SDO_AB_INVALID_VALUE;
}
else if(ODF_arg->subIndex == 4){ /* Transmission_type */
uint8_t *value = (uint8_t*) ODF_arg->data;
if(*value != 254)
return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
}
else if(ODF_arg->subIndex == 5 || ODF_arg->subIndex == 6){ /* COB_ID */
uint32_t value = CO_getUint32(ODF_arg->data);
uint16_t index = ODF_arg->subIndex - 5;
/* check value range, the spec does not specify if COB-ID flags are allowed */
if(value < 0x101 || value > 0x180 || (value & 1) == index)
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(SRDO->nodeId <= 64 && value == (SRDO->defaultCOB_ID[index] + SRDO->nodeId)){
value = SRDO->defaultCOB_ID[index];
CO_setUint32(ODF_arg->data, value);
}
}
*SRDO->SRDOGuard->configurationValid = CO_SRDO_INVALID;
return CO_SDO_AB_NONE;
}
static CO_SDO_abortCode_t CO_ODF_SRDOmap(CO_ODF_arg_t *ODF_arg){
CO_SRDO_t *SRDO;
SRDO = (CO_SRDO_t*) ODF_arg->object;
if(ODF_arg->reading)
return CO_SDO_AB_NONE;
/* Writing Object Dictionary variable */
if(*SRDO->SRDOGuard->operatingState == CO_NMT_OPERATIONAL)
return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */
if(SRDO->SRDOCommPar->informationDirection) /* SRDO must be deleted */
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 > 16 || *value & 1) /*only odd numbers are allowed*/
return CO_SDO_AB_MAP_LEN; /* Number and length of object to be mapped exceeds SRDO length. */
}
else{
if (SRDO->SRDOMapPar->numberOfMappedObjects != 0)
return CO_SDO_AB_UNSUPPORTED_ACCESS;
}
*SRDO->SRDOGuard->configurationValid = CO_SRDO_INVALID;
return CO_SDO_AB_NONE;
}
static CO_SDO_abortCode_t CO_ODF_SRDOcrc(CO_ODF_arg_t *ODF_arg){
CO_SRDOGuard_t *SRDOGuard;
SRDOGuard = (CO_SRDOGuard_t*) ODF_arg->object;
if (!ODF_arg->reading){
if(*SRDOGuard->operatingState == CO_NMT_OPERATIONAL)
return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */
*SRDOGuard->configurationValid = CO_SRDO_INVALID;
}
return CO_SDO_AB_NONE;
}
static CO_SDO_abortCode_t CO_ODF_SRDOvalid(CO_ODF_arg_t *ODF_arg){
CO_SRDOGuard_t *SRDOGuard;
SRDOGuard = (CO_SRDOGuard_t*) ODF_arg->object;
if(!ODF_arg->reading){
if(*SRDOGuard->operatingState == CO_NMT_OPERATIONAL)
return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */
SRDOGuard->checkCRC = ODF_arg->data[0] == CO_SRDO_VALID_MAGIC;
}
return CO_SDO_AB_NONE;
}
CO_ReturnError_t CO_SRDOGuard_init(
CO_SRDOGuard_t *SRDOGuard,
CO_SDO_t *SDO,
CO_NMT_internalState_t *operatingState,
uint8_t *configurationValid,
uint16_t idx_SRDOvalid,
uint16_t idx_SRDOcrc)
{
/* verify arguments */
if(SRDOGuard==NULL || SDO==NULL || operatingState==NULL || configurationValid==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
SRDOGuard->operatingState = operatingState;
SRDOGuard->operatingStatePrev = CO_NMT_INITIALIZING;
SRDOGuard->configurationValid = configurationValid;
SRDOGuard->checkCRC = *configurationValid == CO_SRDO_VALID_MAGIC;
/* Configure Object dictionary entry at index 0x13FE and 0x13FF */
CO_OD_configure(SDO, idx_SRDOvalid, CO_ODF_SRDOvalid, (void*)SRDOGuard, 0, 0);
CO_OD_configure(SDO, idx_SRDOcrc, CO_ODF_SRDOcrc, (void*)SRDOGuard, 0, 0);
return CO_ERROR_NO;
}
uint8_t CO_SRDOGuard_process(
CO_SRDOGuard_t *SRDOGuard)
{
uint8_t result = 0;
CO_NMT_internalState_t operatingState = *SRDOGuard->operatingState;
if(operatingState != SRDOGuard->operatingStatePrev){
SRDOGuard->operatingStatePrev = operatingState;
if (operatingState == CO_NMT_OPERATIONAL)
result |= 1 << 0;
}
if(SRDOGuard->checkCRC){
result |= 1 << 1;
SRDOGuard->checkCRC = 0;
}
return result;
}
#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE
/******************************************************************************/
void CO_SRDO_initCallbackPre(
CO_SRDO_t *SRDO,
void *object,
void (*pFunctSignalPre)(void *object))
{
if(SRDO != NULL){
SRDO->functSignalObjectPre = object;
SRDO->pFunctSignalPre = pFunctSignalPre;
}
}
#endif
/******************************************************************************/
void CO_SRDO_initCallbackEnterSafeState(
CO_SRDO_t *SRDO,
void *object,
void (*pFunctSignalSafe)(void *object))
{
if(SRDO != NULL){
SRDO->functSignalObjectSafe = object;
SRDO->pFunctSignalSafe = pFunctSignalSafe;
}
}
CO_ReturnError_t CO_SRDO_init(
CO_SRDO_t *SRDO,
CO_SRDOGuard_t *SRDOGuard,
CO_EM_t *em,
CO_SDO_t *SDO,
uint8_t nodeId,
uint16_t defaultCOB_ID,
const CO_SRDOCommPar_t *SRDOCommPar,
const CO_SRDOMapPar_t *SRDOMapPar,
const uint16_t *checksum,
uint16_t idx_SRDOCommPar,
uint16_t idx_SRDOMapPar,
CO_CANmodule_t *CANdevRx,
uint16_t CANdevRxIdxNormal,
uint16_t CANdevRxIdxInverted,
CO_CANmodule_t *CANdevTx,
uint16_t CANdevTxIdxNormal,
uint16_t CANdevTxIdxInverted)
{
/* verify arguments */
if(SRDO==NULL || SRDOGuard==NULL || em==NULL || SDO==NULL || checksum==NULL ||
SRDOCommPar==NULL || SRDOMapPar==NULL || CANdevRx==NULL || CANdevTx==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
SRDO->SRDOGuard = SRDOGuard;
SRDO->em = em;
SRDO->SDO = SDO;
SRDO->SRDOCommPar = SRDOCommPar;
SRDO->SRDOMapPar = SRDOMapPar;
SRDO->checksum = checksum;
SRDO->CANdevRx = CANdevRx;
SRDO->CANdevRxIdx[0] = CANdevRxIdxNormal;
SRDO->CANdevRxIdx[1] = CANdevRxIdxInverted;
SRDO->CANdevTx = CANdevTx;
SRDO->CANdevTxIdx[0] = CANdevTxIdxNormal;
SRDO->CANdevTxIdx[1] = CANdevTxIdxInverted;
SRDO->nodeId = nodeId;
SRDO->defaultCOB_ID[0] = defaultCOB_ID;
SRDO->defaultCOB_ID[1] = defaultCOB_ID + 1;
SRDO->valid = CO_SRDO_INVALID;
SRDO->toogle = 0;
SRDO->timer = 0;
SRDO->pFunctSignalSafe = NULL;
SRDO->functSignalObjectSafe = NULL;
#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE
SRDO->pFunctSignalPre = NULL;
SRDO->functSignalObjectPre = NULL;
#endif
/* Configure Object dictionary entry at index 0x1301+ and 0x1381+ */
CO_OD_configure(SDO, idx_SRDOCommPar, CO_ODF_SRDOcom, (void*)SRDO, 0, 0);
CO_OD_configure(SDO, idx_SRDOMapPar, CO_ODF_SRDOmap, (void*)SRDO, 0, 0);
return CO_ERROR_NO;
}
CO_ReturnError_t CO_SRDO_requestSend(
CO_SRDO_t *SRDO)
{
if (*SRDO->SRDOGuard->operatingState != CO_NMT_OPERATIONAL)
return CO_ERROR_WRONG_NMT_STATE;
if (SRDO->valid != CO_SRDO_TX)
return CO_ERROR_TX_UNCONFIGURED;
if(SRDO->toogle != 0)
return CO_ERROR_TX_BUSY;
SRDO->timer = 0;
return CO_ERROR_NO;
}
void CO_SRDO_process(
CO_SRDO_t *SRDO,
uint8_t commands,
uint32_t timeDifference_us,
uint32_t *timerNext_us)
{
(void)timerNext_us; /* may be unused */
if(commands & (1<<1)){
uint16_t crcValue = CO_SRDOcalcCrc(SRDO);
if (*SRDO->checksum != crcValue)
*SRDO->SRDOGuard->configurationValid = 0;
}
if((commands & (1<<0)) && *SRDO->SRDOGuard->configurationValid == CO_SRDO_VALID_MAGIC){
if(CO_SRDOconfigMap(SRDO, SRDO->SRDOMapPar->numberOfMappedObjects) == 0){
CO_SRDOconfigCom(SRDO, SRDO->SRDOCommPar->COB_ID1_normal, SRDO->SRDOCommPar->COB_ID2_inverted);
}
else{
SRDO->valid = CO_SRDO_INVALID;
}
}
if(SRDO->valid && *SRDO->SRDOGuard->operatingState == CO_NMT_OPERATIONAL){
SRDO->timer = (SRDO->timer > timeDifference_us) ? (SRDO->timer - timeDifference_us) : 0;
if(SRDO->valid == CO_SRDO_TX){
if(SRDO->timer == 0){
if(SRDO->toogle){
CO_CANsend(SRDO->CANdevTx, SRDO->CANtxBuff[1]);
SRDO->timer = SRDO->SRDOCommPar->safetyCycleTime * 1000U - CO_CONFIG_SRDO_MINIMUM_DELAY;
}
else{
int16_t i;
uint8_t* pSRDOdataByte_normal;
uint8_t* pSRDOdataByte_inverted;
uint8_t** ppODdataByte_normal;
uint8_t** ppODdataByte_inverted;
bool_t data_ok = true;
#if (CO_CONFIG_SRDO) & CO_CONFIG_TSRDO_CALLS_EXTENSION
if(SRDO->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 = &SRDO->SRDOMapPar->mappedObjects[0];
CO_SDO_t *pSDO = SRDO->SDO;
for(i=SRDO->SRDOMapPar->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 )
{
CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
if( ext->pODFunc != NULL)
{
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
pSRDOdataByte_normal = &SRDO->CANtxBuff[0]->data[0];
ppODdataByte_normal = &SRDO->mapPointer[0][0];
pSRDOdataByte_inverted = &SRDO->CANtxBuff[1]->data[0];
ppODdataByte_inverted = &SRDO->mapPointer[1][0];
#if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_CHECK_TX
/* check data before sending (optional) */
for(i = 0; i<SRDO->dataLength; i++){
uint8_t invert = ~*(ppODdataByte_inverted[i]);
if (*(ppODdataByte_normal[i]) != invert)
{
data_ok = false;
break;
}
}
#endif
if(data_ok){
/* Copy data from Object dictionary. */
for(i = 0; i<SRDO->dataLength; i++){
pSRDOdataByte_normal[i] = *(ppODdataByte_normal[i]);
pSRDOdataByte_inverted[i] = *(ppODdataByte_inverted[i]);
}
CO_CANsend(SRDO->CANdevTx, SRDO->CANtxBuff[0]);
SRDO->timer = CO_CONFIG_SRDO_MINIMUM_DELAY;
}
else{
SRDO->toogle = 1;
/* save state */
if(SRDO->pFunctSignalSafe != NULL){
SRDO->pFunctSignalSafe(SRDO->functSignalObjectSafe);
}
}
}
SRDO->toogle = !SRDO->toogle;
}
#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_TIMERNEXT
if(timerNext_us != NULL){
if(*timerNext_us > SRDO->timer){
*timerNext_us = SRDO->timer; /* Schedule for next message timer */
}
}
#endif
}
else if(SRDO->valid == CO_SRDO_RX){
if(CO_FLAG_READ(SRDO->CANrxNew[SRDO->toogle])){
if(SRDO->toogle){
int16_t i;
uint8_t* pSRDOdataByte_normal;
uint8_t* pSRDOdataByte_inverted;
uint8_t** ppODdataByte_normal;
uint8_t** ppODdataByte_inverted;
bool_t data_ok = true;
pSRDOdataByte_normal = &SRDO->CANrxData[0][0];
pSRDOdataByte_inverted = &SRDO->CANrxData[1][0];
for(i = 0; i<SRDO->dataLength; i++){
uint8_t invert = ~pSRDOdataByte_inverted[i];
if(pSRDOdataByte_normal[i] != invert){
data_ok = false;
break;
}
}
if(data_ok){
ppODdataByte_normal = &SRDO->mapPointer[0][0];
ppODdataByte_inverted = &SRDO->mapPointer[1][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. */
for(i = 0; i<SRDO->dataLength; i++){
*(ppODdataByte_normal[i]) = pSRDOdataByte_normal[i];
*(ppODdataByte_inverted[i]) = pSRDOdataByte_inverted[i];
}
CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
#if (CO_CONFIG_SRDO) & CO_CONFIG_RSRDO_CALLS_EXTENSION
if(SRDO->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 = &SRDO->SRDOMapPar->mappedObjects[0];
CO_SDO_t *pSDO = SRDO->SDO;
for(i=SRDO->SRDOMapPar->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 )
{
CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
if( ext->pODFunc != NULL)
{
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
}
else{
CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
/* save state */
if(SRDO->pFunctSignalSafe != NULL){
SRDO->pFunctSignalSafe(SRDO->functSignalObjectSafe);
}
}
SRDO->timer = SRDO->SRDOCommPar->safetyCycleTime * 1000U;
}
else{
SRDO->timer = SRDO->SRDOCommPar->safetyRelatedValidationTime * 1000U;
}
SRDO->toogle = !SRDO->toogle;
}
if(SRDO->timer == 0){
SRDO->toogle = 0;
SRDO->timer = SRDO->SRDOCommPar->safetyRelatedValidationTime * 1000U;
CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
/* save state */
if(SRDO->pFunctSignalSafe != NULL){
SRDO->pFunctSignalSafe(SRDO->functSignalObjectSafe);
}
}
}
}
else{
SRDO->valid = CO_SRDO_INVALID;
CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
}
}
#endif /* (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE */
View git blame Copy permalink