nikon/CANopenNode
1
0
Fork 0
Code Activity Actions

SYNC timeout bugfix; synchronous PDOs are now processed after next sync.

Browse source
This commit is contained in:
Janez 2016-09-07 16:08:07 +02:00
parent 2e0a086514
commit 5df11b9d74
9 changed files with 76 additions and 138 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
CANopen.c
View file

@ -461,6 +461,7 @@ CO_ReturnError_t CO_init(
CO->RPDO[i],
CO->em,
CO->SDO[0],
CO->SYNC,
&CO->NMT->operatingState,
nodeId,
((i<4) ? (CO_CAN_ID_RPDO_1+i*0x100) : 0),

15
example/main.c
View file

@ -56,14 +56,6 @@
volatile uint16_t CO_timer1ms = 0U; /* variable increments each millisecond */
/* helpers */
void CANrx_lockCbSync(bool_t syncReceived) {
if(syncReceived) {
/* disable CAN receive, untill RPDOs are processed. */
}
}
/* main ***********************************************************************/
int main (void){
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
@ -84,7 +76,6 @@ int main (void){
uint16_t timer1msPrevious;
/* disable CAN and CAN interrupts */
CO->CANmodule[0]->CANnormal = false;
/* initialize CANopen */
@ -95,10 +86,6 @@ int main (void){
}
/* Configure callback functions */
CO_SYNC_initCallback(CO->SYNC, CANrx_lockCbSync);
/* Configure Timer interrupt function for execution every 1 millisecond */
@ -158,8 +145,6 @@ static void tmrTask_thread(void){
/* Process Sync and read inputs */
syncWas = CO_process_SYNC_RPDO(CO, TMR_TASK_INTERVAL);
/* Reenable CANrx, if it was disabled by SYNC callback */
/* Further I/O or nonblocking application code may go here. */
/* Write outputs */

71
stack/CO_PDO.c
View file

@ -70,17 +70,32 @@ static void CO_PDO_receive(void *object, const CO_CANrxMsg_t *msg){
(*RPDO->operatingState == CO_NMT_OPERATIONAL) &&
(msg->DLC >= RPDO->dataLength))
{
/* copy data and set 'new message' flag */
RPDO->CANrxData[0] = msg->data[0];
RPDO->CANrxData[1] = msg->data[1];
RPDO->CANrxData[2] = msg->data[2];
RPDO->CANrxData[3] = msg->data[3];
RPDO->CANrxData[4] = msg->data[4];
RPDO->CANrxData[5] = msg->data[5];
RPDO->CANrxData[6] = msg->data[6];
RPDO->CANrxData[7] = msg->data[7];
if(RPDO->synchronous && RPDO->SYNC->CANrxToggle) {
/* copy data into second buffer and set 'new message' flag */
RPDO->CANrxData[1][0] = msg->data[0];
RPDO->CANrxData[1][1] = msg->data[1];
RPDO->CANrxData[1][2] = msg->data[2];
RPDO->CANrxData[1][3] = msg->data[3];
RPDO->CANrxData[1][4] = msg->data[4];
RPDO->CANrxData[1][5] = msg->data[5];
RPDO->CANrxData[1][6] = msg->data[6];
RPDO->CANrxData[1][7] = msg->data[7];
RPDO->CANrxNew = true;
RPDO->CANrxNew[1] = true;
}
else {
/* copy data into default buffer and set 'new message' flag */
RPDO->CANrxData[0][0] = msg->data[0];
RPDO->CANrxData[0][1] = msg->data[1];
RPDO->CANrxData[0][2] = msg->data[2];
RPDO->CANrxData[0][3] = msg->data[3];
RPDO->CANrxData[0][4] = msg->data[4];
RPDO->CANrxData[0][5] = msg->data[5];
RPDO->CANrxData[0][6] = msg->data[6];
RPDO->CANrxData[0][7] = msg->data[7];
RPDO->CANrxNew[0] = true;
}
}
}
@ -114,7 +129,7 @@ static void CO_RPDOconfigCom(CO_RPDO_t* RPDO, uint32_t COB_IDUsedByRPDO){
else{
ID = 0;
RPDO->valid = false;
RPDO->CANrxNew = false;
RPDO->CANrxNew[0] = RPDO->CANrxNew[1] = false;
}
r = CO_CANrxBufferInit(
RPDO->CANdevRx, /* CAN device */
@ -126,7 +141,7 @@ static void CO_RPDOconfigCom(CO_RPDO_t* RPDO, uint32_t COB_IDUsedByRPDO){
CO_PDO_receive); /* this function will process received message */
if(r != CO_ERROR_NO){
RPDO->valid = false;
RPDO->CANrxNew = false;
RPDO->CANrxNew[0] = RPDO->CANrxNew[1] = false;
}
}
@ -466,10 +481,18 @@ static CO_SDO_abortCode_t CO_ODF_RPDOcom(CO_ODF_arg_t *ODF_arg){
}
else if(ODF_arg->subIndex == 2){ /* Transmission_type */
uint8_t *value = (uint8_t*) ODF_arg->data;
bool_t synchronousPrev = RPDO->synchronous;
/* values from 241...253 are not valid */
if(*value >= 241 && *value <= 253)
return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
RPDO->synchronous = (*value <= 240) ? true : false;
/* Remove old message from second buffer. */
if(RPDO->synchronous != synchronousPrev) {
RPDO->CANrxNew[1] = false;
}
}
return CO_SDO_AB_NONE;
@ -704,6 +727,7 @@ CO_ReturnError_t CO_RPDO_init(
CO_RPDO_t *RPDO,
CO_EM_t *em,
CO_SDO_t *SDO,
CO_SYNC_t *SYNC,
uint8_t *operatingState,
uint8_t nodeId,
uint16_t defaultCOB_ID,
@ -716,7 +740,7 @@ CO_ReturnError_t CO_RPDO_init(
uint16_t CANdevRxIdx)
{
/* verify arguments */
if(RPDO==NULL || em==NULL || SDO==NULL || operatingState==NULL ||
if(RPDO==NULL || em==NULL || SDO==NULL || SYNC==NULL || operatingState==NULL ||
RPDOCommPar==NULL || RPDOMapPar==NULL || CANdevRx==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
@ -724,6 +748,7 @@ CO_ReturnError_t CO_RPDO_init(
/* Configure object variables */
RPDO->em = em;
RPDO->SDO = SDO;
RPDO->SYNC = SYNC;
RPDO->RPDOCommPar = RPDOCommPar;
RPDO->RPDOMapPar = RPDOMapPar;
RPDO->operatingState = operatingState;
@ -736,7 +761,7 @@ CO_ReturnError_t CO_RPDO_init(
CO_OD_configure(SDO, idx_RPDOMapPar, CO_ODF_RPDOmap, (void*)RPDO, 0, 0);
/* configure communication and mapping */
RPDO->CANrxNew = false;
RPDO->CANrxNew[0] = RPDO->CANrxNew[1] = false;
RPDO->CANdevRx = CANdevRx;
RPDO->CANdevRxIdx = CANdevRxIdx;
@ -881,19 +906,27 @@ int16_t CO_TPDOsend(CO_TPDO_t *TPDO){
void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas){
if(RPDO->valid && (*RPDO->operatingState == CO_NMT_OPERATIONAL) &&
((RPDO->synchronous && syncWas) || !RPDO->synchronous)){
while(RPDO->CANrxNew){
((RPDO->synchronous && syncWas) || !RPDO->synchronous))
{
uint8_t bufNo = 0;
/* Determine, which of the two rx buffers, contains relevant message. */
if(RPDO->synchronous && !RPDO->SYNC->CANrxToggle) {
bufNo = 1;
}
while(RPDO->CANrxNew[bufNo]){
int16_t i;
uint8_t* pPDOdataByte;
uint8_t** ppODdataByte;
i = RPDO->dataLength;
pPDOdataByte = &RPDO->CANrxData[0];
pPDOdataByte = &RPDO->CANrxData[bufNo][0];
ppODdataByte = &RPDO->mapPointer[0];
/* Copy data to Object dictionary. If between the copy operation CANrxNew
* is set to true by receive thread, then copy the latest data again. */
RPDO->CANrxNew = false;
RPDO->CANrxNew[bufNo] = false;
for(; i>0; i--) {
**(ppODdataByte++) = *(pPDOdataByte++);
}
@ -929,7 +962,7 @@ void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas){
}
else{
RPDO->CANrxNew = false;
RPDO->CANrxNew[0] = RPDO->CANrxNew[1] = false;
}
}

9
stack/CO_PDO.h
View file

@ -69,7 +69,8 @@
* - Map granularity of one byte.
* - After RPDO is received from CAN bus, its data are copied to buffer.
* Function CO_RPDO_process() (called by application) copies data to
* mapped objects in Object Dictionary.
* mapped objects in Object Dictionary. Synchronous RPDOs are processed AFTER
* reception of the next SYNC message.
* - Function CO_TPDO_process() (called by application) sends TPDO if
* necessary. There are possible different transmission types, including
* automatic detection of Change of State of specific variable.
@ -181,6 +182,7 @@ typedef struct{
typedef struct{
CO_EM_t *em; /**< From CO_RPDO_init() */
CO_SDO_t *SDO; /**< From CO_RPDO_init() */
CO_SYNC_t *SYNC; /**< From CO_RPDO_init() */
const CO_RPDOCommPar_t *RPDOCommPar;/**< From CO_RPDO_init() */
const CO_RPDOMapPar_t *RPDOMapPar; /**< From CO_RPDO_init() */
uint8_t *operatingState; /**< From CO_RPDO_init() */
@ -196,9 +198,9 @@ typedef struct{
/** Pointers to 8 data objects, where PDO will be copied */
uint8_t *mapPointer[8];
/** Variable indicates, if new PDO message received from CAN bus. */
volatile bool_t CANrxNew;
volatile bool_t CANrxNew[2];
/** 8 data bytes of the received message. */
uint8_t CANrxData[8];
uint8_t CANrxData[2][8];
CO_CANmodule_t *CANdevRx; /**< From CO_RPDO_init() */
uint16_t CANdevRxIdx; /**< From CO_RPDO_init() */
}CO_RPDO_t;
@ -273,6 +275,7 @@ CO_ReturnError_t CO_RPDO_init(
CO_RPDO_t *RPDO,
CO_EM_t *em,
CO_SDO_t *SDO,
CO_SYNC_t *SYNC,
uint8_t *operatingState,
uint8_t nodeId,
uint16_t defaultCOB_ID,

28
stack/CO_SYNC.c
View file

@ -50,7 +50,6 @@
#include "CO_NMT_Heartbeat.h"
#include "CO_SYNC.h"
/*
* Read received message from CAN module.
*
@ -83,8 +82,8 @@ static void CO_SYNC_receive(void *object, const CO_CANrxMsg_t *msg){
SYNC->receiveError = (uint16_t)msg->DLC | 0x0200U;
}
}
if(SYNC->CANrxNew && (SYNC->cbSync != NULL)){
SYNC->cbSync(true); //callback
if(SYNC->CANrxNew) {
SYNC->CANrxToggle = SYNC->CANrxToggle ? false : true;
}
}
}
@ -271,13 +270,13 @@ CO_ReturnError_t CO_SYNC_init(
SYNC->curentSyncTimeIsInsideWindow = true;
SYNC->CANrxNew = false;
SYNC->CANrxToggle = false;
SYNC->timer = 0;
SYNC->counter = 0;
SYNC->receiveError = 0U;
SYNC->em = em;
SYNC->operatingState = operatingState;
SYNC->cbSync = NULL;
SYNC->CANdevRx = CANdevRx;
SYNC->CANdevRxIdx = CANdevRxIdx;
@ -312,17 +311,6 @@ CO_ReturnError_t CO_SYNC_init(
}
/******************************************************************************/
void CO_SYNC_initCallback(
CO_SYNC_t *SYNC,
void (*cbSync)(bool_t syncReceived))
{
if(SYNC != NULL){
SYNC->cbSync = cbSync;
}
}
/******************************************************************************/
uint8_t CO_SYNC_process(
CO_SYNC_t *SYNC,
@ -341,6 +329,7 @@ uint8_t CO_SYNC_process(
if(SYNC->CANrxNew){
SYNC->timer = 0;
ret = 1;
SYNC->CANrxNew = false;
}
/* SYNC producer */
@ -349,9 +338,7 @@ uint8_t CO_SYNC_process(
if(++SYNC->counter > SYNC->counterOverflowValue) SYNC->counter = 1;
SYNC->timer = 0;
ret = 1;
if(SYNC->cbSync != NULL){
SYNC->cbSync(false); //callback
}
SYNC->CANrxToggle = SYNC->CANrxToggle ? false : true;
SYNC->CANtxBuff->data[0] = SYNC->counter;
CO_CANsend(SYNC->CANdevTx, SYNC->CANtxBuff);
}
@ -377,6 +364,9 @@ uint8_t CO_SYNC_process(
if(SYNC->periodTime && SYNC->timer > SYNC->periodTimeoutTime && *SYNC->operatingState == CO_NMT_OPERATIONAL)
CO_errorReport(SYNC->em, CO_EM_SYNC_TIME_OUT, CO_EMC_COMMUNICATION, SYNC->timer);
}
else {
SYNC->CANrxNew = false;
}
/* verify error from receive function */
if(SYNC->receiveError != 0U){
@ -384,7 +374,5 @@ uint8_t CO_SYNC_process(
SYNC->receiveError = 0U;
}
SYNC->CANrxNew = false;
return ret;
}

35
stack/CO_SYNC.h
View file

@ -69,16 +69,12 @@
* one data byte: _counter_ incremented by 1 with every SYNC transmission.
*
* ####SYNC in CANopenNode
* If callback is configured by CO_SYNC_initCallback(), then it is called from
* CO_SYNC_receive thread, after SYNC message is received. Or, if node is SYNC
* producer, function is called just before SYNC message is transmitted.
* Usage of callback is microcontroller specific. It may, for example, temporary
* disable CAN message reception. Reception is then reenabled after all RPDOs
* are processed. This is necessary because reception of PDOs must be disabled
* after sync message, before old RPDOs are processed completelly.
*
* Besides callback, information about SYNC message is returned also from
* CO_SYNC_process() function.
* According to CANopen, synchronous RPDOs must be processed after reception of
* the next sync messsage. For that reason, there is a double receive buffer
* for each synchronous RPDO. At the moment, when SYNC is received or
* transmitted, internal variable CANrxToggle toggles. That variable is then
* used by synchronous RPDO to determine, which of the two buffers is used for
* RPDO reception and which for RPDO processing.
*/
@ -108,6 +104,8 @@ typedef struct{
bool_t curentSyncTimeIsInsideWindow;
/** Variable indicates, if new SYNC message received from CAN bus */
bool_t CANrxNew;
/** Variable toggles, if new SYNC message received from CAN bus */
bool_t CANrxToggle;
/** Counter of the SYNC message if counterOverflowValue is different than zero */
uint8_t counter;
/** Timer for the SYNC message in [microseconds].
@ -115,7 +113,6 @@ typedef struct{
uint32_t timer;
/** Set to nonzero value, if SYNC with wrong data length is received from CAN */
uint16_t receiveError;
void (*cbSync)(bool_t syncReceived);/**< From CO_SYNC_initCallback() or NULL */
CO_CANmodule_t *CANdevRx; /**< From CO_SYNC_init() */
uint16_t CANdevRxIdx; /**< From CO_SYNC_init() */
CO_CANmodule_t *CANdevTx; /**< From CO_SYNC_init() */
@ -157,22 +154,6 @@ CO_ReturnError_t CO_SYNC_init(
uint16_t CANdevTxIdx);
/**
* Initialize SYNC callback function.
*
* Function initializes optional callback function, which executes after the SYNC.
*
* @param SYNC This object.
* @param cbSync Callback function, which will be called just after the
* presence of CANopen SYNC message on the bus. If SYNC was received,
* syncReceived will be true, if SYNC will be transmitted, syncReceived
* will be false.
*/
void CO_SYNC_initCallback(
CO_SYNC_t *SYNC,
void (*cbSync)(bool_t syncReceived));
/**
* Process SYNC communication.
*

2
stack/PIC32/CO_driver.c
View file

@ -79,7 +79,7 @@ unsigned int CO_interruptStatus = 0;
/* Number of hardware filters */
/* device PIC32MX530 (and below) has only 16 registers for CAN reception (not 32). */
/* device PIC32MX530, 550 and 570 has only 16 registers for CAN reception (not 32). */
#ifdef __PIC32MX
#if (__PIC32_FEATURE_SET__ == 530) || (__PIC32_FEATURE_SET__ == 550) || (__PIC32_FEATURE_SET__ == 570)
#define NO_CAN_RXF 16

19
stack/PIC32/main_PIC32.c
View file

@ -117,15 +117,6 @@
#endif
/* helpers */
void CANrx_lockCbSync(bool_t syncReceived) {
if(syncReceived) {
CO_CAN_ISR_ENABLE = 0;
CO_CAN_ISR2_ENABLE = 0;
}
}
/* main ***********************************************************************/
int main (void){
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
@ -193,10 +184,6 @@ int main (void){
#endif
/* Configure callback functions */
CO_SYNC_initCallback(CO->SYNC, CANrx_lockCbSync);
/* initialize variables */
timer1msPrevious = CO_timer1ms;
OD_performance[ODA_performance_mainCycleMaxTime] = 0;
@ -316,12 +303,6 @@ void __ISR(_TIMER_2_VECTOR, IPL3SOFT) CO_TimerInterruptHandler(void){
/* Process Sync and read inputs */
syncWas = CO_process_SYNC_RPDO(CO, 1000);
/* Re-enable CANrx, if it was disabled by SYNC callback */
CO_CAN_ISR_ENABLE = 1;
#if CO_NO_CAN_MODULES >= 2
CO_CAN_ISR2_ENABLE = 1;
#endif
/* Further I/O or nonblocking application code may go here. */
#if CO_NO_TRACE > 0
OD_time.epochTimeOffsetMs++;

34
stack/socketCAN/CO_Linux_tasks.c
View file

@ -187,8 +187,6 @@ static struct {
long intervalns;
long intervalus;
uint16_t *maxTime;
int fdEpoll;
bool_t CANrx_locked;
} taskRT;
@ -228,10 +226,6 @@ void CANrx_taskTmr_init(int fdEpoll, long intervalns, uint16_t *maxTime) {
taskRT.intervalns = intervalns;
taskRT.intervalus = intervalns / 1000;
taskRT.maxTime = maxTime;
/* used for CANrx_lockCbSync() */
taskRT.fdEpoll = fdEpoll;
taskRT.CANrx_locked = false;
}
@ -292,19 +286,6 @@ bool_t CANrx_taskTmr_process(int fd) {
/* Process Sync and read inputs */
syncWas = CO_process_SYNC_RPDO(CO, taskRT.intervalus);
/* Re-enable CANrx, if it was disabled by SYNC callback */
if(taskRT.CANrx_locked) {
struct epoll_event ev;
/* enable epool event */
ev.events = EPOLLIN;
ev.data.fd = taskRT.fdRx0;
if(epoll_ctl(taskRT.fdEpoll, EPOLL_CTL_MOD, taskRT.fdRx0, &ev) == -1)
CO_error(0x22500000L + errno);
taskRT.CANrx_locked = false;
}
/* Further I/O or nonblocking application code may go here. */
/* Write outputs */
@ -321,18 +302,3 @@ bool_t CANrx_taskTmr_process(int fd) {
return wasProcessed;
}
void CANrx_lockCbSync(bool_t syncReceived) {
if(syncReceived) {
struct epoll_event ev;
/* disable epool event */
ev.events = 0;
ev.data.fd = taskRT.fdRx0;
if(epoll_ctl(taskRT.fdEpoll, EPOLL_CTL_MOD, taskRT.fdRx0, &ev) == -1)
CO_error(0x24100000L + errno);
taskRT.CANrx_locked = true;
}
}