nikon/CANopenNode
1
0
Fork 0
Code Activity Actions
CANopenNode/301/CO_Emergency.c

811 lines
28 KiB
C
Raw Blame History

/*
* CANopen Emergency object.
*
* @file CO_Emergency.c
* @ingroup CO_Emergency
* @author Janez Paternoster
* @copyright 2020 Janez Paternoster
*
* 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 <string.h>
#include "301/CO_Emergency.h"
/* verify configuration */
#if CO_CONFIG_EM_ERR_STATUS_BITS_COUNT < (6U*8U) \
|| CO_CONFIG_EM_ERR_STATUS_BITS_COUNT > 256U \
|| (CO_CONFIG_EM_ERR_STATUS_BITS_COUNT % 8U) != 0
#error CO_CONFIG_EM_ERR_STATUS_BITS_COUNT is not correct
#endif
/* fifo buffer example for fifoSize = 7 (actual capacity = 6) *
* *
* 0 * * * * *
* 1 pp==wp fifoPpPtr fifoWrPtr * *
* 2 * * * * *
* 3 * * * fifoWrPtr *
* 4 * fifoWrPtr fifoPpPtr fifoPpPtr *
* 5 * * * * *
* 6 * * * * *
* *
* nothing 3 bytes 4 bytes buffer *
* to process to process to process full *
******************************************************************************/
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) != 0
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_CONFIGURABLE) != 0
/*
* Custom functions for read/write OD object "COB-ID EMCY"
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t OD_read_1014(OD_stream_t *stream, void *buf,
OD_size_t count, OD_size_t *countRead)
{
if ((stream == NULL) || (stream->subIndex != 0U) || (buf == NULL)
|| (count < sizeof(uint32_t)) || (countRead == NULL)
) {
return ODR_DEV_INCOMPAT;
}
CO_EM_t *em = (CO_EM_t *)stream->object;
uint16_t canId = (em->producerCanId == CO_CAN_ID_EMERGENCY) ?
(CO_CAN_ID_EMERGENCY + em->nodeId) : em->producerCanId;
uint32_t COB_IDEmergency32 = em->producerEnabled ? 0U : 0x80000000U;
COB_IDEmergency32 |= canId;
(void)CO_setUint32(buf, COB_IDEmergency32);
*countRead = sizeof(uint32_t);
return ODR_OK;
}
static ODR_t OD_write_1014(OD_stream_t *stream, const void *buf,
OD_size_t count, OD_size_t *countWritten)
{
if ((stream == NULL) || (stream->subIndex != 0U) || (buf == NULL)
|| (count != sizeof(uint32_t)) || (countWritten == NULL)
) {
return ODR_DEV_INCOMPAT;
}
CO_EM_t *em = (CO_EM_t *)stream->object;
/* Verify written value. COB ID must not change, if emergency is enabled */
uint32_t COB_IDEmergency32 = CO_getUint32(buf);
uint16_t newCanId = (uint16_t)(COB_IDEmergency32 & 0x7FFU);
uint16_t curCanId = (em->producerCanId == CO_CAN_ID_EMERGENCY) ?
(CO_CAN_ID_EMERGENCY + em->nodeId) : em->producerCanId;
bool_t newEnabled = ((COB_IDEmergency32 & 0x80000000U) == 0U) && (newCanId != 0U);
if (((COB_IDEmergency32 & 0x7FFFF800U)!=0U) || CO_IS_RESTRICTED_CAN_ID(newCanId)
|| ((em->producerEnabled && newEnabled) && (newCanId != curCanId))
) {
return ODR_INVALID_VALUE;
}
/* store values. If default CAN-ID is used, then store only value of
* CO_CAN_ID_EMERGENCY without node id. */
em->producerEnabled = newEnabled;
em->producerCanId = (newCanId == ((uint16_t)CO_CAN_ID_EMERGENCY + em->nodeId)) ?
CO_CAN_ID_EMERGENCY : newCanId;
/* configure emergency message CAN transmission */
if (newEnabled) {
em->CANtxBuff = CO_CANtxBufferInit(
em->CANdevTx,
em->CANdevTxIdx,
newCanId,
false,
8U,
false);
}
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, buf, count, countWritten);
}
#else
/*
* Custom functions for read/write OD object "COB-ID EMCY"
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t OD_read_1014_default(OD_stream_t *stream, void *buf,
OD_size_t count, OD_size_t *countRead)
{
if ((stream == NULL) || (stream->subIndex != 0U) || (buf == NULL)
|| (count < sizeof(uint32_t)) || (countRead == NULL)
) {
return ODR_DEV_INCOMPAT;
}
CO_EM_t *em = (CO_EM_t *)stream->object;
uint32_t COB_IDEmergency32 = em->producerEnabled ? 0U : 0x80000000U;
COB_IDEmergency32 |= CO_CAN_ID_EMERGENCY + (uint32_t)em->nodeId;
(void)CO_setUint32(buf, COB_IDEmergency32);
*countRead = sizeof(uint32_t);
return ODR_OK;
}
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_PROD_CONFIGURABLE */
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT) != 0
/*
* Custom function for writing OD object "Inhibit time EMCY"
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t OD_write_1015(OD_stream_t *stream, const void *buf,
OD_size_t count, OD_size_t *countWritten)
{
if ((stream == NULL) || (stream->subIndex != 0U) || (buf == NULL)
|| (count != sizeof(uint16_t)) || (countWritten == NULL)
) {
return ODR_DEV_INCOMPAT;
}
CO_EM_t *em = (CO_EM_t *)stream->object;
/* update object */
em->inhibitEmTime_us = (uint32_t)CO_getUint16(buf) * 100U;
em->inhibitEmTimer = 0;
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, buf, count, countWritten);
}
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT */
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER */
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_HISTORY) != 0
/*
* Custom functions for read/write OD object _OD_statusBits_, optional
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t OD_read_1003(OD_stream_t *stream, void *buf,
OD_size_t count, OD_size_t *countRead)
{
if ((stream == NULL) || (buf == NULL) || (countRead == NULL)
|| ((count < 4U) && (stream->subIndex > 0U)) || (count < 1U)
) {
return ODR_DEV_INCOMPAT;
}
CO_EM_t *em = (CO_EM_t *)stream->object;
if (em->fifoSize < 2U) {
return ODR_DEV_INCOMPAT;
}
if (stream->subIndex == 0U) {
(void)CO_setUint8(buf, em->fifoCount);
*countRead = sizeof(uint8_t);
return ODR_OK;
}
else if (stream->subIndex <= em->fifoCount) {
/* newest error is reported on subIndex 1 and is stored just behind
* fifoWrPtr. Get correct index in FIFO buffer. */
int16_t index = (int16_t)em->fifoWrPtr - (int16_t)stream->subIndex;
if (index < 0) {
index += (int16_t)em->fifoSize;
}
else if (index >= (int16_t)(em->fifoSize)) {
return ODR_DEV_INCOMPAT;
}
else { /* MISRA C 2004 14.10 */ }
(void)CO_setUint32(buf, em->fifo[index].msg);
*countRead = sizeof(uint32_t);
return ODR_OK;
}
else {
return ODR_NO_DATA;
}
}
static ODR_t OD_write_1003(OD_stream_t *stream, const void *buf,
OD_size_t count, OD_size_t *countWritten)
{
if ((stream == NULL) || (stream->subIndex != 0U) || (buf == NULL) || (count != 1U)
|| (countWritten == NULL))
{
return ODR_DEV_INCOMPAT;
}
if (CO_getUint8(buf) != 0U) {
return ODR_INVALID_VALUE;
}
CO_EM_t *em = (CO_EM_t *)stream->object;
/* clear error history */
em->fifoCount = 0;
*countWritten = sizeof(uint8_t);
return ODR_OK;
}
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_HISTORY */
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_STATUS_BITS) != 0
/*
* Custom functions for read/write OD object _OD_statusBits_, optional
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static ODR_t OD_read_statusBits(OD_stream_t *stream, void *buf,
OD_size_t count, OD_size_t *countRead)
{
if ((stream == NULL) || (stream->subIndex != 0U)
|| (buf == NULL) || (countRead == NULL))
{
return ODR_DEV_INCOMPAT;
}
CO_EM_t *em = (CO_EM_t *)stream->object;
/* get MAX(errorStatusBitsSize, bufSize, ODsizeIndication) */
OD_size_t countReadLocal = CO_CONFIG_EM_ERR_STATUS_BITS_COUNT / 8U;
if (countReadLocal > count) {
countReadLocal = count;
}
if ((stream->dataLength != 0U) && (countReadLocal > stream->dataLength)) {
countReadLocal = stream->dataLength;
}
else {
stream->dataLength = countReadLocal;
}
(void)memcpy ((void *)(buf), (const void *)(&em->errorStatusBits[0]), countReadLocal);
*countRead = countReadLocal;
return ODR_OK;
}
static ODR_t OD_write_statusBits(OD_stream_t *stream, const void *buf,
OD_size_t count, OD_size_t *countWritten)
{
if ((stream == NULL) || (stream->subIndex != 0U)
|| (buf == NULL) || (countWritten == NULL)
) {
return ODR_DEV_INCOMPAT;
}
CO_EM_t *em = (CO_EM_t *)stream->object;
/* get MAX(errorStatusBitsSize, bufSize, ODsizeIndication) */
OD_size_t countWrite = CO_CONFIG_EM_ERR_STATUS_BITS_COUNT / 8U;
if (countWrite > count) {
countWrite = count;
}
if ((stream->dataLength != 0U) && (countWrite > stream->dataLength)) {
countWrite = stream->dataLength;
}
else {
stream->dataLength = countWrite;
}
(void)memcpy ((void *)(&em->errorStatusBits[0]), (const void *)(buf), countWrite);
*countWritten = countWrite;
return ODR_OK;
}
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_STATUS_BITS */
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) != 0
/*
* 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.
*/
static void CO_EM_receive(void *object, void *msg) {
CO_EM_t *em = (CO_EM_t*)object;
if ((em != NULL) && (em->pFunctSignalRx != NULL)) {
uint16_t ident = CO_CANrxMsg_readIdent(msg);
/* ignore sync messages (necessary if sync object is not used) */
if (ident != 0x80U) {
const uint8_t *data = CO_CANrxMsg_readData(msg);
uint16_t errorCode;
uint32_t infoCode;
(void)memcpy((void *)(&errorCode), (const void *)(&data[0]), sizeof(errorCode));
(void)memcpy((void *)(&infoCode), (const void *)(&data[4]), sizeof(infoCode));
em->pFunctSignalRx(ident,
CO_SWAP_16(errorCode),
data[2],
data[3],
CO_SWAP_32(infoCode));
}
}
}
#endif
/******************************************************************************/
CO_ReturnError_t CO_EM_init(CO_EM_t *em,
CO_CANmodule_t *CANdevTx,
const OD_entry_t *OD_1001_errReg,
#if ((CO_CONFIG_EM) & (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)) != 0
CO_EM_fifo_t *fifo,
uint8_t fifoSize,
#endif
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) != 0
OD_entry_t *OD_1014_cobIdEm,
uint16_t CANdevTxIdx,
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT) != 0
OD_entry_t *OD_1015_InhTime,
#endif
#endif
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_HISTORY) != 0
OD_entry_t *OD_1003_preDefErr,
#endif
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_STATUS_BITS) != 0
OD_entry_t *OD_statusBits,
#endif
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) != 0
CO_CANmodule_t *CANdevRx,
uint16_t CANdevRxIdx,
#endif
const uint8_t nodeId,
uint32_t *errInfo)
{
(void) nodeId; /* may be unused */
CO_ReturnError_t ret = CO_ERROR_NO;
/* verify arguments */
if ((em == NULL) || (OD_1001_errReg == NULL)
#if ((CO_CONFIG_EM) & (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)) != 0
|| ((fifo == NULL) && (fifoSize >= 2U))
#endif
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) != 0
|| (OD_1014_cobIdEm == NULL) || (CANdevTx == NULL)
|| (nodeId < 1U) || (nodeId > 127U)
#endif
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_HISTORY) != 0
|| (OD_1003_preDefErr == NULL)
#endif
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) != 0
|| (CANdevRx == NULL)
#endif
) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* clear the object */
(void)memset(em, 0, sizeof(CO_EM_t));
/* set object variables */
em->CANdevTx = CANdevTx;
/* get and verify "Error register" from Object Dictionary */
em->errorRegister = OD_getPtr(OD_1001_errReg, 0, sizeof(uint8_t), NULL);
if (em->errorRegister == NULL) {
if (errInfo != NULL) { *errInfo = OD_getIndex(OD_1001_errReg); }
return CO_ERROR_OD_PARAMETERS;
}
*em->errorRegister = 0;
#if ((CO_CONFIG_EM) & (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)) != 0
em->fifo = fifo;
em->fifoSize = fifoSize;
#endif
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) != 0
/* get initial and verify "COB-ID EMCY" from Object Dictionary */
uint32_t COB_IDEmergency32;
ODR_t odRet;
odRet = OD_get_u32(OD_1014_cobIdEm, 0, &COB_IDEmergency32, true);
if ((odRet != ODR_OK) || ((COB_IDEmergency32 & 0x7FFFF800U) != 0U)) {
if (errInfo != NULL) { *errInfo = OD_getIndex(OD_1014_cobIdEm); }
/* don't break a program, if only value of a parameter is wrong */
if (odRet != ODR_OK) { return CO_ERROR_OD_PARAMETERS; }
}
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_CONFIGURABLE) != 0
uint16_t producerCanId = (uint16_t)(COB_IDEmergency32 & 0x7FFU);
em->producerEnabled = ((COB_IDEmergency32 & 0x80000000U) == 0U)
&& (producerCanId != 0U);
em->OD_1014_extension.object = em;
em->OD_1014_extension.read = OD_read_1014;
em->OD_1014_extension.write = OD_write_1014;
odRet = OD_extension_init(OD_1014_cobIdEm, &em->OD_1014_extension);
if (odRet != ODR_OK) {
if (errInfo != NULL) {
*errInfo = OD_getIndex(OD_1014_cobIdEm);
}
return CO_ERROR_OD_PARAMETERS;
}
/* following two variables are used inside OD_read_1014 and OD_write_1014 */
em->producerCanId = producerCanId;
em->CANdevTxIdx = CANdevTxIdx;
/* if default producerCanId is used, then value of CO_CAN_ID_EMERGENCY
* (0x80) is stored into non-volatile memory. In that case it is necessary
* to add nodeId of this node to the stored value. */
if (producerCanId == CO_CAN_ID_EMERGENCY) {
producerCanId += nodeId;
}
#else
uint16_t producerCanId = CO_CAN_ID_EMERGENCY + (uint16_t)nodeId;
em->producerEnabled = (COB_IDEmergency32 & 0x80000000U) == 0U;
em->OD_1014_extension.object = em;
em->OD_1014_extension.read = OD_read_1014_default;
em->OD_1014_extension.write = OD_writeOriginal;
odRet = OD_extension_init(OD_1014_cobIdEm, &em->OD_1014_extension);
if (odRet != ODR_OK) {
if (errInfo != NULL) { *errInfo = OD_getIndex(OD_1014_cobIdEm); }
return CO_ERROR_OD_PARAMETERS;
}
#endif
/* configure parameters and emergency message CAN transmission */
em->nodeId = nodeId;
em->CANtxBuff = CO_CANtxBufferInit(
CANdevTx,
CANdevTxIdx,
producerCanId,
false,
8U,
false);
if (em->CANtxBuff == NULL) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT) != 0
/* get and verify optional "Inhibit time EMCY" from Object Dictionary */
em->inhibitEmTime_us = 0;
em->inhibitEmTimer = 0;
uint16_t inhibitTime_100us;
odRet = OD_get_u16(OD_1015_InhTime, 0, &inhibitTime_100us, true);
if (odRet == ODR_OK) {
em->inhibitEmTime_us = (uint32_t)inhibitTime_100us * 100U;
em->OD_1015_extension.object = em;
em->OD_1015_extension.read = OD_readOriginal;
em->OD_1015_extension.write = OD_write_1015;
(void)OD_extension_init(OD_1015_InhTime, &em->OD_1015_extension);
}
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT */
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER */
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_HISTORY) != 0
/* If OD entry available, make access to em->preDefErr */
em->OD_1003_extension.object = em;
em->OD_1003_extension.read = OD_read_1003;
em->OD_1003_extension.write = OD_write_1003;
(void)OD_extension_init(OD_1003_preDefErr, &em->OD_1003_extension);
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_HISTORY */
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_STATUS_BITS) != 0
/* If OD entry available, make access to em->errorStatusBits */
em->OD_statusBits_extension.object = em;
em->OD_statusBits_extension.read = OD_read_statusBits;
em->OD_statusBits_extension.write = OD_write_statusBits;
(void)OD_extension_init(OD_statusBits, &em->OD_statusBits_extension);
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_STATUS_BITS */
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) != 0
em->pFunctSignalRx = NULL;
/* configure SDO server CAN reception */
ret = CO_CANrxBufferInit(
CANdevRx,
CANdevRxIdx,
CO_CAN_ID_EMERGENCY,
0x780,
false,
(void*)em,
CO_EM_receive);
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER */
return ret;
}
/******************************************************************************/
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) != 0
void CO_EM_initCallbackRx(CO_EM_t *em,
void (*pFunctSignalRx)(const uint16_t ident,
const uint16_t errorCode,
const uint8_t errorRegister,
const uint8_t errorBit,
const uint32_t infoCode))
{
if (em != NULL) {
em->pFunctSignalRx = pFunctSignalRx;
}
}
#endif
#if ((CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE) != 0
void CO_EM_initCallbackPre(CO_EM_t *em,
void *object,
void (*pFunctSignal)(void *object))
{
if (em != NULL) {
em->functSignalObjectPre = object;
em->pFunctSignalPre = pFunctSignal;
}
}
#endif
/******************************************************************************/
void CO_EM_process(CO_EM_t *em,
bool_t NMTisPreOrOperational,
uint32_t timeDifference_us,
uint32_t *timerNext_us)
{
(void)timerNext_us; /* may be unused */
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT) == 0
(void)timeDifference_us; /* may be unused */
#endif
/* verify errors from driver */
uint16_t CANerrSt = em->CANdevTx->CANerrorStatus;
if (CANerrSt != em->CANerrorStatusOld) {
uint16_t CANerrStChanged = CANerrSt ^ em->CANerrorStatusOld;
em->CANerrorStatusOld = CANerrSt;
if ((CANerrStChanged & (CO_CAN_ERRTX_WARNING | CO_CAN_ERRRX_WARNING)) != 0U) {
CO_error(em,
(CANerrSt & (CO_CAN_ERRTX_WARNING | CO_CAN_ERRRX_WARNING)) != 0U,
CO_EM_CAN_BUS_WARNING, CO_EMC_NO_ERROR, 0);
}
if ((CANerrStChanged & CO_CAN_ERRTX_PASSIVE) != 0U) {
CO_error(em, (CANerrSt & CO_CAN_ERRTX_PASSIVE) != 0U,
CO_EM_CAN_TX_BUS_PASSIVE, CO_EMC_CAN_PASSIVE, 0);
}
if ((CANerrStChanged & CO_CAN_ERRTX_BUS_OFF) != 0U) {
CO_error(em, (CANerrSt & CO_CAN_ERRTX_BUS_OFF) != 0U,
CO_EM_CAN_TX_BUS_OFF, CO_EMC_BUS_OFF_RECOVERED, 0);
}
if ((CANerrStChanged & CO_CAN_ERRTX_OVERFLOW) != 0U) {
CO_error(em, (CANerrSt & CO_CAN_ERRTX_OVERFLOW) != 0U,
CO_EM_CAN_TX_OVERFLOW, CO_EMC_CAN_OVERRUN, 0);
}
if ((CANerrStChanged & CO_CAN_ERRTX_PDO_LATE) != 0U) {
CO_error(em, (CANerrSt & CO_CAN_ERRTX_PDO_LATE) != 0U,
CO_EM_TPDO_OUTSIDE_WINDOW, CO_EMC_COMMUNICATION, 0);
}
if ((CANerrStChanged & CO_CAN_ERRRX_PASSIVE) != 0U) {
CO_error(em, (CANerrSt & CO_CAN_ERRRX_PASSIVE) != 0U,
CO_EM_CAN_RX_BUS_PASSIVE, CO_EMC_CAN_PASSIVE, 0);
}
if ((CANerrStChanged & CO_CAN_ERRRX_OVERFLOW) != 0U) {
CO_error(em, (CANerrSt & CO_CAN_ERRRX_OVERFLOW) != 0U,
CO_EM_CAN_RXB_OVERFLOW, CO_EMC_CAN_OVERRUN, 0);
}
}
/* calculate Error register */
uint8_t errorRegister = 0U;
if (CO_CONFIG_ERR_CONDITION_GENERIC) {
errorRegister |= (uint8_t)CO_ERR_REG_GENERIC_ERR;
}
#ifdef CO_CONFIG_ERR_CONDITION_CURRENT
if (CO_CONFIG_ERR_CONDITION_CURRENT) {
errorRegister |= (uint8_t)CO_ERR_REG_CURRENT;
}
#endif
#ifdef CO_CONFIG_ERR_CONDITION_VOLTAGE
if (CO_CONFIG_ERR_CONDITION_VOLTAGE) {
errorRegister |= (uint8_t)CO_ERR_REG_VOLTAGE;
}
#endif
#ifdef CO_CONFIG_ERR_CONDITION_TEMPERATURE
if (CO_CONFIG_ERR_CONDITION_TEMPERATURE) {
errorRegister |= (uint8_t)CO_ERR_REG_TEMPERATURE;
}
#endif
if (CO_CONFIG_ERR_CONDITION_COMMUNICATION) {
errorRegister |= (uint8_t)CO_ERR_REG_COMMUNICATION;
}
#ifdef CO_CONFIG_ERR_CONDITION_DEV_PROFILE
if (CO_CONFIG_ERR_CONDITION_DEV_PROFILE) {
errorRegister |= (uint8_t)CO_ERR_REG_DEV_PROFILE;
}
#endif
if (CO_CONFIG_ERR_CONDITION_MANUFACTURER) {
errorRegister |= (uint8_t)CO_ERR_REG_MANUFACTURER;
}
*em->errorRegister = errorRegister;
if (!NMTisPreOrOperational) {
return;
}
/* post-process Emergency message in fifo buffer. */
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) != 0
if (em->fifoSize >= 2U) {
uint8_t fifoPpPtr = em->fifoPpPtr;
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT) != 0
if (em->inhibitEmTimer < em->inhibitEmTime_us) {
em->inhibitEmTimer += timeDifference_us;
}
if (!em->CANtxBuff->bufferFull && (fifoPpPtr != em->fifoWrPtr)
&& (em->inhibitEmTimer >= em->inhibitEmTime_us)
) {
em->inhibitEmTimer = 0;
#else
if ((!em->CANtxBuff->bufferFull) && (fifoPpPtr != em->fifoWrPtr)) {
#endif
/* add error register to emergency message */
em->fifo[fifoPpPtr].msg |= (uint32_t) errorRegister << 16;
/* send emergency message */
(void)memcpy((void *)em->CANtxBuff->data, (void *)&em->fifo[fifoPpPtr].msg,
sizeof(em->CANtxBuff->data));
(void)CO_CANsend(em->CANdevTx, em->CANtxBuff);
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) != 0
/* report also own emergency messages */
if (em->pFunctSignalRx != NULL) {
uint32_t errMsg = em->fifo[fifoPpPtr].msg;
em->pFunctSignalRx(0,
CO_SWAP_16((uint16_t) errMsg),
errorRegister,
(uint8_t) (errMsg >> 24),
CO_SWAP_32(em->fifo[fifoPpPtr].info));
}
#endif
/* increment pointer */
fifoPpPtr++;
em->fifoPpPtr = (fifoPpPtr < em->fifoSize) ? fifoPpPtr : 0U;
/* verify message buffer overflow. Clear error condition if all
* messages from fifo buffer are processed */
if (em->fifoOverflow == 1U) {
em->fifoOverflow = 2;
CO_errorReport(em, CO_EM_EMERGENCY_BUFFER_FULL,
CO_EMC_GENERIC, 0);
}
else if ((em->fifoOverflow == 2U) && (em->fifoPpPtr == em->fifoWrPtr)) {
em->fifoOverflow = 0;
CO_errorReset(em, CO_EM_EMERGENCY_BUFFER_FULL, 0);
}
else { /* MISRA C 2004 14.10 */ }
}
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT) != 0
#if ((CO_CONFIG_EM) & CO_CONFIG_FLAG_TIMERNEXT) != 0
else if ((timerNext_us != NULL)
&& (em->inhibitEmTimer < em->inhibitEmTime_us))
{
/* check again after inhibit time elapsed */
uint32_t diff = em->inhibitEmTime_us - em->inhibitEmTimer;
if (*timerNext_us > diff) {
*timerNext_us = diff;
}
}
else { /* MISRA C 2004 14.10 */ }
#endif
#endif
}
#elif ((CO_CONFIG_EM) & CO_CONFIG_EM_HISTORY) != 0
if (em->fifoSize >= 2) {
uint8_t fifoPpPtr = em->fifoPpPtr;
while (fifoPpPtr != em->fifoWrPtr) {
/* add error register to emergency message and increment pointers */
em->fifo[fifoPpPtr].msg |= (uint32_t) errorRegister << 16;
if (++fifoPpPtr >= em->fifoSize) {
fifoPpPtr = 0;
}
}
em->fifoPpPtr = fifoPpPtr;
}
#endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER, #elif CO_CONFIG_EM_HISTORY */
return;
}
/******************************************************************************/
void CO_error(CO_EM_t *em, bool_t setError, const uint8_t errorBit,
uint16_t errorCode, uint32_t infoCode)
{
if (em == NULL) { return; }
uint8_t index = errorBit >> 3;
uint8_t bitmask = 1U << (errorBit & 0x7U);
/* if unsupported errorBit, change to 'CO_EM_WRONG_ERROR_REPORT' */
if (index >= (CO_CONFIG_EM_ERR_STATUS_BITS_COUNT / 8U)) {
index = CO_EM_WRONG_ERROR_REPORT >> 3;
bitmask = 1U << (CO_EM_WRONG_ERROR_REPORT & 0x7U);
errorCode = CO_EMC_SOFTWARE_INTERNAL;
infoCode = errorBit;
}
uint8_t *errorStatusBits = &em->errorStatusBits[index];
uint8_t errorStatusBitMasked = *errorStatusBits & bitmask;
/* If error is already set (or unset), return without further actions,
* otherwise toggle bit and continue with error indication. */
if (setError) {
if (errorStatusBitMasked != 0U) {
return;
}
}
else {
if (errorStatusBitMasked == 0U) {
return;
}
errorCode = CO_EMC_NO_ERROR;
}
#if ((CO_CONFIG_EM) & (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)) != 0
/* prepare emergency message. Error register will be added in post-process*/
uint32_t errMsg = ((uint32_t)errorBit << 24) | CO_SWAP_16(errorCode);
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) != 0
uint32_t infoCodeSwapped = CO_SWAP_32(infoCode);
#endif
#endif
/* safely write data, and increment pointers */
CO_LOCK_EMCY(em->CANdevTx);
if (setError) { *errorStatusBits |= bitmask; }
else { *errorStatusBits &= ~bitmask; }
#if ((CO_CONFIG_EM) & (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)) != 0
if (em->fifoSize >= 2U) {
uint8_t fifoWrPtr = em->fifoWrPtr;
uint8_t fifoWrPtrNext = fifoWrPtr + 1U;
if (fifoWrPtrNext >= em->fifoSize) {
fifoWrPtrNext = 0;
}
if (fifoWrPtrNext == em->fifoPpPtr) {
em->fifoOverflow = 1;
}
else {
em->fifo[fifoWrPtr].msg = errMsg;
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) != 0
em->fifo[fifoWrPtr].info = infoCodeSwapped;
#endif
em->fifoWrPtr = fifoWrPtrNext;
if (em->fifoCount < (em->fifoSize - 1U)) { em->fifoCount++; }
}
}
#endif /* (CO_CONFIG_EM) & (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY) */
CO_UNLOCK_EMCY(em->CANdevTx);
#if ((CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE) != 0
#if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) != 0
/* Optional signal to RTOS, which can resume task, which handles
* CO_EM_process */
if ((em->pFunctSignalPre != NULL) && em->producerEnabled) {
em->pFunctSignalPre(em->functSignalObjectPre);
}
#endif
#endif
}
View git blame Copy permalink