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

354 lines
12 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 "CO_driver.h"
#include "CO_SDO.h"
#include "CO_Emergency.h"
#include "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, const CO_CANrxMsg_t *msg){
CO_NMT_t *NMT;
uint8_t nodeId;
NMT = (CO_NMT_t*)object; /* this is the correct pointer type of the first argument */
nodeId = msg->data[1];
if((msg->DLC == 2) && ((nodeId == 0) || (nodeId == NMT->nodeId))){
uint8_t command = msg->data[0];
uint8_t currentOperatingState = NMT->operatingState;
switch(command){
case CO_NMT_ENTER_OPERATIONAL:
if((*NMT->emPr->errorRegister) == 0U){
NMT->operatingState = CO_NMT_OPERATIONAL;
}
break;
case CO_NMT_ENTER_STOPPED:
NMT->operatingState = CO_NMT_STOPPED;
break;
case CO_NMT_ENTER_PRE_OPERATIONAL:
NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
break;
case CO_NMT_RESET_NODE:
NMT->resetCommand = CO_RESET_APP;
break;
case CO_NMT_RESET_COMMUNICATION:
NMT->resetCommand = CO_RESET_COMM;
break;
default:
break;
}
if(NMT->pFunctNMT!=NULL && currentOperatingState!=NMT->operatingState){
NMT->pFunctNMT(NMT->operatingState);
}
}
}
/******************************************************************************/
CO_ReturnError_t CO_NMT_init(
CO_NMT_t *NMT,
CO_EMpr_t *emPr,
uint8_t nodeId,
uint16_t firstHBTime,
CO_CANmodule_t *NMT_CANdev,
uint16_t NMT_rxIdx,
uint16_t CANidRxNMT,
CO_CANmodule_t *HB_CANdev,
uint16_t HB_txIdx,
uint16_t CANidTxHB)
{
/* verify arguments */
if(NMT==NULL || emPr==NULL || NMT_CANdev==NULL || HB_CANdev==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* blinking bytes */
#ifdef CO_USE_LEDS
NMT->LEDflickering = 0;
NMT->LEDblinking = 0;
NMT->LEDsingleFlash = 0;
NMT->LEDdoubleFlash = 0;
NMT->LEDtripleFlash = 0;
NMT->LEDquadrupleFlash = 0;
#endif /* CO_USE_LEDS */
/* Configure object variables */
NMT->operatingState = CO_NMT_INITIALIZING;
#ifdef CO_USE_LEDS
NMT->LEDgreenRun = -1;
NMT->LEDredError = 1;
#endif /* CO_USE_LEDS */
NMT->nodeId = nodeId;
NMT->firstHBTime = firstHBTime;
NMT->resetCommand = 0;
NMT->HBproducerTimer = 0xFFFF;
NMT->emPr = emPr;
NMT->pFunctNMT = NULL;
/* configure NMT CAN reception */
CO_CANrxBufferInit(
NMT_CANdev, /* 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 */
/* configure HB CAN transmission */
NMT->HB_CANdev = HB_CANdev;
NMT->HB_TXbuff = CO_CANtxBufferInit(
HB_CANdev, /* 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 */
return CO_ERROR_NO;
}
/******************************************************************************/
void CO_NMT_initCallback(
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);
}
}
}
/******************************************************************************/
#ifdef CO_USE_LEDS
void CO_NMT_blinkingProcess50ms(CO_NMT_t *NMT){
if(++NMT->LEDflickering >= 1) NMT->LEDflickering = -1;
if(++NMT->LEDblinking >= 4) NMT->LEDblinking = -4;
if(++NMT->LEDsingleFlash >= 4) NMT->LEDsingleFlash = -20;
switch(++NMT->LEDdoubleFlash){
case 4: NMT->LEDdoubleFlash = -104; break;
case -100: NMT->LEDdoubleFlash = 100; break;
case 104: NMT->LEDdoubleFlash = -20; break;
default: break;
}
switch(++NMT->LEDtripleFlash){
case 4: NMT->LEDtripleFlash = -104; break;
case -100: NMT->LEDtripleFlash = 100; break;
case 104: NMT->LEDtripleFlash = -114; break;
case -110: NMT->LEDtripleFlash = 110; break;
case 114: NMT->LEDtripleFlash = -20; break;
default: break;
}
switch(++NMT->LEDquadrupleFlash){
case 4: NMT->LEDquadrupleFlash = -104; break;
case -100: NMT->LEDquadrupleFlash = 100; break;
case 104: NMT->LEDquadrupleFlash = -114; break;
case -110: NMT->LEDquadrupleFlash = 110; break;
case 114: NMT->LEDquadrupleFlash = -124; break;
case -120: NMT->LEDquadrupleFlash = 120; break;
case 124: NMT->LEDquadrupleFlash = -20; break;
default: break;
}
}
#endif /* CO_USE_LEDS */
/******************************************************************************/
CO_NMT_reset_cmd_t CO_NMT_process(
CO_NMT_t *NMT,
uint16_t timeDifference_ms,
uint16_t HBtime,
uint32_t NMTstartup,
uint8_t errorRegister,
const uint8_t errorBehavior[],
uint16_t *timerNext_ms)
{
uint8_t CANpassive;
uint8_t currentOperatingState = NMT->operatingState;
NMT->HBproducerTimer += timeDifference_ms;
/* Heartbeat producer message & Bootup message */
if((HBtime != 0 && NMT->HBproducerTimer >= HBtime) || NMT->operatingState == CO_NMT_INITIALIZING){
/* Start from the beginning. If OS is slow, time sliding may occur. However, heartbeat is
* not for synchronization, it is for health report. */
NMT->HBproducerTimer = 0;
NMT->HB_TXbuff->data[0] = NMT->operatingState;
CO_CANsend(NMT->HB_CANdev, NMT->HB_TXbuff);
if(NMT->operatingState == CO_NMT_INITIALIZING){
if(HBtime > NMT->firstHBTime) NMT->HBproducerTimer = HBtime - NMT->firstHBTime;
else NMT->HBproducerTimer = 0;
/* NMT slave self starting */
if (NMTstartup == 0x00000008U) NMT->operatingState = CO_NMT_OPERATIONAL;
else NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
}
}
/* Calculate, when next Heartbeat needs to be send and lower timerNext_ms if necessary. */
if(HBtime != 0 && timerNext_ms != NULL){
if(NMT->HBproducerTimer < HBtime){
uint16_t diff = HBtime - NMT->HBproducerTimer;
if(*timerNext_ms > diff){
*timerNext_ms = diff;
}
}else{
*timerNext_ms = 0;
}
}
/* CAN passive flag */
CANpassive = 0;
if(CO_isError(NMT->emPr->em, CO_EM_CAN_TX_BUS_PASSIVE) || CO_isError(NMT->emPr->em, CO_EM_CAN_RX_BUS_PASSIVE))
CANpassive = 1;
#ifdef CO_USE_LEDS
/* CANopen green RUN LED (DR 303-3) */
switch(NMT->operatingState){
case CO_NMT_STOPPED: NMT->LEDgreenRun = NMT->LEDsingleFlash; break;
case CO_NMT_PRE_OPERATIONAL: NMT->LEDgreenRun = NMT->LEDblinking; break;
case CO_NMT_OPERATIONAL: NMT->LEDgreenRun = 1; break;
default: break;
}
/* CANopen red ERROR LED (DR 303-3) */
if(CO_isError(NMT->emPr->em, CO_EM_CAN_TX_BUS_OFF))
NMT->LEDredError = 1;
else if(CO_isError(NMT->emPr->em, CO_EM_SYNC_TIME_OUT))
NMT->LEDredError = NMT->LEDtripleFlash;
else if(CO_isError(NMT->emPr->em, CO_EM_HEARTBEAT_CONSUMER) || CO_isError(NMT->emPr->em, CO_EM_HB_CONSUMER_REMOTE_RESET))
NMT->LEDredError = NMT->LEDdoubleFlash;
else if(CANpassive || CO_isError(NMT->emPr->em, CO_EM_CAN_BUS_WARNING))
NMT->LEDredError = NMT->LEDsingleFlash;
else if(errorRegister)
NMT->LEDredError = (NMT->LEDblinking>=0)?-1:1;
else
NMT->LEDredError = -1;
#endif /* CO_USE_LEDS */
/* in case of error enter pre-operational state */
if(errorBehavior && (NMT->operatingState == CO_NMT_OPERATIONAL)){
if(CANpassive && (errorBehavior[2] == 0 || errorBehavior[2] == 2)) errorRegister |= 0x10;
if(errorRegister){
/* Communication error */
if(errorRegister & CO_ERR_REG_COMM_ERR){
if(errorBehavior[1] == 0){
NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
}
else if(errorBehavior[1] == 2){
NMT->operatingState = CO_NMT_STOPPED;
}
else if(CO_isError(NMT->emPr->em, CO_EM_CAN_TX_BUS_OFF)
|| CO_isError(NMT->emPr->em, CO_EM_HEARTBEAT_CONSUMER)
|| CO_isError(NMT->emPr->em, CO_EM_HB_CONSUMER_REMOTE_RESET))
{
if(errorBehavior[0] == 0){
NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
}
else if(errorBehavior[0] == 2){
NMT->operatingState = CO_NMT_STOPPED;
}
}
}
/* Generic error */
if(errorRegister & CO_ERR_REG_GENERIC_ERR){
if (errorBehavior[3] == 0) NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
else if (errorBehavior[3] == 2) NMT->operatingState = CO_NMT_STOPPED;
}
/* Device profile error */
if(errorRegister & CO_ERR_REG_DEV_PROFILE){
if (errorBehavior[4] == 0) NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
else if (errorBehavior[4] == 2) NMT->operatingState = CO_NMT_STOPPED;
}
/* Manufacturer specific error */
if(errorRegister & CO_ERR_REG_MANUFACTURER){
if (errorBehavior[5] == 0) NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
else if (errorBehavior[5] == 2) NMT->operatingState = CO_NMT_STOPPED;
}
/* if operational state is lost, send HB immediately. */
if(NMT->operatingState != CO_NMT_OPERATIONAL)
NMT->HBproducerTimer = HBtime;
}
}
if(NMT->pFunctNMT!=NULL && currentOperatingState!=NMT->operatingState){
NMT->pFunctNMT(NMT->operatingState);
}
return NMT->resetCommand;
}
/******************************************************************************/
CO_NMT_internalState_t CO_NMT_getInternalState(
CO_NMT_t *NMT)
{
if(NMT != NULL){
return NMT->operatingState;
}
return CO_NMT_INITIALIZING;
}
Reference in New Issue View Git Blame Copy Permalink