%% Authors:
%% - Carlos Jenkins <carlos@jenkins.co.cr>
%% - Michal Sojka <sojkam1@fel.cvut.cz>
+%% - Michal Horn <hornmich@fel.cvut.cz>
%%
%% This document contains proprietary information belonging to Czech
%% Technical University in Prague. Passing on and copying of this
%% The mr prefix is standard to mark Multi Tasking main, which is the case. See
%% rpp_file_process.m description above for more information about this.
%%
-%% !!!!!!!!!!!!!!!!!!!!!!!!
-%% !!! This is not ready to use source code. It is just a concept with some ideas
-%% !!! from our experiments and has to be reworked.
-%% !!!!!!!!!!!!!!!!!!!!!!!!
%% References:
%% Example in <matlabroot>/rtw/c/tlc/mw/bareboard_mrmain.tlc
/* Definitions */
#define STEP_SIZE_MILLIS %<CompiledModel.FundamentalStepSize>*1000.0
- #define CONTROL_PRIORITY 3
- #define WORKING_PRIORITY 2
+ #define NUM_SAMPLE_TIMES %<numSampleTimes>
+ #define CONTROL_PRIORITY (configMAX_PRIORITIES - 1)
#define EXTMODE_PRIORITY 1
+
+ %foreach j = numSampleTimes - 1
+ %assign i = j + 1
+ %if i == 1 && tid01Eq
+ %continue
+ %endif
+ #define WORKING%<i>_PRIORITY %<CompiledModel.SampleTime[i].Priority>
+ %endforeach
+
%closefile tmpBuf
%<LibSetSourceFileSection(cFile, "Defines", tmpBuf)>
%%%%%%%%
%openfile tmpBuf
+ typedef struct sub_rate_st {
+ xTaskHandle thread;
+ xSemaphoreHandle sem;
+ } sub_rate_t;
- static boolean_t working = FALSE;
- static xSemaphoreHandle step_signal = NULL;
- static xSemaphoreHandle initialized_signal = NULL;
%if extMode == 1
static xSemaphoreHandle ext_mode_ready = NULL;
%endif
static uint32_t steps_control = 0;
- static uint32_t steps_working = 0;
+ static uint32_t steps_working[NUM_SAMPLE_TIMES];
boolean_t overrun_flag = FALSE;
+ static sub_rate_t sub_rate[NUM_SAMPLE_TIMES];
+ static boolean_t simulationFinished = 0;
%closefile tmpBuf
%<LibSetSourceFileSection(cFile, "Declarations", tmpBuf)>
*/
void control_task(void* p)
{
-
- xSemaphoreTake(initialized_signal, portMAX_DELAY);
+ uint32_t i;
+ uint32_t taskCounter[NUM_SAMPLE_TIMES];
+ boolean_t eventFlags[NUM_SAMPLE_TIMES];
+ boolean_t OverrunFlags[NUM_SAMPLE_TIMES];
+ uint32_t step_sem_value;
+
+ steps_control = 0;
+ for (i = 0; i < NUM_SAMPLE_TIMES; i++) {
+ taskCounter[i] = 0;
+ eventFlags[i] = FALSE;
+ OverrunFlags[i] = FALSE;
+ }
%if extMode == 1
xSemaphoreTake(ext_mode_ready, portMAX_DELAY);
%endif
%endif
/* Wait until next step */
vTaskDelayUntil(&last_wake_time, freq_ticks);
+
+ /* Check subrate overrun, set rates that need to run this time step*/
+ %<LibCallSetEventForThisBaseStep("eventFlags")>\
+
+ /* Trigger sub-rate threads */
+ %foreach i = numSampleTimes
+ %if i == 0 || i == 1 && tid01Eq
+ %continue
+ %endif
+ %assign s = CompiledModel.SampleTime[i].PeriodAndOffset[0]
+ %assign o = CompiledModel.SampleTime[i].PeriodAndOffset[1]
+ /* Sampling rate %<i>, sample time = %<s>, offset = %<o> */
+ if (OverrunFlags[i] == TRUE) {
+ overrun_flag = TRUE;
+ break;
+ }
+
+ if (eventFlags[%<i>]) {
+ eventFlags[%<i>] = FALSE;
+ step_sem_value = uxQueueMessagesWaiting(sub_rate[%<i>].sem);
+ if (step_sem_value) {
+ rtmSetErrorStatus(%<modelName>_M, "Overrun");
+ overrun_flag = TRUE;
+ break;
+ }
+ else {
+ overrun_flag = FALSE;
+ }
+ if (xSemaphoreGive(sub_rate[%<i>].sem) == pdFALSE) {
+ rpp_sci_printk("ERROR: Task %<i> semaphore 1 give./r/n");
+ while (1) ;
+ }
+ if (xSemaphoreGive(sub_rate[%<i>].sem) == pdFALSE) {
+ rpp_sci_printk("ERROR: Task %<i> semaphore 2 give./r/n");
+ while (1) ;
+ }
+ }
+ %endforeach
- if (working) {
- /* Overrun detected */
- overrun_flag = TRUE;
- } else {
- overrun_flag = FALSE;
- /* Release semaphore */
- xSemaphoreGive(step_signal);
- }
steps_control++;
%if extMode == 1
%endif
}
+ simulationFinished = 1;
+
+ /* Final step */
+ for (i = %<1 + tid01Eq>; i < NUM_SAMPLE_TIMES; i++) {
+ if (xSemaphoreGive(sub_rate[%<i>].sem) == pdFALSE) {
+ rpp_sci_printk("ERROR: Task %d semaphore 1 give./r/n", i);
+ while (1);
+ }
+ if (xSemaphoreGive(sub_rate[%<i>].sem) == pdFALSE) {
+ rpp_sci_printk("ERROR: Task %d semaphore 2 give./r/n", i);
+ while (1);
+ }
+ }
/* In case of shutdown, delete this task */
vTaskDelete(NULL);
}
- /**
- * Model step logic execution task.
- */
- void working_task(void* p)
- {
- %% This idea might be useful in the MR implementation
- %if LibNumSynchronousSampleTimes() > 1
- static int_T taskCounter[%<FcnNumST()>] = %<FcnInitializeTaskCounter()>;
- %endif
- %% ----
- /* Initialize model */
- %<LibCallModelInitialize()>\
- /* Step the model for any subrate */
- xSemaphoreGive(initialized_signal);
-
- while (1) {
- if (xSemaphoreTake(step_signal, portMAX_DELAY)) {
- working = TRUE;
-
- %<LibCallModelStep(0)>\
-
- %% This idea might be useful in the MR implementation
-
- %if LibNumSynchronousSampleTimes() > 1
- %foreach idx = LibGetNumSyncPeriodicTasks() - 1
- %assign tid = 1 + idx + tid01Eq
- /* Task %<tid> handling */
- if (taskCounter[%<tid>] == 0) {
- %<LibCallModelStep(tid)>\
- }
- if (++taskCounter[%<tid>] == %< FcnComputeTaskTickLimit(tid)>)
- taskCounter[%<tid>]=0;
- %endforeach
- %endif
-
- %% ----
-
- steps_working++;
- working = FALSE;
- }
- }
-
- %% /* In case of shutdown, delete this task */
- %% vTaskDelete(NULL);
- }
+ %foreach j = numSampleTimes - 1
+ %assign i = j + 1
+ %if i == 1 && tid01Eq
+ %continue
+ %endif
+ /**
+ * Model step logic execution task for sample time %<CompiledModel.SampleTime[i].ClockTickStepSize> .
+ */
+ void working_task%<i>(void *p)
+ {
+ steps_working[%<i>] = 0;
+
+ while(!simulationFinished) {
+ xSemaphoreTake(sub_rate[%<i>].sem, portMAX_DELAY); /* Sem_val = 1 */
+ %<LibCallModelStep(i)>\
+ steps_working[%<i>]++;
+ xSemaphoreTake(sub_rate[%<i>].sem, portMAX_DELAY); /* Sem_val = 0 */
+ }
+
+ %% /* In case of shutdown, delete this task */
+ vTaskDelete(NULL);
+ }
+ %endforeach
%if extMode == 1
- void ext_mode_comm_task(void* p)
- {
- drv_sci_set_crlf_conv_en(FALSE); /* Disable CR->CRLF conversion */
-
- %<SLibGenERTExtModeInit()>
- xSemaphoreGive(ext_mode_ready);
-
- while (rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {
- rtExtModeOneStep(rtmGetRTWExtModeInfo(%<modelName>_M), %<numSampleTimes>, (boolean_T *)&rtmGetStopRequested(%<modelName>_M));
- }
- rtExtModeShutdown(%<numSampleTimes>);
- /* In case of shutdown, delete this task */
- vTaskDelete(NULL);
- }
+ void ext_mode_comm_task(void* p)
+ {
+ drv_sci_set_crlf_conv_en(FALSE); /* Disable CR->CRLF conversion */
+
+ %<SLibGenERTExtModeInit()>
+ xSemaphoreGive(ext_mode_ready);
+
+ while (rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {
+ rtExtModeOneStep(rtmGetRTWExtModeInfo(%<modelName>_M), %<numSampleTimes>, (boolean_T *)&rtmGetStopRequested(%<modelName>_M));
+ }
+ rtExtModeShutdown(%<numSampleTimes>);
+ /* In case of shutdown, delete this task */
+ vTaskDelete(NULL);
+ }
%endif
/**
void main(void)
{
/* Initialize RPP board */
+ /* Initialize RPP board */
%if EXISTS(::rpp_ain_present)
rpp_adc_init();
%endif
- %if EXISTS(::rpp_din_present) || EXISTS(::rpp_dout_present)
+ %if EXISTS(::rpp_aout_present)
+ rpp_dac_init();
+ %endif
+ %if EXISTS(::rpp_din_present)
+ rpp_din_init();
+ %endif
+ %if EXISTS(::rpp_hbr_present)
+ rpp_hbr_init();
+ %endif
+ %if EXISTS(::rpp_irc_present)
+ rpp_irc_init();
+ %endif
+ %if EXISTS(::rpp_lout_present)
+ rpp_lout_init();
+ %endif
+ %if EXISTS(::rpp_mout_present)
+ rpp_mout_init();
+ %endif
+ %if EXISTS(::rpp_sdr_present)
+ rpp_sdr_init();
+ %endif
+ %if EXISTS(::rpp_gio_in_present) || EXISTS(::rpp_gio_out_present)
rpp_gio_init(RPP_GIO_PORT_ALL);
%endif
rpp_sci_init();
// Speed up the SCI
rpp_sci_setup(115200);
+
+ /* Initialize model */
+ %<LibCallModelInitialize()>\
%if rppPrintMeta
- %assign model_info = SPRINTF("'%s' - %s (TLC %s)\\r\\n", LibGetMdlPubHdrBaseName(), TLC_TIME, TLC_VERSION)
- rpp_sci_printk("%<model_info>");
- %endif
+ %assign model_info = SPRINTF("'%s' - %s (TLC %s)\\r\\n", LibGetMdlPubHdrBaseName(), TLC_TIME, TLC_VERSION)
+ rpp_sci_printk("%<model_info>");
+ %endif
- /* Create and lock semaphore */
- vSemaphoreCreateBinary(step_signal);
- xSemaphoreTake(step_signal, 0);
- vSemaphoreCreateBinary(initialized_signal);
- xSemaphoreTake(initialized_signal, 0);
%if extMode == 1
vSemaphoreCreateBinary(ext_mode_ready);
xSemaphoreTake(ext_mode_ready, 0);
%endif
+ %foreach i = numSampleTimes
+ %if i == 0 || i == 1 && tid01Eq
+ %continue
+ %endif
+ %assign s = CompiledModel.SampleTime[i].PeriodAndOffset[0]
+ %assign o = CompiledModel.SampleTime[i].PeriodAndOffset[1]
+
+ /* Initializing the step semaphore of the loop %<i> */
+ sub_rate[%<i>].sem = xSemaphoreCreateCounting(10, 0);
+ if (sub_rate[%<i>].sem == NULL) {
+ rpp_sci_printk("ERROR: Can not create semaphore for task %<i>.\r\n");
+ while (1) ;
+ }
+
+ /* Starting loop %<i> thread for sample time = %<s>s, offset = %<o>s. */
+ if (xTaskCreate(working_task%<i>,
+ "working_task%<i>",
+ %<rppModelTaskStack>,
+ NULL,
+ WORKING%<i>_PRIORITY,
+ &sub_rate[%<i>].thread ) != pdPASS) {
+ rpp_sci_printk("ERROR: Cannot spawn working task %<i>.\r\n");
+ while (1);
+ }
+ %endforeach
+
/* Create tasks to step model and model task */
- if (xTaskCreate(control_task, "control_task",
- configMINIMAL_STACK_SIZE, NULL, CONTROL_PRIORITY, NULL) != pdPASS) {
- rpp_sci_printk("ERROR: Cannot spawn control task.\r\n");
- return;
- }
- if (xTaskCreate(working_task, "working_task",
- %<rppModelTaskStack>, NULL, WORKING_PRIORITY, NULL) != pdPASS) {
- rpp_sci_printk("ERROR: Cannot spawn model task.\r\n"
- );
- return;
+
+ if (xTaskCreate(control_task,
+ "control_task",
+ %<rppModelTaskStack>,
+ NULL,
+ CONTROL_PRIORITY,
+ NULL) != pdPASS) {
+ rpp_sci_printk("ERROR: Cannot spawn control task.\r\n");
+ while (1);
}
+
%if extMode == 1
/* External mode */
rtParseArgsForExtMode(0, NULL);
if (xTaskCreate(ext_mode_comm_task, "ext_mode_comm_task", 1024, NULL, EXTMODE_PRIORITY, NULL) != pdPASS) {
rpp_sci_printk("ERROR: Cannot spawn model task.\r\n");
- return;
+ while (1);
}
%endif
vTaskStartScheduler();
/* We should never get here */
- %<LibCallModelTerminate()>
+ %<LibCallModelTerminate()>\
rpp_sci_printk("ERROR: Problem allocating memory for idle task.\r\n");
+ while (1);
}
*/
void vApplicationMallocFailedHook(void) {
rpp_sci_printk("ERROR: Memory allocation failed.\r\n");
+ while (1);
}
#endif
void vApplicationStackOverflowHook(xTaskHandle xTask, signed portCHAR *pcTaskName)
{
rpp_sci_printk("ERROR: Stack overflow : \"%s\".\r\n", pcTaskName);
+ while (1);
}
#endif
projects / jenkicar / rpp-simulink.git / commitdiff