306 lines
8.3 KiB
C
Executable File
306 lines
8.3 KiB
C
Executable File
|
|
#include <main.h>
|
|
#include <mqtt/mqtt_client.h>
|
|
#include <can/can_client.h>
|
|
#include <io/io.h>
|
|
#include <settings/settings.h>
|
|
|
|
// Period info of the realtime task
|
|
struct period_info pinfo;
|
|
int iThreadControl = 0; // 0: thread is running, <0: thread shall exit, >0 thread has exited
|
|
int iLogToConsole = 1;
|
|
|
|
|
|
/// @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() && (iThreadControl == 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 (iThreadControl == 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
|
|
iThreadControl = 1;
|
|
return NULL;
|
|
}
|
|
|
|
|
|
/// @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);
|
|
iThreadControl = -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();
|
|
|
|
// 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, ¶m);
|
|
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 */
|
|
iThreadControl = 0;
|
|
ret = pthread_create(&thread, &attr, thread_func, NULL);
|
|
if (ret)
|
|
{
|
|
mylog(LOG_ERR, "create pthread failed");
|
|
goto out;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|