nikon/CANopenNode
1
0
Fork 0
Code Activity Actions

socketCAN, main_utils.c, Heartbeat triggering rule changed.

Browse source
This commit is contained in:
JanezXD 2015-09-06 23:34:09 +02:00
parent 149c8cb89c
commit 909427c978
3 changed files with 146 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
stack/CO_NMT_Heartbeat.c
View file

@ -183,7 +183,9 @@ CO_NMT_reset_cmd_t CO_NMT_process(
/* Heartbeat producer message & Bootup message */
if((HBtime != 0 && NMT->HBproducerTimer >= HBtime) || NMT->operatingState == CO_NMT_INITIALIZING){
NMT->HBproducerTimer = NMT->HBproducerTimer - HBtime;
/* Start from the beginning. If OS is slow, time sliding may occur. However, heartbeat is
* not for synchronization, it is for health report. */
NMT->HBproducerTimer = 0;
NMT->HB_TXbuff->data[0] = NMT->operatingState;
CO_CANsend(NMT->HB_CANdev, NMT->HB_TXbuff);
@ -288,7 +290,7 @@ CO_NMT_reset_cmd_t CO_NMT_process(
else if (errorBehavior[5] == 2) NMT->operatingState = CO_NMT_STOPPED;
}
/* if operational state is lost, send HB immediatelly. */
/* if operational state is lost, send HB immediately. */
if(NMT->operatingState != CO_NMT_OPERATIONAL)
NMT->HBproducerTimer = HBtime;
}

88
stack/socketCAN/main_utils.c
View file

@ -27,7 +27,7 @@
#include "main_utils.h"
/******************************************************************************/
/* Utilities for 'Timer interval task'. ***************************************/
int taskTmr_init(taskTmr_t *tt, long intervalns, uint16_t *maxTime){
int ret;
@ -55,8 +55,8 @@ int taskTmr_init(taskTmr_t *tt, long intervalns, uint16_t *maxTime){
}
int taskTmr_wait(taskTmr_t *tt, struct timespec *sync){
uint64_t tmrExp;
int ret = 0;
uint64_t tmrExp;
/* Wait for timer to expire */
if(read(tt->fd, &tmrExp, sizeof(tmrExp)) != sizeof(uint64_t)) ret = -1;
@ -88,3 +88,87 @@ int taskTmr_wait(taskTmr_t *tt, struct timespec *sync){
return ret;
}
/* Utilities for 'Mainline task'. *********************************************/
int taskMain_init(taskMain_t *tt, volatile uint16_t *tmr1ms, uint16_t *maxTime){
int ret;
ret = tt->fd = timerfd_create(CLOCK_MONOTONIC, 0);
tt->tmrSpec.it_interval.tv_sec = 0;
tt->tmrSpec.it_interval.tv_nsec = 0;
tt->tmrSpec.it_value.tv_sec = 0;
tt->tmrSpec.it_value.tv_nsec = 1;
tt->tmr1ms = tmr1ms;
tt->tmr1msPrev = *tmr1ms;
tt->maxTime = maxTime;
if(ret != -1){
ret = timerfd_settime(tt->fd, 0, &tt->tmrSpec, NULL);
}
return ret;
}
int taskMain_wait(taskMain_t *tt, uint16_t *timeDiff){
int ret = 0;
uint64_t tmrExp;
uint16_t tmr1msCopy;
/* Wait for timer to expire */
if(read(tt->fd, &tmrExp, sizeof(tmrExp)) != sizeof(uint64_t)) ret = -1;
/* Calculate time difference */
tmr1msCopy = *tt->tmr1ms;
*timeDiff = tmr1msCopy - tt->tmr1msPrev;
tt->tmr1msPrev = tmr1msCopy;
/* Calculate maximum interval in milliseconds (informative) */
if(tt->maxTime != NULL){
if(*timeDiff > *tt->maxTime){
*tt->maxTime = *timeDiff;
}
}
return ret;
}
int taskMain_setDelay(taskMain_t *tt, uint16_t delay){
int ret = 0;
/* Set next shot for the timer */
tt->tmrSpec.it_value.tv_nsec = (long)(++delay) * NSEC_PER_MSEC;
ret = timerfd_settime(tt->fd, 0, &tt->tmrSpec, NULL);
return ret;
}
/* Helpers ********************************************************************/
void printSocketCanOptions(int fdSocket){
struct can_filter rfilter[150];
can_err_mask_t err_mask;
int loopback, recv_own_msgs, enable_can_fd, i, nfilters;
socklen_t len1 = sizeof(rfilter);
socklen_t len2 = sizeof(err_mask);
socklen_t len3 = sizeof(loopback);
socklen_t len4 = sizeof(recv_own_msgs);
socklen_t len5 = sizeof(enable_can_fd);
getsockopt(fdSocket, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, &len1);
getsockopt(fdSocket, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, &len2);
getsockopt(fdSocket, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, &len3);
getsockopt(fdSocket, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, &len4);
getsockopt(fdSocket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_can_fd, &len5);
nfilters = len1 / sizeof(struct can_filter);
for(i=0; i<nfilters; i++){
printf("filter[%02d]: id=0x%08X, mask=0x%08X\n", i, rfilter[i].can_id, rfilter[i].can_mask);
}
printf("err_filter_mask=0x%08X, loopback=%d, recv_own_msgs=%d, enable_can_fd=%d\n",
err_mask, loopback, recv_own_msgs, enable_can_fd);
}

57
stack/socketCAN/main_utils.h
View file

@ -28,10 +28,14 @@
#define MAIN_UTILS_H
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <time.h>
#include <sys/timerfd.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#define NSEC_PER_SEC (1000000000) /* The number of nanoseconds per second. */
@ -40,7 +44,7 @@
/* Utilities for 'Timer interval task'. ***************************************/
/* It is used for short, realtime processing of CANopen SYNC and PDO objects,
* triggered in constant intervals.*/
* triggered in constant, non-sliding intervals. */
typedef struct{
int fd; /* file descriptor */
struct itimerspec tmrSpec;
@ -74,4 +78,55 @@ int taskTmr_init(taskTmr_t *tt, long intervalns, uint16_t *maxTime);
int taskTmr_wait(taskTmr_t *tt, struct timespec *sync);
/* Utilities for 'Mainline task'. *********************************************/
/* It is used for delay in mainline */
typedef struct{
int fd; /* file descriptor */
struct itimerspec tmrSpec;
volatile uint16_t *tmr1ms;
uint16_t tmr1msPrev;
uint16_t *maxTime;
}taskMain_t;
/* Create Linux timerfd and configures tt object.
*
* @param tt This object.
* @param tmr1ms Variable, which is externally incremented each millisecond.
* @param maxTime Pointer to value, which will indicate longest interval in
* milliseconds. If NULL, calculations won't be made.
*
* @return 0 on success, -1 on error, info in errno.
*/
int taskMain_init(taskMain_t *tt, volatile uint16_t *tmr1ms, uint16_t *maxTime);
/* Block for defined delay.
*
* @param tt This object.
* @param timeDiff Return value - time difference from previous call in milliseconds.
*
* @return 0 on success, -1 on error, info in errno.
*/
int taskMain_wait(taskMain_t *tt, uint16_t *timeDiff);
/* Set next delay.
*
* @param tt This object.
* @param delay Delay in milliseconds.
*
* @return 0 on success, -1 on error, info in errno.
*/
int taskMain_setDelay(taskMain_t *tt, uint16_t delay);
/* Helpers ********************************************************************/
/* Print all options of CAN socket using printf function.
*
* @param fdSocket File descriptor for CAN socket.
*/
void printSocketCanOptions(int fdSocket);
#endif