nikon/CANopenNode
1
0
Fork 0
Code Activity Actions

Implementing PDO and SYNC timerNext_us return value

Browse source
This commit is contained in:
Miles Simpson 2020-01-30 08:58:12 -08:00 committed by Janez
parent 0efcf4b1b5
commit 729a139c12
6 changed files with 94 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

38
CANopen.c
View file

@ -841,7 +841,6 @@ CO_NMT_reset_cmd_t CO_process(
}
}
for(i=0; i<CO_NO_SDO_SERVER; i++){
CO_SDO_process(
co->SDO[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; i<CO_NO_TPDO; i++){
if(!co->TPDO[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; i<CO_NO_RPDO; i++){
CO_RPDO_process(CO->RPDO[i], syncWas);
}
/* Verify PDO Change Of State and process TPDOs */
for(i=0; i<CO_NO_TPDO; i++){
if(!CO->TPDO[i]->sendRequest) CO->TPDO[i]->sendRequest = CO_TPDOisCOS(CO->TPDO[i]);
CO_TPDO_process(CO->TPDO[i], syncWas, timeDifference_us, timerNext_us);
}
return syncWas;
}

24
CANopen.h
View file

@ -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*/

18
stack/CO_PDO.c
View file

@ -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;
}

4
stack/CO_PDO.h
View file

@ -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
}

18
stack/CO_SYNC.c
View file

@ -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{

4
stack/CO_SYNC.h
View file

@ -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
}