socketCAN now compiles and runs.
This commit is contained in:
Janez
parent
fb2274fc9f
commit
e6b4c1f5b1
9 changed files with 356 additions and 208 deletions
2
Makefile
2
Makefile
|
|
@ -42,7 +42,7 @@ SOURCES = \
|
|||
|
||||
OBJS = $(SOURCES:%.c=%.o)
|
||||
CC ?= gcc
|
||||
OPT = -Og
|
||||
OPT = -g
|
||||
CFLAGS = -Wall $(OPT) $(INCLUDE_DIRS)
|
||||
LDFLAGS = -pthread
|
||||
|
||||
|
|
|
|||
|
|
@ -192,7 +192,7 @@ Some tested USB to CAN interfaces, which are natively integrated into Linux are:
|
|||
- Simple serial [USBtin](http://www.fischl.de/usbtin/) - Start with: `sudo slcand -f -o -c -s8 /dev/ttyACM0 can0; sudo ip link set up can0`
|
||||
- [EMS CPC-USB](http://www.ems-wuensche.com/product/datasheet/html/can-usb-adapter-converter-interface-cpcusb.html) - Start with: `sudo ip link set up can0 type can bitrate 250000`
|
||||
- [PCAN-USB FD](http://www.peak-system.com/PCAN-USB-FD.365.0.html?&L=1) - Needs newer Linux kernel, supports CAN flexible data rate.
|
||||
- You can get the idea of other supported CAN interfaces in [Linux kernel source](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/drivers/net/can).
|
||||
- You can get the idea of other supported CAN interfaces in [Linux kernel source](https://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/drivers/net/can) (Kconfig files).
|
||||
- Beaglebone or Paspberry PI or similar has CAN capes available. On RPI worked
|
||||
also the above USB interfaces, but it was necessary to compile the kernel.
|
||||
|
||||
|
|
|
|||
|
|
@ -25,13 +25,17 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include <sys/timerfd.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <time.h>
|
||||
#include <sys/epoll.h>
|
||||
#include <sys/eventfd.h>
|
||||
#include <sys/timerfd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include "CANopen.h"
|
||||
|
||||
|
||||
/* Helper function - get monotonic clock time in microseconds */
|
||||
static uint64_t CO_LinuxThreads_clock_gettime_us(void)
|
||||
{
|
||||
|
|
@ -41,7 +45,8 @@ static uint64_t CO_LinuxThreads_clock_gettime_us(void)
|
|||
return ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
|
||||
}
|
||||
|
||||
/* Mainline thread (threadMain) ***********************************************/
|
||||
|
||||
/* Mainline thread - basic (threadMain) ***************************************/
|
||||
static struct
|
||||
{
|
||||
uint64_t start; /* time value CO_process() was called last time in us */
|
||||
|
|
@ -69,6 +74,7 @@ void threadMain_process(CO_NMT_reset_cmd_t *reset)
|
|||
|
||||
now = CO_LinuxThreads_clock_gettime_us();
|
||||
diff = (uint32_t)(now - threadMain.start);
|
||||
threadMain.start = now;
|
||||
|
||||
/* we use timerNext_us in CO_process() as indication if processing is
|
||||
* finished. We ignore any calculated values for maximum delay times. */
|
||||
|
|
@ -78,10 +84,160 @@ void threadMain_process(CO_NMT_reset_cmd_t *reset)
|
|||
diff = 0;
|
||||
} while ((*reset == CO_RESET_NOT) && (finished == 0));
|
||||
|
||||
/* prepare next call */
|
||||
threadMain.start = now;
|
||||
}
|
||||
|
||||
|
||||
/* Mainline thread - Blocking (threadMainWait) ********************************/
|
||||
static struct
|
||||
{
|
||||
uint64_t start; /* time value CO_process() was called last time in us */
|
||||
int epoll_fd; /* epoll file descriptor */
|
||||
int event_fd; /* notification event file descriptor */
|
||||
int timer_fd; /* interval timer file descriptor */
|
||||
uint32_t interval_us; /* interval for threadMainWait_process */
|
||||
struct itimerspec tm; /* structure for timer configuration */
|
||||
} threadMainWait;
|
||||
|
||||
static void threadMainWait_callback(void *object)
|
||||
{
|
||||
/* send event to wake threadMainWait_process() */
|
||||
uint64_t u = 1;
|
||||
ssize_t s;
|
||||
s = write(threadMainWait.event_fd, &u, sizeof(uint64_t));
|
||||
if (s != sizeof(uint64_t)) {
|
||||
log_printf(LOG_DEBUG, DBG_ERRNO, "write()");
|
||||
}
|
||||
}
|
||||
|
||||
void threadMainWait_init(uint32_t interval_us)
|
||||
{
|
||||
int32_t ret;
|
||||
struct epoll_event ev;
|
||||
|
||||
/* Configure callback functions */
|
||||
CO_SDO_initCallback(CO->SDO[0], NULL, threadMainWait_callback);
|
||||
CO_EM_initCallback(CO->em, NULL, threadMainWait_callback);
|
||||
|
||||
/* Initial value for time calculation */
|
||||
threadMainWait.start = CO_LinuxThreads_clock_gettime_us();
|
||||
|
||||
/* Configure epoll for mainline */
|
||||
threadMainWait.epoll_fd = epoll_create(1);
|
||||
if (threadMainWait.epoll_fd < 0) {
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_create()");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
/* Configure eventfd for notifications and add it to epoll */
|
||||
threadMainWait.event_fd = eventfd(0, 0);
|
||||
if (threadMainWait.event_fd < 0) {
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "eventfd()");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
ev.events = EPOLLIN;
|
||||
ev.data.fd = threadMainWait.event_fd;
|
||||
ret = epoll_ctl(threadMainWait.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
|
||||
if (ret < 0){
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(event_fd)");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
/* Configure timer for interval_us and add it to epoll */
|
||||
threadMainWait.timer_fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK);
|
||||
if (threadMainWait.timer_fd < 0) {
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_create()");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
threadMainWait.interval_us = interval_us;
|
||||
threadMainWait.tm.it_interval.tv_sec = interval_us / 1000000;
|
||||
threadMainWait.tm.it_interval.tv_nsec = (interval_us % 1000000) * 1000;
|
||||
threadMainWait.tm.it_value.tv_sec = 0;
|
||||
threadMainWait.tm.it_value.tv_nsec = 1;
|
||||
ret = timerfd_settime(threadMainWait.timer_fd, 0, &threadMainWait.tm, NULL);
|
||||
if (ret < 0){
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_settime");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
ev.events = EPOLLIN;
|
||||
ev.data.fd = threadMainWait.timer_fd;
|
||||
ret = epoll_ctl(threadMainWait.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
|
||||
if (ret < 0){
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(timer_fd)");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
}
|
||||
|
||||
void threadMainWait_close(void)
|
||||
{
|
||||
CO_SDO_initCallback(CO->SDO[0], NULL, NULL);
|
||||
CO_EM_initCallback(CO->em, NULL, NULL);
|
||||
|
||||
close(threadMainWait.epoll_fd);
|
||||
close(threadMainWait.event_fd);
|
||||
close(threadMainWait.timer_fd);
|
||||
threadMainWait.epoll_fd = -1;
|
||||
threadMainWait.event_fd = -1;
|
||||
threadMainWait.timer_fd = -1;
|
||||
}
|
||||
|
||||
uint32_t threadMainWait_process(CO_NMT_reset_cmd_t *reset)
|
||||
{
|
||||
int ready, ret;
|
||||
struct epoll_event ev;
|
||||
uint64_t ull;
|
||||
ssize_t s;
|
||||
uint32_t diff, timerNext_us;
|
||||
|
||||
/* wait for event or timer expiration and read data from file descriptors */
|
||||
ready = epoll_wait(threadMainWait.epoll_fd, &ev, 1, -1);
|
||||
if (ready != 1 && errno != EINTR) {
|
||||
log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_wait");
|
||||
}
|
||||
else if (ev.data.fd == threadMainWait.event_fd) {
|
||||
s = read(threadMainWait.event_fd, &ull, sizeof(uint64_t));
|
||||
if (s != sizeof(uint64_t)) {
|
||||
log_printf(LOG_DEBUG, DBG_ERRNO, "read(event_fd)");
|
||||
}
|
||||
}
|
||||
else if (ev.data.fd == threadMainWait.timer_fd) {
|
||||
s = read(threadMainWait.timer_fd, &ull, sizeof(uint64_t));
|
||||
if (s != sizeof(uint64_t) && errno != EAGAIN) {
|
||||
log_printf(LOG_DEBUG, DBG_ERRNO, "read(timer_fd)");
|
||||
}
|
||||
}
|
||||
|
||||
/* calculate time difference since last call */
|
||||
ull = CO_LinuxThreads_clock_gettime_us();
|
||||
diff = (uint32_t)(ull - threadMainWait.start);
|
||||
threadMainWait.start = ull;
|
||||
|
||||
/* stack will lower this, if necessary */
|
||||
timerNext_us = threadMainWait.interval_us;
|
||||
|
||||
/* process CANopen objects */
|
||||
*reset = CO_process(CO, diff, &timerNext_us);
|
||||
|
||||
/* lower next timer interval if necessary */
|
||||
if (timerNext_us < threadMainWait.interval_us) {
|
||||
/* add one microsecond extra delay and make sure it is not zero */
|
||||
timerNext_us += 1;
|
||||
if (threadMainWait.interval_us < 1000000) {
|
||||
threadMainWait.tm.it_value.tv_nsec = timerNext_us * 1000;
|
||||
} else {
|
||||
threadMainWait.tm.it_value.tv_sec = timerNext_us / 1000000;
|
||||
threadMainWait.tm.it_value.tv_nsec = (timerNext_us % 1000000) * 1000;
|
||||
}
|
||||
ret = timerfd_settime(threadMainWait.timer_fd, 0,
|
||||
&threadMainWait.tm, NULL);
|
||||
if (ret < 0){
|
||||
log_printf(LOG_DEBUG, DBG_ERRNO, "timerfd_settime");
|
||||
}
|
||||
}
|
||||
|
||||
return diff;
|
||||
}
|
||||
|
||||
|
||||
/* Realtime thread (threadRT) *************************************************/
|
||||
static struct {
|
||||
uint32_t us_interval; /* configured interval in us */
|
||||
|
|
@ -108,12 +264,12 @@ void CANrx_threadTmr_close(void)
|
|||
threadRT.interval_fd = -1;
|
||||
}
|
||||
|
||||
void CANrx_threadTmr_process(void)
|
||||
uint32_t CANrx_threadTmr_process(void)
|
||||
{
|
||||
int32_t result;
|
||||
uint64_t i;
|
||||
bool_t syncWas;
|
||||
uint64_t missed;
|
||||
uint64_t missed = 0;
|
||||
|
||||
result = CO_CANrxWait(CO->CANmodule[0], threadRT.interval_fd, NULL);
|
||||
if (result < 0) {
|
||||
|
|
@ -143,4 +299,6 @@ void CANrx_threadTmr_process(void)
|
|||
CO_UNLOCK_OD();
|
||||
}
|
||||
}
|
||||
|
||||
return (uint32_t) missed;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,12 +38,17 @@ extern "C" {
|
|||
* @defgroup CO_socketCAN socketCAN
|
||||
* @{
|
||||
*
|
||||
* Linux specific interface to CANopenNode
|
||||
* Linux specific interface to CANopenNode.
|
||||
*
|
||||
* CANopenNode runs on top of SocketCAN interface, which is part of the Linux
|
||||
* kernel. For more info on Linux SocketCAN see
|
||||
* https://www.kernel.org/doc/html/latest/networking/can.html
|
||||
*
|
||||
* CANopenNode runs in two threads:
|
||||
* - timer based real-time thread for CAN receive, SYNC and PDO, see
|
||||
* CANrx_threadTmr_process()
|
||||
* - mainline thread for other processing, see threadMain_process()
|
||||
* - mainline thread for other processing, see threadMain_process() or
|
||||
* threadMainWait_process()
|
||||
*
|
||||
* The "threads" specified here do not fork threads themselves, but require
|
||||
* that two threads are provided by the calling application.
|
||||
|
|
@ -51,28 +56,29 @@ extern "C" {
|
|||
|
||||
|
||||
/**
|
||||
* Initialize mainline thread.
|
||||
* Initialize mainline thread - basic.
|
||||
*
|
||||
* @param callback this function is called to indicate #threadMain_process() has
|
||||
* work to do
|
||||
* @param object this pointer is given to _callback()_
|
||||
*/
|
||||
extern void threadMain_init(void (*callback)(void*), void *object);
|
||||
void threadMain_init(void (*callback)(void*), void *object);
|
||||
|
||||
|
||||
/**
|
||||
* Cleanup mainline thread.
|
||||
* Cleanup mainline thread - basic.
|
||||
*/
|
||||
extern void threadMain_close(void);
|
||||
void threadMain_close(void);
|
||||
|
||||
|
||||
/**
|
||||
* Process mainline thread.
|
||||
* Process mainline thread - basic.
|
||||
*
|
||||
* threadMain is non-realtime thread for CANopenNode processing. It is
|
||||
* initialized by threadMain_init(). There is no configuration for CANopen
|
||||
* objects. There is also no configuration for epool or interval timer or notify
|
||||
* pipe. These must be specified externally.
|
||||
* objects. There is also no configuration for epool or interval timer or
|
||||
* eventfd. These must be specified externally. For more complete function see
|
||||
* threadMainWait_process(), they are included.
|
||||
*
|
||||
* threadMain_process() calls CO_process() function for processing mainline
|
||||
* CANopen objects. It is non-blocking and should be called cyclically in 50 ms
|
||||
|
|
@ -81,7 +87,39 @@ extern void threadMain_close(void);
|
|||
*
|
||||
* @param reset return value from CO_process() function.
|
||||
*/
|
||||
extern void threadMain_process(CO_NMT_reset_cmd_t *reset);
|
||||
void threadMain_process(CO_NMT_reset_cmd_t *reset);
|
||||
|
||||
|
||||
/**
|
||||
* Initialize mainline thread - blocking.
|
||||
*
|
||||
* @param interval_us interval of the threadMainWait_process()
|
||||
*/
|
||||
void threadMainWait_init(uint32_t interval_us);
|
||||
|
||||
|
||||
/**
|
||||
* Cleanup mainline thread - blocking.
|
||||
*/
|
||||
void threadMainWait_close(void);
|
||||
|
||||
|
||||
/**
|
||||
* Process mainline thread - blocking.
|
||||
*
|
||||
* threadMainWait is non-realtime thread for CANopenNode processing. It is
|
||||
* initialized by threadMainWait_init(). There is no configuration for CANopen
|
||||
* objects. But there is configuration for epool, interval timer and eventfd.
|
||||
* Function must be called inside loop. It blocks for correct time and unblocks
|
||||
* automatically in case of event. It calls CO_process() function for processing
|
||||
* mainline CANopen objects.
|
||||
* For more basic function see threadMain_process() alternative.
|
||||
*
|
||||
* @param reset return value from CO_process() function.
|
||||
*
|
||||
* @return time difference since last call in microseconds
|
||||
*/
|
||||
uint32_t threadMainWait_process(CO_NMT_reset_cmd_t *reset);
|
||||
|
||||
|
||||
/**
|
||||
|
|
@ -90,13 +128,13 @@ extern void threadMain_process(CO_NMT_reset_cmd_t *reset);
|
|||
* @param interval_us Interval of periodic timer in microseconds, recommended
|
||||
* value for realtime response: 1000 us
|
||||
*/
|
||||
extern void CANrx_threadTmr_init(uint32_t interval_us);
|
||||
void CANrx_threadTmr_init(uint32_t interval_us);
|
||||
|
||||
|
||||
/**
|
||||
* Terminate realtime thread.
|
||||
*/
|
||||
extern void CANrx_threadTmr_close(void);
|
||||
void CANrx_threadTmr_close(void);
|
||||
|
||||
|
||||
/**
|
||||
|
|
@ -117,8 +155,10 @@ extern void CANrx_threadTmr_close(void);
|
|||
*
|
||||
* @remark If realtime is required, this thread must be registered as such in
|
||||
* the Linux kernel.
|
||||
*
|
||||
* @return Number of interval timer passes since last call.
|
||||
*/
|
||||
extern void CANrx_threadTmr_process();
|
||||
void CANrx_threadTmr_process(void);
|
||||
|
||||
/** @} */
|
||||
|
||||
|
|
|
|||
|
|
@ -241,8 +241,7 @@ CO_ReturnError_t CO_OD_storage_init(
|
|||
odStor->odSize = odSize;
|
||||
odStor->filename = filename;
|
||||
odStor->fp = NULL;
|
||||
odStor->tmr1msPrev = 0;
|
||||
odStor->lastSavedMs = 0;
|
||||
odStor->lastSavedUs = 0;
|
||||
|
||||
buf = malloc(odStor->odSize);
|
||||
if(buf == NULL) {
|
||||
|
|
@ -292,8 +291,8 @@ CO_ReturnError_t CO_OD_storage_init(
|
|||
/******************************************************************************/
|
||||
CO_ReturnError_t CO_OD_storage_autoSave(
|
||||
CO_OD_storage_t *odStor,
|
||||
uint16_t timer1ms,
|
||||
uint16_t delay)
|
||||
uint32_t timer1usDiff,
|
||||
uint32_t delay_us)
|
||||
{
|
||||
CO_ReturnError_t ret = CO_ERROR_NO;
|
||||
|
||||
|
|
@ -303,10 +302,8 @@ CO_ReturnError_t CO_OD_storage_autoSave(
|
|||
}
|
||||
|
||||
/* don't save file more often than delay */
|
||||
if(odStor->lastSavedMs < delay) {
|
||||
odStor->lastSavedMs += timer1ms - odStor->tmr1msPrev;
|
||||
}
|
||||
else {
|
||||
odStor->lastSavedUs += timer1usDiff;
|
||||
if (odStor->lastSavedUs > delay_us) {
|
||||
void *buf = NULL;
|
||||
bool_t saveData = false;
|
||||
|
||||
|
|
@ -360,14 +357,12 @@ CO_ReturnError_t CO_OD_storage_autoSave(
|
|||
|
||||
fflush(odStor->fp);
|
||||
|
||||
odStor->lastSavedMs = 0;
|
||||
odStor->lastSavedUs = 0;
|
||||
}
|
||||
|
||||
free(buf);
|
||||
}
|
||||
|
||||
odStor->tmr1msPrev = timer1ms;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -108,8 +108,7 @@ typedef struct {
|
|||
char *filename; /**< From CO_OD_storage_init() */
|
||||
/** If CO_OD_storage_autoSave() is used, file stays opened and fp is stored here. */
|
||||
FILE *fp;
|
||||
uint16_t tmr1msPrev; /**< used with CO_OD_storage_autoSave. */
|
||||
uint32_t lastSavedMs; /**< used with CO_OD_storage_autoSave. */
|
||||
uint32_t lastSavedUs; /**< used with CO_OD_storage_autoSave. */
|
||||
} CO_OD_storage_t;
|
||||
|
||||
|
||||
|
|
@ -143,16 +142,17 @@ CO_ReturnError_t CO_OD_storage_init(
|
|||
* CRC bytes. File remains opened.
|
||||
*
|
||||
* @param odStor OD storage object.
|
||||
* @param timer1ms Variable, which must increment each millisecond.
|
||||
* @param delay Delay (inhibit) time between writes to disk in milliseconds (60000 for example).
|
||||
* @param timer1usDiff Time difference in microseconds since last call.
|
||||
* @param delay_us Delay (inhibit) time between writes to disk in microseconds
|
||||
* (60000 for example).
|
||||
*
|
||||
* @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_DATA_CORRUPT (Data in file corrupt),
|
||||
* CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_OUT_OF_MEMORY (malloc failed).
|
||||
* @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_DATA_CORRUPT (Data in file
|
||||
* corrupt), CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_OUT_OF_MEMORY (malloc failed).
|
||||
*/
|
||||
CO_ReturnError_t CO_OD_storage_autoSave(
|
||||
CO_OD_storage_t *odStor,
|
||||
uint16_t timer1ms,
|
||||
uint16_t delay);
|
||||
uint32_t timer1usDiff,
|
||||
uint32_t delay_us);
|
||||
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -59,10 +59,10 @@ extern "C" {
|
|||
* default system stores messages in /var/log/syslog file.
|
||||
* Log can optionally be configured before, for example to filter out less
|
||||
* critical errors than LOG_NOTICE, specify program name, print also process PID
|
||||
* and print also to standard error, use:
|
||||
* and print also to standard error, set 'user' type of program, use:
|
||||
* @code
|
||||
* setlogmask (LOG_UPTO (LOG_NOTICE));
|
||||
* openlog ("exampleprog", LOG_PID | LOG_PERROR);
|
||||
* openlog ("exampleprog", LOG_PID | LOG_PERROR, LOG_USER);
|
||||
* @endcode
|
||||
*
|
||||
* @param priority one of LOG_EMERG, LOG_ALERT, LOG_CRIT, LOG_ERR, LOG_WARNING,
|
||||
|
|
|
|||
|
|
@ -77,6 +77,18 @@ extern "C" {
|
|||
#define DBG_CAN_SET_LISTEN_ONLY "(%s) %s Set Listen Only", __func__
|
||||
#define DBG_CAN_CLR_LISTEN_ONLY "(%s) %s Leave Listen Only", __func__
|
||||
|
||||
/* mainline */
|
||||
#define DBG_EMERGENCY_RX "CANopen Emergency message from node 0x%02X: errorCode=0x%04X, errorRegister=0x%02X, errorBit=0x%02X, infoCode=0x%08X"
|
||||
#define DBG_NOT_TCP_PORT "(%s) -t argument \'%s\' is not a valid tcp port", __func__
|
||||
#define DBG_WRONG_NODE_ID "(%s) Wrong node ID \"%d\"", __func__
|
||||
#define DBG_WRONG_PRIORITY "(%s) Wrong RT priority \"%d\"", __func__
|
||||
#define DBG_NO_CAN_DEVICE "(%s) Can't find CAN device \"%s\"", __func__
|
||||
#define DBG_OBJECT_DICTIONARY "(%s) Error in Object Dictionary \"%s\"", __func__
|
||||
#define DBG_CAN_OPEN "(%s) CANopen error in %s, err=%d", __func__
|
||||
#define DBG_CAN_OPEN_INFO "(%s) CANopen device, Node ID = %d(0x%02X), %s", __func__
|
||||
#define DBG_COMMAND_LOCAL_INFO "(%s) Command interface on socket \"%s\" started", __func__
|
||||
#define DBG_COMMAND_TCP_INFO "(%s) Command interface on tcp port \"%hu\" started", __func__
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
|
|
|||
|
|
@ -55,16 +55,20 @@
|
|||
|
||||
/* Use DS309-3 standard - ASCII command interface to CANopen: NMT master,
|
||||
* LSS master and SDO client */
|
||||
#ifdef CO_309
|
||||
#if CO_CONFIG_309 > 0
|
||||
#include "CO_command.h"
|
||||
#endif
|
||||
|
||||
/* Interval of real-time thread in microseconds */
|
||||
/* Interval of mainline and real-time thread in microseconds */
|
||||
#ifndef MAIN_THREAD_INTERVAL_US
|
||||
#define MAIN_THREAD_INTERVAL_US 100000
|
||||
#endif
|
||||
#ifndef TMR_THREAD_INTERVAL_US
|
||||
#define TMR_THREAD_INTERVAL_US 1000
|
||||
#endif
|
||||
|
||||
#ifdef CO_309
|
||||
|
||||
#if CO_CONFIG_309 > 0
|
||||
/* Mutex is locked, when CAN is not valid (configuration state). May be used
|
||||
* from command interface. RT threads may use CO->CANmodule[0]->CANnormal instead. */
|
||||
pthread_mutex_t CO_CAN_VALID_mtx = PTHREAD_MUTEX_INITIALIZER;
|
||||
|
|
@ -72,25 +76,21 @@ pthread_mutex_t CO_CAN_VALID_mtx = PTHREAD_MUTEX_INITIALIZER;
|
|||
|
||||
/* Other variables and objects */
|
||||
static int rtPriority = -1; /* Real time priority, configurable by arguments. (-1=RT disabled) */
|
||||
static int mainline_epoll_fd; /* epoll file descriptor for mainline */
|
||||
static int nodeId = -1; /* Use value from Object Dictionary or set to 1..127 by arguments */
|
||||
static CO_OD_storage_t odStor; /* Object Dictionary storage object for CO_OD_ROM */
|
||||
static CO_OD_storage_t odStorAuto; /* Object Dictionary storage object for CO_OD_EEPROM */
|
||||
static char *odStorFile_rom = "od_storage"; /* Name of the file */
|
||||
static char *odStorFile_eeprom = "od_storage_auto"; /* Name of the file */
|
||||
#ifdef CO_309
|
||||
#if CO_CONFIG_309 > 0
|
||||
static in_port_t CO_command_socket_tcp_port = 60000; /* default port when used in tcp gateway mode */
|
||||
#endif
|
||||
#if CO_NO_TRACE > 0
|
||||
static CO_time_t CO_time; /* Object for current time */
|
||||
#endif
|
||||
|
||||
/* Helper functions ***********************************************************/
|
||||
/* Realtime thread */
|
||||
#ifdef CO_MULTI_THREAD
|
||||
static void* rt_thread(void* arg);
|
||||
static pthread_t rt_thread_id;
|
||||
static int rt_thread_epoll_fd;
|
||||
#endif
|
||||
|
||||
static void* rt_thread(void* arg);
|
||||
|
||||
/* Signal handler */
|
||||
volatile sig_atomic_t CO_endProgram = 0;
|
||||
|
|
@ -98,24 +98,25 @@ static void sigHandler(int sig) {
|
|||
CO_endProgram = 1;
|
||||
}
|
||||
|
||||
/* callback for emergency messages */
|
||||
static void EmergencyRxCallback(const uint16_t ident,
|
||||
const uint16_t errorCode,
|
||||
const uint8_t errorRegister,
|
||||
const uint8_t errorBit,
|
||||
const uint32_t infoCode)
|
||||
{
|
||||
int16_t nodeIdRx = ident ? (ident&0x7F) : nodeId;
|
||||
|
||||
/* Helper functions ***********************************************************/
|
||||
void CO_errExit(char* msg) {
|
||||
perror(msg);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
/* send CANopen generic emergency message */
|
||||
void CO_error(const uint32_t info) {
|
||||
CO_errorReport(CO->em, CO_EM_GENERIC_SOFTWARE_ERROR, CO_EMC_SOFTWARE_INTERNAL, info);
|
||||
fprintf(stderr, "canopend generic error: 0x%X\n", info);
|
||||
log_printf(LOG_NOTICE, DBG_EMERGENCY_RX, nodeIdRx, errorCode,
|
||||
errorRegister, errorBit, infoCode);
|
||||
}
|
||||
|
||||
|
||||
/* Print usage */
|
||||
static void printUsage(char *progName) {
|
||||
fprintf(stderr,
|
||||
printf(
|
||||
"Usage: %s <CAN device name> [options]\n", progName);
|
||||
fprintf(stderr,
|
||||
printf(
|
||||
"\n"
|
||||
"Options:\n"
|
||||
" -i <Node ID> CANopen Node-id (1..127). If not specified, value from\n"
|
||||
|
|
@ -126,31 +127,30 @@ fprintf(stderr,
|
|||
" -s <ODstorage file> Set Filename for OD storage ('od_storage' is default).\n"
|
||||
" -a <ODstorageAuto> Set Filename for automatic storage variables from\n"
|
||||
" Object dictionary. ('od_storage_auto' is default).\n");
|
||||
#ifdef CO_309
|
||||
fprintf(stderr,
|
||||
#if CO_CONFIG_309 > 0
|
||||
printf(
|
||||
" -c <Socket path> Enable command interface for master functionality. \n"
|
||||
" If socket path is specified as empty string \"\",\n"
|
||||
" default '%s' will be used.\n"
|
||||
" Note that location of socket path may affect security.\n"
|
||||
" See 'canopencomm/canopencomm --help' for more info.\n"
|
||||
, CO_command_socketPath);
|
||||
fprintf(stderr,
|
||||
" -t <port> Enable command interface for master functionality over tcp, \n"
|
||||
" listen to <port>.\n"
|
||||
printf(
|
||||
" -t <port> Enable command interface for master functionality over\n"
|
||||
" tcp, listen to <port>.\n"
|
||||
" Note that using this mode may affect security.\n"
|
||||
);
|
||||
#endif
|
||||
fprintf(stderr,
|
||||
printf(
|
||||
"\n"
|
||||
"See also: https://github.com/CANopenNode/CANopenNode\n"
|
||||
"\n");
|
||||
}
|
||||
|
||||
|
||||
/******************************************************************************/
|
||||
/** Mainline and RT thread **/
|
||||
/******************************************************************************/
|
||||
/*Mainline thread *************************************************************/
|
||||
int main (int argc, char *argv[]) {
|
||||
pthread_t rt_thread_id;
|
||||
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
|
||||
CO_ReturnError_t err;
|
||||
CO_ReturnError_t odStorStatus_rom, odStorStatus_eeprom;
|
||||
|
|
@ -160,20 +160,21 @@ int main (int argc, char *argv[]) {
|
|||
|
||||
char* CANdevice = NULL; /* CAN device, configurable by arguments. */
|
||||
bool_t nodeIdFromArgs = false; /* True, if program arguments are used for CANopen Node Id */
|
||||
int nodeId = -1; /* Use value from Object Dictionary or set to 1..127 by arguments */
|
||||
bool_t rebootEnable = false; /* Configurable by arguments */
|
||||
#ifdef CO_309
|
||||
#if CO_CONFIG_309 > 0
|
||||
typedef enum CMD_MODE {CMD_NONE, CMD_LOCAL, CMD_REMOTE} cmdMode_t;
|
||||
cmdMode_t commandEnable = CMD_NONE; /* Configurable by arguments */
|
||||
#endif
|
||||
|
||||
/* configure system log */
|
||||
setlogmask(LOG_UPTO (LOG_DEBUG)); /* LOG_DEBUG - log all meessages */
|
||||
openlog(argv[0], LOG_PID | LOG_PERROR, LOG_USER); /* print also to standard error */
|
||||
|
||||
/* Get program options */
|
||||
if(argc < 2 || strcmp(argv[1], "--help") == 0){
|
||||
printUsage(argv[0]);
|
||||
exit(EXIT_SUCCESS);
|
||||
}
|
||||
|
||||
|
||||
/* Get program options */
|
||||
while((opt = getopt(argc, argv, "i:p:rc:t:s:a:")) != -1) {
|
||||
switch (opt) {
|
||||
case 'i':
|
||||
|
|
@ -182,7 +183,7 @@ int main (int argc, char *argv[]) {
|
|||
break;
|
||||
case 'p': rtPriority = strtol(optarg, NULL, 0); break;
|
||||
case 'r': rebootEnable = true; break;
|
||||
#ifdef CO_309
|
||||
#if CO_CONFIG_309 > 0
|
||||
case 'c':
|
||||
/* In case of empty string keep default name, just enable interface. */
|
||||
if(strlen(optarg) != 0) {
|
||||
|
|
@ -196,8 +197,8 @@ int main (int argc, char *argv[]) {
|
|||
//CO_command_socket_tcp_port = optarg;
|
||||
int scanResult = sscanf(optarg, "%hu", &CO_command_socket_tcp_port);
|
||||
if(scanResult != 1){ //expect one argument to be extracted
|
||||
printf("ERROR: -t argument \'%s\' is not a valid tcp port\n", optarg);
|
||||
exit(EXIT_FAILURE);
|
||||
log_printf(LOG_CRIT, DBG_NOT_TCP_PORT, optarg);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
}
|
||||
commandEnable = CMD_REMOTE;
|
||||
|
|
@ -217,47 +218,46 @@ int main (int argc, char *argv[]) {
|
|||
}
|
||||
|
||||
if(nodeIdFromArgs && (nodeId < 1 || nodeId > 127)) {
|
||||
fprintf(stderr, "Wrong node ID (%d)\n", nodeId);
|
||||
log_printf(LOG_CRIT, DBG_WRONG_NODE_ID, nodeId);
|
||||
printUsage(argv[0]);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if(rtPriority != -1 && (rtPriority < sched_get_priority_min(SCHED_FIFO)
|
||||
|| rtPriority > sched_get_priority_max(SCHED_FIFO))) {
|
||||
fprintf(stderr, "Wrong RT priority (%d)\n", rtPriority);
|
||||
log_printf(LOG_CRIT, DBG_WRONG_PRIORITY, rtPriority);
|
||||
printUsage(argv[0]);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if(CANdevice0Index == 0) {
|
||||
char s[120];
|
||||
snprintf(s, 120, "Can't find CAN device \"%s\"", CANdevice);
|
||||
CO_errExit(s);
|
||||
log_printf(LOG_CRIT, DBG_NO_CAN_DEVICE, CANdevice);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
|
||||
printf("%s - starting CANopen device with Node ID %d(0x%02X)", argv[0], nodeId, nodeId);
|
||||
log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, nodeId, nodeId, "starting");
|
||||
|
||||
|
||||
/* Allocate memory for CANopen objects */
|
||||
err = CO_new();
|
||||
err = CO_new(NULL);
|
||||
if (err != CO_ERROR_NO) {
|
||||
fprintf(stderr, "Program init - %s - CO_new() failed.\n", argv[0]);
|
||||
log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_new()", err);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
|
||||
/* Verify, if OD structures have proper alignment of initial values */
|
||||
if(CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord) {
|
||||
fprintf(stderr, "Program init - %s - Error in CO_OD_RAM.\n", argv[0]);
|
||||
log_printf(LOG_CRIT, DBG_OBJECT_DICTIONARY, "CO_OD_RAM");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
if(CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord) {
|
||||
fprintf(stderr, "Program init - %s - Error in CO_OD_EEPROM.\n", argv[0]);
|
||||
log_printf(LOG_CRIT, DBG_OBJECT_DICTIONARY, "CO_OD_EEPROM");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
if(CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord) {
|
||||
fprintf(stderr, "Program init - %s - Error in CO_OD_ROM.\n", argv[0]);
|
||||
log_printf(LOG_CRIT, DBG_OBJECT_DICTIONARY, "CO_OD_ROM");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
|
|
@ -268,22 +268,23 @@ int main (int argc, char *argv[]) {
|
|||
|
||||
|
||||
/* Catch signals SIGINT and SIGTERM */
|
||||
if(signal(SIGINT, sigHandler) == SIG_ERR)
|
||||
CO_errExit("Program init - SIGINIT handler creation failed");
|
||||
if(signal(SIGTERM, sigHandler) == SIG_ERR)
|
||||
CO_errExit("Program init - SIGTERM handler creation failed");
|
||||
|
||||
/* increase variable each startup. Variable is automatically stored in non-volatile memory. */
|
||||
printf(", count=%u ...\n", ++OD_powerOnCounter);
|
||||
if(signal(SIGINT, sigHandler) == SIG_ERR) {
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "signal(SIGINT, sigHandler)");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
if(signal(SIGTERM, sigHandler) == SIG_ERR) {
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "signal(SIGTERM, sigHandler)");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
|
||||
while(reset != CO_RESET_APP && reset != CO_RESET_QUIT && CO_endProgram == 0) {
|
||||
/* CANopen communication reset - initialize CANopen objects *******************/
|
||||
|
||||
printf("%s - communication reset ...\n", argv[0]);
|
||||
log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, nodeId, nodeId, "communication reset");
|
||||
|
||||
|
||||
#ifdef CO_309
|
||||
#if CO_CONFIG_309 > 0
|
||||
/* Wait other threads (command interface). */
|
||||
pthread_mutex_lock(&CO_CAN_VALID_mtx);
|
||||
#endif
|
||||
|
|
@ -307,16 +308,19 @@ int main (int argc, char *argv[]) {
|
|||
}
|
||||
|
||||
err = CO_CANinit((void *)CANdevice0Index, 0 /* bit rate not used */);
|
||||
if (err == CO_ERROR_NO) {
|
||||
err = CO_CANopenInit(nodeId);
|
||||
}
|
||||
|
||||
if(err != CO_ERROR_NO) {
|
||||
char s[120];
|
||||
snprintf(s, 120, "Communication reset - CANopen initialization failed, err=%d", err);
|
||||
CO_errExit(s);
|
||||
log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANinit()", err);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
err = CO_CANopenInit(nodeId);
|
||||
if(err != CO_ERROR_NO) {
|
||||
log_printf(LOG_CRIT, DBG_CAN_OPEN, "CO_CANopenInit()", err);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
/* initialize callbacks */
|
||||
CO_EM_initCallbackRx(CO->em, EmergencyRxCallback);
|
||||
|
||||
/* initialize OD objects 1010 and 1011 and verify errors. */
|
||||
CO_OD_configure(CO->SDO[0], OD_H1010_STORE_PARAM_FUNC, CO_ODF_1010, (void*)&odStor, 0, 0U);
|
||||
|
|
@ -329,13 +333,6 @@ int main (int argc, char *argv[]) {
|
|||
}
|
||||
|
||||
|
||||
/* Configure callback functions for thread control */
|
||||
// void CO_CANopenInitCallback(void *object, void (*pFunctSignal)(void *object));
|
||||
// CO_EM_initCallback(CO->em, threadMain_cbSignal);
|
||||
// CO_SDO_initCallback(CO->SDO[0], threadMain_cbSignal);
|
||||
// CO_SDOclient_initCallback(CO->SDOclient, threadMain_cbSignal);
|
||||
|
||||
|
||||
#if CO_NO_TRACE > 0
|
||||
/* Initialize time */
|
||||
CO_time_init(&CO_time, CO->SDO[0], &OD_time.epochTimeBaseMs, &OD_time.epochTimeOffsetMs, 0x2130);
|
||||
|
|
@ -345,52 +342,43 @@ int main (int argc, char *argv[]) {
|
|||
if(firstRun) {
|
||||
firstRun = false;
|
||||
|
||||
/* Configure epoll for mainline */
|
||||
mainline_epoll_fd = epoll_create(4);
|
||||
if(mainline_epoll_fd == -1)
|
||||
CO_errExit("Program init - epoll_create mainline failed");
|
||||
|
||||
/* Init mainline */
|
||||
threadMain_init(mainline_epoll_fd, &OD_performance[ODA_performance_mainCycleMaxTime]);
|
||||
/* Init threadMainWait structure and file descriptors */
|
||||
threadMainWait_init(MAIN_THREAD_INTERVAL_US);
|
||||
|
||||
|
||||
#ifdef CO_MULTI_THREAD
|
||||
/* Configure epoll for rt_thread */
|
||||
rt_thread_epoll_fd = epoll_create(2);
|
||||
if(rt_thread_epoll_fd == -1)
|
||||
CO_errExit("Program init - epoll_create rt_thread failed");
|
||||
/* Init threadRT structure and file descriptors */
|
||||
CANrx_threadTmr_init(TMR_THREAD_INTERVAL_US);
|
||||
|
||||
/* Init threadRT */
|
||||
CANrx_threadTmr_init(rt_thread_epoll_fd, TMR_THREAD_INTERVAL_NS, &OD_performance[ODA_performance_timerCycleMaxTime]);
|
||||
|
||||
/* Create rt_thread */
|
||||
if(pthread_create(&rt_thread_id, NULL, rt_thread, NULL) != 0)
|
||||
CO_errExit("Program init - rt_thread creation failed");
|
||||
|
||||
/* Set priority for rt_thread */
|
||||
/* Create rt_thread and set priority */
|
||||
if(pthread_create(&rt_thread_id, NULL, rt_thread, NULL) != 0) {
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "pthread_create(rt_thread)");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
if(rtPriority > 0) {
|
||||
struct sched_param param;
|
||||
|
||||
param.sched_priority = rtPriority;
|
||||
if(pthread_setschedparam(rt_thread_id, SCHED_FIFO, ¶m) != 0)
|
||||
CO_errExit("Program init - rt_thread set scheduler failed");
|
||||
if (pthread_setschedparam(rt_thread_id, SCHED_FIFO, ¶m) != 0) {
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "pthread_setschedparam()");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CO_309
|
||||
#if CO_CONFIG_309 > 0
|
||||
/* Initialize socket command interface */
|
||||
switch(commandEnable) {
|
||||
case CMD_LOCAL:
|
||||
if(CO_command_init() != 0) {
|
||||
CO_errExit("Socket command interface initialization failed");
|
||||
}
|
||||
printf("%s - Command interface on socket '%s' started ...\n", argv[0], CO_command_socketPath);
|
||||
log_printf(LOG_INFO, DBG_COMMAND_LOCAL_INFO, CO_command_socketPath);
|
||||
break;
|
||||
case CMD_REMOTE:
|
||||
if(CO_command_init_tcp(CO_command_socket_tcp_port) != 0) {
|
||||
CO_errExit("Socket command interface initialization failed");
|
||||
}
|
||||
printf("%s - Command interface on tcp port '%hu' started ...\n", argv[0], CO_command_socket_tcp_port);
|
||||
log_printf(LOG_INFO, DBG_COMMAND_TCP_INFO, CO_command_socket_tcp_port);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
@ -413,58 +401,32 @@ int main (int argc, char *argv[]) {
|
|||
/* start CAN */
|
||||
CO_CANsetNormalMode(CO->CANmodule[0]);
|
||||
|
||||
#ifdef CO_309
|
||||
#if CO_CONFIG_309 > 0
|
||||
pthread_mutex_unlock(&CO_CAN_VALID_mtx);
|
||||
#endif
|
||||
|
||||
|
||||
reset = CO_RESET_NOT;
|
||||
|
||||
printf("%s - running ...\n", argv[0]);
|
||||
log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, nodeId, nodeId, "running ...");
|
||||
|
||||
|
||||
while(reset == CO_RESET_NOT && CO_endProgram == 0) {
|
||||
/* loop for normal program execution ******************************************/
|
||||
int ready;
|
||||
struct epoll_event ev;
|
||||
|
||||
ready = epoll_wait(mainline_epoll_fd, &ev, 1, -1);
|
||||
|
||||
if(ready != 1) {
|
||||
if(errno != EINTR) {
|
||||
CO_error(0x11100000L + errno);
|
||||
}
|
||||
}
|
||||
|
||||
else if(threadMain_process(ev.data.fd, &reset, CO_timer1ms)) {
|
||||
uint16_t timer1msDiff;
|
||||
static uint16_t tmr1msPrev = 0;
|
||||
|
||||
/* Calculate time difference */
|
||||
timer1msDiff = CO_timer1ms - tmr1msPrev;
|
||||
tmr1msPrev = CO_timer1ms;
|
||||
|
||||
/* code was processed in the above function. Additional code process below */
|
||||
uint32_t timer1usDiff = threadMainWait_process(&reset);
|
||||
|
||||
#ifdef CO_USE_APPLICATION
|
||||
/* Execute optional additional application code */
|
||||
app_programAsync(timer1msDiff);
|
||||
app_programAsync(timer1usDiff);
|
||||
#endif
|
||||
|
||||
CO_OD_storage_autoSave(&odStorAuto, CO_timer1ms, 60000);
|
||||
}
|
||||
|
||||
else {
|
||||
/* No file descriptor was processed. */
|
||||
CO_error(0x11200000L);
|
||||
}
|
||||
CO_OD_storage_autoSave(&odStorAuto, timer1usDiff, 60000000);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* program exit ***************************************************************/
|
||||
/* join threads */
|
||||
#ifdef CO_309
|
||||
#if CO_CONFIG_309 > 0
|
||||
switch (commandEnable)
|
||||
{
|
||||
case CMD_LOCAL:
|
||||
|
|
@ -481,11 +443,10 @@ int main (int argc, char *argv[]) {
|
|||
#endif
|
||||
|
||||
CO_endProgram = 1;
|
||||
#ifdef CO_MULTI_THREAD
|
||||
if(pthread_join(rt_thread_id, NULL) != 0) {
|
||||
CO_errExit("Program end - pthread_join failed");
|
||||
if (pthread_join(rt_thread_id, NULL) != 0) {
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "pthread_join()");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CO_USE_APPLICATION
|
||||
/* Execute optional additional application code */
|
||||
|
|
@ -498,16 +459,17 @@ int main (int argc, char *argv[]) {
|
|||
|
||||
/* delete objects from memory */
|
||||
CANrx_threadTmr_close();
|
||||
threadMain_close();
|
||||
threadMainWait_close();
|
||||
CO_delete((void *)CANdevice0Index);
|
||||
|
||||
printf("%s on %s (nodeId=0x%02X) - finished.\n\n", argv[0], CANdevice, nodeId);
|
||||
log_printf(LOG_INFO, DBG_CAN_OPEN_INFO, nodeId, nodeId, "finished");
|
||||
|
||||
/* Flush all buffers (and reboot) */
|
||||
if(rebootEnable && reset == CO_RESET_APP) {
|
||||
sync();
|
||||
if(reboot(LINUX_REBOOT_CMD_RESTART) != 0) {
|
||||
CO_errExit("Program end - reboot failed");
|
||||
log_printf(LOG_CRIT, DBG_ERRNO, "reboot()");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -515,47 +477,28 @@ int main (int argc, char *argv[]) {
|
|||
}
|
||||
|
||||
|
||||
#ifdef CO_MULTI_THREAD
|
||||
/* Realtime thread for CAN receive and threadTmr ******************************/
|
||||
static void* rt_thread(void* arg) {
|
||||
|
||||
/* Endless loop */
|
||||
while(CO_endProgram == 0) {
|
||||
int ready;
|
||||
struct epoll_event ev;
|
||||
|
||||
ready = epoll_wait(rt_thread_epoll_fd, &ev, 1, -1);
|
||||
|
||||
if(ready != 1) {
|
||||
if(errno != EINTR) {
|
||||
CO_error(0x12100000L + errno);
|
||||
}
|
||||
}
|
||||
|
||||
else if(CANrx_threadTmr_process(ev.data.fd)) {
|
||||
int i;
|
||||
CANrx_threadTmr_process();
|
||||
|
||||
#if CO_NO_TRACE > 0
|
||||
/* Monitor variables with trace objects */
|
||||
CO_time_process(&CO_time);
|
||||
for(i=0; i<OD_traceEnable && i<CO_NO_TRACE; i++) {
|
||||
CO_trace_process(CO->trace[i], *CO_time.epochTimeOffsetMs);
|
||||
}
|
||||
/* Monitor variables with trace objects */
|
||||
CO_time_process(&CO_time);
|
||||
for(i=0; i<OD_traceEnable && i<CO_NO_TRACE; i++) {
|
||||
CO_trace_process(CO->trace[i], *CO_time.epochTimeOffsetMs);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CO_USE_APPLICATION
|
||||
/* Execute optional additional application code */
|
||||
app_program1ms();
|
||||
/* Execute optional additional application code */
|
||||
app_program1ms();
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
else {
|
||||
/* No file descriptor was processed. */
|
||||
CO_error(0x12200000L);
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
#endif /* CO_MULTI_THREAD */
|
||||
|
|
|
|||
Loading…
Reference in a new issue