From 729a139c127bf41b666c548c3dfeed0a5b58725d Mon Sep 17 00:00:00 2001 From: Miles Simpson Date: Thu, 30 Jan 2020 08:58:12 -0800 Subject: [PATCH] Implementing PDO and SYNC timerNext_us return value --- CANopen.c | 38 +++++++++++++++++++++++++++++++++++--- CANopen.h | 24 +++++++++++++++++++++++- stack/CO_PDO.c | 18 +++++++++++++----- stack/CO_PDO.h | 4 +++- stack/CO_SYNC.c | 18 +++++++++++++++++- stack/CO_SYNC.h | 4 +++- 6 files changed, 94 insertions(+), 12 deletions(-) diff --git a/CANopen.c b/CANopen.c index 5443aea..916cb39 100644 --- a/CANopen.c +++ b/CANopen.c @@ -841,7 +841,6 @@ CO_NMT_reset_cmd_t CO_process( } } - for(i=0; iSDO[i], @@ -892,7 +891,7 @@ bool_t CO_process_SYNC( { bool_t syncWas = false; - switch(CO_SYNC_process(co->SYNC, timeDifference_us, OD_synchronousWindowLength)){ + switch(CO_SYNC_process(co->SYNC, timeDifference_us, OD_synchronousWindowLength, NULL)){ case 1: //immediately after the SYNC message syncWas = true; break; @@ -930,6 +929,39 @@ void CO_process_TPDO( for(i=0; iTPDO[i]->sendRequest) co->TPDO[i]->sendRequest = CO_TPDOisCOS(co->TPDO[i]); - CO_TPDO_process(co->TPDO[i], syncWas, timeDifference_us); + CO_TPDO_process(co->TPDO[i], syncWas, timeDifference_us, NULL); } } + + +/******************************************************************************/ +bool_t CO_process_SYNC_PDO( + CO_t *CO, + uint32_t timeDifference_us, + uint32_t *timerNext_us) +{ + int16_t i; + bool_t syncWas = false; + + switch(CO_SYNC_process(CO->SYNC, timeDifference_us, OD_synchronousWindowLength, timerNext_us)){ + case 1: //immediately after the SYNC message + syncWas = true; + break; + case 2: //outside SYNC window + CO_CANclearPendingSyncPDOs(CO->CANmodule[0]); + break; + } + + /* Process RPDOs */ + for(i=0; iRPDO[i], syncWas); + } + + /* Verify PDO Change Of State and process TPDOs */ + for(i=0; iTPDO[i]->sendRequest) CO->TPDO[i]->sendRequest = CO_TPDOisCOS(CO->TPDO[i]); + CO_TPDO_process(CO->TPDO[i], syncWas, timeDifference_us, timerNext_us); + } + + return syncWas; +} diff --git a/CANopen.h b/CANopen.h index e97bb9a..569fdbe 100644 --- a/CANopen.h +++ b/CANopen.h @@ -60,7 +60,6 @@ extern "C" { #include "CO_LSSmaster.h" #endif - /* * CANopen stack object combines pointers to all CANopen objects. */ @@ -262,6 +261,29 @@ void CO_process_TPDO( bool_t syncWas, uint32_t timeDifference_us); + +/** + * Process CANopen SYNC and PDO objects. + * + * Function must be called cyclically from real time thread with constant. + * interval (1ms typically). It processes SYNC and PDO CANopen objects. + * + * @param CO This object. + * @param timeDifference_us Time difference from previous function call in [microseconds]. + * @param timerNext_us Return value - info to OS - maximum delay after function + * should be called next time in [microseconds]. Value can be used for OS + * sleep time. Initial value must be set to something, 1000us typically. + * Output will be equal or lower to initial value. If there is new object + * to process, delay should be suspended and this function should be + * called immediately. Parameter is ignored if NULL. + * + * @return True, if CANopen SYNC message was just received or transmitted. + */ +bool_t CO_process_SYNC_PDO( + CO_t *CO, + uint32_t timeDifference_us, + uint32_t *timerNext_us); + #ifdef __cplusplus } #endif /*__cplusplus*/ diff --git a/stack/CO_PDO.c b/stack/CO_PDO.c index c37a23e..7204990 100644 --- a/stack/CO_PDO.c +++ b/stack/CO_PDO.c @@ -973,8 +973,13 @@ void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas){ void CO_TPDO_process( CO_TPDO_t *TPDO, bool_t syncWas, - uint32_t timeDifference_us) + uint32_t timeDifference_us, + uint32_t *timerNext_us) { + /* update timers */ + TPDO->inhibitTimer = (TPDO->inhibitTimer > timeDifference_us) ? (TPDO->inhibitTimer - timeDifference_us) : 0; + TPDO->eventTimer = (TPDO->eventTimer > timeDifference_us) ? (TPDO->eventTimer - timeDifference_us) : 0; + if(TPDO->valid && *TPDO->operatingState == CO_NMT_OPERATIONAL){ /* Send PDO by application request or by Event timer */ @@ -986,6 +991,13 @@ void CO_TPDO_process( TPDO->eventTimer = ((uint32_t) TPDO->TPDOCommPar->eventTimer) * 1000; } } + if(timerNext_us != NULL){ + if(TPDO->TPDOCommPar->inhibitTime && *timerNext_us > TPDO->inhibitTimer){ + *timerNext_us = TPDO->inhibitTimer + 1; /* Schedule for just beyond inhibit window */ + }else if(TPDO->TPDOCommPar->eventTimer && *timerNext_us > TPDO->eventTimer){ + *timerNext_us = TPDO->eventTimer; /* Schedule for next maximum event time */ + } + } } /* Synchronous PDOs */ @@ -1024,8 +1036,4 @@ void CO_TPDO_process( if(TPDO->TPDOCommPar->transmissionType>=254) TPDO->sendRequest = 1; else TPDO->sendRequest = 0; } - - /* update timers */ - TPDO->inhibitTimer = (TPDO->inhibitTimer > timeDifference_us) ? (TPDO->inhibitTimer - timeDifference_us) : 0; - TPDO->eventTimer = (TPDO->eventTimer > timeDifference_us) ? (TPDO->eventTimer - timeDifference_us) : 0; } diff --git a/stack/CO_PDO.h b/stack/CO_PDO.h index 0f7b4a2..b76d47e 100644 --- a/stack/CO_PDO.h +++ b/stack/CO_PDO.h @@ -374,11 +374,13 @@ void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas); * @param TPDO This object. * @param syncWas True, if CANopen SYNC message was just received or transmitted. * @param timeDifference_us Time difference from previous function call in [microseconds]. + * @param timerNext_us Return value - info to OS - see CO_process_SYNC_PDO(). */ void CO_TPDO_process( CO_TPDO_t *TPDO, bool_t syncWas, - uint32_t timeDifference_us); + uint32_t timeDifference_us, + uint32_t *timerNext_us); #ifdef __cplusplus } diff --git a/stack/CO_SYNC.c b/stack/CO_SYNC.c index c598313..c299ef0 100644 --- a/stack/CO_SYNC.c +++ b/stack/CO_SYNC.c @@ -301,7 +301,8 @@ CO_ReturnError_t CO_SYNC_init( uint8_t CO_SYNC_process( CO_SYNC_t *SYNC, uint32_t timeDifference_us, - uint32_t ObjDict_synchronousWindowLength) + uint32_t ObjDict_synchronousWindowLength, + uint32_t *timerNext_us) { uint8_t ret = 0; uint32_t timerNew; @@ -320,6 +321,7 @@ uint8_t CO_SYNC_process( /* SYNC producer */ if(SYNC->isProducer && SYNC->periodTime){ + uint32_t diff; if(SYNC->timer >= SYNC->periodTime){ if(++SYNC->counter > SYNC->counterOverflowValue) SYNC->counter = 1; SYNC->timer = 0; @@ -327,6 +329,16 @@ uint8_t CO_SYNC_process( SYNC->CANrxToggle = SYNC->CANrxToggle ? false : true; SYNC->CANtxBuff->data[0] = SYNC->counter; CO_CANsend(SYNC->CANdevTx, SYNC->CANtxBuff); + diff = SYNC->periodTime; + }else{ + /* Calculate when next SYNC needs to be sent */ + diff = SYNC->periodTime - SYNC->timer; + } + /* Set lower timerNext_us if necessary. */ + if(timerNext_us != NULL){ + if(*timerNext_us > diff){ + *timerNext_us = diff; + } } } @@ -340,6 +352,10 @@ uint8_t CO_SYNC_process( } else{ SYNC->curentSyncTimeIsInsideWindow = true; + /* Immediately continue while inside the window */ + if(timerNext_us != NULL){ + *timerNext_us = 0; + } } } else{ diff --git a/stack/CO_SYNC.h b/stack/CO_SYNC.h index 82463f5..80e4449 100644 --- a/stack/CO_SYNC.h +++ b/stack/CO_SYNC.h @@ -146,6 +146,7 @@ CO_ReturnError_t CO_SYNC_init( * @param timeDifference_us Time difference from previous function call in [microseconds]. * @param ObjDict_synchronousWindowLength _Synchronous window length_ variable from * Object dictionary (index 0x1007). + * @param timerNext_us Return value - info to OS - see CO_process_SYNC_PDO(). * * @return 0: No special meaning. * @return 1: New SYNC message recently received or was just transmitted. @@ -154,7 +155,8 @@ CO_ReturnError_t CO_SYNC_init( uint8_t CO_SYNC_process( CO_SYNC_t *SYNC, uint32_t timeDifference_us, - uint32_t ObjDict_synchronousWindowLength); + uint32_t ObjDict_synchronousWindowLength, + uint32_t *timerNext_us); #ifdef __cplusplus }