CanRtDriver/main.c

#include <main.h>
#include <mqtt/mqtt_client.h>
#include <can/can_client.h>
#include <io/io.h>
#include <settings/settings.h>
#include <termios.h>

// Period info of the realtime task
struct period_info pinfo;
int iLogToConsole = 1;
atomic_short asThreadControl = ATOMIC_VAR_INIT(0);      // 0: thread is running, <0: thread shall exit, >0 thread has exited

// values for simulation
atomic_bool abKeyPlus = ATOMIC_VAR_INIT(false);
atomic_bool abKeyMinus = ATOMIC_VAR_INIT(false);
atomic_bool abKeyStop = ATOMIC_VAR_INIT(false);

/// @brief send a log message
/// @param prio 
/// @param format 
/// @param  
void mylog(int prio, const char *format, ...)
{
        if (prio <= settings.iDebugLevel)
        {
                va_list args;

                // 1. Initialize the argument list with the last fixed argument
                va_start(args, format);

                // 2. Transfer to vsyslog (instead of syslog)
                // vsyslog accepts a va_list
                vsyslog(prio, format, args);

                // 3. Optional: Output additionally to the console
                // We have to reinitialize the list because va_list is "consumed."
                if (iLogToConsole)
                {
                        va_end(args);
                        va_start(args, format);
                        vfprintf(stderr, format, args);
                        fprintf(stderr, "\n");
                }

                // 4. Cleanup
                va_end(args);
        }
}


/// @brief Initialize period_info with period_ms for cyclic task
/// @param period_ms 
/// @param pinfo 
static void periodic_task_init(long period_ms, struct period_info *pinfo)
{
        /* for simplicity, hardcoding a 1ms period */
        pinfo->period_ns = period_ms * 1000000;
        pinfo->cyclecounter = 0;

        clock_gettime(CLOCK_MONOTONIC, &(pinfo->next_period));
        pinfo->fStartTime = (float)pinfo->next_period.tv_sec + ((float)pinfo->next_period.tv_nsec / 1000000000.0);
}


/// @brief Wait in cyclic task the rest of time until the next cycle
/// @param pinfo 
static void wait_rest_of_period(struct period_info *pinfo)
{
        pinfo->next_period.tv_nsec += pinfo->period_ns;
 
        while (pinfo->next_period.tv_nsec >= 1000000000) {
                /* timespec nsec overflow */
                pinfo->next_period.tv_sec++;
                pinfo->next_period.tv_nsec -= 1000000000;
        }
 
        /* for simplicity, ignoring possibilities of signal wakes */
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &pinfo->next_period, NULL);
}


/// @brief Get monotonic time in sec as float
/// @return 
float clock_gettime_s()
{
        struct timespec tm;
        clock_gettime(CLOCK_MONOTONIC, &tm);
        return (float)tm.tv_sec + ((float)tm.tv_nsec / 1000000000.0);
}


/// @brief Cyclic task, is called from thread_func every ms
/// @param pinfo 
static void do_cyclic_1ms(struct period_info *pinfo)
{
        uint16_t nCalled = 0;

        // read each cycle CAN data
        Can_ReadData(0);

        if ((pinfo->cyclecounter % 10) == 0)
        {
                // called every 10ms
                nCalled |= 0x1000;
                IO_DoCyclic();               
        }

        if ((pinfo->cyclecounter % 50) == 0)
        {
                // called every 50ms
                nCalled |= 0x0001;
                Can_TransmitMotorPower(0);
        }

        if (((pinfo->cyclecounter + 10) % 100) == 0)
        {
                // called every 100ms
                nCalled |= 0x0010;
                Can_TransmitMotorGear(0);
        }

        if (((pinfo->cyclecounter + 20) % 100) == 0)
        {
                // called every 100ms
                nCalled |= 0x0100;
                MqttClient_Refresher();
        }

        if (((pinfo->cyclecounter + 30) % 500) == 0)
        {
                // called every 500ms
                MqttClient_Publisher();
        }
}
 

/// @brief Our one and only realtime task
/// @param data 
/// @return 
void *thread_func(void *data)
{
        // Initialize IO Ports
        if (IO_Init())
        {
                mylog(LOG_ERR, "IO_Init() failed!");
                return NULL;
        }

        // Open CAN interface first motor
        if (Can_OpenInterface(0, "can0"))
        {
                mylog(LOG_ERR, "Can_OpenInterface() failed!");
                return NULL;
        }

        // Connect to mqtt broker
        while (MqttClient_Connect() && atomic_load(&asThreadControl) == 0)
        {
                mylog(LOG_ERR, "MqttClient_Connect() failed!");
                sleep(10);
        }

        // initialize cyclic task
        periodic_task_init(1, &pinfo);
 
        // Ignition on
        WriteOutputPin(GPIO_OUT_PWRON, HIGH);

        // cyclic call of do_cyclic_1ms()
        while (atomic_load(&asThreadControl) == 0)
        {
                pinfo.cyclecounter++;
                if (pinfo.cyclecounter > CYCLE_COUNTER_MAX)
                {
                        // Reset cycle counter every 24h
                        pinfo.cyclecounter = 1;
                } 
                do_cyclic_1ms(&pinfo);
                wait_rest_of_period(&pinfo);
        }

        // Ignition off
        WriteOutputPin(GPIO_OUT_PWRON, LOW);

        // Disconnect from mqtt broker
        MqttClient_Close();

        // Close CAN interface
        Can_CloseInterface(0);
        
        // signal thread has finnished
        atomic_store(&asThreadControl, 1);
        return NULL;
}


// Funktion, um das Terminal in den "Raw Mode" zu versetzen
void set_conio_terminal_mode() 
{
    struct termios new_termios;
    tcgetattr(0, &new_termios);
    new_termios.c_lflag &= ~ICANON; // Deaktiviert den zeilenweisen Modus
    new_termios.c_lflag &= ~ECHO;   // Verhindert, dass die Taste angezeigt wird
    tcsetattr(0, TCSANOW, &new_termios);
}


/// @brief catch signals and set flag to terminate for the realtime thread
/// @param signo 
void sig_handler(int signo)
{
        if ((signo == SIGINT) || (signo == SIGTERM))
        {
                mylog(LOG_INFO, "Received signal %d", signo);
                atomic_store(&asThreadControl, -1);     // signal realtime thread to exit
        }
}


/// @brief Main function, create and start realtime task
/// @param argc 
/// @param argv 
/// @return 
int main(int argc, char* argv[])
{
        struct sched_param param;
        pthread_attr_t attr;
        pthread_t thread;
        int ret;

        // First we have to get the default values of our settings
        Settings_InitDefaultValues();

        openlog("CanRtDriver", LOG_PID | LOG_CONS, LOG_DAEMON);
        mylog(LOG_INFO, "Service started. PID: %d", getpid());

        // Read the settings file after opening the log
        Settings_ReadConfFile();

        if (settings.iCanSimu)
        {
                // in simulation mode switch terminal to raw mode
                set_conio_terminal_mode();
        }
        else
        {
                // catch signals
                if (signal(SIGTERM, sig_handler) == SIG_ERR)
                {
                        mylog(LOG_ERR, "Can't catch SIGTERM");
                        exit(-1);
                }
                if (signal(SIGINT, sig_handler) == SIG_ERR)
                {
                        mylog(LOG_ERR, "Can't catch SIGINT");
                        exit(-2);
                }
        }

        /* Lock memory */
        if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1) 
        {
                mylog(LOG_ERR, "mlockall failed: %m");
                exit(-3);
        }

        /* Initialize pthread attributes (default values) */
        ret = pthread_attr_init(&attr);
        if (ret) 
        {
                mylog(LOG_ERR, "init pthread attributes failed");
                goto out;
        }

        /* Set a specific stack size  */
        ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN);
        if (ret) 
        {
            mylog(LOG_ERR, "pthread setstacksize failed");
            goto out;
        }

        /* Set scheduler policy and priority of pthread */
        ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
        if (ret) 
        {
                mylog(LOG_ERR, "pthread setschedpolicy failed");
                goto out;
        }
        param.sched_priority = 99;  // Priority between 1 (low) and 99() high)
        ret = pthread_attr_setschedparam(&attr, &param);
        if (ret) 
        {
                mylog(LOG_ERR, "pthread setschedparam failed");
                goto out;
        }
        /* Use scheduling parameters of attr */
        ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
        if (ret) 
        {
                mylog(LOG_ERR, "pthread setinheritsched failed");
                goto out;
        }

        
        /* Create a pthread with specified attributes */
        atomic_store(&asThreadControl, 0);
        ret = pthread_create(&thread, &attr, thread_func, NULL);
        if (ret) 
        {
                mylog(LOG_ERR, "create pthread failed");
                goto out;
        }

        if (settings.iCanSimu)
        {
                // in simulation mode we use keys to control the motors
                mylog(LOG_INFO, "*** CanRtDriver in Simu-Mode ***");
                while (1)
                {
                        char ch = getchar();
                        mylog(LOG_INFO, "SIMU: Taste gedrückt: %c", ch);

                        if (ch == '+')
                        {
                                atomic_store(&abKeyPlus, true);
                        }
                        else if (ch == '-')
                        {
                                atomic_store(&abKeyMinus, true);
                        }
                        else if (ch == 's')
                        {
                                atomic_store(&abKeyStop, true);
                        }
                        else if (ch == 'p')
                        {
                                if (iPowerSupplyOn)
                                {
                                        MqttClient_SwitchPowerSupply(0);
                                        iPowerSupplyOn = 0;
                                }
                                else
                                {
                                        MqttClient_SwitchPowerSupply(1);
                                        iPowerSupplyOn = 1;
                                }
                        }
                        else if ( ch == 'q')
                        {
                                atomic_store(&asThreadControl, -1);     // signal realtime thread to exit
                                break;
                        }
                }
        }

        // join the thread and wait for it to exit
        ret = pthread_join(thread, NULL);
        if (ret)
        {
                mylog(LOG_ERR, "faild to join thread!");
        }

out:
        mylog(LOG_INFO, "Service quit.");
        closelog();

        return ret;
}