nikon/CANopenNode
1
0
Fork 0
Code Activity Actions

CANopen Safety (SRDO according to DIN EN 50325-5:2016) (#196)

Browse source 
* CANopen Safety (SRDO according to DIN EN 50325-5:2016)

* GFC code and docs

* clang-format on GFC

Co-authored-by: rg1 <r.gruening@miunske.com>
This commit is contained in:
gotocoffee 2020-06-22 10:14:14 +02:00 committed by GitHub
parent aed6a472d4
commit 0f7b3e2d17
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
13 changed files with 1565 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
301/CO_SDOserver.h
View file

@ -565,6 +565,11 @@ typedef enum{
OD_H1029_ERR_BEHAVIOR = 0x1029U,/**< Error behaviour */
OD_H1200_SDO_SERVER_PARAM = 0x1200U,/**< SDO server parameters */
OD_H1280_SDO_CLIENT_PARAM = 0x1280U,/**< SDO client parameters */
OD_H1300_GFC_PARAM = 0x1300U,/**< GFC parameter */
OD_H1301_SRDO_1_PARAM = 0x1301U,/**< SRDO communication parameters */
OD_H1381_SRDO_1_MAPPING = 0x1381U,/**< SRDO mapping parameters */
OD_H13FE_SRDO_VALID = 0x13FEU,/**< SRDO valid flag */
OD_H13FF_SRDO_CHECKSUM = 0x13FFU,/**< SRDO checksum */
OD_H1400_RXPDO_1_PARAM = 0x1400U,/**< RXPDO communication parameter */
OD_H1401_RXPDO_2_PARAM = 0x1401U,/**< RXPDO communication parameter */
OD_H1402_RXPDO_3_PARAM = 0x1402U,/**< RXPDO communication parameter */

43
301/CO_config.h
View file

@ -185,6 +185,49 @@ extern "C" {
#define CO_CONFIG_HB_CONS_CALLBACK_MULTI 0x02
#define CO_CONFIG_HB_CONS_QUERY_FUNCT 0x04
/**
* Configuration of GFC
*
* Possible flags, can be ORed:
* - CO_CONFIG_GFC_CONSUMER - Enable the GFC consumer
* - CO_CONFIG_GFC_PRODUCER - Enable the GFC producer
*/
#ifdef CO_DOXYGEN
#define CO_CONFIG_GFC (CO_CONFIG_GFC_CONSUMER | CO_CONFIG_GFC_PRODUCER)
#endif
#define CO_CONFIG_GFC_CONSUMER 0x01
#define CO_CONFIG_GFC_PRODUCER 0x02
/**
* Configuration of SRDO
*
* Possible flags, can be ORed:
* - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
* received RSRDO CAN message.
* Callback is configured by CO_SRDO_initCallbackPre().
* - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
* inside CO_SRDO_process() (Tx SRDO only).
* - CO_CONFIG_RSRDO_CALLS_EXTENSION - Enable calling configured extension
* callbacks when received RSRDO CAN message modifies OD entries.
* - CO_CONFIG_TRSRDO_CALLS_EXTENSION - Enable calling configured extension
* callbacks before TSRDO CAN message is sent.
*/
#ifdef CO_DOXYGEN
#define CO_CONFIG_SRDO (CO_CONFIG_FLAG_CALLBACK_PRE | CO_CONFIG_FLAG_TIMERNEXT | CO_CONFIG_SRDO_CHECK_TX | CO_CONFIG_RSRDO_CALLS_EXTENSION | CO_CONFIG_TSRDO_CALLS_EXTENSION)
#endif
#define CO_CONFIG_SRDO_CHECK_TX 0x01
#define CO_CONFIG_RSRDO_CALLS_EXTENSION 0x02
#define CO_CONFIG_TSRDO_CALLS_EXTENSION 0x04
/**
* SRDO Tx time delay
*
* minimum time between the first and second SRDO (Tx) message
* in us
*/
#ifdef CO_DOXYGEN
#define CO_CONFIG_SRDO_MINIMUM_DELAY 0
#endif
/**
* Configuration of PDO

2
301/CO_driver.h
View file

@ -454,9 +454,11 @@ typedef struct {
*/
typedef enum {
CO_CAN_ID_NMT_SERVICE = 0x000, /**< 0x000, Network management */
CO_CAN_ID_GFC = 0x001, /**< 0x001, Global fail-safe command */
CO_CAN_ID_SYNC = 0x080, /**< 0x080, Synchronous message */
CO_CAN_ID_EMERGENCY = 0x080, /**< 0x080, Emergency messages (+nodeID) */
CO_CAN_ID_TIME = 0x100, /**< 0x100, Time message */
CO_CAN_ID_SRDO_1 = 0x0FF, /**< 0x0FF, Default SRDO1 (+2*nodeID) */
CO_CAN_ID_TPDO_1 = 0x180, /**< 0x180, Default TPDO1 (+nodeID) */
CO_CAN_ID_RPDO_1 = 0x200, /**< 0x200, Default RPDO1 (+nodeID) */
CO_CAN_ID_TPDO_2 = 0x280, /**< 0x280, Default TPDO2 (+nodeID) */

122
304/CO_GFC.c Normal file
View file

@ -0,0 +1,122 @@
/**
* CANopen Global fail-safe command protocol.
*
* @file CO_GFC.c
* @ingroup CO_GFC
* @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_GFC.h"
#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER
static void CO_GFC_receive(void *object, void *msg)
{
CO_GFC_t *GFC;
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
GFC = (CO_GFC_t *)
object; /* this is the correct pointer type of the first argument */
if ((*GFC->valid == 0x01) && (DLC == 0)) {
#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER
/* Optional signal to RTOS, which can resume task, which handles SRDO.
*/
if (GFC->pFunctSignalSafe != NULL) {
GFC->pFunctSignalSafe(GFC->functSignalObjectSafe);
}
#endif
}
}
void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC,
void *object,
void (*pFunctSignalSafe)(void *object))
{
if (GFC != NULL) {
GFC->functSignalObjectSafe = object;
GFC->pFunctSignalSafe = pFunctSignalSafe;
}
}
#endif
CO_ReturnError_t CO_GFC_init(CO_GFC_t *GFC,
uint8_t *valid,
CO_CANmodule_t *GFC_CANdevRx,
uint16_t GFC_rxIdx,
uint16_t CANidRxGFC,
CO_CANmodule_t *GFC_CANdevTx,
uint16_t GFC_txIdx,
uint16_t CANidTxGFC)
{
CO_ReturnError_t r;
if (GFC == NULL || valid == NULL || GFC_CANdevRx == NULL ||
GFC_CANdevTx == NULL) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
GFC->valid = valid;
#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER
GFC->CANdevTx = GFC_CANdevTx;
#endif
#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER
GFC->functSignalObjectSafe = NULL;
GFC->pFunctSignalSafe = NULL;
#endif
#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER
GFC->CANtxBuff = CO_CANtxBufferInit(
GFC->CANdevTx, /* CAN device */
GFC_txIdx, /* index of specific buffer inside CAN module */
CANidTxGFC, /* CAN identifier */
0, /* rtr */
0, /* number of data bytes */
0); /* synchronous message flag bit */
if (GFC->CANtxBuff == 0) {
return CO_ERROR_TX_UNCONFIGURED;
}
#endif
#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER
r = CO_CANrxBufferInit(
GFC_CANdevRx, /* CAN device */
GFC_rxIdx, /* rx buffer index */
CANidRxGFC, /* CAN identifier */
0x7FF, /* mask */
0, /* rtr */
(void *)GFC, /* object passed to receive function */
CO_GFC_receive); /* this function will process received message */
if (r != CO_ERROR_NO) {
return r;
}
#endif
return CO_ERROR_NO;
}
#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER
CO_ReturnError_t CO_GFCsend(CO_GFC_t *GFC)
{
if (*GFC->valid == 0x01)
return CO_CANsend(GFC->CANdevTx, GFC->CANtxBuff);
return CO_ERROR_NO;
}
#endif

128
304/CO_GFC.h Normal file
View file

@ -0,0 +1,128 @@
/**
* CANopen Global fail-safe command protocol.
*
* @file CO_GFC.h
* @ingroup CO_GFC
* @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 "301/CO_driver.h"
#ifndef CO_GFC_H
#define CO_GFC_H
#ifdef __cplusplus
extern "C" {
#endif
/**
* @defgroup CO_GFC GFC
* @ingroup CO_CANopen_304
* @{
*
* Global fail-safe command protocol.
*
* Very simple consumer/producer protocol.
* A net can have multiple GFC producer and multiple GFC consumer.
* On a safety-relevant the producer can send a GFC message (ID 0, DLC 0).
* The consumer can use this message to start the transition to a safe state.
* The GFC is optional for the security protocol and is not monitored (timed).
*/
/**
* GFC object.
*/
typedef struct {
uint8_t *valid; /**< From CO_GFC_init() */
#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN
CO_CANmodule_t *CANdevTx; /**< From CO_GFC_init() */
CO_CANtx_t *CANtxBuff; /**< CAN transmit buffer inside CANdevTx */
#endif
#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN
/** From CO_GFC_initCallbackEnterSafeState() or NULL */
void (*pFunctSignalSafe)(void *object);
/** From CO_GFC_initCallbackEnterSafeState() or NULL */
void *functSignalObjectSafe;
#endif
} CO_GFC_t;
/**
* Initialize GFC object.
*
* Function must be called in the communication reset section.
*
* @param GFC This object will be initialized.
* @param valid pointer to the valid flag in OD (0x1300)
* @param GFC_CANdevRx CAN device used for SRDO reception.
* @param GFC_rxIdx Index of receive buffer in the above CAN device.
* @param CANidRxGFC GFC CAN ID for reception
* @param GFC_CANdevTx AN device used for SRDO transmission.
* @param GFC_txIdx Index of transmit buffer in the above CAN device.
* @param CANidTxGFC GFC CAN ID for transmission
*
* @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
*/
CO_ReturnError_t CO_GFC_init(CO_GFC_t *GFC,
uint8_t *valid,
CO_CANmodule_t *GFC_CANdevRx,
uint16_t GFC_rxIdx,
uint16_t CANidRxGFC,
CO_CANmodule_t *GFC_CANdevTx,
uint16_t GFC_txIdx,
uint16_t CANidTxGFC);
#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN
/**
* Initialize GFC callback function.
*
* Function initializes optional callback function, that is called when GFC is
* received. Callback is called from receive function (interrupt).
*
* @param GFC This object.
* @param object Pointer to object, which will be passed to pFunctSignalSafe().
* Can be NULL
* @param pFunctSignalSafe Pointer to the callback function. Not called if NULL.
*/
void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC,
void *object,
void (*pFunctSignalSafe)(void *object));
#endif
#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN
/**
* Send GFC message.
*
* It should be called by application, for example after a safety-relevant
* change.
*
* @param GFC GFC object.
*
* @return Same as CO_CANsend().
*/
CO_ReturnError_t CO_GFCsend(CO_GFC_t *GFC);
#endif
#ifdef __cplusplus
}
#endif /*__cplusplus*/
/** @} */
#endif

752
304/CO_SRDO.c Normal file
View file

@ -0,0 +1,752 @@
/**
* 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"
#include "301/crc16-ccitt.h"
#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] ^ i) & 1)){
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_SRDOCommPar_t *com, const CO_SRDOMapPar_t *map){
uint16_t i;
uint16_t result = 0x0000;
uint8_t buffer[4];
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);
CO_memcpySwap4(&buffer[0], &com->COB_ID1_normal);
result = crc16_ccitt(&buffer[0], 4, result);
CO_memcpySwap4(&buffer[0], &com->COB_ID2_inverted);
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->defaultCOB_ID[index])
value += SRDO->nodeId;
/* If PDO is not valid, set bit 31 */
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;
/* bits 11...29 must be zero */
if(value & 0x3FFFF800L)
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)&0x7FF) == (SRDO->defaultCOB_ID[index] + SRDO->nodeId)){
value &= 0xC0000000L;
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;
/* 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(!SRDO->dataLength) *value = 0;
}
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 PDO 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 && operatingState == CO_NMT_OPERATIONAL){
SRDOGuard->operatingStatePrev = operatingState;
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 0x1341+ */
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;
}
void CO_SRDO_process(
CO_SRDO_t *SRDO,
uint8_t commands,
uint32_t timeDifference_us,
uint32_t *timerNext_us)
{
if(commands & (1<<1)){
uint16_t crcValue = CO_SRDOcalcCrc(SRDO->SRDOCommPar, SRDO->SRDOMapPar);
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* pPDOdataByte_normal;
uint8_t* pPDOdataByte_inverted;
uint8_t** ppODdataByte_normal;
uint8_t** ppODdataByte_inverted;
bool_t data_ok = true;
pPDOdataByte_normal = &SRDO->CANtxBuff[0]->data[0];
ppODdataByte_normal = &SRDO->mapPointer[0][0];
pPDOdataByte_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 = ~pPDOdataByte_inverted[i];
if (pPDOdataByte_normal[i] != invert)
{
data_ok = false;
break;
}
}
#else
(void)timerNext_us;
#endif
if(data_ok){
/* Copy data from Object dictionary. */
for(i = 0; i<SRDO->dataLength; i++){
pPDOdataByte_normal[i] = *(ppODdataByte_normal[i]);
pPDOdataByte_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* pPDOdataByte_normal;
uint8_t* pPDOdataByte_inverted;
uint8_t** ppODdataByte_normal;
uint8_t** ppODdataByte_inverted;
bool_t data_ok = true;
pPDOdataByte_normal = &SRDO->CANrxData[0][0];
pPDOdataByte_inverted = &SRDO->CANrxData[1][0];
for(i = 0; i<SRDO->dataLength; i++){
uint8_t invert = ~pPDOdataByte_inverted[i];
if(pPDOdataByte_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]) = pPDOdataByte_normal[i];
*(ppODdataByte_inverted[i]) = pPDOdataByte_inverted[i];
}
CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
}
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]);
}
}

293
304/CO_SRDO.h Normal file
View file

@ -0,0 +1,293 @@
/**
* CANopen Safety Related Data Object protocol.
*
* @file CO_SRDO.h
* @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 "301/CO_driver.h"
#include "301/CO_SDOserver.h"
#include "301/CO_Emergency.h"
#include "301/CO_NMT_Heartbeat.h"
#ifndef CO_SRDO_H
#define CO_SRDO_H
#ifdef __cplusplus
extern "C" {
#endif
/**
* @defgroup CO_SRDO SRDO
* @ingroup CO_CANopen_304
* @{
*
* CANopen Safety Related Data Object protocol.
*
* The functionality is very similar to that of the PDOs.
* The main differences is every message is send and received twice.
* The second message must be bitwise inverted. The delay between the two messages and between each message pair is monitored.
* The distinction between sending and receiving SRDO is made at runtime (for PDO it is compile time).
* If the security protocol is used, at least one SRDO is mandatory.
*/
/**
* SRDO communication parameter. The same as record from Object dictionary (index 0x1301-0x1340).
*/
typedef struct{
uint8_t maxSubIndex; /**< Equal to 6 */
/** Direction of the SRDO. Values:
- 0: SRDO is invalid (deleted)
- 1: SRDO is transmiting data
- 2: SRDO is receive data */
uint8_t informationDirection;
/** Refresh-time / SCT
- in tx mode (Refresh-time): transmission interval
- in rx mode (SCT): receive timeout between two SRDO */
uint16_t safetyCycleTime;
/** SRVT
- in tx mode: unsed
- in rx mode: receive timeout between first and second SRDO message */
uint8_t safetyRelatedValidationTime;
/** Transmission type. Values:
- 254: Manufacturer specific.*/
uint8_t transmissionType;
/** Communication object identifier for message received. Meaning of the specific bits:
- Bit 0-10: COB-ID for PDO, to change it bit 31 must be set.
- Bit 11-29: set to 0 for 11 bit COB-ID.
- Bit 30: If true, rtr are NOT allowed for PDO.
- Bit 31: If true, node does NOT use the PDO. */
uint32_t COB_ID1_normal;
/** Communication object identifier for message received. Meaning of the specific bits:
- Bit 0-10: COB-ID for PDO, to change it bit 31 must be set.
- Bit 11-29: set to 0 for 11 bit COB-ID.
- Bit 30: If true, rtr are NOT allowed for PDO.
- Bit 31: If true, node does NOT use the PDO. */
uint32_t COB_ID2_inverted;
}CO_SRDOCommPar_t;
typedef struct{
/** Actual number of mapped objects from 0 to 16. Only even numbers are allowed. To change mapped object,
this value must be 0. */
uint8_t numberOfMappedObjects;
/** Location and size of the mapped object.
Even index is the normal object. Odd index is the inverted object. Bit meanings `0xIIIISSLL`:
- Bit 0-7: Data Length in bits.
- Bit 8-15: Subindex from object distionary.
- Bit 16-31: Index from object distionary. */
uint32_t mappedObjects[16];
}CO_SRDOMapPar_t;
/**
* Gurad Object for SRDO
* monitors:
* - access to CRC objects
* - access configuration valid flag
* - change in operation state
*/
typedef struct{
CO_NMT_internalState_t *operatingState; /**< pointer to current operation state */
CO_NMT_internalState_t operatingStatePrev; /**< last operation state */
uint8_t *configurationValid; /**< pointer to the configuration valid flag in OD */
uint8_t checkCRC; /**< specifies whether a CRC check should be performed */
}CO_SRDOGuard_t;
/**
* SRDO object.
*/
typedef struct{
CO_EM_t *em; /**< From CO_SRDO_init() */
CO_SDO_t *SDO; /**< From CO_SRDO_init() */
CO_SRDOGuard_t *SRDOGuard; /**< From CO_SRDO_init() */
/** Pointers to 2*8 data objects, where SRDO will be copied */
uint8_t *mapPointer[2][8];
/** Data length of the received PDO message. Calculated from mapping */
uint8_t dataLength;
uint8_t nodeId; /**< From CO_SRDO_init() */
uint16_t defaultCOB_ID[2]; /**< From CO_SRDO_init() */
/** 0 - invalid, 1 - tx, 2 - rx */
uint8_t valid;
const CO_SRDOCommPar_t *SRDOCommPar; /**< From CO_SRDO_init() */
const CO_SRDOMapPar_t *SRDOMapPar; /**< From CO_SRDO_init() */
const uint16_t *checksum; /**< From CO_SRDO_init() */
CO_CANmodule_t *CANdevRx; /**< From CO_SRDO_init() */
CO_CANmodule_t *CANdevTx; /**< From CO_SRDO_init() */
CO_CANtx_t *CANtxBuff[2]; /**< CAN transmit buffer inside CANdevTx */
uint16_t CANdevRxIdx[2]; /**< From CO_SRDO_init() */
uint16_t CANdevTxIdx[2]; /**< From CO_SRDO_init() */
uint8_t toogle; /**< defines the current state */
uint32_t timer; /**< transmit timer and receive timeout */
/** Variable indicates, if new SRDO message received from CAN bus. */
volatile void *CANrxNew[2];
/** 2*8 data bytes of the received message. */
uint8_t CANrxData[2][8];
/** From CO_SRDO_initCallbackEnterSafeState() or NULL */
void (*pFunctSignalSafe)(void *object);
/** From CO_SRDO_initCallbackEnterSafeState() or NULL */
void *functSignalObjectSafe;
#if ((CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
/** From CO_SRDO_initCallbackPre() or NULL */
void (*pFunctSignalPre)(void *object);
/** From CO_SRDO_initCallbackPre() or NULL */
void *functSignalObjectPre;
#endif
}CO_SRDO_t;
/**
* Initialize SRDOGuard object.
*
* Function must be called in the communication reset section.
*
* @param SRDOGuard This object will be initialized.
* @param SDO SDO object.
* @param operatingState Pointer to variable indicating CANopen device NMT internal state.
* @param configurationValid Pointer to variable with the SR valid flag
* @param idx_SRDOvalid Index in Object Dictionary
* @param idx_SRDOcrc Index in Object Dictionary
*
* @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
*/
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);
/**
* Process operation and valid state changes.
*
* @param SRDOGuard This object.
* @return uint8_t command for CO_SRDO_process().
* - bit 0 entered operational
* - bit 1 validate checksum
*/
uint8_t CO_SRDOGuard_process(
CO_SRDOGuard_t *SRDOGuard);
/**
* Initialize SRDO object.
*
* Function must be called in the communication reset section.
*
* @param SRDO This object will be initialized.
* @param SRDOGuard SRDOGuard object.
* @param em Emergency object.
* @param SDO SDO object.
* @param nodeId CANopen Node ID of this device. If default COB_ID is used, value will be added.
* @param defaultCOB_ID Default COB ID for this SRDO (without NodeId).
* @param SRDOCommPar Pointer to _SRDO communication parameter_ record from Object
* dictionary (index 0x1301+).
* @param SRDOMapPar Pointer to _SRDO mapping parameter_ record from Object
* dictionary (index 0x1381+).
* @param checksum
* @param idx_SRDOCommPar Index in Object Dictionary
* @param idx_SRDOMapPar Index in Object Dictionary
* @param CANdevRx CAN device used for SRDO reception.
* @param CANdevRxIdxNormal Index of receive buffer in the above CAN device.
* @param CANdevRxIdxInverted Index of receive buffer in the above CAN device.
* @param CANdevTx CAN device used for SRDO transmission.
* @param CANdevTxIdxNormal Index of transmit buffer in the above CAN device.
* @param CANdevTxIdxInverted Index of transmit buffer in the above CAN device.
*
* @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
*/
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);
#if ((CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
/**
* Initialize SRDO callback function.
*
* Function initializes optional callback function, which should immediately
* start processing of CO_SRDO_process() function.
* Callback is called after SRDO message is received from the CAN bus.
*
* @param SRDO This object.
* @param object Pointer to object, which will be passed to pFunctSignalPre(). Can be NULL
* @param pFunctSignalPre Pointer to the callback function. Not called if NULL.
*/
void CO_SRDO_initCallbackPre(
CO_SRDO_t *RPDO,
void *object,
void (*pFunctSignalPre)(void *object));
#endif
/**
* Initialize SRDO callback function.
*
* Function initializes optional callback function, that is called when SRDO enters a safe state.
* This happens when a timeout is reached or the data is inconsistent. The safe state itself is not further defined.
* One measure, for example, would be to go back to the pre-operational state
* Callback is called from CO_SRDO_process().
*
* @param SRDO This object.
* @param object Pointer to object, which will be passed to pFunctSignalSafe(). Can be NULL
* @param pFunctSignalSafe Pointer to the callback function. Not called if NULL.
*/
void CO_SRDO_initCallbackEnterSafeState(
CO_SRDO_t *SRDO,
void *object,
void (*pFunctSignalSafe)(void *object));
/**
* Process transmitting/receiving SRDO messages.
*
* This function verifies the checksum on demand.
* This function also configures the SRDO on operation state change to operational
*
* @param SRDO This object.
* @param commands result from CO_SRDOGuard_process().
* @param timeDifference_us Time difference from previous function call in [microseconds].
* @param [out] timerNext_us info to OS.
*/
void CO_SRDO_process(
CO_SRDO_t *SRDO,
uint8_t commands,
uint32_t timeDifference_us,
uint32_t *timerNext_us);
#ifdef __cplusplus
}
#endif /*__cplusplus*/
/** @} */
#endif

142
CANopen.c
View file

@ -61,6 +61,8 @@ static uint32_t CO_traceBufferSize[CO_NO_TRACE];
|| (CO_NO_SDO_SERVER < 1 || CO_NO_SDO_SERVER > 128) \
|| CO_NO_TIME > 1 \
|| CO_NO_SDO_CLIENT > 128 \
|| CO_NO_GFC > 1 \
|| CO_NO_SRDO > 64 \
|| (CO_NO_RPDO < 1 || CO_NO_RPDO > 0x200) \
|| (CO_NO_TPDO < 1 || CO_NO_TPDO > 0x200) \
|| ODL_consumerHeartbeatTime_arrayLength == 0 \
@ -75,7 +77,9 @@ static uint32_t CO_traceBufferSize[CO_NO_TRACE];
#define CO_RXCAN_SYNC (CO_RXCAN_NMT + CO_NO_NMT)
#define CO_RXCAN_EMERG (CO_RXCAN_SYNC + CO_NO_SYNC)
#define CO_RXCAN_TIME (CO_RXCAN_EMERG + CO_NO_EM_CONS)
#define CO_RXCAN_RPDO (CO_RXCAN_TIME + CO_NO_TIME)
#define CO_RXCAN_GFC (CO_RXCAN_TIME + CO_NO_TIME)
#define CO_RXCAN_SRDO (CO_RXCAN_GFC + CO_NO_GFC)
#define CO_RXCAN_RPDO (CO_RXCAN_SRDO + CO_NO_SRDO*2)
#define CO_RXCAN_SDO_SRV (CO_RXCAN_RPDO + CO_NO_RPDO)
#define CO_RXCAN_SDO_CLI (CO_RXCAN_SDO_SRV + CO_NO_SDO_SERVER)
#define CO_RXCAN_CONS_HB (CO_RXCAN_SDO_CLI + CO_NO_SDO_CLIENT)
@ -85,6 +89,8 @@ static uint32_t CO_traceBufferSize[CO_NO_TRACE];
CO_NO_SYNC + \
CO_NO_EM_CONS + \
CO_NO_TIME + \
CO_NO_GFC + \
CO_NO_SRDO*2 + \
CO_NO_RPDO + \
CO_NO_SDO_SERVER + \
CO_NO_SDO_CLIENT + \
@ -97,7 +103,9 @@ static uint32_t CO_traceBufferSize[CO_NO_TRACE];
#define CO_TXCAN_SYNC (CO_TXCAN_NMT + CO_NO_NMT_MST)
#define CO_TXCAN_EMERG (CO_TXCAN_SYNC + CO_NO_SYNC)
#define CO_TXCAN_TIME (CO_TXCAN_EMERG + CO_NO_EMERGENCY)
#define CO_TXCAN_TPDO (CO_TXCAN_TIME + CO_NO_TIME)
#define CO_TXCAN_GFC (CO_TXCAN_TIME + CO_NO_TIME)
#define CO_TXCAN_SRDO (CO_TXCAN_GFC + CO_NO_GFC)
#define CO_TXCAN_TPDO (CO_TXCAN_SRDO + CO_NO_SRDO*2)
#define CO_TXCAN_SDO_SRV (CO_TXCAN_TPDO + CO_NO_TPDO)
#define CO_TXCAN_SDO_CLI (CO_TXCAN_SDO_SRV + CO_NO_SDO_SERVER)
#define CO_TXCAN_HB (CO_TXCAN_SDO_CLI + CO_NO_SDO_CLIENT)
@ -107,6 +115,8 @@ static uint32_t CO_traceBufferSize[CO_NO_TRACE];
CO_NO_SYNC + \
CO_NO_EMERGENCY + \
CO_NO_TIME + \
CO_NO_GFC + \
CO_NO_SRDO*2 + \
CO_NO_TPDO + \
CO_NO_SDO_SERVER + \
CO_NO_SDO_CLIENT + \
@ -182,6 +192,22 @@ CO_ReturnError_t CO_new(uint32_t *heapMemoryUsed) {
CO->TIME = NULL;
#endif
#if CO_NO_GFC == 1
CO->GFC = (CO_GFC_t *)calloc(1, sizeof(CO_GFC_t));
if (CO->GFC == NULL) errCnt++;
CO_memoryUsed += sizeof(CO_GFC_t);
#endif
#if CO_NO_SRDO != 0
/* SRDO */
CO->SRDOGuard = (CO_SRDOGuard_t *)calloc(1, sizeof(CO_SRDOGuard_t));
if (CO->SRDOGuard == NULL) errCnt++;
for (i = 0; i < CO_NO_SRDO; i++) {
CO->SRDO[i] = (CO_SRDO_t *)calloc(1, sizeof(CO_SRDO_t));
if (CO->SRDO[i] == NULL) errCnt++;
}
CO_memoryUsed += sizeof(CO_SRDO_t) * CO_NO_SRDO + sizeof(CO_SRDOGuard_t);
#endif
/* RPDO */
for (i = 0; i < CO_NO_RPDO; i++) {
CO->RPDO[i] = (CO_RPDO_t *)calloc(1, sizeof(CO_RPDO_t));
@ -321,6 +347,19 @@ void CO_delete(void *CANptr) {
free(CO_HBcons_monitoredNodes);
free(CO->HBcons);
#if CO_NO_GFC == 1
/* GFC */
free(CO->GFC);
#endif
#if CO_NO_SRDO != 0
/* SRDO */
for (i = 0; i < CO_NO_SRDO; i++) {
free(CO->SRDO[i]);
}
free(CO->SRDOGuard);
#endif
/* TPDO */
for (i = 0; i < CO_NO_TPDO; i++) {
free(CO->TPDO[i]);
@ -380,6 +419,13 @@ void CO_delete(void *CANptr) {
#endif
#if CO_NO_TIME == 1
static CO_TIME_t COO_TIME;
#endif
#if CO_NO_GFC == 1
static CO_GFC_t COO_GFC;
#endif
#if CO_NO_SRDO != 0
static CO_SRDOGuard_t COO_SRDOGuard;
static CO_SRDO_t COO_SRDO[CO_NO_SRDO];
#endif
static CO_RPDO_t COO_RPDO[CO_NO_RPDO];
static CO_TPDO_t COO_TPDO[CO_NO_TPDO];
@ -450,6 +496,19 @@ CO_ReturnError_t CO_new(uint32_t *heapMemoryUsed) {
CO->TIME = NULL;
#endif
#if CO_NO_GFC == 1
/* GFC */
CO->GFC = &COO_GFC;
#endif
#if CO_NO_SRDO != 0
/* SRDO */
CO->SRDOGuard = &COO_SRDOGuard;
for (i = 0; i < CO_NO_SRDO; i++) {
CO->SRDO[i] = &COO_SRDO[i];
}
#endif
/* RPDO */
for (i = 0; i < CO_NO_RPDO; i++) {
CO->RPDO[i] = &COO_RPDO[i];
@ -583,8 +642,13 @@ CO_ReturnError_t CO_CANopenInit(uint8_t nodeId) {
if (nodeId < 1 || nodeId > 127) {
return CO_ERROR_PARAMETERS;
}
/* Verify parameters from CO_OD */
#if CO_NO_SRDO != 0
if (sizeof(OD_SRDOCommunicationParameter_t) != sizeof(CO_SRDOCommPar_t) ||
sizeof(OD_SRDOMappingParameter_t) != sizeof(CO_SRDOMapPar_t)) {
return CO_ERROR_PARAMETERS;
}
#endif
if (sizeof(OD_TPDOCommunicationParameter_t) != sizeof(CO_TPDOCommPar_t) ||
sizeof(OD_TPDOMappingParameter_t) != sizeof(CO_TPDOMapPar_t) ||
sizeof(OD_RPDOCommunicationParameter_t) != sizeof(CO_RPDOCommPar_t) ||
@ -712,6 +776,54 @@ CO_ReturnError_t CO_CANopenInit(uint8_t nodeId) {
if (err) return err;
#endif
#if CO_NO_GFC == 1
/* GFC */
CO_GFC_init(CO->GFC,
&OD_globalFailSafeCommandParameter,
CO->CANmodule[0],
CO_RXCAN_GFC,
CO_CAN_ID_GFC,
CO->CANmodule[0],
CO_TXCAN_GFC,
CO_CAN_ID_GFC);
#endif
#if CO_NO_SRDO != 0
/* SRDO */
err = CO_SRDOGuard_init(CO->SRDOGuard,
CO->SDO[0],
&CO->NMT->operatingState,
&OD_configurationValid,
OD_H13FE_SRDO_VALID,
OD_H13FF_SRDO_CHECKSUM);
if (err) return err;
for (i = 0; i < CO_NO_SRDO; i++) {
CO_CANmodule_t *CANdev = CO->CANmodule[0];
uint16_t CANdevRxIdx = CO_RXCAN_SRDO + 2*i;
uint16_t CANdevTxIdx = CO_TXCAN_SRDO + 2*i;
err = CO_SRDO_init(CO->SRDO[i],
CO->SRDOGuard,
CO->em,
CO->SDO[0],
nodeId,
((i == 0) ? CO_CAN_ID_SRDO_1 : 0),
(CO_SRDOCommPar_t*)&OD_SRDOCommunicationParameter[i],
(CO_SRDOMapPar_t *)&OD_SRDOMappingParameter[i],
&OD_safetyConfigurationChecksum[i],
OD_H1301_SRDO_1_PARAM + i,
OD_H1381_SRDO_1_MAPPING + i,
CANdev,
CANdevRxIdx,
CANdevRxIdx + 1,
CANdev,
CANdevTxIdx,
CANdevTxIdx + 1);
if (err) return err;
}
#endif
/* RPDO */
for (i = 0; i < CO_NO_RPDO; i++) {
CO_CANmodule_t *CANdevRx = CO->CANmodule[0];
@ -1020,3 +1132,27 @@ void CO_process_TPDO(CO_t *co,
CO_TPDO_process(co->TPDO[i], syncWas, timeDifference_us, timerNext_us);
}
}
/******************************************************************************/
#if CO_NO_SRDO != 0
void CO_process_SRDO(CO_t *co,
uint32_t timeDifference_us,
uint32_t *timerNext_us)
{
int16_t i;
uint8_t firstOperational;
#if CO_NO_LSS_SLAVE == 1
if (co->nodeIdUnconfigured) {
return;
}
#endif
firstOperational = CO_SRDOGuard_process(co->SRDOGuard);
/* Verify PDO Change Of State and process PDOs */
for (i = 0; i < CO_NO_SRDO; i++) {
CO_SRDO_process(co->SRDO[i], firstOperational, timeDifference_us, timerNext_us);
}
}
#endif

45
CANopen.h
View file

@ -87,6 +87,18 @@ extern "C" {
* @}
*/
/**
* @defgroup CO_CANopen_304 CANopen_304
* @{
*
* CANopen Safety (EN 50325­-5:2010 (formerly CiA 304))
*
* Standard defines the usage of Safety Related Data Objects (SRDO) and the GFC.
* This is an additional protocol (to SDO, PDO) to exchange data.
* The meaning of "security" here refers not to security (crypto) but to data consistency.
* @}
*/
/**
* @defgroup CO_CANopen_305 CANopen_305
* @{
@ -163,6 +175,10 @@ extern "C" {
#define CO_NO_EM_CONS (0 - 1)
/** Number of Time-stamp objects, 0 or 1 (consumer(CANrx) + producer(CANtx)) */
#define CO_NO_TIME (0 - 1)
/** Number of GFC objects, 0 or 1 (consumer(CANrx) + producer(CANtx)) */
#define CO_NO_GFC (0 - 1)
/** Number of SRDO objects, 0 to 64 (consumer(CANrx) + producer(CANtx)) */
#define CO_NO_RPDO (0 - 64)
/** Number of RPDO objects, 1 to 512 consumers (CANrx) */
#define CO_NO_RPDO (1 - 512)
/** Number of TPDO objects, 1 to 512 producers (CANtx) */
@ -237,6 +253,12 @@ extern "C" {
#if CO_NO_SDO_CLIENT != 0 || defined CO_DOXYGEN
#include "301/CO_SDOclient.h"
#endif
#if CO_NO_GFC != 0 || defined CO_DOXYGEN
#include "304/CO_GFC.h"
#endif
#if CO_NO_SRDO != 0 || defined CO_DOXYGEN
#include "304/CO_SRDO.h"
#endif
#if CO_NO_LSS_SLAVE != 0 || defined CO_DOXYGEN
#include "305/CO_LSSslave.h"
#endif
@ -277,6 +299,13 @@ typedef struct {
#if ((CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE) || defined CO_DOXYGEN
CO_LEDs_t *LEDs; /**< LEDs object */
#endif
#if CO_NO_GFC != 0 || defined CO_DOXYGEN
CO_GFC_t *GFC; /**< GFC objects */
#endif
#if CO_NO_SRDO != 0 || defined CO_DOXYGEN
CO_SRDOGuard_t *SRDOGuard; /**< SRDO objects */
CO_SRDO_t *SRDO[CO_NO_SRDO]; /**< SRDO objects */
#endif
#if CO_NO_LSS_SLAVE == 1 || defined CO_DOXYGEN
CO_LSSslave_t *LSSslave; /**< LSS slave object */
#endif
@ -446,6 +475,22 @@ void CO_process_TPDO(CO_t *co,
uint32_t timeDifference_us,
uint32_t *timerNext_us);
#if CO_NO_SRDO != 0 || defined CO_DOXYGEN
/**
* Process CANopen SRDO objects.
*
* Function must be called cyclically from real time thread with constant.
* interval (1ms typically). It processes receive SRDO CANopen objects.
*
* @param co CANopen object.
* @param timeDifference_us Time difference from previous function call in
* microseconds.
* @param [out] timerNext_us info to OS - see CO_process().
*/
void CO_process_SRDO(CO_t *co,
uint32_t timeDifference_us,
uint32_t *timerNext_us);
#endif /* CO_NO_SRDO != 0 */
/** @} */

2
Makefile
View file

@ -33,6 +33,8 @@ SOURCES = \
$(CANOPEN_SRC)/301/crc16-ccitt.c \
$(CANOPEN_SRC)/301/CO_fifo.c \
$(CANOPEN_SRC)/303/CO_LEDs.c \
$(CANOPEN_SRC)/304/CO_GFC.c \
$(CANOPEN_SRC)/304/CO_SRDO.c \
$(CANOPEN_SRC)/305/CO_LSSslave.c \
$(CANOPEN_SRC)/305/CO_LSSmaster.c \
$(CANOPEN_SRC)/309/CO_gateway_ascii.c \

16
example/CO_OD.h
View file

@ -94,6 +94,8 @@
#define CO_NO_EMERGENCY 1 //Associated objects: 1014, 1015
#define CO_NO_SDO_SERVER 1 //Associated objects: 1200
#define CO_NO_SDO_CLIENT 1 //Associated objects: 1280
#define CO_NO_GFC 0 //Associated objects: 1300
#define CO_NO_SRDO 0 //Associated objects: 1301-13FF
#define CO_NO_RPDO 4 //Associated objects: 1400, 1401, 1402, 1403, 1600, 1601, 1602, 1603
#define CO_NO_TPDO 4 //Associated objects: 1800, 1801, 1802, 1803, 1A00, 1A01, 1A02, 1A03
#define CO_NO_TRACE 0
@ -127,7 +129,19 @@
UNSIGNED32 COB_IDServerToClient;
UNSIGNED8 nodeIDOfTheSDOServer;
} OD_SDOClientParameter_t;
/*1301 */ typedef struct {
UNSIGNED8 maxSubIndex;
UNSIGNED8 informationDirection;
UNSIGNED16 safetyCycleTime;
UNSIGNED8 safetyRelatedValidationTime;
UNSIGNED8 transmissionType;
UNSIGNED32 COB_ID1_normal;
UNSIGNED32 COB_ID2_inverted;
} OD_SRDOCommunicationParameter_t;
/*1381 */ typedef struct {
UNSIGNED8 numberOfMappedObjects;
UNSIGNED32 mappedObject[16];
} OD_SRDOMappingParameter_t;
/*1400[4] */ typedef struct{
UNSIGNED8 maxSubIndex;
UNSIGNED32 COB_IDUsedByRPDO;

17
example/CO_driver_target.h
View file

@ -76,6 +76,23 @@ extern "C" {
CO_CONFIG_HB_CONS_QUERY_FUNCT)
#endif
#ifndef CO_CONFIG_GFC
#define CO_CONFIG_GFC (CO_CONFIG_GFC_CONSUMER | \
CO_CONFIG_GFC_PRODUCER)
#endif
#ifndef CO_CONFIG_SRDO
#define CO_CONFIG_SRDO (CO_CONFIG_FLAG_CALLBACK_PRE | \
CO_CONFIG_FLAG_TIMERNEXT | \
CO_CONFIG_SRDO_CHECK_TX | \
CO_CONFIG_RSRDO_CALLS_EXTENSION | \
CO_CONFIG_TSRDO_CALLS_EXTENSION)
#endif
#ifndef CO_CONFIG_SRDO_MINIMUM_DELAY
#define CO_CONFIG_SRDO_MINIMUM_DELAY 0
#endif
#ifndef CO_CONFIG_PDO
#define CO_CONFIG_PDO (CO_CONFIG_FLAG_CALLBACK_PRE | \
CO_CONFIG_FLAG_TIMERNEXT | \

2
example/Makefile
View file

@ -31,6 +31,8 @@ SOURCES = \
$(CANOPEN_SRC)/301/crc16-ccitt.c \
$(CANOPEN_SRC)/301/CO_fifo.c \
$(CANOPEN_SRC)/303/CO_LEDs.c \
$(CANOPEN_SRC)/304/CO_GFC.c \
$(CANOPEN_SRC)/304/CO_SRDO.c \
$(CANOPEN_SRC)/305/CO_LSSslave.c \
$(CANOPEN_SRC)/305/CO_LSSmaster.c \
$(CANOPEN_SRC)/309/CO_gateway_ascii.c \