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

353 lines
13 KiB
C
Raw Blame History

/*
* CANopen NMT and Heartbeat producer object.
*
* @file CO_NMT_Heartbeat.c
* @ingroup CO_NMT_Heartbeat
* @author Janez Paternoster
* @copyright 2004 - 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 "301/CO_NMT_Heartbeat.h"
/*
* Read received message from CAN module.
*
* Function will be called (by CAN receive interrupt) every time, when CAN
* message with correct identifier will be received. For more information and
* description of parameters see file CO_driver.h.
*/
static void CO_NMT_receive(void *object, void *msg) {
uint8_t DLC = CO_CANrxMsg_readDLC(msg);
uint8_t *data = CO_CANrxMsg_readData(msg);
uint8_t command = data[0];
uint8_t nodeId = data[1];
CO_NMT_t *NMT = (CO_NMT_t*)object;
if (DLC == 2 && (nodeId == 0 || nodeId == NMT->nodeId)) {
NMT->internalCommand = command;
#if (CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE
/* Optional signal to RTOS, which can resume task, which handles NMT. */
if (NMT->pFunctSignalPre != NULL) {
NMT->pFunctSignalPre(NMT->functSignalObjectPre);
}
#endif
}
}
/*
* Custom function for writing OD object "Producer heartbeat time"
*
* For more information see file CO_ODinterface.h, OD_IO_t.
*/
static OD_size_t OD_write_1017(OD_stream_t *stream, uint8_t subIndex,
const void *buf, OD_size_t count,
ODR_t *returnCode)
{
/* "count" is already verified in *_init() function */
if (stream == NULL || subIndex != 0 || buf == NULL || returnCode == NULL) {
if (returnCode != NULL) *returnCode = ODR_DEV_INCOMPAT;
return 0;
}
CO_NMT_t *NMT = (CO_NMT_t *)stream->object;
*returnCode = ODR_OK;
/* update object, send Heartbeat immediately */
NMT->HBproducerTime_us = (uint32_t)CO_getUint16(buf) * 1000;
NMT->HBproducerTimer = 0;
/* write value to the original location in the Object Dictionary */
return OD_writeOriginal(stream, subIndex, buf, count, returnCode);
}
/******************************************************************************/
CO_ReturnError_t CO_NMT_init(CO_NMT_t *NMT,
OD_entry_t *OD_1017_ProducerHbTime,
CO_EM_t *em,
uint8_t nodeId,
CO_NMT_control_t NMTcontrol,
uint16_t firstHBTime_ms,
CO_CANmodule_t *NMT_CANdevRx,
uint16_t NMT_rxIdx,
uint16_t CANidRxNMT,
#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER
CO_CANmodule_t *NMT_CANdevTx,
uint16_t NMT_txIdx,
uint16_t CANidTxNMT,
#endif
CO_CANmodule_t *HB_CANdevTx,
uint16_t HB_txIdx,
uint16_t CANidTxHB,
uint32_t *errInfo)
{
CO_ReturnError_t ret = CO_ERROR_NO;
/* verify arguments */
if (NMT == NULL || OD_1017_ProducerHbTime == NULL || em == NULL
|| NMT_CANdevRx == NULL || HB_CANdevTx == NULL
#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER
|| NMT_CANdevTx == NULL
#endif
) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* clear the object */
memset(NMT, 0, sizeof(CO_NMT_t));
/* Configure object variables */
NMT->operatingState = CO_NMT_INITIALIZING;
NMT->operatingStatePrev = CO_NMT_INITIALIZING;
NMT->nodeId = nodeId;
NMT->NMTcontrol = NMTcontrol;
NMT->em = em;
NMT->HBproducerTimer = (int32_t)firstHBTime_ms * 1000;
/* get and verify required "Producer heartbeat time" from Object Dict. */
uint16_t HBprodTime_ms;
ODR_t odRet = OD_get_u16(OD_1017_ProducerHbTime, 0, &HBprodTime_ms, true);
if (odRet != ODR_OK) {
if (errInfo != NULL) *errInfo = OD_getIndex(OD_1017_ProducerHbTime);
return CO_ERROR_OD_PARAMETERS;
}
NMT->HBproducerTime_us = (uint32_t)HBprodTime_ms * 1000;
NMT->OD_1017_extension.object = NMT;
NMT->OD_1017_extension.read = OD_readOriginal;
NMT->OD_1017_extension.write = OD_write_1017;
odRet = OD_extension_init(OD_1017_ProducerHbTime, &NMT->OD_1017_extension);
if (odRet != ODR_OK) {
if (errInfo != NULL) *errInfo = OD_getIndex(OD_1017_ProducerHbTime);
return CO_ERROR_OD_PARAMETERS;
}
if (NMT->HBproducerTimer > NMT->HBproducerTime_us) {
NMT->HBproducerTimer = NMT->HBproducerTime_us;
}
/* configure NMT CAN reception */
ret = CO_CANrxBufferInit(
NMT_CANdevRx, /* CAN device */
NMT_rxIdx, /* rx buffer index */
CANidRxNMT, /* CAN identifier */
0x7FF, /* mask */
0, /* rtr */
(void*)NMT, /* object passed to receive function */
CO_NMT_receive); /* this function will process received message*/
if (ret != CO_ERROR_NO) {
return ret;
}
#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER
/* configure NMT CAN transmission */
NMT->NMT_CANdevTx = NMT_CANdevTx;
NMT->NMT_TXbuff = CO_CANtxBufferInit(
NMT_CANdevTx, /* CAN device */
NMT_txIdx, /* index of specific buffer inside CAN module */
CANidTxNMT, /* CAN identifier */
0, /* rtr */
2, /* number of data bytes */
0); /* synchronous message flag bit */
if (NMT->NMT_TXbuff == NULL) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
#endif
/* configure HB CAN transmission */
NMT->HB_CANdevTx = HB_CANdevTx;
NMT->HB_TXbuff = CO_CANtxBufferInit(
HB_CANdevTx, /* CAN device */
HB_txIdx, /* index of specific buffer inside CAN module */
CANidTxHB, /* CAN identifier */
0, /* rtr */
1, /* number of data bytes */
0); /* synchronous message flag bit */
if (NMT->HB_TXbuff == NULL) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
return ret;
}
#if (CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE
void CO_NMT_initCallbackPre(CO_NMT_t *NMT,
void *object,
void (*pFunctSignal)(void *object))
{
if (NMT != NULL) {
NMT->pFunctSignalPre = pFunctSignal;
NMT->functSignalObjectPre = object;
}
}
#endif
#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE
/******************************************************************************/
void CO_NMT_initCallbackChanged(CO_NMT_t *NMT,
void (*pFunctNMT)(CO_NMT_internalState_t state))
{
if (NMT != NULL) {
NMT->pFunctNMT = pFunctNMT;
if (NMT->pFunctNMT != NULL) {
NMT->pFunctNMT(NMT->operatingState);
}
}
}
#endif
/******************************************************************************/
CO_NMT_reset_cmd_t CO_NMT_process(CO_NMT_t *NMT,
CO_NMT_internalState_t *NMTstate,
uint32_t timeDifference_us,
uint32_t *timerNext_us)
{
(void)timerNext_us; /* may be unused */
CO_NMT_internalState_t NMTstateCpy = NMT->operatingState;
CO_NMT_reset_cmd_t resetCommand = CO_RESET_NOT;
bool_t NNTinit = NMTstateCpy == CO_NMT_INITIALIZING;
NMT->HBproducerTimer = (NMT->HBproducerTimer > timeDifference_us )
? (NMT->HBproducerTimer - timeDifference_us) : 0;
/* Send heartbeat producer message if:
* - First start, send bootup message or
* - HB producer enabled and: Timer expired or NMT->operatingState changed*/
if (NNTinit || (NMT->HBproducerTime_us != 0
&& (NMT->HBproducerTimer == 0
|| NMTstateCpy != NMT->operatingStatePrev)
)) {
NMT->HB_TXbuff->data[0] = (uint8_t) NMTstateCpy;
CO_CANsend(NMT->HB_CANdevTx, NMT->HB_TXbuff);
if (NMTstateCpy == CO_NMT_INITIALIZING) {
/* NMT slave self starting */
NMTstateCpy = (NMT->NMTcontrol & CO_NMT_STARTUP_TO_OPERATIONAL) != 0
? CO_NMT_OPERATIONAL : CO_NMT_PRE_OPERATIONAL;
}
else {
/* Start timer from the beginning. If OS is slow, time sliding may
* occur. However, heartbeat is not for synchronization, it is for
* health report. In case of initializing, timer is set in the
* CO_NMT_init() function with pre-defined value. */
NMT->HBproducerTimer = NMT->HBproducerTime_us;
}
}
NMT->operatingStatePrev = NMTstateCpy;
/* process internal NMT commands, received from CO_NMT_receive() or
* CO_NMT_sendCommand() */
if (NMT->internalCommand != 0) {
switch (NMT->internalCommand) {
case CO_NMT_ENTER_OPERATIONAL:
NMTstateCpy = CO_NMT_OPERATIONAL;
break;
case CO_NMT_ENTER_STOPPED:
NMTstateCpy = CO_NMT_STOPPED;
break;
case CO_NMT_ENTER_PRE_OPERATIONAL:
NMTstateCpy = CO_NMT_PRE_OPERATIONAL;
break;
case CO_NMT_RESET_NODE:
resetCommand = CO_RESET_APP;
break;
case CO_NMT_RESET_COMMUNICATION:
resetCommand = CO_RESET_COMM;
break;
default:
break;
}
NMT->internalCommand = 0;
}
/* verify NMT transitions based on error register */
bool_t busOff_HB = (NMT->NMTcontrol & CO_NMT_ERR_ON_BUSOFF_HB) != 0
&& (CO_isError(NMT->em, CO_EM_CAN_TX_BUS_OFF)
|| CO_isError(NMT->em, CO_EM_HEARTBEAT_CONSUMER)
|| CO_isError(NMT->em, CO_EM_HB_CONSUMER_REMOTE_RESET));
bool_t errRegMasked = (NMT->NMTcontrol & CO_NMT_ERR_ON_ERR_REG) != 0
&& (CO_getErrorRegister(NMT->em) & NMT->NMTcontrol) != 0;
if (NMTstateCpy == CO_NMT_OPERATIONAL && (busOff_HB || errRegMasked)) {
NMTstateCpy = (NMT->NMTcontrol & CO_NMT_ERR_TO_STOPPED) != 0
? CO_NMT_STOPPED : CO_NMT_PRE_OPERATIONAL;
}
else if ((NMT->NMTcontrol & CO_NMT_ERR_FREE_TO_OPERATIONAL) != 0
&& NMTstateCpy == CO_NMT_PRE_OPERATIONAL && !busOff_HB && !errRegMasked
) {
NMTstateCpy = CO_NMT_OPERATIONAL;
}
#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE
/* Notify operating state change */
if (NMT->operatingStatePrev != NMTstateCpy || NNTinit) {
if (NMT->pFunctNMT != NULL) {
NMT->pFunctNMT(NMTstateCpy);
}
}
#endif
#if (CO_CONFIG_NMT) & CO_CONFIG_FLAG_TIMERNEXT
/* Calculate, when next Heartbeat needs to be send */
if (NMT->HBproducerTime_us != 0 && timerNext_us != NULL) {
if (NMT->operatingStatePrev != NMTstateCpy) {
*timerNext_us = 0;
}
else if (*timerNext_us > NMT->HBproducerTimer) {
*timerNext_us = NMT->HBproducerTimer;
}
}
#endif
NMT->operatingState = NMTstateCpy;
if (NMTstate != NULL) *NMTstate = NMTstateCpy;
return resetCommand;
}
#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER
/******************************************************************************/
CO_ReturnError_t CO_NMT_sendCommand(CO_NMT_t *NMT,
CO_NMT_command_t command,
uint8_t nodeID)
{
/* verify arguments */
if (NMT == NULL) {
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* Apply NMT command also to this node, if set so. */
if (nodeID == 0 || nodeID == NMT->nodeId) {
NMT->internalCommand = command;
}
/* Send NMT master message. */
NMT->NMT_TXbuff->data[0] = command;
NMT->NMT_TXbuff->data[1] = nodeID;
return CO_CANsend(NMT->NMT_CANdevTx, NMT->NMT_TXbuff);
}
#endif
View git blame Copy permalink