nikon/CANopenNode
1
0
Fork 0
Code Activity Actions

301/CO_time.c updated to newOD. Some principles changed.

Browse source
This commit is contained in:
Janez 2021-01-25 12:52:52 +01:00
parent 9dce037ecb
commit 464a856a3f
8 changed files with 283 additions and 236 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
301/CO_Emergency.h
View file

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

270
301/CO_TIME.c
View file

@ -25,188 +25,212 @@
#include <string.h>
#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 */

202
301/CO_TIME.h
View file

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

2
301/CO_config.h
View file

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

17
CANopen.c
View file

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

3
example/CO_driver_target.h
View file

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

4
socketCAN/CO_driver_target.h
View file

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

15
socketCAN/CO_main_basic.c
View file

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