Embedded_SDK
/
MindSDK_MM32F5270
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
MindSDK_MM32F5270/components/canopen/CO_HBconsumer.c

421 lines
14 KiB
C
Raw Blame History

/*
* CANopen Heartbeat consumer object.
*
* @file CO_HBconsumer.c
* @ingroup CO_HBconsumer
* @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 "CANopen.h"
#include "CO_HBconsumer.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_HBcons_receive(void *object, const CO_CANrxMsg_t *msg){
CO_HBconsNode_t *HBconsNode;
HBconsNode = (CO_HBconsNode_t*) object; /* this is the correct pointer type of the first argument */
/* verify message length */
if(msg->DLC == 1){
/* copy data and set 'new message' flag. */
HBconsNode->NMTstate = (CO_NMT_internalState_t)msg->data[0];
SET_CANrxNew(HBconsNode->CANrxNew);
}
}
/*
* Configure one monitored node.
*/
static void CO_HBcons_monitoredNodeConfig(
CO_HBconsumer_t *HBcons,
uint8_t idx,
uint8_t nodeId,
uint16_t time)
{
uint16_t COB_ID;
CO_HBconsNode_t *monitoredNode;
if(idx >= HBcons->numberOfMonitoredNodes) return;
monitoredNode = &HBcons->monitoredNodes[idx];
monitoredNode->nodeId = nodeId;
monitoredNode->time = time;
monitoredNode->NMTstate = CO_NMT_INITIALIZING;
monitoredNode->HBstate = CO_HBconsumer_UNCONFIGURED;
/* is channel used */
if(monitoredNode->nodeId && monitoredNode->time){
COB_ID = monitoredNode->nodeId + CO_CAN_ID_HEARTBEAT;
monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
}
else{
COB_ID = 0;
monitoredNode->time = 0;
}
/* configure Heartbeat consumer CAN reception */
if (monitoredNode->HBstate != CO_HBconsumer_UNCONFIGURED) {
CO_CANrxBufferInit(
HBcons->CANdevRx,
HBcons->CANdevRxIdxStart + idx,
COB_ID,
0x7FF,
0,
(void*)&HBcons->monitoredNodes[idx],
CO_HBcons_receive);
}
}
/*
* OD function for accessing _Consumer Heartbeat Time_ (index 0x1016) from SDO server.
*
* For more information see file CO_SDO.h.
*/
static CO_SDO_abortCode_t CO_ODF_1016(CO_ODF_arg_t *ODF_arg)
{
CO_HBconsumer_t *HBcons;
uint8_t NodeID;
uint16_t HBconsTime;
uint32_t value;
CO_ReturnError_t ret;
if(ODF_arg->reading){
return CO_SDO_AB_NONE;
}
HBcons = (CO_HBconsumer_t*) ODF_arg->object;
value = CO_getUint32(ODF_arg->data);
NodeID = (value >> 16U) & 0xFFU;
HBconsTime = value & 0xFFFFU;
if((value & 0xFF800000U) != 0){
return CO_SDO_AB_PRAM_INCOMPAT;
}
ret = CO_HBconsumer_initEntry(HBcons, ODF_arg->subIndex-1U, NodeID, HBconsTime);
if (ret != CO_ERROR_NO) {
return CO_SDO_AB_PRAM_INCOMPAT;
}
return CO_SDO_AB_NONE;
}
/******************************************************************************/
CO_ReturnError_t CO_HBconsumer_init(
CO_HBconsumer_t *HBcons,
CO_EM_t *em,
CO_SDO_t *SDO,
const uint32_t HBconsTime[],
CO_HBconsNode_t monitoredNodes[],
uint8_t numberOfMonitoredNodes,
CO_CANmodule_t *CANdevRx,
uint16_t CANdevRxIdxStart)
{
uint8_t i;
/* verify arguments */
if(HBcons==NULL || em==NULL || SDO==NULL || HBconsTime==NULL ||
monitoredNodes==NULL || CANdevRx==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* Configure object variables */
HBcons->em = em;
HBcons->HBconsTime = HBconsTime;
HBcons->monitoredNodes = monitoredNodes;
HBcons->numberOfMonitoredNodes = numberOfMonitoredNodes;
HBcons->allMonitoredOperational = 0;
HBcons->CANdevRx = CANdevRx;
HBcons->CANdevRxIdxStart = CANdevRxIdxStart;
for(i=0; i<HBcons->numberOfMonitoredNodes; i++) {
uint8_t nodeId = (HBcons->HBconsTime[i] >> 16U) & 0xFFU;
uint16_t time = HBcons->HBconsTime[i] & 0xFFFFU;
CO_HBconsumer_initEntry(HBcons, i, nodeId, time);
}
/* Configure Object dictionary entry at index 0x1016 */
CO_OD_configure(SDO, OD_H1016_CONSUMER_HB_TIME, CO_ODF_1016, (void*)HBcons, 0, 0);
return CO_ERROR_NO;
}
/******************************************************************************/
CO_ReturnError_t CO_HBconsumer_initEntry(
CO_HBconsumer_t *HBcons,
uint8_t idx,
uint8_t nodeId,
uint16_t consumerTime)
{
CO_ReturnError_t ret = CO_ERROR_NO;
/* verify arguments */
if(HBcons==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
if((consumerTime != 0) && (nodeId != 0)){
uint8_t i;
/* there must not be more entries with same index and time different than zero */
for(i = 0U; i<HBcons->numberOfMonitoredNodes; i++){
uint32_t objectCopy = HBcons->HBconsTime[i];
uint8_t NodeIDObj = (objectCopy >> 16U) & 0xFFU;
uint16_t HBconsTimeObj = objectCopy & 0xFFFFU;
if((idx != i) && (HBconsTimeObj != 0) && (nodeId == NodeIDObj)){
ret = CO_ERROR_ILLEGAL_ARGUMENT;
}
}
}
/* Configure */
if(ret == CO_ERROR_NO){
CO_HBcons_monitoredNodeConfig(HBcons, idx, nodeId, consumerTime);
}
return ret;
}
/******************************************************************************/
void CO_HBconsumer_initCallbackHeartbeatStarted(
CO_HBconsumer_t *HBcons,
uint8_t idx,
void *object,
void (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
{
CO_HBconsNode_t *monitoredNode;
if (HBcons==NULL || idx>HBcons->numberOfMonitoredNodes) {
return;
}
monitoredNode = &HBcons->monitoredNodes[idx];
monitoredNode->pFunctSignalHbStarted = pFunctSignal;
monitoredNode->functSignalObjectHbStarted = object;
}
/******************************************************************************/
void CO_HBconsumer_initCallbackTimeout(
CO_HBconsumer_t *HBcons,
uint8_t idx,
void *object,
void (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
{
CO_HBconsNode_t *monitoredNode;
if (HBcons==NULL || idx>HBcons->numberOfMonitoredNodes) {
return;
}
monitoredNode = &HBcons->monitoredNodes[idx];
monitoredNode->pFunctSignalTimeout = pFunctSignal;
monitoredNode->functSignalObjectTimeout = object;
}
/******************************************************************************/
void CO_HBconsumer_initCallbackRemoteReset(
CO_HBconsumer_t *HBcons,
uint8_t idx,
void *object,
void (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
{
CO_HBconsNode_t *monitoredNode;
if (HBcons==NULL || idx>HBcons->numberOfMonitoredNodes) {
return;
}
monitoredNode = &HBcons->monitoredNodes[idx];
monitoredNode->pFunctSignalRemoteReset = pFunctSignal;
monitoredNode->functSignalObjectRemoteReset = object;
}
/******************************************************************************/
void CO_HBconsumer_process(
CO_HBconsumer_t *HBcons,
bool_t NMTisPreOrOperational,
uint16_t timeDifference_ms)
{
uint8_t i;
uint8_t emcyHeartbeatTimeoutActive = 0;
uint8_t emcyRemoteResetActive = 0;
uint8_t AllMonitoredOperationalCopy;
CO_HBconsNode_t *monitoredNode;
AllMonitoredOperationalCopy = 5;
monitoredNode = &HBcons->monitoredNodes[0];
if(NMTisPreOrOperational){
for(i=0; i<HBcons->numberOfMonitoredNodes; i++){
if(monitoredNode->time > 0){/* is node monitored */
/* Verify if received message is heartbeat or bootup */
if(IS_CANrxNew(monitoredNode->CANrxNew)){
if(monitoredNode->NMTstate == CO_NMT_INITIALIZING){
/* bootup message, call callback */
if (monitoredNode->pFunctSignalRemoteReset != NULL) {
monitoredNode->pFunctSignalRemoteReset(monitoredNode->nodeId, i,
monitoredNode->functSignalObjectRemoteReset);
}
}
else {
/* heartbeat message */
if (monitoredNode->HBstate!=CO_HBconsumer_ACTIVE &&
monitoredNode->pFunctSignalHbStarted!=NULL) {
monitoredNode->pFunctSignalHbStarted(monitoredNode->nodeId, i,
monitoredNode->functSignalObjectHbStarted);
}
monitoredNode->HBstate = CO_HBconsumer_ACTIVE;
monitoredNode->timeoutTimer = 0; /* reset timer */
timeDifference_ms = 0;
}
CLEAR_CANrxNew(monitoredNode->CANrxNew);
}
/* Verify timeout */
if(monitoredNode->timeoutTimer < monitoredNode->time) {
monitoredNode->timeoutTimer += timeDifference_ms;
}
if(monitoredNode->HBstate!=CO_HBconsumer_UNCONFIGURED &&
monitoredNode->HBstate!=CO_HBconsumer_UNKNOWN) {
if(monitoredNode->timeoutTimer >= monitoredNode->time){
/* timeout expired */
CO_errorReport(HBcons->em, CO_EM_HEARTBEAT_CONSUMER, CO_EMC_HEARTBEAT, i);
emcyHeartbeatTimeoutActive = 1;
monitoredNode->NMTstate = CO_NMT_INITIALIZING;
if (monitoredNode->HBstate!=CO_HBconsumer_TIMEOUT &&
monitoredNode->pFunctSignalTimeout!=NULL) {
monitoredNode->pFunctSignalTimeout(monitoredNode->nodeId, i,
monitoredNode->functSignalObjectTimeout);
}
monitoredNode->HBstate = CO_HBconsumer_TIMEOUT;
}
else if(monitoredNode->NMTstate == CO_NMT_INITIALIZING){
/* there was a bootup message */
CO_errorReport(HBcons->em, CO_EM_HB_CONSUMER_REMOTE_RESET, CO_EMC_HEARTBEAT, i);
emcyRemoteResetActive = 1;
monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
}
}
if(monitoredNode->NMTstate != CO_NMT_OPERATIONAL) {
AllMonitoredOperationalCopy = 0;
}
}
monitoredNode++;
}
}
else{ /* not in (pre)operational state */
for(i=0; i<HBcons->numberOfMonitoredNodes; i++){
monitoredNode->NMTstate = CO_NMT_INITIALIZING;
CLEAR_CANrxNew(monitoredNode->CANrxNew);
if(monitoredNode->HBstate != CO_HBconsumer_UNCONFIGURED){
monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
}
monitoredNode++;
}
AllMonitoredOperationalCopy = 0;
}
/* clear emergencies. We only have one emergency index for all
* monitored nodes! */
if ( ! emcyHeartbeatTimeoutActive) {
CO_errorReset(HBcons->em, CO_EM_HEARTBEAT_CONSUMER, 0);
}
if ( ! emcyRemoteResetActive) {
CO_errorReset(HBcons->em, CO_EM_HB_CONSUMER_REMOTE_RESET, 0);
}
HBcons->allMonitoredOperational = AllMonitoredOperationalCopy;
}
/******************************************************************************/
int8_t CO_HBconsumer_getIdxByNodeId(
CO_HBconsumer_t *HBcons,
uint8_t nodeId)
{
uint8_t i;
CO_HBconsNode_t *monitoredNode;
if (HBcons == NULL) {
return -1;
}
/* linear search for the node */
monitoredNode = &HBcons->monitoredNodes[0];
for(i=0; i<HBcons->numberOfMonitoredNodes; i++){
if (monitoredNode->nodeId == nodeId) {
return i;
}
monitoredNode ++;
}
/* not found */
return -1;
}
/******************************************************************************/
CO_HBconsumer_state_t CO_HBconsumer_getState(
CO_HBconsumer_t *HBcons,
uint8_t idx)
{
CO_HBconsNode_t *monitoredNode;
if (HBcons==NULL || idx>HBcons->numberOfMonitoredNodes) {
return CO_HBconsumer_UNCONFIGURED;
}
monitoredNode = &HBcons->monitoredNodes[idx];
return monitoredNode->HBstate;
}
/******************************************************************************/
int8_t CO_HBconsumer_getNmtState(
CO_HBconsumer_t *HBcons,
uint8_t idx,
CO_NMT_internalState_t *nmtState)
{
CO_HBconsNode_t *monitoredNode;
if (HBcons==NULL || nmtState==NULL || idx>HBcons->numberOfMonitoredNodes) {
return -1;
}
*nmtState = CO_NMT_INITIALIZING;
monitoredNode = &HBcons->monitoredNodes[idx];
if (monitoredNode->HBstate == CO_HBconsumer_ACTIVE) {
*nmtState = monitoredNode->NMTstate;
return 0;
}
return -1;
}
Reference in New Issue View Git Blame Copy Permalink