bernhard/CanRtDriver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Kommunikation hat funktioniert

Browse Source
This commit is contained in:
Bernhard
2025-12-06 19:20:49 +01:00
commit 9b3c4b3fd2
13 changed files with 1115 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

253
main.c Executable file
View File

@@ -0,0 +1,253 @@
#include <main.h>
#include <mqtt/mqtt_client.h>
#include <can/can_client.h>
#include <io/io.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
/// @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;
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);
//printf("%.3f: 100ms-Cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, pinfo->cyclecounter);
}
if (((pinfo->cyclecounter + 20) % 100) == 0)
{
// called every 100ms
nCalled |= 0x0100;
MqttClient_Refresher();
}
if (((pinfo->cyclecounter + 30) % 500) == 0)
{
// called every 250ms
MqttClient_Publisher();
}
if (nCalled > 0)
{
//printf("%.3f: Called 0x%.4X at cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, nCalled, pinfo->cyclecounter);
}
}
/// @brief Our one and only realtime task
/// @param data
/// @return
void *thread_func(void *data)
{
// Open CAN interface first motor
if (Can_OpenInterface(0, "can0"))
{
printf("Can_OpenInterface() failed!\n");
return NULL;
}
// Connect to mqtt broker
while (MqttClient_Connect() && (iThreadControl == 0))
{
printf("MqttClient_Connect() failed!\n");
sleep(10);
}
// Initialize IO Ports
if (IO_Init())
{
printf("IO_Init() failed!\n");
return NULL;
}
// initialize cyclic task
periodic_task_init(1, &pinfo);
// cyclic call of do_cyclic_1ms()
while (iThreadControl == 0)
{
pinfo.cyclecounter++;
if (pinfo.cyclecounter >= 86400000)
{
// Reset cycle counter every 24h
pinfo.cyclecounter = 0;
}
do_cyclic_1ms(&pinfo);
wait_rest_of_period(&pinfo);
}
// Disconnect from mqtt broker
MqttClient_Close();
// Close CAN interface
Can_CloseInterface(0);
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))
{
printf("Received signal %d\n", signo);
iThreadControl = -1; // signal realtime thread to exit
}
}
/// @brief Hauptfunktion Echtzeit-Task erstellen und starten
/// @param argc
/// @param argv
/// @return
int main(int argc, char* argv[])
{
struct sched_param param;
pthread_attr_t attr;
pthread_t thread;
int ret;
// catch signals
if (signal(SIGTERM, sig_handler) == SIG_ERR)
{
printf("Can't catch SIGTERM\n");
exit(-1);
}
if (signal(SIGINT, sig_handler) == SIG_ERR)
{
printf("Can't catch SIGINT\n");
exit(-2);
}
/* Lock memory */
if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
{
printf("mlockall failed: %m\n");
exit(-3);
}
/* Initialize pthread attributes (default values) */
ret = pthread_attr_init(&attr);
if (ret)
{
printf("init pthread attributes failed\n");
goto out;
}
/* Set a specific stack size */
ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN);
if (ret)
{
printf("pthread setstacksize failed\n");
goto out;
}
/* Set scheduler policy and priority of pthread */
ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
if (ret)
{
printf("pthread setschedpolicy failed\n");
goto out;
}
param.sched_priority = 99; // Priority between 1 (low) and 99() high)
ret = pthread_attr_setschedparam(&attr, &param);
if (ret)
{
printf("pthread setschedparam failed\n");
goto out;
}
/* Use scheduling parameters of attr */
ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
if (ret)
{
printf("pthread setinheritsched failed\n");
goto out;
}
/* Create a pthread with specified attributes */
iThreadControl = 0;
ret = pthread_create(&thread, &attr, thread_func, NULL);
if (ret)
{
printf("create pthread failed\n");
goto out;
}
// join the thread and wait for it to exit
ret = pthread_join(thread, NULL);
if (ret)
{
printf("faild to join thread!\n");
}
out:
return ret;
}