From 464a856a3f13417769f0f36eba3ce2901df381f1 Mon Sep 17 00:00:00 2001 From: Janez Date: Mon, 25 Jan 2021 12:52:52 +0100 Subject: [PATCH] 301/CO_time.c updated to newOD. Some principles changed. --- 301/CO_Emergency.h | 6 +- 301/CO_TIME.c | 270 +++++++++++++++++++---------------- 301/CO_TIME.h | 202 +++++++++++++------------- 301/CO_config.h | 2 + CANopen.c | 17 ++- example/CO_driver_target.h | 3 +- socketCAN/CO_driver_target.h | 4 +- socketCAN/CO_main_basic.c | 15 ++ 8 files changed, 283 insertions(+), 236 deletions(-) diff --git a/301/CO_Emergency.h b/301/CO_Emergency.h index c59df37..dc67e6f 100644 --- a/301/CO_Emergency.h +++ b/301/CO_Emergency.h @@ -198,8 +198,6 @@ typedef enum { CO_EMC_SYNC_DATA_LENGTH = 0x8240U, /** 0x8250, RPDO timeout */ CO_EMC_RPDO_TIMEOUT = 0x8250U, - /** 0x8260, Unexpected TIME data length */ - CO_EMC_TIME_DATA_LENGTH = 0x8260U, /** 0x90xx, External Error */ CO_EMC_EXTERNAL_ERROR = 0x9000U, /** 0xF0xx, Additional Functions */ @@ -267,8 +265,8 @@ typedef enum { CO_EM_NMT_WRONG_COMMAND = 0x08U, /** 0x09, communication, info, TIME message timeout */ CO_EM_TIME_TIMEOUT = 0x09U, - /** 0x0A, communication, info, Unexpected TIME data length */ - CO_EM_TIME_LENGTH = 0x0AU, + /** 0x0A, communication, info, (unused) */ + CO_EM_0A_unused = 0x0AU, /** 0x0B, communication, info, (unused) */ CO_EM_0B_unused = 0x0BU, /** 0x0C, communication, info, (unused) */ diff --git a/301/CO_TIME.c b/301/CO_TIME.c index 403a9fb..1dd0297 100644 --- a/301/CO_TIME.c +++ b/301/CO_TIME.c @@ -25,188 +25,212 @@ #include -#include "301/CO_SDOserver.h" -#include "301/CO_Emergency.h" -#include "301/CO_NMT_Heartbeat.h" #include "301/CO_TIME.h" #if (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE -#define DIV_ROUND_UP(_n, _d) (((_n) + (_d) - 1) / (_d)) - /* * 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. */ -static void CO_TIME_receive(void *object, void *msg){ - CO_TIME_t *TIME; - CO_NMT_internalState_t operState; +static void CO_TIME_receive(void *object, void *msg) { + CO_TIME_t *TIME = object; uint8_t DLC = CO_CANrxMsg_readDLC(msg); uint8_t *data = CO_CANrxMsg_readData(msg); - TIME = (CO_TIME_t*)object; /* this is the correct pointer type of the first argument */ - operState = *TIME->operatingState; - - if((operState == CO_NMT_OPERATIONAL) || (operState == CO_NMT_PRE_OPERATIONAL)){ - // Process Time from msg buffer - memcpy(&TIME->Time.ullValue, data, DLC); + if (DLC == CO_TIME_MSG_LENGTH) { + memcpy(TIME->timeStamp, data, sizeof(TIME->timeStamp)); CO_FLAG_SET(TIME->CANrxNew); #if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE - /* Optional signal to RTOS, which can resume task, which handles TIME. */ - if(TIME->pFunctSignalPre != NULL) { + /* Optional signal to RTOS, which can resume task, which handles TIME.*/ + if (TIME->pFunctSignalPre != NULL) { TIME->pFunctSignalPre(TIME->functSignalObjectPre); } #endif } - else{ - TIME->receiveError = (uint16_t)DLC; - } } -/******************************************************************************/ -CO_ReturnError_t CO_TIME_init( - CO_TIME_t *TIME, - CO_EM_t *em, - CO_SDO_t *SDO, - CO_NMT_internalState_t *operatingState, - uint32_t COB_ID_TIMEMessage, - uint32_t TIMECyclePeriod, - CO_CANmodule_t *CANdevRx, - uint16_t CANdevRxIdx, - CO_CANmodule_t *CANdevTx, - uint16_t CANdevTxIdx) -{ - CO_ReturnError_t ret = CO_ERROR_NO; - /* verify arguments */ - if(TIME==NULL || em==NULL || SDO==NULL || operatingState==NULL || - CANdevRx==NULL || CANdevTx==NULL){ - return CO_ERROR_ILLEGAL_ARGUMENT; +#if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_OD_DYNAMIC +/* + * Custom function for writing OD object "COB-ID time stamp" + * + * For more information see file CO_ODinterface.h, OD_IO_t. + */ +static OD_size_t OD_write_1012(OD_stream_t *stream, uint8_t subIndex, + const void *buf, OD_size_t count, + ODR_t *returnCode) +{ + /* "count" is already verified in *_init() function */ + if (stream == NULL || subIndex != 0 || buf == NULL || returnCode == NULL) { + if (returnCode != NULL) *returnCode = ODR_DEV_INCOMPAT; + return 0; } - /* Configure object variables */ - TIME->isConsumer = (COB_ID_TIMEMessage&0x80000000L) ? true : false; - TIME->isProducer = (COB_ID_TIMEMessage&0x40000000L) ? true : false; - TIME->COB_ID = COB_ID_TIMEMessage&0x7FF; // 11 bit ID + CO_TIME_t *TIME = stream->object; + *returnCode = ODR_OK; - TIME->periodTime = TIMECyclePeriod; - TIME->periodTimeoutTime = TIMECyclePeriod / 2 * 3; - /* overflow? */ - if(TIME->periodTimeoutTime < TIMECyclePeriod) - TIME->periodTimeoutTime = 0xFFFFFFFFL; + /* update object */ + uint32_t cobIdTimeStamp = CO_getUint32(buf); + TIME->isConsumer = (cobIdTimeStamp & 0x80000000L) != 0; + TIME->isProducer = (cobIdTimeStamp & 0x40000000L) != 0; - CO_FLAG_CLEAR(TIME->CANrxNew); - TIME->timer = 0; - TIME->receiveError = 0U; - - TIME->em = em; - TIME->operatingState = operatingState; - -#if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE - TIME->pFunctSignalPre = NULL; - TIME->functSignalObjectPre = NULL; + /* write value to the original location in the Object Dictionary */ + return OD_writeOriginal(stream, subIndex, buf, count, returnCode); +} #endif - /* configure TIME consumer message reception */ - TIME->CANdevRx = CANdevRx; - TIME->CANdevRxIdx = CANdevRxIdx; - if (TIME->isConsumer) { - ret = CO_CANrxBufferInit( - CANdevRx, /* CAN device */ - CANdevRxIdx, /* rx buffer index */ - TIME->COB_ID, /* CAN identifier */ - 0x7FF, /* mask */ - 0, /* rtr */ - (void*)TIME, /* object passed to receive function */ - CO_TIME_receive); /* this function will process received message */ +CO_ReturnError_t CO_TIME_init(CO_TIME_t *TIME, + OD_entry_t *OD_1012_cobIdTimeStamp, + CO_CANmodule_t *CANdevRx, + uint16_t CANdevRxIdx, +#if (CO_CONFIG_TIME) & CO_CONFIG_TIME_PRODUCER + CO_CANmodule_t *CANdevTx, + uint16_t CANdevTxIdx, +#endif + uint32_t *errInfo) +{ + /* verify arguments */ + if (TIME == NULL || OD_1012_cobIdTimeStamp == NULL || CANdevRx == NULL +#if (CO_CONFIG_TIME) & CO_CONFIG_TIME_PRODUCER + || CANdevTx == NULL +#endif + ) { + return CO_ERROR_ILLEGAL_ARGUMENT; } + memset(TIME, 0, sizeof(CO_TIME_t)); + + /* get parameters from object dictionary and configure extension */ + uint32_t cobIdTimeStamp; + ODR_t odRet = OD_get_u32(OD_1012_cobIdTimeStamp, 0, &cobIdTimeStamp, true); + if (odRet != ODR_OK) { + if (errInfo != NULL) *errInfo = OD_getIndex(OD_1012_cobIdTimeStamp); + return CO_ERROR_OD_PARAMETERS; + } +#if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_OD_DYNAMIC + TIME->OD_1012_extension.object = TIME; + TIME->OD_1012_extension.read = OD_readOriginal; + TIME->OD_1012_extension.write = OD_write_1012; + OD_extension_init(OD_1012_cobIdTimeStamp, &TIME->OD_1012_extension); +#endif + + /* Configure object variables */ + uint16_t cobId = cobIdTimeStamp & 0x7FF; + TIME->isConsumer = (cobIdTimeStamp & 0x80000000L) != 0; + TIME->isProducer = (cobIdTimeStamp & 0x40000000L) != 0; + CO_FLAG_CLEAR(TIME->CANrxNew); + + /* configure TIME consumer message reception */ + if (TIME->isConsumer) { + CO_ReturnError_t ret = CO_CANrxBufferInit( + CANdevRx, /* CAN device */ + CANdevRxIdx, /* rx buffer index */ + cobId, /* CAN identifier */ + 0x7FF, /* mask */ + 0, /* rtr */ + (void*)TIME, /* object passed to receive function */ + CO_TIME_receive);/*this function will process received message*/ + if (ret != CO_ERROR_NO) + return ret; + } + +#if (CO_CONFIG_TIME) & CO_CONFIG_TIME_PRODUCER /* configure TIME producer message transmission */ TIME->CANdevTx = CANdevTx; - TIME->CANdevTxIdx = CANdevTxIdx; - if (TIME->isProducer) { - TIME->TXbuff = CO_CANtxBufferInit( - CANdevTx, /* CAN device */ - CANdevTxIdx, /* index of specific buffer inside CAN module */ - TIME->COB_ID, /* CAN identifier */ - 0, /* rtr */ - TIME_MSG_LENGTH, /* number of data bytes */ - 0); /* synchronous message flag bit */ + TIME->CANtxBuff = CO_CANtxBufferInit( + CANdevTx, /* CAN device */ + CANdevTxIdx, /* index of specific buffer inside CAN module */ + cobId, /* CAN identifier */ + 0, /* rtr */ + CO_TIME_MSG_LENGTH, /* number of data bytes */ + 0); /* synchronous message flag bit */ - if (TIME->TXbuff == NULL) { - ret = CO_ERROR_ILLEGAL_ARGUMENT; - } + if (TIME->CANtxBuff == NULL) { + return CO_ERROR_ILLEGAL_ARGUMENT; } +#endif - return ret; + return CO_ERROR_NO; } + #if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE -/******************************************************************************/ -void CO_TIME_initCallbackPre( - CO_TIME_t *TIME, - void *object, - void (*pFunctSignalPre)(void *object)) +void CO_TIME_initCallbackPre(CO_TIME_t *TIME, + void *object, + void (*pFunctSignalPre)(void *object)) { - if(TIME != NULL){ + if (TIME != NULL) { TIME->functSignalObjectPre = object; TIME->pFunctSignalPre = pFunctSignalPre; } } #endif -/******************************************************************************/ -uint8_t CO_TIME_process( - CO_TIME_t *TIME, - uint32_t timeDifference_us) + +bool_t CO_TIME_process(CO_TIME_t *TIME, + bool_t NMTisPreOrOperational, + uint32_t timeDifference_us) { - uint8_t ret = 0; - uint32_t timerNew; + bool_t timestampReceived = false; - if(*TIME->operatingState == CO_NMT_OPERATIONAL || *TIME->operatingState == CO_NMT_PRE_OPERATIONAL){ - /* update TIME timer, no overflow */ - uint32_t timeDifference_ms = DIV_ROUND_UP(timeDifference_us, 1000); - timerNew = TIME->timer + timeDifference_ms; - if(timerNew > TIME->timer) - TIME->timer = timerNew; + /* Was TIME stamp message just received */ + if (NMTisPreOrOperational && TIME->isConsumer) { + if(CO_FLAG_READ(TIME->CANrxNew)) { + uint32_t ms_swapped = CO_getUint32(&TIME->timeStamp[0]); + uint16_t days_swapped = CO_getUint16(&TIME->timeStamp[4]); + TIME->ms = CO_SWAP_32(ms_swapped) & 0x0FFFFFFF; + TIME->days = CO_SWAP_16(days_swapped); + TIME->residual_us = 0; + timestampReceived = true; - /* was TIME just received */ - if(CO_FLAG_READ(TIME->CANrxNew)){ - TIME->timer = 0; - ret = 1; CO_FLAG_CLEAR(TIME->CANrxNew); } - - /* TIME producer */ - if(TIME->isProducer && TIME->periodTime){ - if(TIME->timer >= TIME->periodTime){ - TIME->timer = 0; - ret = 1; - memcpy(TIME->TXbuff->data, &TIME->Time.ullValue, TIME_MSG_LENGTH); - CO_CANsend(TIME->CANdevTx, TIME->TXbuff); - } - } - - /* Verify TIME timeout if node is consumer */ - if(TIME->isConsumer && TIME->periodTime && TIME->timer > TIME->periodTimeoutTime - && *TIME->operatingState == CO_NMT_OPERATIONAL) - CO_errorReport(TIME->em, CO_EM_TIME_TIMEOUT, CO_EMC_COMMUNICATION, TIME->timer); } else { CO_FLAG_CLEAR(TIME->CANrxNew); } - /* verify error from receive function */ - if(TIME->receiveError != 0U){ - CO_errorReport(TIME->em, CO_EM_TIME_LENGTH, CO_EMC_TIME_DATA_LENGTH, (uint32_t)TIME->receiveError); - TIME->receiveError = 0U; + /* Update time */ + uint32_t ms = 0; + if (!timestampReceived && timeDifference_us > 0) { + uint32_t us = timeDifference_us + TIME->residual_us; + ms = us / 1000; + TIME->residual_us = us % 1000; + TIME->ms += ms; + if (TIME->ms >= (1000*60*60*24)) { + TIME->ms -= (1000*60*60*24); + TIME->days += 1; + } } - return ret; +#if (CO_CONFIG_TIME) & CO_CONFIG_TIME_PRODUCER + if (NMTisPreOrOperational && TIME->isProducer + && TIME->producerInterval_ms > 0 + ) { + if (TIME->producerTimer_ms >= TIME->producerInterval_ms) { + TIME->producerTimer_ms -= TIME->producerInterval_ms; + + uint32_t ms_swapped = CO_SWAP_32(TIME->ms); + uint16_t days_swapped = CO_SWAP_16(TIME->days); + CO_setUint32(&TIME->CANtxBuff->data[0], ms_swapped); + CO_setUint16(&TIME->CANtxBuff->data[4], days_swapped); + CO_CANsend(TIME->CANdevTx, TIME->CANtxBuff); + } + else { + TIME->producerTimer_ms += ms; + } + } + else { + TIME->producerTimer_ms = TIME->producerInterval_ms; + } +#endif + + return timestampReceived; } #endif /* (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE */ diff --git a/301/CO_TIME.h b/301/CO_TIME.h index 44cdf3c..7af36ba 100644 --- a/301/CO_TIME.h +++ b/301/CO_TIME.h @@ -27,10 +27,13 @@ #define CO_TIME_H #include "301/CO_driver.h" +#include "301/CO_ODinterface.h" +#include "301/CO_NMT_Heartbeat.h" + /* default configuration, see CO_config.h */ #ifndef CO_CONFIG_TIME -#define CO_CONFIG_TIME (0) +#define CO_CONFIG_TIME (CO_CONFIG_TIME_ENABLE) #endif #if ((CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE) || defined CO_DOXYGEN @@ -46,92 +49,73 @@ extern "C" { * * CANopen Time-stamp protocol. * - * For CAN identifier see #CO_Default_CAN_ID_t + * For CAN identifier see @ref CO_Default_CAN_ID_t * - * TIME message is used for time synchronization of the nodes on network. One node - * should be TIME producer, others can be TIME consumers. This is configured with - * COB_ID_TIME object 0x1012 : + * TIME message is used for time synchronization of the nodes on the network. + * One node should be TIME producer, others can be TIME consumers. This is + * configured by COB_ID_TIME object 0x1012: * * - bit 31 should be set for a consumer * - bit 30 should be set for a producer + * - bits 0..10 is CAN-ID, 0x100 by default * + * Current time can be read from @p CO_TIME_t->ms (milliseconds after midnight) + * and @p CO_TIME_t->days (number of days since January 1, 1984). Those values + * are updated on each @ref CO_TIME_process() call, either from internal timer + * or from received time stamp message. * - * ###TIME CONSUMER - * - * CO_TIME_init() configuration : - * - COB_ID_TIME : 0x80000100L -> TIME consumer with TIME_COB_ID = 0x100 - * - TIMECyclePeriod : - * - 0 -> no EMCY will be transmitted in case of TIME timeout - * - X -> an EMCY will be transmitted in case of TIME timeout (X * 1.5) ms - * - * Latest time value is stored in \p CO->TIME->Time variable. - * - * - * ###TIME PRODUCER - * - * CO_TIME_init() configuration : - * - COB_ID_TIME : 0x40000100L -> TIME producer with TIME_COB_ID = 0x100 - * - TIMECyclePeriod : Time transmit period in ms - * - * Write time value in \p CO->TIME->Time variable, this will be sent at TIMECyclePeriod. + * Current time can be set with @ref CO_TIME_set() function, which is necessary + * at least once, if time producer. If configured, time stamp message is + * send from @ref CO_TIME_process() in intervals specified by @ref CO_TIME_set() */ -#define TIME_MSG_LENGTH 6U -#ifndef timeOfDay_t -typedef union { - unsigned long long ullValue; - struct { - unsigned long ms:28; - unsigned reserved:4; - unsigned days:16; - unsigned reserved2:16; - }; -} timeOfDay_t; -#endif +/** Length of the TIME message */ +#define CO_TIME_MSG_LENGTH 6 -typedef timeOfDay_t TIME_OF_DAY; -typedef timeOfDay_t TIME_DIFFERENCE; /** * TIME producer and consumer object. */ -typedef struct{ - CO_EM_t *em; /**< From CO_TIME_init() */ - CO_NMT_internalState_t *operatingState; /**< From CO_TIME_init() */ +typedef struct { + /** Received timestamp data */ + uint8_t timeStamp[CO_TIME_MSG_LENGTH]; + /** Milliseconds after midnight */ + uint32_t ms; + /** Number of days since January 1, 1984 */ + uint16_t days; + /** Residual microseconds calculated inside CO_TIME_process() */ + uint16_t residual_us; /** True, if device is TIME consumer. Calculated from _COB ID TIME Message_ variable from Object dictionary (index 0x1012). */ - bool_t isConsumer; - /** True, if device is TIME producer. Calculated from _COB ID TIME Message_ + bool_t isConsumer; + /** True, if device is TIME producer. Calculated from _COB ID TIME Message_ variable from Object dictionary (index 0x1012). */ - bool_t isProducer; - uint16_t COB_ID; /**< From CO_TIME_init() */ - /** TIME period time in [milliseconds]. Set to TIME period to enable - timeout detection */ - uint32_t periodTime; - /** TIME period timeout time in [milliseconds]. - (periodTimeoutTime = periodTime * 1,5) */ - uint32_t periodTimeoutTime; + bool_t isProducer; /** Variable indicates, if new TIME message received from CAN bus */ - volatile void *CANrxNew; - /** Timer for the TIME message in [microseconds]. - Set to zero after received or transmitted TIME message */ - uint32_t timer; - /** Set to nonzero value, if TIME with wrong data length is received from CAN */ - uint16_t receiveError; + volatile void *CANrxNew; +#if ((CO_CONFIG_TIME) & CO_CONFIG_TIME_PRODUCER) || defined CO_DOXYGEN + /** Interval for time producer in milli seconds */ + uint32_t producerInterval_ms; + /** Sync producer timer */ + uint32_t producerTimer_ms; + /** From CO_TIME_init() */ + CO_CANmodule_t *CANdevTx; + /** CAN transmit buffer */ + CO_CANtx_t *CANtxBuff; +#endif #if ((CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN /** From CO_TIME_initCallbackPre() or NULL */ - void (*pFunctSignalPre)(void *object); + void (*pFunctSignalPre)(void *object); /** From CO_TIME_initCallbackPre() or NULL */ - void *functSignalObjectPre; + void *functSignalObjectPre; #endif - CO_CANmodule_t *CANdevRx; /**< From CO_TIME_init() */ - uint16_t CANdevRxIdx; /**< From CO_TIME_init() */ - CO_CANmodule_t *CANdevTx; /**< From CO_TIME_init() */ - uint16_t CANdevTxIdx; /**< From CO_TIME_init() */ - CO_CANtx_t *TXbuff; /**< CAN transmit buffer */ - TIME_OF_DAY Time; -}CO_TIME_t; +#if ((CO_CONFIG_TIME) & CO_CONFIG_FLAG_OD_DYNAMIC) || defined CO_DOXYGEN + /** Extension for OD object */ + OD_extension_t OD_1012_extension; +#endif +} CO_TIME_t; + /** * Initialize TIME object. @@ -139,29 +123,26 @@ typedef struct{ * Function must be called in the communication reset section. * * @param TIME This object will be initialized. - * @param em Emergency object. - * @param SDO SDO server object. - * @param operatingState Pointer to variable indicating CANopen device NMT internal state. - * @param COB_ID_TIMEMessage Should be intialized with CO_CAN_ID_TIME_STAMP - * @param TIMECyclePeriod TIME period in ms (may also be used in consumer mode for timeout detection (1.5x period)). + * @param OD_1012_cobIdTimeStamp OD entry for 0x1012 -"COB-ID time stamp", + * entry is required. * @param CANdevRx CAN device for TIME reception. * @param CANdevRxIdx Index of receive buffer in the above CAN device. * @param CANdevTx CAN device for TIME transmission. * @param CANdevTxIdx Index of transmit buffer in the above CAN device. + * @param [out] errInfo Additional information in case of error, may be NULL. * - * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT. + * @return #CO_ReturnError_t CO_ERROR_NO on success. */ -CO_ReturnError_t CO_TIME_init( - CO_TIME_t *TIME, - CO_EM_t *em, - CO_SDO_t *SDO, - CO_NMT_internalState_t *operatingState, - uint32_t COB_ID_TIMEMessage, - uint32_t TIMECyclePeriod, - CO_CANmodule_t *CANdevRx, - uint16_t CANdevRxIdx, - CO_CANmodule_t *CANdevTx, - uint16_t CANdevTxIdx); +CO_ReturnError_t CO_TIME_init(CO_TIME_t *TIME, + OD_entry_t *OD_1012_cobIdTimeStamp, + CO_CANmodule_t *CANdevRx, + uint16_t CANdevRxIdx, +#if ((CO_CONFIG_TIME) & CO_CONFIG_TIME_PRODUCER) || defined CO_DOXYGEN + CO_CANmodule_t *CANdevTx, + uint16_t CANdevTxIdx, +#endif + uint32_t *errInfo); + #if ((CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN /** @@ -172,29 +153,56 @@ CO_ReturnError_t CO_TIME_init( * Callback is called after TIME message is received from the CAN bus. * * @param TIME This object. - * @param object Pointer to object, which will be passed to pFunctSignalPre(). Can be NULL + * @param object Pointer to object, which will be passed to pFunctSignalPre(). * @param pFunctSignalPre Pointer to the callback function. Not called if NULL. */ -void CO_TIME_initCallbackPre( - CO_TIME_t *TIME, - void *object, - void (*pFunctSignalPre)(void *object)); +void CO_TIME_initCallbackPre(CO_TIME_t *TIME, + void *object, + void (*pFunctSignalPre)(void *object)); #endif + /** - * Process TIME communication. - * - * Function must be called cyclically. + * Set current time * * @param TIME This object. - * @param timeDifference_us Time difference from previous function call in [microseconds]. - * - * @return 0: No special meaning. - * @return 1: New TIME message recently received (consumer) / transmited (producer). + * @param ms Milliseconds after midnight + * @param days Number of days since January 1, 1984 + * @param producerInterval_ms Interval time for time producer in milliseconds */ -uint8_t CO_TIME_process( - CO_TIME_t *TIME, - uint32_t timeDifference_us); +static inline void CO_TIME_set(CO_TIME_t *TIME, + uint32_t ms, + uint16_t days, + uint32_t producerInterval_ms) +{ + if (TIME != NULL) { + TIME->residual_us = 0; + TIME->ms = ms; + TIME->days = days; + TIME->producerTimer_ms = TIME->producerInterval_ms =producerInterval_ms; + } +} + + +/** + * Process TIME object. + * + * Function must be called cyclically. It updates internal time from received + * time stamp message or from timeDifference_us. It also sends produces + * timestamp message, if producer and producerInterval_ms is set. + * + * @param TIME This object. + * @param timeDifference_us Time difference from previous function call in + * [microseconds]. + * @param NMTisPreOrOperational True if this node is NMT_PRE_OPERATIONAL or + * NMT_OPERATIONAL state. + * + * @return True if new TIME stamp message recently received (consumer). + */ +bool_t CO_TIME_process(CO_TIME_t *TIME, + bool_t NMTisPreOrOperational, + uint32_t timeDifference_us); + /** @} */ /* CO_TIME */ diff --git a/301/CO_config.h b/301/CO_config.h index 701be25..4d44005 100644 --- a/301/CO_config.h +++ b/301/CO_config.h @@ -401,6 +401,8 @@ extern "C" { * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing * received TIME CAN message. * Callback is configured by CO_TIME_initCallbackPre(). + * - #CO_CONFIG_FLAG_OD_DYNAMIC - Enable dynamic configuration - writing to + * object 0x1012 enables / disables time producer or consumer. */ #ifdef CO_DOXYGEN #define CO_CONFIG_TIME (0) diff --git a/CANopen.c b/CANopen.c index e44aaba..e7f1da0 100644 --- a/CANopen.c +++ b/CANopen.c @@ -1067,7 +1067,7 @@ CO_ReturnError_t CO_CANopenInit(CO_t *co, /* SDOserver */ if (CO_GET_CNT(SDO_SRV) > 0) { - OD_entry_t *SDOsrvPar = OD_GET(H1200,OD_H1200_SDO_SERVER_1_PARAM); + OD_entry_t *SDOsrvPar = OD_GET(H1200, OD_H1200_SDO_SERVER_1_PARAM); for (int16_t i = 0; i < CO_GET_CNT(SDO_SRV); i++) { err = CO_SDOserver_init(&co->SDOserver[i], od, @@ -1085,7 +1085,7 @@ CO_ReturnError_t CO_CANopenInit(CO_t *co, #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE if (CO_GET_CNT(SDO_CLI) > 0) { - OD_entry_t *SDOcliPar = OD_GET(H1280,OD_H1280_SDO_CLIENT_1_PARAM); + OD_entry_t *SDOcliPar = OD_GET(H1280, OD_H1280_SDO_CLIENT_1_PARAM); for (int16_t i = 0; i < CO_GET_CNT(SDO_CLI); i++) { err = CO_SDOclient_init(&co->SDOclient[i], od, @@ -1104,15 +1104,14 @@ CO_ReturnError_t CO_CANopenInit(CO_t *co, #if (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE if (CO_GET_CNT(TIME) == 1) { err = CO_TIME_init(co->TIME, - em, - co->SDO[0], - &co->NMT->operatingState, - OD_COB_ID_TIME, - 0, + OD_GET(H1012, OD_H1012_COBID_TIME), co->CANmodule, CO_GET_CO(RX_IDX_TIME), +#if (CO_CONFIG_TIME) & CO_CONFIG_TIME_PRODUCER co->CANmodule, - CO_GET_CO(TX_IDX_TIME)); + CO_GET_CO(TX_IDX_TIME), +#endif + errInfo); if (err) return err; } #endif @@ -1398,7 +1397,7 @@ CO_NMT_reset_cmd_t CO_process(CO_t *co, #if (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE if (CO_GET_CNT(TIME) == 1) { - CO_TIME_process(co->TIME, timeDifference_us); + CO_TIME_process(co->TIME, NMTisPreOrOperational, timeDifference_us); } #endif diff --git a/example/CO_driver_target.h b/example/CO_driver_target.h index d31025f..4aabcb2 100644 --- a/example/CO_driver_target.h +++ b/example/CO_driver_target.h @@ -95,7 +95,8 @@ extern "C" { #ifndef CO_CONFIG_TIME #define CO_CONFIG_TIME (CO_CONFIG_TIME_ENABLE | \ CO_CONFIG_TIME_PRODUCER | \ - CO_CONFIG_FLAG_CALLBACK_PRE) + CO_CONFIG_FLAG_CALLBACK_PRE | \ + CO_CONFIG_FLAG_OD_DYNAMIC) #endif #ifndef CO_CONFIG_SYNC diff --git a/socketCAN/CO_driver_target.h b/socketCAN/CO_driver_target.h index 9003dad..b1febbc 100644 --- a/socketCAN/CO_driver_target.h +++ b/socketCAN/CO_driver_target.h @@ -55,7 +55,6 @@ extern "C" { /* TODO some parts are disabled in non-finished pre-release */ #define CO_CONFIG_HB_CONS (0) -#define CO_CONFIG_TIME (0) #define CO_CONFIG_SYNC (0) #define CO_CONFIG_PDO (0) #define CO_CONFIG_TRACE (0) @@ -119,7 +118,8 @@ extern "C" { #ifndef CO_CONFIG_TIME #define CO_CONFIG_TIME (CO_CONFIG_TIME_ENABLE | \ CO_CONFIG_TIME_PRODUCER | \ - CO_CONFIG_FLAG_CALLBACK_PRE_USED) + CO_CONFIG_FLAG_CALLBACK_PRE_USED | \ + CO_CONFIG_FLAG_OD_DYNAMIC) #endif #ifndef CO_CONFIG_SYNC diff --git a/socketCAN/CO_main_basic.c b/socketCAN/CO_main_basic.c index 8c2d1b6..dbdd227 100644 --- a/socketCAN/CO_main_basic.c +++ b/socketCAN/CO_main_basic.c @@ -90,6 +90,10 @@ #ifndef GATEWAY_ENABLE #define GATEWAY_ENABLE true #endif +/* Interval for time stamp message in milliseconds */ +#ifndef TIME_STAMP_INTERVAL_MS +#define TIME_STAMP_INTERVAL_MS 10000 +#endif /* CANopen object */ CO_t *CO = NULL; @@ -491,6 +495,16 @@ int main (int argc, char *argv[]) { exit(EXIT_FAILURE); } + /* get current time for CO_TIME_set(), since January 1, 1984, UTC. */ + struct timespec ts; + if (clock_gettime(CLOCK_REALTIME, &ts) == -1) { + log_printf(LOG_CRIT, DBG_GENERAL, "clock_gettime(main)", 0); + exit(EXIT_FAILURE); + } + uint16_t time_days = (uint16_t)(ts.tv_sec / (24 * 60 * 60)); + time_days -= 5113; /* difference between Unix epoch and CANopen Epoch */ + uint32_t time_ms = (uint32_t)(ts.tv_sec % (24 * 60 * 60)) * 1000; + time_ms += ts.tv_nsec / 1000000; /* Create epoll functions */ err = CO_epoll_create(&epMain, MAIN_THREAD_INTERVAL_US); @@ -625,6 +639,7 @@ int main (int argc, char *argv[]) { /* First time only initialization. */ if(firstRun) { firstRun = false; + CO_TIME_set(CO->TIME, time_ms, time_days, TIME_STAMP_INTERVAL_MS); #ifndef CO_SINGLE_THREAD /* Create rt_thread and set priority */ if(pthread_create(&rt_thread_id, NULL, rt_thread, NULL) != 0) {