AtguiguCorp/linear-Slide
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
25d82ccd6608b82c86ab70f8008b958a29604f6b
linear-Slide/Middleware/CANopenNode/301/CO_PDO.c

1473 lines
54 KiB
C
Raw Blame History

/*
* CANopen Receive Process Data Object protocol.
*
* @file CO_PDO.c
* @ingroup CO_PDO
* @author Janez Paternoster
* @copyright 2021 Janez Paternoster
*
* This file is part of <https://github.com/CANopenNode/CANopenNode>, a CANopen Stack.
*
* 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 <string.h>
#include "301/CO_PDO.h"
#if ((CO_CONFIG_PDO) & (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE)) != 0
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_OD_IO_ACCESS) == 0
#error Dynamic PDO mapping is not possible without CO_CONFIG_PDO_OD_IO_ACCESS
#endif
#endif
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING) != 0
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_OD_IO_ACCESS) == 0
#error Bitwise PDO mapping is not possible without CO_CONFIG_PDO_OD_IO_ACCESS
#endif
#endif
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_OD_IO_ACCESS) != 0
/*
* Custom function for write dummy OD object. Will be used only from RPDO.
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t
OD_write_dummy(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
(void)stream;
(void)buf;
if (countWritten != NULL) {
*countWritten = count;
}
return ODR_OK;
}
/*
* Custom function for read dummy OD object. Will be used only from TPDO.
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t
OD_read_dummy(OD_stream_t* stream, void* buf, OD_size_t count, OD_size_t* countRead) {
if ((buf == NULL) || (stream == NULL) || (countRead == NULL)) {
return ODR_DEV_INCOMPAT;
}
if (count > stream->dataLength) {
count = stream->dataLength;
}
(void)memset(buf, 0, count);
*countRead = count;
return ODR_OK;
}
/*
* Find mapped variable in Object Dictionary and configure entry in RPDO or TPDO
*
* @param PDO This object will be configured. If map is erroneous, then it will stay unchanged.
* @param map PDO mapping parameter.
* @param mapIndex from 0 to CO_PDO_MAX_MAPPED_ENTRIES
* @param isRPDO True for RPDO and false for TPDO.
* @param OD Object Dictionary.
*
* @return ODR_OK on success, otherwise error reason.
*/
static ODR_t
PDOconfigMap(CO_PDO_common_t* PDO, uint32_t map, uint8_t mapIndex, bool_t isRPDO, OD_t* OD) {
uint16_t index = (uint16_t)(map >> 16);
uint8_t subIndex = (uint8_t)(map >> 8);
uint8_t mappedLengthBits = (uint8_t)map;
uint8_t mappedLength = mappedLengthBits >> 3;
OD_IO_t* OD_IO = &PDO->OD_IO[mapIndex];
/* total PDO length can not be more than CO_PDO_MAX_SIZE bytes */
if (mappedLength > CO_PDO_MAX_SIZE) {
return ODR_MAP_LEN; /* PDO length exceeded */
}
/* is there a reference to the dummy entry */
if ((index < 0x20U) && (subIndex == 0U)) {
OD_stream_t* stream = &OD_IO->stream;
(void)memset(stream, 0, sizeof(OD_stream_t));
stream->dataLength = mappedLength;
stream->dataOffset = mappedLength;
OD_IO->read = OD_read_dummy;
OD_IO->write = OD_write_dummy;
return ODR_OK;
}
/* find entry in the Object Dictionary */
OD_IO_t OD_IOcopy;
OD_entry_t* entry = OD_find(OD, index);
ODR_t odRet = OD_getSub(entry, subIndex, &OD_IOcopy, false);
if (odRet != ODR_OK) {
return odRet;
}
/* verify access attributes, byte alignment and length */
OD_attr_t testAttribute = isRPDO ? (OD_attr_t)(ODA_RPDO) : (OD_attr_t)(ODA_TPDO);
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
/* If the bitwise mapping is enabled, check only length in bits */
if (((OD_IOcopy.stream.attribute & testAttribute) == 0U)
|| ((OD_IOcopy.stream.dataLength * 8) < mappedLengthBits)) {
#else
/* If the bitwise mapping is disabled, theck the alignment and length in bytes*/
if (((OD_IOcopy.stream.attribute & testAttribute) == 0U) || ((mappedLengthBits & 0x07U) != 0U)
|| (OD_IOcopy.stream.dataLength < mappedLength)) {
#endif /* (CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING) */
return ODR_NO_MAP; /* Object cannot be mapped to the PDO. */
}
/* Copy values and store mappedLength temporary. */
*OD_IO = OD_IOcopy;
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
OD_IO->stream.dataOffset = mappedLengthBits;
#else
OD_IO->stream.dataOffset = mappedLength;
#endif
/* get TPDO request flag byte from extension */
#if OD_FLAGS_PDO_SIZE > 0
if (!isRPDO) {
if ((subIndex < (OD_FLAGS_PDO_SIZE * 8U)) && (entry->extension != NULL)) {
PDO->flagPDObyte[mapIndex] = &entry->extension->flagsPDO[subIndex >> 3];
PDO->flagPDObitmask[mapIndex] = 1U << (subIndex & 0x07U);
} else {
PDO->flagPDObyte[mapIndex] = NULL;
}
}
#endif
return ODR_OK;
}
/*
* Initialize PDO mapping parameters
*
* @param PDO This object.
* @param OD Object Dictionary.
* @param OD_PDOMapPar OD entry for "PDO mapping parameter".
* @param isRPDO True for RPDO and false for TPDO.
* @param [out] errInfo Additional information in case of error, may be NULL.
* @param [out] erroneousMap Additional information about erroneous map.
*
* @return #CO_ReturnError_t CO_ERROR_NO on success.
*/
static CO_ReturnError_t
PDO_initMapping(CO_PDO_common_t* PDO, OD_t* OD, OD_entry_t* OD_PDOMapPar, bool_t isRPDO, uint32_t* errInfo,
uint32_t* erroneousMap) {
ODR_t odRet;
size_t pdoDataLength = 0;
uint8_t mappedObjectsCount = 0;
/* number of mapped application objects in PDO */
odRet = OD_get_u8(OD_PDOMapPar, 0, &mappedObjectsCount, true);
if (odRet != ODR_OK) {
if (errInfo != NULL) {
*errInfo = ((uint32_t)OD_getIndex(OD_PDOMapPar)) << 8;
}
return CO_ERROR_OD_PARAMETERS;
}
for (uint8_t i = 0; i < CO_PDO_MAX_MAPPED_ENTRIES; i++) {
OD_IO_t* OD_IO = &PDO->OD_IO[i];
uint32_t map = 0;
odRet = OD_get_u32(OD_PDOMapPar, i + 1U, &map, true);
if (odRet == ODR_SUB_NOT_EXIST) {
continue;
}
if (odRet != ODR_OK) {
if (errInfo != NULL) {
*errInfo = (((uint32_t)OD_getIndex(OD_PDOMapPar)) << 8) | i;
}
return CO_ERROR_OD_PARAMETERS;
}
odRet = PDOconfigMap(PDO, map, i, isRPDO, OD);
if (odRet != ODR_OK) {
/* indicate erroneous mapping in initialization phase */
OD_IO->stream.dataLength = 0;
OD_IO->stream.dataOffset = 0xFF;
if (*erroneousMap == 0U) {
*erroneousMap = map;
}
}
if (i < mappedObjectsCount) {
pdoDataLength += OD_IO->stream.dataOffset;
}
}
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
if ((pdoDataLength > CO_PDO_MAX_SIZE * 8) || ((pdoDataLength == 0U) && (mappedObjectsCount > 0U))) {
#else
if ((pdoDataLength > CO_PDO_MAX_SIZE) || ((pdoDataLength == 0U) && (mappedObjectsCount > 0U))) {
#endif
if (*erroneousMap == 0U) {
*erroneousMap = 1;
}
}
if (*erroneousMap == 0U) {
PDO->dataLength = (CO_PDO_size_t)pdoDataLength;
PDO->mappedObjectsCount = mappedObjectsCount;
}
return CO_ERROR_NO;
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
/*
* Custom function for writing OD object "PDO mapping parameter"
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t
OD_write_PDO_mapping(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
/* "count" is already verified in *_init() function */
if ((stream == NULL) || (buf == NULL) || (countWritten == NULL) || (stream->subIndex > CO_PDO_MAX_MAPPED_ENTRIES)) {
return ODR_DEV_INCOMPAT;
}
/* Only common part of the CO_RPDO_t or CO_TPDO_t will be used */
CO_PDO_common_t* PDO = stream->object;
/* PDO must be disabled before mapping configuration */
if ((PDO->valid) || ((PDO->mappedObjectsCount != 0U) && (stream->subIndex > 0U))) {
return ODR_UNSUPP_ACCESS;
}
if (stream->subIndex == 0U) {
uint8_t mappedObjectsCount = CO_getUint8(buf);
size_t pdoDataLength = 0;
if (mappedObjectsCount > CO_PDO_MAX_MAPPED_ENTRIES) {
return ODR_MAP_LEN;
}
/* validate enabled mapping parameters */
for (uint8_t i = 0; i < mappedObjectsCount; i++) {
OD_IO_t* OD_IO = &PDO->OD_IO[i];
size_t dataLength = (size_t)OD_IO->stream.dataLength;
size_t mappedLength = (size_t)OD_IO->stream.dataOffset;
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
if (mappedLength > dataLength * 8) {
#else
if (mappedLength > dataLength) {
#endif
/* erroneous map since device initial values */
return ODR_NO_MAP;
}
pdoDataLength += mappedLength;
}
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
if (pdoDataLength > CO_PDO_MAX_SIZE * 8) {
#else
if (pdoDataLength > CO_PDO_MAX_SIZE) {
#endif
return ODR_MAP_LEN;
}
if ((pdoDataLength == 0U) && (mappedObjectsCount > 0U)) {
return ODR_INVALID_VALUE;
}
/* success, update PDO */
PDO->dataLength = (CO_PDO_size_t)pdoDataLength;
PDO->mappedObjectsCount = mappedObjectsCount;
} else {
uint32_t val = CO_getUint32(buf);
ODR_t odRet = PDOconfigMap(PDO, val, stream->subIndex - 1U, PDO->isRPDO, PDO->OD);
if (odRet != ODR_OK) {
return odRet;
}
}
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, buf, count, countWritten);
}
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_FLAG_OD_DYNAMIC */
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_PDO_OD_IO_ACCESS */
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_OD_IO_ACCESS) == 0
static CO_ReturnError_t
PDO_initMapping(CO_PDO_common_t* PDO, OD_t* OD, OD_entry_t* OD_PDOMapPar, bool_t isRPDO, uint32_t* errInfo,
uint32_t* erroneousMap) {
ODR_t odRet;
size_t pdoDataLength = 0;
/* number of mapped application objects in PDO */
uint8_t mappedObjectsCount = 0;
odRet = OD_get_u8(OD_PDOMapPar, 0, &mappedObjectsCount, true);
if (odRet != ODR_OK) {
if (errInfo != NULL) {
*errInfo = ((uint32_t)OD_getIndex(OD_PDOMapPar)) << 8;
}
return CO_ERROR_OD_PARAMETERS;
}
if (mappedObjectsCount > CO_PDO_MAX_MAPPED_ENTRIES) {
*erroneousMap = 1;
return CO_ERROR_NO;
}
/* iterate mapped OD variables */
for (uint8_t i = 0; i < mappedObjectsCount; i++) {
uint32_t map = 0;
odRet = OD_get_u32(OD_PDOMapPar, i + 1, &map, true);
if (odRet != ODR_OK) {
if (errInfo != NULL) {
*errInfo = (((uint32_t)OD_getIndex(OD_PDOMapPar)) << 8) | i;
}
return CO_ERROR_OD_PARAMETERS;
}
uint16_t index = (uint16_t)(map >> 16);
uint8_t subIndex = (uint8_t)(map >> 8);
uint8_t mappedLengthBits = (uint8_t)map;
uint8_t mappedLength = mappedLengthBits >> 3;
uint8_t pdoDataStart = pdoDataLength;
pdoDataLength += mappedLength;
if ((mappedLengthBits & 0x07) != 0 || pdoDataLength > CO_PDO_MAX_SIZE) {
*erroneousMap = map;
return CO_ERROR_NO;
}
/* is there a reference to the dummy entry */
if (index < 0x20 && subIndex == 0) {
for (uint8_t j = pdoDataStart; j < pdoDataLength; j++) {
static uint8_t dummyTX = 0;
static uint8_t dummyRX;
PDO->mapPointer[j] = isRPDO ? &dummyRX : &dummyTX;
}
continue;
}
/* find entry in the Object Dictionary, original location */
OD_IO_t OD_IO;
OD_entry_t* entry = OD_find(OD, index);
OD_attr_t testAttribute = isRPDO ? ODA_RPDO : ODA_TPDO;
ODR_t odRet = OD_getSub(entry, subIndex, &OD_IO, true);
if (odRet != ODR_OK || (OD_IO.stream.attribute & testAttribute) == 0 || OD_IO.stream.dataLength < mappedLength
|| OD_IO.stream.dataOrig == NULL) {
*erroneousMap = map;
return CO_ERROR_NO;
}
/* write locations to OD variable data bytes into PDO map pointers */
#ifdef CO_BIG_ENDIAN
if ((OD_IO.stream.attribute & ODA_MB) != 0) {
uint8_t* odDataPointer = OD_IO.stream.dataOrig + OD_IO.stream.dataLength - 1;
for (uint8_t j = pdoDataStart; j < pdoDataLength; j++) {
PDO->mapPointer[j] = odDataPointer--;
}
} else
#endif
{
uint8_t* odDataPointer = OD_IO.stream.dataOrig;
for (uint8_t j = pdoDataStart; j < pdoDataLength; j++) {
PDO->mapPointer[j] = odDataPointer++;
}
}
/* get TPDO request flag byte from extension */
#if OD_FLAGS_PDO_SIZE > 0
if (!isRPDO && subIndex < (OD_FLAGS_PDO_SIZE * 8) && entry->extension != NULL) {
PDO->flagPDObyte[pdoDataStart] = &entry->extension->flagsPDO[subIndex >> 3];
PDO->flagPDObitmask[pdoDataStart] = 1 << (subIndex & 0x07);
}
#endif
}
PDO->dataLength = PDO->mappedObjectsCount = pdoDataLength;
return CO_ERROR_NO;
}
#endif /* ((CO_CONFIG_PDO) & CO_CONFIG_PDO_OD_IO_ACCESS) == 0 */
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
/*
* Custom function for reading OD object "PDO communication parameter"
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t
OD_read_PDO_commParam(OD_stream_t* stream, void* buf, OD_size_t count, OD_size_t* countRead) {
ODR_t returnCode = OD_readOriginal(stream, buf, count, countRead);
/* When reading COB_ID, add Node-Id to the read value, if necessary */
if ((returnCode == ODR_OK) && (stream->subIndex == 1U) && (*countRead == 4U)) {
/* Only common part of the CO_RPDO_t or CO_TPDO_t will be used */
CO_PDO_common_t* PDO = stream->object;
uint32_t COB_ID = CO_getUint32(buf);
uint16_t CAN_ID = (uint16_t)(COB_ID & 0x7FFU);
/* If default CAN-ID is stored in OD (without Node-ID), add Node-ID */
if ((CAN_ID != 0U) && (CAN_ID == (PDO->preDefinedCanId & 0xFF80U))) {
COB_ID = (COB_ID & 0xFFFF0000U) | PDO->preDefinedCanId;
}
/* If PDO is not valid, set bit 31 */
if (!PDO->valid) {
COB_ID |= 0x80000000U;
}
(void)CO_setUint32(buf, COB_ID);
}
return returnCode;
}
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_FLAG_OD_DYNAMIC */
/*******************************************************************************
* R P D O
******************************************************************************/
#if ((CO_CONFIG_PDO)&CO_CONFIG_RPDO_ENABLE) != 0
/*
* @defgroup CO_PDO_receiveErrors_t States for RPDO->receiveError indicates received RPDOs with wrong length.
* @{
*
*/
#define CO_RPDO_RX_ACK_NO_ERROR 0U /* No error */
#define CO_RPDO_RX_ACK_ERROR 1U /* Error is acknowledged */
#define CO_RPDO_RX_ACK 10U /* Auxiliary value */
#define CO_RPDO_RX_OK 11U /* Correct RPDO received, not acknowledged */
#define CO_RPDO_RX_SHORT 12U /* Too short RPDO received, not acknowledged */
#define CO_RPDO_RX_LONG 13U /* Too long RPDO received, not acknowledged */
/* @} */ /* CO_PDO_receiveErrors_t */
/*
* 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 the new message.
*/
static void
CO_PDO_receive(void* object, void* msg) {
CO_RPDO_t* RPDO = object;
CO_PDO_common_t* PDO = &RPDO->PDO_common;
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
const uint8_t* data = CO_CANrxMsg_readData(msg);
uint8_t err = RPDO->receiveError;
if (PDO->valid) {
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING) != 0
if (DLC >= (PDO->dataLength + 7) / 8) {
#else
if (DLC >= PDO->dataLength) {
#endif
/* indicate errors in PDO length */
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING) != 0
if (DLC == (PDO->dataLength + 7) / 8) {
#else
if (DLC == PDO->dataLength) {
#endif
if (err == CO_RPDO_RX_ACK_ERROR) {
err = CO_RPDO_RX_OK;
}
} else {
if (err == CO_RPDO_RX_ACK_NO_ERROR) {
err = CO_RPDO_RX_LONG;
}
}
/* Determine, to which of the two rx buffers copy the message. */
uint8_t bufNo = 0;
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
if (RPDO->synchronous && (RPDO->SYNC != NULL) && RPDO->SYNC->CANrxToggle) {
bufNo = 1;
}
#endif
/* copy data into appropriate buffer and set 'new message' flag */
(void)memcpy(RPDO->CANrxData[bufNo], data, CO_PDO_MAX_SIZE);
CO_FLAG_SET(RPDO->CANrxNew[bufNo]);
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_CALLBACK_PRE) != 0
/* Optional signal to RTOS, which can resume task, which handles the RPDO. */
if (RPDO->pFunctSignalPre != NULL) {
RPDO->pFunctSignalPre(RPDO->functSignalObjectPre);
}
#endif
} else if (err == CO_RPDO_RX_ACK_NO_ERROR) {
err = CO_RPDO_RX_SHORT;
} else { /* MISRA C 2004 14.10 */
}
}
RPDO->receiveError = err;
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
/*
* Custom function for writing OD object "RPDO communication parameter"
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t
OD_write_14xx(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
/* "count" is also verified in *_init() function */
if ((stream == NULL) || (buf == NULL) || (countWritten == NULL) || (count > 4U)) {
return ODR_DEV_INCOMPAT;
}
CO_RPDO_t* RPDO = stream->object;
CO_PDO_common_t* PDO = &RPDO->PDO_common;
uint8_t bufCopy[4];
(void)memcpy((void*)bufCopy, (const void*)buf, count);
switch (stream->subIndex) {
case 1: { /* COB-ID used by PDO */
uint32_t COB_ID = CO_getUint32(buf);
uint16_t CAN_ID = (uint16_t)(COB_ID & 0x7FFU);
bool_t valid = (COB_ID & 0x80000000U) == 0U;
/* bits 11...29 must be zero, PDO must be disabled on change, CAN_ID == 0 is
* not allowed, mapping must be configured before enabling the PDO */
if (((COB_ID & 0x3FFFF800U) != 0U) || (valid && PDO->valid && (CAN_ID != PDO->configuredCanId))
|| (valid && CO_IS_RESTRICTED_CAN_ID(CAN_ID)) || (valid && (PDO->mappedObjectsCount == 0U))) {
return ODR_INVALID_VALUE;
}
/* parameter changed? */
if ((valid != PDO->valid) || (CAN_ID != PDO->configuredCanId)) {
/* if default CAN-ID is written, store to OD without Node-ID */
if (CAN_ID == PDO->preDefinedCanId) {
(void)CO_setUint32(bufCopy, COB_ID & 0xFFFFFF80U);
}
if (!valid) {
CAN_ID = 0;
}
CO_ReturnError_t ret = CO_CANrxBufferInit(PDO->CANdev, PDO->CANdevIdx, CAN_ID, 0x7FF, false,
(void*)RPDO, CO_PDO_receive);
if (valid && (ret == CO_ERROR_NO)) {
PDO->valid = true;
PDO->configuredCanId = CAN_ID;
} else {
PDO->valid = false;
CO_FLAG_CLEAR(RPDO->CANrxNew[0]);
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
CO_FLAG_CLEAR(RPDO->CANrxNew[1]);
#endif
if (ret != CO_ERROR_NO) {
return ODR_DEV_INCOMPAT;
}
}
}
break;
}
case 2: { /* transmission type */ uint8_t transmissionType = CO_getUint8(buf);
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
if ((transmissionType > (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_240)
&& (transmissionType < (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO)) {
return ODR_INVALID_VALUE;
}
bool_t synchronous = transmissionType <= (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_240;
/* Remove old message from the second buffer. */
if (RPDO->synchronous != synchronous) {
CO_FLAG_CLEAR(RPDO->CANrxNew[1]);
}
RPDO->synchronous = synchronous;
#else
if (transmissionType < CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO) {
return ODR_INVALID_VALUE;
}
#endif
break;
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_RPDO_TIMERS_ENABLE) != 0
case 5: { /* event-timer */
uint32_t eventTime = CO_getUint16(buf);
RPDO->timeoutTime_us = eventTime * 1000U;
RPDO->timeoutTimer = 0;
break;
}
#endif
default:
/* MISRA C 2004 15.3 */
break;
}
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, bufCopy, count, countWritten);
}
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_FLAG_OD_DYNAMIC */
CO_ReturnError_t
CO_RPDO_init(CO_RPDO_t* RPDO, OD_t* OD, CO_EM_t* em,
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
CO_SYNC_t* SYNC,
#endif
uint16_t preDefinedCanId, OD_entry_t* OD_14xx_RPDOCommPar, OD_entry_t* OD_16xx_RPDOMapPar,
CO_CANmodule_t* CANdevRx, uint16_t CANdevRxIdx, uint32_t* errInfo) {
CO_PDO_common_t* PDO = &RPDO->PDO_common;
CO_ReturnError_t ret;
ODR_t odRet;
/* verify arguments */
if ((RPDO == NULL) || (OD == NULL) || (em == NULL) || (OD_14xx_RPDOCommPar == NULL) || (OD_16xx_RPDOMapPar == NULL)
|| (CANdevRx == NULL)) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* clear object */
(void)memset(RPDO, 0, sizeof(CO_RPDO_t));
/* Configure object variables */
PDO->em = em;
PDO->CANdev = CANdevRx;
/* Configure mapping parameters */
uint32_t erroneousMap = 0;
ret = PDO_initMapping(PDO, OD, OD_16xx_RPDOMapPar, true, errInfo, &erroneousMap);
if (ret != CO_ERROR_NO) {
return ret;
}
/* Configure communication parameter - COB-ID */
uint32_t COB_ID = 0;
odRet = OD_get_u32(OD_14xx_RPDOCommPar, 1, &COB_ID, true);
if (odRet != ODR_OK) {
if (errInfo != NULL) {
*errInfo = (((uint32_t)OD_getIndex(OD_14xx_RPDOCommPar)) << 8) | 1U;
}
return CO_ERROR_OD_PARAMETERS;
}
bool_t valid = (COB_ID & 0x80000000U) == 0U;
uint16_t CAN_ID = (uint16_t)(COB_ID & 0x7FFU);
if (valid && ((PDO->mappedObjectsCount == 0U) || (CAN_ID == 0U))) {
valid = false;
if (erroneousMap == 0U) {
erroneousMap = 1;
}
}
if (erroneousMap != 0U) {
CO_errorReport(PDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR,
(erroneousMap != 1U) ? erroneousMap : COB_ID);
}
if (!valid) {
CAN_ID = 0;
}
/* If default CAN-ID is stored in OD (without Node-ID), add Node-ID */
if ((CAN_ID != 0U) && (CAN_ID == (preDefinedCanId & 0xFF80U))) {
CAN_ID = preDefinedCanId;
}
ret = CO_CANrxBufferInit(CANdevRx, CANdevRxIdx, CAN_ID, 0x7FF, false, (void*)RPDO, CO_PDO_receive);
if (ret != CO_ERROR_NO) {
return ret;
}
PDO->valid = valid;
/* Configure communication parameter - transmission type */
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
uint8_t transmissionType = (uint8_t)(CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO);
odRet = OD_get_u8(OD_14xx_RPDOCommPar, 2, &transmissionType, true);
if (odRet != ODR_OK) {
if (errInfo != NULL) {
*errInfo = (((uint32_t)OD_getIndex(OD_14xx_RPDOCommPar)) << 8) | 2U;
}
return CO_ERROR_OD_PARAMETERS;
}
RPDO->SYNC = SYNC;
RPDO->synchronous = transmissionType <= (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_240;
#endif
/* Configure communication parameter - event-timer (optional) */
#if ((CO_CONFIG_PDO)&CO_CONFIG_RPDO_TIMERS_ENABLE) != 0
uint16_t eventTime = 0;
(void)OD_get_u16(OD_14xx_RPDOCommPar, 5, &eventTime, true);
RPDO->timeoutTime_us = (uint32_t)eventTime * 1000U;
#endif
/* Configure OD extensions */
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
PDO->isRPDO = true;
PDO->OD = OD;
PDO->CANdevIdx = CANdevRxIdx;
PDO->preDefinedCanId = preDefinedCanId;
PDO->configuredCanId = CAN_ID;
PDO->OD_communicationParam_ext.object = RPDO;
PDO->OD_communicationParam_ext.read = OD_read_PDO_commParam;
PDO->OD_communicationParam_ext.write = OD_write_14xx;
PDO->OD_mappingParam_extension.object = RPDO;
PDO->OD_mappingParam_extension.read = OD_readOriginal;
PDO->OD_mappingParam_extension.write = OD_write_PDO_mapping;
(void)OD_extension_init(OD_14xx_RPDOCommPar, &PDO->OD_communicationParam_ext);
(void)OD_extension_init(OD_16xx_RPDOMapPar, &PDO->OD_mappingParam_extension);
#endif
return CO_ERROR_NO;
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_CALLBACK_PRE) != 0
void
CO_RPDO_initCallbackPre(CO_RPDO_t* RPDO, void* object, void (*pFunctSignalPre)(void* object)) {
if (RPDO != NULL) {
RPDO->functSignalObjectPre = object;
RPDO->pFunctSignalPre = pFunctSignalPre;
}
}
#endif
void
CO_RPDO_process(CO_RPDO_t* RPDO,
#if ((CO_CONFIG_PDO)&CO_CONFIG_RPDO_TIMERS_ENABLE) != 0
uint32_t timeDifference_us, uint32_t* timerNext_us,
#endif
bool_t NMTisOperational, bool_t syncWas) {
(void)syncWas;
#if ((CO_CONFIG_PDO)&CO_CONFIG_RPDO_TIMERS_ENABLE) != 0
(void)timerNext_us;
#endif
CO_PDO_common_t* PDO = &RPDO->PDO_common;
if (PDO->valid && NMTisOperational
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
&& (syncWas || !RPDO->synchronous)
#endif
) {
/* Verify errors in length of received RPDO CAN message */
if (RPDO->receiveError > CO_RPDO_RX_ACK) {
bool_t setError = RPDO->receiveError != CO_RPDO_RX_OK;
uint16_t code = (RPDO->receiveError == CO_RPDO_RX_SHORT) ? CO_EMC_PDO_LENGTH : CO_EMC_PDO_LENGTH_EXC;
CO_error(PDO->em, setError, CO_EM_RPDO_WRONG_LENGTH, code, PDO->dataLength);
RPDO->receiveError = setError ? CO_RPDO_RX_ACK_ERROR : CO_RPDO_RX_ACK_NO_ERROR;
}
/* Determine, which of the two rx buffers contains relevant message. */
uint8_t bufNo = 0;
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
if (RPDO->synchronous && (RPDO->SYNC != NULL) && !RPDO->SYNC->CANrxToggle) {
bufNo = 1;
}
#endif
/* copy RPDO into OD variables according to mappings */
bool_t rpdoReceived = false;
while (CO_FLAG_READ(RPDO->CANrxNew[bufNo])) {
rpdoReceived = true;
uint8_t* dataRPDO = RPDO->CANrxData[bufNo];
OD_size_t verifyLength = 0U;
/* Clear the flag. If between the copy operation CANrxNew is set
* by receive thread, then copy the latest data again. */
CO_FLAG_CLEAR(RPDO->CANrxNew[bufNo]);
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING) != 0
/* Copy everything regardless of the actual PDO size to UINT64 to simplify bit shifting */
uint64_t buf64 = 0;
(void)memcpy(&buf64, dataRPDO, CO_PDO_MAX_SIZE);
#ifdef CO_BIG_ENDIAN
/* For big endian we need to swap all bytes */
buf64 = CO_SWAP_64(buf64);
#endif /* CO_BIG_ENDIAN */
#endif /* ((CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING) */
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_OD_IO_ACCESS) != 0
for (uint8_t i = 0; i < PDO->mappedObjectsCount; i++) {
OD_IO_t* OD_IO = &PDO->OD_IO[i];
/* get mappedLength from temporary storage */
OD_size_t* dataOffset = &OD_IO->stream.dataOffset;
uint8_t mappedLength = (uint8_t)(*dataOffset);
/* additional safety check. */
verifyLength += (OD_size_t)mappedLength;
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING) != 0
if (verifyLength > CO_PDO_MAX_SIZE * 8) {
#else
if (verifyLength > CO_PDO_MAX_SIZE) {
#endif
break;
}
/* length of OD variable may be larger than mappedLength */
OD_size_t ODdataLength = OD_IO->stream.dataLength;
if (ODdataLength > CO_PDO_MAX_SIZE) {
ODdataLength = CO_PDO_MAX_SIZE;
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING) != 0
/* Prepare data for writing into OD variable. If mappedLength
* is smaller than ODdataLength, then use auxiliary buffer */
uint8_t* dataOD;
/* Apply the bitmask */
uint64_t shiftedData = buf64 & (UINT64_MAX >> (64 - mappedLength));
/* Shift the original buffer to get ready for the next mapping */
buf64 >>= mappedLength;
#ifdef CO_BIG_ENDIAN
/* Adjust the pointer ignoring unused most significant bytes */
dataOD = (uint8_t *)&shiftedData + 8 - ODdataLength;
#else
/* For little-endian we have a pointer to LSB */
dataOD = (uint8_t *)&shiftedData;
#endif
#else
/* Prepare data for writing into OD variable. If mappedLength
* is smaller than ODdataLength, then use auxiliary buffer */
uint8_t buf[CO_PDO_MAX_SIZE];
uint8_t* dataOD;
if (ODdataLength > mappedLength) {
(void)memset(buf, 0, sizeof(buf));
(void)memcpy(buf, dataRPDO, mappedLength);
dataOD = buf;
} else {
dataOD = dataRPDO;
}
/* swap multibyte data if big-endian */
#ifdef CO_BIG_ENDIAN
if ((OD_IO->stream.attribute & ODA_MB) != 0) {
uint8_t* lo = dataOD;
uint8_t* hi = dataOD + ODdataLength - 1;
while (lo < hi) {
uint8_t swap = *lo;
*lo++ = *hi;
*hi-- = swap;
}
}
#endif
#endif /* (CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING */
/* Set stream.dataOffset to zero, perform OD_IO.write()
* and store mappedLength back to stream.dataOffset */
*dataOffset = 0;
OD_size_t countWritten;
OD_IO->write(&OD_IO->stream, dataOD, ODdataLength, &countWritten);
*dataOffset = mappedLength;
dataRPDO += mappedLength;
}
#else
verifyLength = (OD_size_t)PDO->dataLength;
for (uint8_t i = 0; i < PDO->dataLength; i++) {
*PDO->mapPointer[i] = dataRPDO[i];
}
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_PDO_OD_IO_ACCESS */
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING) != 0
if ((verifyLength > (CO_PDO_MAX_SIZE * 8)) || (verifyLength != (OD_size_t)PDO->dataLength)) {
#else
if ((verifyLength > CO_PDO_MAX_SIZE) || (verifyLength != (OD_size_t)PDO->dataLength)) {
#endif
/* bug in software, should not happen */
CO_errorReport(PDO->em, CO_EM_GENERIC_SOFTWARE_ERROR, CO_EMC_SOFTWARE_INTERNAL,
(0x100000U | verifyLength));
}
} /* while (CO_FLAG_READ(RPDO->CANrxNew[bufNo])) */
/* verify RPDO timeout */
(void)rpdoReceived;
#if ((CO_CONFIG_PDO)&CO_CONFIG_RPDO_TIMERS_ENABLE) != 0
if (RPDO->timeoutTime_us > 0U) {
if (rpdoReceived) {
if (RPDO->timeoutTimer > RPDO->timeoutTime_us) {
CO_errorReset(PDO->em, CO_EM_RPDO_TIME_OUT, RPDO->timeoutTimer);
}
/* enable monitoring */
RPDO->timeoutTimer = 1;
} else if ((RPDO->timeoutTimer > 0U) && (RPDO->timeoutTimer < RPDO->timeoutTime_us)) {
RPDO->timeoutTimer += timeDifference_us;
if (RPDO->timeoutTimer > RPDO->timeoutTime_us) {
CO_errorReport(PDO->em, CO_EM_RPDO_TIME_OUT, CO_EMC_RPDO_TIMEOUT, RPDO->timeoutTimer);
}
} else { /* MISRA C 2004 14.10 */
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_TIMERNEXT) != 0
if ((timerNext_us != NULL) && (RPDO->timeoutTimer < RPDO->timeoutTime_us)) {
uint32_t diff = RPDO->timeoutTime_us - RPDO->timeoutTimer;
if (*timerNext_us > diff) {
*timerNext_us = diff;
}
}
#endif
}
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_RPDO_TIMERS_ENABLE */
} /* if (PDO->valid && NMTisOperational) */
else {
/* not valid and operational, clear CAN receive flags and timeoutTimer */
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
if (!PDO->valid || !NMTisOperational) {
CO_FLAG_CLEAR(RPDO->CANrxNew[0]);
CO_FLAG_CLEAR(RPDO->CANrxNew[1]);
#if ((CO_CONFIG_PDO)&CO_CONFIG_RPDO_TIMERS_ENABLE) != 0
RPDO->timeoutTimer = 0;
#endif
}
#else
CO_FLAG_CLEAR(RPDO->CANrxNew[0]);
#if ((CO_CONFIG_PDO)&CO_CONFIG_RPDO_TIMERS_ENABLE) != 0
RPDO->timeoutTimer = 0;
#endif
#endif
}
}
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_RPDO_ENABLE */
/*******************************************************************************
* T P D O
******************************************************************************/
#if ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_ENABLE) != 0
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
/*
* Custom function for writing OD object "TPDO communication parameter"
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t
OD_write_18xx(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
/* "count" is also verified in *_init() function */
if ((stream == NULL) || (buf == NULL) || (countWritten == NULL) || (count > 4U)) {
return ODR_DEV_INCOMPAT;
}
CO_TPDO_t* TPDO = stream->object;
CO_PDO_common_t* PDO = &TPDO->PDO_common;
uint8_t bufCopy[4];
(void)memcpy((void*)bufCopy, (const void*)buf, count);
switch (stream->subIndex) {
case 1: { /* COB-ID used by PDO */
uint32_t COB_ID = CO_getUint32(buf);
uint16_t CAN_ID = (uint16_t)(COB_ID & 0x7FFU);
bool_t valid = (COB_ID & 0x80000000U) == 0U;
/* bits 11...29 must be zero, PDO must be disabled on change, CAN_ID == 0 is
* not allowed, mapping must be configured before enabling the PDO */
if (((COB_ID & 0x3FFFF800U) != 0U) || (valid && (PDO->valid && (CAN_ID != PDO->configuredCanId)))
|| (valid && CO_IS_RESTRICTED_CAN_ID(CAN_ID)) || (valid && (PDO->mappedObjectsCount == 0U))) {
return ODR_INVALID_VALUE;
}
/* parameter changed? */
if ((valid != PDO->valid) || (CAN_ID != PDO->configuredCanId)) {
/* if default CAN-ID is written, store to OD without Node-ID */
if (CAN_ID == PDO->preDefinedCanId) {
(void)CO_setUint32(bufCopy, COB_ID & 0xFFFFFF80U);
}
if (!valid) {
CAN_ID = 0;
}
CO_CANtx_t* CANtxBuff = CO_CANtxBufferInit(
PDO->CANdev, PDO->CANdevIdx, CAN_ID, false, PDO->dataLength,
TPDO->transmissionType <= (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_240);
if (CANtxBuff == NULL) {
return ODR_DEV_INCOMPAT;
}
TPDO->CANtxBuff = CANtxBuff;
PDO->valid = valid;
PDO->configuredCanId = CAN_ID;
}
break;
}
case 2: { /* transmission type */ uint8_t transmissionType = CO_getUint8(buf);
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
if ((transmissionType > (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_240)
&& (transmissionType < (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO)) {
return ODR_INVALID_VALUE;
}
TPDO->CANtxBuff->syncFlag = transmissionType <= (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_240;
TPDO->syncCounter = 255;
#else
if (transmissionType < CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO) {
return ODR_INVALID_VALUE;
}
#endif
TPDO->transmissionType = transmissionType;
TPDO->sendRequest = true;
#if ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE) != 0
TPDO->inhibitTimer = 0;
TPDO->eventTimer = 0;
#endif
break;
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE) != 0
case 3: { /* inhibit time */
if (PDO->valid) {
return ODR_INVALID_VALUE;
}
uint32_t inhibitTime = CO_getUint16(buf);
TPDO->inhibitTime_us = inhibitTime * 100U;
TPDO->inhibitTimer = 0;
break;
}
case 5: { /* event-timer */
uint32_t eventTime = CO_getUint16(buf);
TPDO->eventTime_us = eventTime * 1000U;
TPDO->eventTimer = 0;
break;
}
#endif
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
case 6: { /* SYNC start value */
uint8_t syncStartValue = CO_getUint8(buf);
if (PDO->valid || (syncStartValue > 240U)) {
return ODR_INVALID_VALUE;
}
TPDO->syncStartValue = syncStartValue;
break;
}
#endif
default:
/* MISRA C 2004 15.3 */
break;
}
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, bufCopy, count, countWritten);
}
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_FLAG_OD_DYNAMIC */
CO_ReturnError_t
CO_TPDO_init(CO_TPDO_t* TPDO, OD_t* OD, CO_EM_t* em,
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
CO_SYNC_t* SYNC,
#endif
uint16_t preDefinedCanId, OD_entry_t* OD_18xx_TPDOCommPar, OD_entry_t* OD_1Axx_TPDOMapPar,
CO_CANmodule_t* CANdevTx, uint16_t CANdevTxIdx, uint32_t* errInfo) {
CO_PDO_common_t* PDO = &TPDO->PDO_common;
ODR_t odRet;
/* verify arguments */
if ((TPDO == NULL) || (OD == NULL) || (em == NULL) || (OD_18xx_TPDOCommPar == NULL) || (OD_1Axx_TPDOMapPar == NULL)
|| (CANdevTx == NULL)) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* clear object */
(void)memset(TPDO, 0, sizeof(CO_TPDO_t));
/* Configure object variables */
PDO->em = em;
PDO->CANdev = CANdevTx;
/* Configure mapping parameters */
uint32_t erroneousMap = 0;
CO_ReturnError_t ret = PDO_initMapping(PDO, OD, OD_1Axx_TPDOMapPar, false, errInfo, &erroneousMap);
if (ret != CO_ERROR_NO) {
return ret;
}
/* Configure communication parameter - transmission type */
uint8_t transmissionType = (uint8_t)(CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO);
odRet = OD_get_u8(OD_18xx_TPDOCommPar, 2, &transmissionType, true);
if (odRet != ODR_OK) {
if (errInfo != NULL) {
*errInfo = (((uint32_t)OD_getIndex(OD_18xx_TPDOCommPar)) << 8) | 2U;
}
return CO_ERROR_OD_PARAMETERS;
}
if ((transmissionType < (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO)
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
&& (transmissionType > (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_240)
#endif
) {
transmissionType = (uint8_t)(CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO);
}
TPDO->transmissionType = transmissionType;
TPDO->sendRequest = true;
/* Configure communication parameter - COB-ID */
uint32_t COB_ID = 0;
odRet = OD_get_u32(OD_18xx_TPDOCommPar, 1, &COB_ID, true);
if (odRet != ODR_OK) {
if (errInfo != NULL) {
*errInfo = (((uint32_t)OD_getIndex(OD_18xx_TPDOCommPar)) << 8) | 1U;
}
return CO_ERROR_OD_PARAMETERS;
}
bool_t valid = (COB_ID & 0x80000000U) == 0U;
uint16_t CAN_ID = (uint16_t)(COB_ID & 0x7FFU);
if (valid && ((PDO->mappedObjectsCount == 0U) || (CAN_ID == 0U))) {
valid = false;
if (erroneousMap == 0U) {
erroneousMap = 1;
}
}
if (erroneousMap != 0U) {
CO_errorReport(PDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR,
(erroneousMap != 1U) ? erroneousMap : COB_ID);
}
if (!valid) {
CAN_ID = 0;
}
/* If default CAN-ID is stored in OD (without Node-ID), add Node-ID */
if ((CAN_ID != 0U) && (CAN_ID == (preDefinedCanId & 0xFF80U))) {
CAN_ID = preDefinedCanId;
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_BITWISE_MAPPING) != 0
TPDO->CANtxBuff = CO_CANtxBufferInit(CANdevTx, CANdevTxIdx, CAN_ID, false, (PDO->dataLength + 7) / 8,
TPDO->transmissionType <= (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_240);
#else
TPDO->CANtxBuff = CO_CANtxBufferInit(CANdevTx, CANdevTxIdx, CAN_ID, false, PDO->dataLength,
TPDO->transmissionType <= (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_240);
#endif
if (TPDO->CANtxBuff == NULL) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
PDO->valid = valid;
/* Configure communication parameter - inhibit time and event-timer (opt) */
#if ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE) != 0
uint16_t inhibitTime = 0;
uint16_t eventTime = 0;
(void)OD_get_u16(OD_18xx_TPDOCommPar, 3, &inhibitTime, true);
(void)OD_get_u16(OD_18xx_TPDOCommPar, 5, &eventTime, true);
TPDO->inhibitTime_us = (uint32_t)inhibitTime * 100U;
TPDO->eventTime_us = (uint32_t)eventTime * 1000U;
#endif
/* Configure communication parameter - SYNC start value (optional) */
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
TPDO->syncStartValue = 0;
(void)OD_get_u8(OD_18xx_TPDOCommPar, 6, &TPDO->syncStartValue, true);
TPDO->SYNC = SYNC;
TPDO->syncCounter = 255;
#endif
/* Configure OD extensions */
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
PDO->isRPDO = false;
PDO->OD = OD;
PDO->CANdevIdx = CANdevTxIdx;
PDO->preDefinedCanId = preDefinedCanId;
PDO->configuredCanId = CAN_ID;
PDO->OD_communicationParam_ext.object = TPDO;
PDO->OD_communicationParam_ext.read = OD_read_PDO_commParam;
PDO->OD_communicationParam_ext.write = OD_write_18xx;
PDO->OD_mappingParam_extension.object = TPDO;
PDO->OD_mappingParam_extension.read = OD_readOriginal;
PDO->OD_mappingParam_extension.write = OD_write_PDO_mapping;
(void)OD_extension_init(OD_18xx_TPDOCommPar, &PDO->OD_communicationParam_ext);
(void)OD_extension_init(OD_1Axx_TPDOMapPar, &PDO->OD_mappingParam_extension);
#endif
return CO_ERROR_NO;
}
/*
* Send TPDO message.
*
* Function prepares TPDO data from Object Dictionary variables. It is called
* from CO_TPDO_process() according to TPDO communication parameters.
*
* @param TPDO TPDO object.
*
* @return Same as CO_CANsend().
*/
static CO_ReturnError_t
CO_TPDOsend(CO_TPDO_t* TPDO) {
CO_PDO_common_t* PDO = &TPDO->PDO_common;
uint8_t* dataTPDO = &TPDO->CANtxBuff->data[0];
OD_size_t verifyLength = 0U;
#if OD_FLAGS_PDO_SIZE > 0
bool_t eventDriven = ((TPDO->transmissionType == (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_ACYCLIC)
|| (TPDO->transmissionType >= (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO));
#endif
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_OD_IO_ACCESS) != 0
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
uint64_t buf64 = 0;
#endif
for (uint8_t i = 0; i < PDO->mappedObjectsCount; i++) {
OD_IO_t* OD_IO = &PDO->OD_IO[i];
OD_stream_t* stream = &OD_IO->stream;
/* get mappedLength from temporary storage */
uint8_t mappedLength = (uint8_t)stream->dataOffset;
/* additional safety check */
verifyLength += (OD_size_t)mappedLength;
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
if (verifyLength > CO_PDO_MAX_SIZE * 8) {
#else
if (verifyLength > CO_PDO_MAX_SIZE) {
#endif
break;
}
/* length of OD variable may be larger than mappedLength */
OD_size_t ODdataLength = stream->dataLength;
if (ODdataLength > CO_PDO_MAX_SIZE) {
ODdataLength = CO_PDO_MAX_SIZE;
}
/* If mappedLength is smaller than ODdataLength, use auxiliary buffer */
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
uint64_t buf = 0;
#else
uint8_t buf[CO_PDO_MAX_SIZE];
uint8_t* dataTPDOCopy;
if (ODdataLength > mappedLength) {
(void)memset(buf, 0, sizeof(buf));
dataTPDOCopy = buf;
} else {
dataTPDOCopy = dataTPDO;
}
#endif /* (CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING) */
/* Set stream.dataOffset to zero, perform OD_IO.read() and store mappedLength back to stream.dataOffset */
stream->dataOffset = 0;
OD_size_t countRd;
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
OD_IO->read(stream, &buf, ODdataLength, &countRd);
#ifdef CO_BIG_ENDIAN
/* Shift right according to the OD data length in bytes to compensate the difference in byte length
* For LE not needed, low bytes from the OD entry are copied to low bytes of u64 */
buf >>= 64 - 8 * ODdataLength;
#endif /* CO_BIG_ENDIAN */
/* Apply the mask and merge with the rest */
buf &= (UINT64_MAX >> (64 - mappedLength));
buf <<= (verifyLength - mappedLength);
buf64 |= buf;
#else
OD_IO->read(stream, dataTPDOCopy, ODdataLength, &countRd);
#endif /* (CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING) */
stream->dataOffset = mappedLength;
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) == 0
/* swap multibyte data if big-endian */
#ifdef CO_BIG_ENDIAN
if ((stream->attribute & ODA_MB) != 0) {
uint8_t* lo = dataTPDOCopy;
uint8_t* hi = dataTPDOCopy + ODdataLength - 1;
while (lo < hi) {
uint8_t swap = *lo;
*lo++ = *hi;
*hi-- = swap;
}
}
#endif
/* If auxiliary buffer, copy it to the TPDO */
if (ODdataLength > mappedLength) {
(void)memcpy(dataTPDO, buf, mappedLength);
}
#endif /* (CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING) == 0*/
/* In event driven TPDO indicate transmission of OD variable */
#if OD_FLAGS_PDO_SIZE > 0
uint8_t* flagPDObyte = PDO->flagPDObyte[i];
if ((flagPDObyte != NULL) && eventDriven) {
*flagPDObyte |= PDO->flagPDObitmask[i];
}
#endif
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) == 0
dataTPDO += mappedLength;
#endif
}
#else
verifyLength = (OD_size_t)PDO->dataLength;
for (uint8_t i = 0; i < PDO->dataLength; i++) {
dataTPDO[i] = *PDO->mapPointer[i];
/* In event driven TPDO indicate transmission of OD variable */
#if OD_FLAGS_PDO_SIZE > 0
uint8_t* flagPDObyte = PDO->flagPDObyte[i];
if (flagPDObyte != NULL && eventDriven) {
*flagPDObyte |= PDO->flagPDObitmask[i];
}
#endif
}
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_PDO_OD_IO_ACCESS */
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
if ((verifyLength > (CO_PDO_MAX_SIZE * 8)) || (verifyLength != (OD_size_t)PDO->dataLength)) {
#else
if ((verifyLength > CO_PDO_MAX_SIZE) || (verifyLength != (OD_size_t)PDO->dataLength)) {
#endif
/* bug in software, should not happen */
CO_errorReport(PDO->em, CO_EM_GENERIC_SOFTWARE_ERROR, CO_EMC_SOFTWARE_INTERNAL, (0x200000U | verifyLength));
return CO_ERROR_DATA_CORRUPT;
}
#if ((CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING)) != 0
#ifdef CO_BIG_ENDIAN
buf64 = CO_SWAP_64(buf64);
#endif
/* Copy the calculated uint64 data to the PDO buffer */
(void)memcpy(dataTPDO, &buf64, CO_PDO_MAX_SIZE);
#endif /* (CO_CONFIG_PDO) & (CO_CONFIG_PDO_BITWISE_MAPPING) */
TPDO->sendRequest = false;
#if ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE) != 0
TPDO->eventTimer = TPDO->eventTime_us;
TPDO->inhibitTimer = TPDO->inhibitTime_us;
#endif
return CO_CANsend(PDO->CANdev, TPDO->CANtxBuff);
}
void
CO_TPDO_process(CO_TPDO_t* TPDO,
#if (((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE) != 0) || defined CO_DOXYGEN
uint32_t timeDifference_us, uint32_t* timerNext_us,
#endif
bool_t NMTisOperational, bool_t syncWas) {
CO_PDO_common_t* PDO = &TPDO->PDO_common;
#if (((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE)) != 0
(void)timerNext_us;
#endif
(void)syncWas;
if (PDO->valid && NMTisOperational) {
/* check for event timer or application event */
#if (((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE) != 0) || (OD_FLAGS_PDO_SIZE > 0)
if ((TPDO->transmissionType == (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_ACYCLIC)
|| (TPDO->transmissionType >= (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO)) {
/* event timer */
#if ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE) != 0
if (TPDO->eventTime_us != 0U) {
TPDO->eventTimer = (TPDO->eventTimer > timeDifference_us) ? (TPDO->eventTimer - timeDifference_us) : 0U;
if (TPDO->eventTimer == 0U) {
TPDO->sendRequest = true;
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_TIMERNEXT) != 0
if ((timerNext_us != NULL) && (*timerNext_us > TPDO->eventTimer)) {
/* Schedule for next event time */
*timerNext_us = TPDO->eventTimer;
}
#endif
}
#endif
/* check for any OD_requestTPDO() */
#if OD_FLAGS_PDO_SIZE > 0
if (!TPDO->sendRequest) {
for (uint8_t i = 0; i < PDO->mappedObjectsCount; i++) {
uint8_t* flagPDObyte = PDO->flagPDObyte[i];
if (flagPDObyte != NULL) {
if ((*flagPDObyte & PDO->flagPDObitmask[i]) == 0U) {
TPDO->sendRequest = true;
break;
}
}
}
}
#endif
}
#endif /* ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE)||(OD_FLAGS_PDO_SIZE>0) */
/* Send PDO by application request or by Event timer */
if (TPDO->transmissionType >= (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO) {
#if ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE) != 0
TPDO->inhibitTimer = (TPDO->inhibitTimer > timeDifference_us) ? (TPDO->inhibitTimer - timeDifference_us)
: 0U;
/* send TPDO */
if (TPDO->sendRequest && (TPDO->inhibitTimer == 0U)) {
(void)CO_TPDOsend(TPDO);
}
#if ((CO_CONFIG_PDO)&CO_CONFIG_FLAG_TIMERNEXT) != 0
if (TPDO->sendRequest && (timerNext_us != NULL) && (*timerNext_us > TPDO->inhibitTimer)) {
/* Schedule for just beyond inhibit window */
*timerNext_us = TPDO->inhibitTimer;
}
#endif
#else
if (TPDO->sendRequest) {
(void)CO_TPDOsend(TPDO);
}
#endif
} /* if (TPDO->transmissionType >= CO_PDO_TRANSM_TYPE_SYNC_EVENT_LO) */
/* Synchronous PDOs */
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
else if ((TPDO->SYNC != NULL) && syncWas) {
/* send synchronous acyclic TPDO */
if (TPDO->transmissionType == (uint8_t)CO_PDO_TRANSM_TYPE_SYNC_ACYCLIC) {
if (TPDO->sendRequest) {
(void)CO_TPDOsend(TPDO);
}
}
/* send synchronous cyclic TPDO */
else {
/* is the start of synchronous TPDO transmission */
if (TPDO->syncCounter == 255U) {
if ((TPDO->SYNC->counterOverflowValue != 0U) && (TPDO->syncStartValue != 0U)) {
/* syncStartValue is in use */
TPDO->syncCounter = 254;
} else {
/* Send first TPDO somewhere in the middle */
TPDO->syncCounter = (TPDO->transmissionType / 2U) + 1U;
}
}
/* If the syncStartValue is in use, start first TPDO after SYNC with matched syncStartValue. */
if (TPDO->syncCounter == 254U) {
if (TPDO->SYNC->counter == TPDO->syncStartValue) {
TPDO->syncCounter = TPDO->transmissionType;
(void)CO_TPDOsend(TPDO);
}
}
/* Send TPDO after every N-th Sync */
else if (--TPDO->syncCounter == 0U) {
TPDO->syncCounter = TPDO->transmissionType;
(void)CO_TPDOsend(TPDO);
} else { /* MISRA C 2004 14.10 */
}
}
} /* else if (TPDO->SYNC && syncWas) */
else { /* MISRA C 2004 14.10 */
}
#endif
} else {
/* Not operational or valid, reset triggers */
TPDO->sendRequest = true;
#if ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_TIMERS_ENABLE) != 0
TPDO->inhibitTimer = 0;
TPDO->eventTimer = 0;
#endif
#if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0
TPDO->syncCounter = 255;
#endif
}
}
#endif /* (CO_CONFIG_PDO) & CO_CONFIG_TPDO_ENABLE */
#endif /* (CO_CONFIG_PDO) & (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE) */
Reference in New Issue View Git Blame Copy Permalink