forked from xtdrone/XTDrone
94 lines
1.8 KiB
C++
94 lines
1.8 KiB
C++
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include "stepvel.h"
|
|
#include "stepvel_private.h"
|
|
#include "rtwtypes.h"
|
|
#include "limits.h"
|
|
#include "linuxinitialize.h"
|
|
#define UNUSED(x) x = x
|
|
|
|
/* Function prototype declaration*/
|
|
void exitFcn(int sig);
|
|
void *terminateTask(void *arg);
|
|
void *baseRateTask(void *arg);
|
|
void *subrateTask(void *arg);
|
|
volatile boolean_T stopRequested = false;
|
|
volatile boolean_T runModel = true;
|
|
sem_t stopSem;
|
|
sem_t baserateTaskSem;
|
|
pthread_t schedulerThread;
|
|
pthread_t baseRateThread;
|
|
unsigned long threadJoinStatus[8];
|
|
int terminatingmodel = 0;
|
|
void *baseRateTask(void *arg)
|
|
{
|
|
runModel = (rtmGetErrorStatus(stepvel_M) == (NULL));
|
|
while (runModel) {
|
|
sem_wait(&baserateTaskSem);
|
|
stepvel_step();
|
|
|
|
/* Get model outputs here */
|
|
stopRequested = !((rtmGetErrorStatus(stepvel_M) == (NULL)));
|
|
runModel = !stopRequested;
|
|
}
|
|
|
|
runModel = 0;
|
|
terminateTask(arg);
|
|
pthread_exit((void *)0);
|
|
return NULL;
|
|
}
|
|
|
|
void exitFcn(int sig)
|
|
{
|
|
UNUSED(sig);
|
|
rtmSetErrorStatus(stepvel_M, "stopping the model");
|
|
}
|
|
|
|
void *terminateTask(void *arg)
|
|
{
|
|
UNUSED(arg);
|
|
terminatingmodel = 1;
|
|
|
|
{
|
|
runModel = 0;
|
|
}
|
|
|
|
/* Disable rt_OneStep() here */
|
|
|
|
/* Terminate model */
|
|
stepvel_terminate();
|
|
sem_post(&stopSem);
|
|
return NULL;
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
UNUSED(argc);
|
|
UNUSED(argv);
|
|
void slros_node_init(int argc, char** argv);
|
|
slros_node_init(argc, argv);
|
|
rtmSetErrorStatus(stepvel_M, 0);
|
|
|
|
/* Initialize model */
|
|
stepvel_initialize();
|
|
|
|
/* Call RTOS Initialization function */
|
|
myRTOSInit(0.05, 0);
|
|
|
|
/* Wait for stop semaphore */
|
|
sem_wait(&stopSem);
|
|
|
|
#if (MW_NUMBER_TIMER_DRIVEN_TASKS > 0)
|
|
|
|
{
|
|
int i;
|
|
for (i=0; i < MW_NUMBER_TIMER_DRIVEN_TASKS; i++) {
|
|
CHECK_STATUS(sem_destroy(&timerTaskSem[i]), 0, "sem_destroy");
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
return 0;
|
|
}
|