os_interface.hpp

Go to the documentation of this file.

#pragma once

#ifdef __XENO__

#include <native/task.h>
#include <native/timer.h>
#include <native/mutex.h>
#include <native/cond.h>
#include <rtdk.h>
#include <rtdm/rtcan.h>

#else

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>

#include <net/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>

#include <linux/can.h>
#include <linux/can/raw.h>

#include <limits.h>

#include <real_time_tools/timer.hpp>

#include <iostream>

typedef struct can_frame can_frame_t;
typedef canid_t can_id_t;
typedef uint64_t nanosecs_abs_t;

#define rt_fprintf fprintf

#define rt_printf printf

#define rt_dev_socket socket

#define rt_dev_ioctl ioctl

#define rt_dev_close close

#define rt_dev_setsockopt setsockopt

#define rt_dev_bind bind

#define rt_dev_recvmsg recvmsg

#define rt_dev_sendto sendto

#endif

#include <sstream>
#include <mutex>
#include <condition_variable>

#include <sys/mman.h>

namespace osi
{

#ifdef __XENO__

namespace xenomai
{
class mutex
{
public:
    RT_MUTEX rt_mutex_;

    mutex()
    {
        rt_mutex_create(&rt_mutex_, NULL);
    }

    void lock()
    {
        rt_mutex_acquire(&rt_mutex_, TM_INFINITE);
    }

    void unlock()
    {
        rt_mutex_release(&rt_mutex_);
    }
};

class condition_variable
{
public:
    RT_COND rt_condition_variable_;

    condition_variable()
    {
        rt_cond_create(&rt_condition_variable_, NULL);
    }

    void wait(std::unique_lock<mutex> &lock)
    {
        rt_cond_wait(&rt_condition_variable_,
                     &lock.mutex()->rt_mutex_, TM_INFINITE);
    }

    void notify_all()
    {
        rt_cond_broadcast(&rt_condition_variable_);
    }
};
} // namespace xenomai
typedef xenomai::mutex Mutex;

typedef xenomai::condition_variable ConditionVariable;

#else

typedef std::mutex Mutex;

typedef std::condition_variable ConditionVariable;
#endif

inline void send_to_can_device(int fd, const void *buf, size_t len,
                               int flags, const struct sockaddr *to,
                               socklen_t tolen)
{
    // int ret = rt_dev_sendto(fd, buf, len, flags, to, tolen);

    // if (ret < 0)
    // {
    //     std::ostringstream oss;
    //     oss << "something went wrong with sending "
    //         << "CAN frame, error code: "
    //         << ret << ", errno=" << errno << std::endl;
    //     throw std::runtime_error(oss.str());
    // }

    for (size_t i = 0; true; i++)
    {
        int ret = rt_dev_sendto(fd, buf, len, flags, to, tolen);
        if (ret >= 0)
        {
            if (i > 0)
            {
                std::cout << " Managed to send after "
                          << i << " attempts." << std::endl;
            }
            return;
        }

        if (i == 0)
        {
            std::cout << "WARNING: Something went wrong with sending "
                      << "CAN frame, error code: "
                      << ret << ", errno: " << errno << ". Possibly you have "
                      << "been attempting to send at a rate which is too "
                      << "high. We keep trying" << std::flush;
        }

        real_time_tools::Timer::sleep_ms(0.1);
    }
}

inline void close_can_device(int socket)
{
    int ret = rt_dev_close(socket);
    if (ret)
    {
        rt_fprintf(stderr, "rt_dev_close: %s\n", strerror(-ret));
        exit(-1);
    }
}

inline void receive_message_from_can_device(int fd, struct msghdr *msg, int flags)
{
    int ret = rt_dev_recvmsg(fd, msg, flags);
    if (ret < 0)
    {
        std::ostringstream oss;
        oss << "something went wrong with receiving "
            << "CAN frame, error code: "
            << ret << ", errno=" << errno << std::endl;
        throw std::runtime_error(oss.str());
    }
}

inline void initialize_realtime_printing()
{
#ifdef __XENO__
    rt_print_auto_init(1);
#endif
}

inline void sleep_ms(const double &sleep_time_ms)
{
#ifdef __XENO__
    rt_task_sleep(int(sleep_time_ms * 1000000.));
#else
    usleep(sleep_time_ms * 1000.); // nano_sleep
#endif
}

inline double get_current_time_ms()
{
#ifdef __XENO__
    return double(rt_timer_read()) / 1000000.;
#else
    struct timespec now;
    clock_gettime(CLOCK_REALTIME, &now);
    double current_time_ms = (double)(now.tv_sec * 1e3) + (now.tv_nsec / 1e6);

    return current_time_ms;
#endif
}

inline void make_this_thread_realtime()
{
#ifdef __XENO__

    rt_task_shadow(NULL, NULL, 0, 0);
#endif
}

} // namespace osi