int main(void)
{
volatile boolean_T runModel = 1;
float modelBaseRate = 2.0;
float systemClock = 0;
init();
MW_Arduino_Init();
rtmSetErrorStatus(test_motor_M, 0);
/* initialize external mode */
rtParseArgsForExtMode(0, NULL);
test_motor_initialize();
sei();
/* External mode */
rtSetTFinalForExtMode(&rtmGetTFinal(test_motor_M));
rtExtModeCheckInit(1);
{
boolean_T rtmStopReq = false;
rtExtModeWaitForStartPkt(test_motor_M->extModeInfo, 1, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(test_motor_M, true);
}
}
rtERTExtModeStartMsg();
cli();
configureArduinoAVRTimer();
runModel =
(rtmGetErrorStatus(test_motor_M) == (NULL)) && !rtmGetStopRequested
(test_motor_M);
sei();
sei();
while (runModel) {
/* External mode */
{
boolean_T rtmStopReq = false;
rtExtModeOneStep(test_motor_M->extModeInfo, 1, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(test_motor_M, true);
}
}
runModel =
(rtmGetErrorStatus(test_motor_M) == (NULL)) && !rtmGetStopRequested
(test_motor_M);
}
rtExtModeShutdown(1);
/* Disable rt_OneStep() here */
/* Terminate model */
test_motor_terminate();
cli();
return 0;
}
void baseRateTask(void *arg)
{
baseRateInfo_t info = *((baseRateInfo_t *)arg);
MW_setTaskPeriod(info.period, info.signo);
while ((rtmGetErrorStatus(motorControlTask_M) == (NULL)) &&
!rtmGetStopRequested(motorControlTask_M) ) {
/* Wait for the next timer interrupt */
if (MW_sigWaitWithOverrunDetection(&info.sigMask) == 1) {
printf("Overrun - rate for base rate task too fast.\n");
fflush(stdout);
}
/* External mode */
{
boolean_T rtmStopReq = false;
rtExtModePauseIfNeeded(motorControlTask_M->extModeInfo, 2, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(motorControlTask_M, true);
}
if (rtmGetStopRequested(motorControlTask_M) == true) {
rtmSetErrorStatus(motorControlTask_M, "Simulation finished");
break;
}
}
/* External mode */
{
boolean_T rtmStopReq = false;
rtExtModeOneStep(motorControlTask_M->extModeInfo, 2, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(motorControlTask_M, true);
}
}
motorControlTask_output();
/* Get model outputs here */
/* External mode */
rtExtModeUploadCheckTrigger(2);
{ /* Sample time: [0.0s, 0.0s] */
rtExtModeUpload(0, motorControlTask_M->Timing.t[0]);
}
{ /* Sample time: [0.02s, 0.0s] */
rtExtModeUpload(1, ((motorControlTask_M->Timing.clockTick1) * 0.02));
}
motorControlTask_update();
rtExtModeCheckEndTrigger();
} /* while */
sem_post(&stopSem);
}
void baseRateTask(void *arg)
{
baseRateInfo_t info = *((baseRateInfo_t *)arg);
MW_setTaskPeriod(info.period, info.signo);
while ((rtmGetErrorStatus(Serial_M) == (NULL)) && !rtmGetStopRequested
(Serial_M) ) {
/* Wait for the next timer interrupt */
MW_sigWait(&info.sigMask);
/* External mode */
{
boolean_T rtmStopReq = false;
rtExtModePauseIfNeeded(Serial_M->extModeInfo, 2, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(Serial_M, true);
}
if (rtmGetStopRequested(Serial_M) == true) {
rtmSetErrorStatus(Serial_M, "Simulation finished");
break;
}
}
/* External mode */
{
boolean_T rtmStopReq = false;
rtExtModeOneStep(Serial_M->extModeInfo, 2, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(Serial_M, true);
}
}
Serial_output();
/* Get model outputs here */
/* External mode */
rtExtModeUploadCheckTrigger(2);
{ /* Sample time: [0.0s, 0.0s] */
rtExtModeUpload(0, Serial_M->Timing.t[0]);
}
{ /* Sample time: [0.05s, 0.0s] */
rtExtModeUpload(1, ((Serial_M->Timing.clockTick1) * 0.05));
}
Serial_update();
rtExtModeCheckEndTrigger();
} /* while */
sem_post(&stopSem);
}
void baseRateTask(void *arg)
{
baseRateInfo_t info = *((baseRateInfo_t *)arg);
MW_setTaskPeriod(info.period, info.signo);
while ((rtmGetErrorStatus(raspberrypi_audioequalizer_M) == (NULL)) &&
!rtmGetStopRequested(raspberrypi_audioequalizer_M) ) {
/* Wait for the next timer interrupt */
MW_sigWait(&info.sigMask);
/* External mode */
{
boolean_T rtmStopReq = FALSE;
rtExtModePauseIfNeeded(raspberrypi_audioequalizer_M->extModeInfo, 1,
&rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(raspberrypi_audioequalizer_M, TRUE);
}
if (rtmGetStopRequested(raspberrypi_audioequalizer_M) == TRUE) {
rtmSetErrorStatus(raspberrypi_audioequalizer_M, "Simulation finished");
break;
}
}
/* External mode */
{
boolean_T rtmStopReq = FALSE;
rtExtModeOneStep(raspberrypi_audioequalizer_M->extModeInfo, 1, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(raspberrypi_audioequalizer_M, TRUE);
}
}
raspberrypi_audioequalizer_output();
/* Get model outputs here */
/* External mode */
rtExtModeUploadCheckTrigger(1);
{ /* Sample time: [0.1s, 0.0s] */
rtExtModeUpload(0, raspberrypi_audioequalizer_M->Timing.taskTime0);
}
raspberrypi_audioequalizer_update();
rtExtModeCheckEndTrigger();
} /* while */
sem_post(&stopSem);
}
/* Model update function */
static void CelpSimulink_update(int_T tid)
{
{
static char sErr[512];
void *device = CelpSimulink_DWork.ToAudioDevice_AudioDevice;
AudioDeviceLibrary *adl = (AudioDeviceLibrary*)
&CelpSimulink_DWork.ToAudioDevice_AudioDeviceLib[0];
sErr[0] = 0;
if (device)
adl->deviceUpdate(device, sErr, CelpSimulink_B.DeemphasisFilter, 1, 1);
if (*sErr) {
DestroyAudioDeviceLibrary(adl);
rtmSetErrorStatus(CelpSimulink_M, sErr);
rtmSetStopRequested(CelpSimulink_M, 1);
}
}
/* Update absolute time for base rate */
if (!(++CelpSimulink_M->Timing.clockTick0))
++CelpSimulink_M->Timing.clockTickH0;
CelpSimulink_M->Timing.t[0] = CelpSimulink_M->Timing.clockTick0 *
CelpSimulink_M->Timing.stepSize0 + CelpSimulink_M->Timing.clockTickH0 *
CelpSimulink_M->Timing.stepSize0 * 4294967296.0;
UNUSED_PARAMETER(tid);
}
void baseRateTask(void *arg)
{
runModel = (rtmGetErrorStatus(Model_M) == (NULL)) && !rtmGetStopRequested
(Model_M);
while (runModel) {
mw_osSemaphoreWaitEver(&baserateTaskSem);
/* External mode */
{
boolean_T rtmStopReq = false;
rtExtModePauseIfNeeded(Model_M->extModeInfo, 1, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(Model_M, true);
}
if (rtmGetStopRequested(Model_M) == true) {
rtmSetErrorStatus(Model_M, "Simulation finished");
break;
}
}
Model_step();
/* Get model outputs here */
rtExtModeCheckEndTrigger();
runModel = (rtmGetErrorStatus(Model_M) == (NULL)) && !rtmGetStopRequested
(Model_M);
}
runModel = 0;
terminateTask(arg);
taskDelete((void *)0);
}
int main(int argc, char **argv)
{
printf("**starting the model**\n");
fflush(stdout);
rtmSetErrorStatus(Model_M, 0);
rtExtModeParseArgs(argc, (const char_T **)argv, NULL);
/* Initialize model */
Model_initialize();
/* External mode */
rtSetTFinalForExtMode(&rtmGetTFinal(Model_M));
rtExtModeCheckInit(1);
{
boolean_T rtmStopReq = false;
rtExtModeWaitForStartPkt(Model_M->extModeInfo, 1, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(Model_M, true);
}
}
rtERTExtModeStartMsg();
/* Call RTOS Initialization funcation */
prosRTOSInit(0.2, 0);
/* Wait for stop semaphore */
mw_osSemaphoreWaitEver(&stopSem);
return 0;
}
void backgroundTask(int sig)
{
while (runModel) {
/* External mode */
{
boolean_T rtmStopReq = false;
rtExtModeOneStep(Model_M->extModeInfo, 1, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(Model_M, true);
}
}
}
}
/* Model terminate function */
void udpRead_terminate(void)
{
char_T *sErr;
/* Terminate for S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
sErr = GetErrorBuffer(&udpRead_DW.UDPReceive_NetworkLib[0U]);
LibTerminate(&udpRead_DW.UDPReceive_NetworkLib[0U]);
if (*sErr != 0) {
rtmSetErrorStatus(udpRead_M, sErr);
rtmSetStopRequested(udpRead_M, 1);
}
LibDestroy(&udpRead_DW.UDPReceive_NetworkLib[0U], 0);
DestroyUDPInterface(&udpRead_DW.UDPReceive_NetworkLib[0U]);
/* End of Terminate for S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
}
/* Model terminate function */
void beagleboard_communication_terminate(void)
{
char_T *sErr;
/* Terminate for S-Function (sdspToNetwork): '<Root>/UDP Send' */
sErr = GetErrorBuffer(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
LibTerminate(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
if (*sErr != 0) {
rtmSetErrorStatus(beagleboard_communication_M, sErr);
rtmSetStopRequested(beagleboard_communication_M, 1);
}
LibDestroy(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U], 1);
DestroyUDPInterface(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
/* End of Terminate for S-Function (sdspToNetwork): '<Root>/UDP Send' */
}
/* Model initialize function */
void beagleboard_communication_initialize(void)
{
/* Registration code */
/* initialize real-time model */
(void) memset((void *)beagleboard_communication_M, 0,
sizeof(RT_MODEL_beagleboard_communicat));
/* block I/O */
(void) memset(((void *) &beagleboard_communication_B), 0,
sizeof(BlockIO_beagleboard_communicati));
/* states (dwork) */
(void) memset((void *)&beagleboard_communication_DWork, 0,
sizeof(D_Work_beagleboard_communicatio));
{
char_T *sErr;
/* Start for S-Function (sdspToNetwork): '<Root>/UDP Send' */
sErr = GetErrorBuffer(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
CreateUDPInterface(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
if (*sErr == 0) {
LibCreate_Network(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U],
1, "255.255.255.255", -1, "192.168.1.35", 25000, 8192, 8,
0);
}
if (*sErr == 0) {
LibStart(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
}
if (*sErr != 0) {
DestroyUDPInterface(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
if (*sErr != 0) {
rtmSetErrorStatus(beagleboard_communication_M, sErr);
rtmSetStopRequested(beagleboard_communication_M, 1);
}
}
/* End of Start for S-Function (sdspToNetwork): '<Root>/UDP Send' */
}
}
void MdlStart(void)
{
/* Start for iterator SubSystem: '<Root>/Codebook Search' */
/* InitializeConditions for S-Function (sdspstatminmax): '<S6>/Maximum1' */
CelpSimulink_DWork.Maximum1_Valdata = rtMinusInf;
/* InitializeConditions for UnitDelay: '<S5>/Unit Delay' */
CelpSimulink_DWork.UnitDelay_DSTATE = CelpSimulink_P.UnitDelay_X0;
/* end of Start for SubSystem: '<Root>/Codebook Search' */
{
/* Signal Processing Blockset ToAudioDevice (sdspToAudioDevice) - '<Root>/To Audio Device' - Start */
void *device = NULL;
AudioDeviceLibrary adl;
static char sErr[512];
sErr[0] = 0;
adl = CreateAudioDeviceLibrary(&sErr[0]);
memcpy(&CelpSimulink_DWork.ToAudioDevice_AudioDeviceLib[0], &adl, sizeof
(AudioDeviceLibrary));
if (!*sErr) { /* Create the device */
adl.deviceCreate(&sErr[0], NULL, "Default", 1, &device,
80, 8000.0, 1, 512, 8000.0,
1, 1);
}
if (!*sErr) {
adl.deviceStart(device, &sErr[0]);
}
if (*sErr) {
DestroyAudioDeviceLibrary(&adl);
rtmSetErrorStatus(CelpSimulink_M, sErr);
rtmSetStopRequested(CelpSimulink_M, 1);
}
CelpSimulink_DWork.ToAudioDevice_AudioDevice = device;
}
MdlInitialize();
}
/* Model update function */
void beagleboard_communication_update(void)
{
char_T *sErr;
/* Update for Sin: '<Root>/Sine Wave' */
beagleboard_communication_DWork.counter++;
if (beagleboard_communication_DWork.counter ==
beagleboard_communication_P.SineWave_NumSamp) {
beagleboard_communication_DWork.counter = 0;
}
/* End of Update for Sin: '<Root>/Sine Wave' */
/* Update for S-Function (sdspToNetwork): '<Root>/UDP Send' */
sErr = GetErrorBuffer(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
LibUpdate_Network(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U],
&beagleboard_communication_B.sine_wave, 1);
if (*sErr != 0) {
rtmSetErrorStatus(beagleboard_communication_M, sErr);
rtmSetStopRequested(beagleboard_communication_M, 1);
}
/* End of Update for S-Function (sdspToNetwork): '<Root>/UDP Send' */
}
/* Model output function */
void omni_interface_output(int_T tid)
{
/* local block i/o variables */
real_T rtb_HILRead_o3[2];
real_T rtb_Saturation[3];
real_T rtb_StreamRead1_o2[3];
t_stream_ptr rtb_StreamAnswer_o1;
t_stream_ptr rtb_StreamWrite_o1;
t_stream_ptr rtb_StreamAnswer1_o1;
t_stream_ptr rtb_StreamRead1_o1;
uint8_T rtb_StreamAnswer1_o2;
boolean_T rtb_LogicalOperator2[2];
boolean_T rtb_StreamRead1_o4;
boolean_T rtb_StreamRead1_o3;
/* S-Function Block: omni_interface/HIL Read (hil_read_block) */
{
t_error result = hil_read(omni_interface_DWork.HILInitialize_Card,
omni_interface_P.HILRead_AnalogChannels, 3U,
omni_interface_P.HILRead_EncoderChannels, 3U,
omni_interface_P.HILRead_DigitalChannels, 2U,
NULL, 0U,
rtb_Saturation,
&omni_interface_DWork.HILRead_EncoderBuffer[0],
&omni_interface_DWork.HILRead_DigitalBuffer[0],
NULL
);
if (result < 0) {
msg_get_error_messageA(NULL, result, _rt_error_message, sizeof
(_rt_error_message));
rtmSetErrorStatus(omni_interface_M, _rt_error_message);
} else {
rtb_StreamRead1_o2[0] = omni_interface_DWork.HILRead_EncoderBuffer[0];
rtb_StreamRead1_o2[1] = omni_interface_DWork.HILRead_EncoderBuffer[1];
rtb_StreamRead1_o2[2] = omni_interface_DWork.HILRead_EncoderBuffer[2];
rtb_HILRead_o3[0] = omni_interface_DWork.HILRead_DigitalBuffer[0];
rtb_HILRead_o3[1] = omni_interface_DWork.HILRead_DigitalBuffer[1];
}
}
/* Gain: '<Root>/Gain2' incorporates:
* Bias: '<Root>/Bias3'
* Gain: '<Root>/Gain1'
* Gain: '<Root>/Gear Ratio'
*/
omni_interface_B.Gain2[0] = (rtb_StreamRead1_o2[0] +
omni_interface_P.Bias3_Bias[0]) * omni_interface_P.Gain1_Gain *
omni_interface_P.GearRatio_Gain[0] * omni_interface_P.Gain2_Gain;
omni_interface_B.Gain2[1] = (rtb_StreamRead1_o2[1] +
omni_interface_P.Bias3_Bias[1]) * omni_interface_P.Gain1_Gain *
omni_interface_P.GearRatio_Gain[1] * omni_interface_P.Gain2_Gain;
omni_interface_B.Gain2[2] = (rtb_StreamRead1_o2[2] +
omni_interface_P.Bias3_Bias[2]) * omni_interface_P.Gain1_Gain *
omni_interface_P.GearRatio_Gain[2] * omni_interface_P.Gain2_Gain;
/* Switch: '<S8>/Reset' incorporates:
* Constant: '<S8>/Initial Condition'
* MinMax: '<S2>/MinMax'
* UnitDelay: '<S8>/FixPt Unit Delay1'
*/
if (0.0 != 0.0) {
omni_interface_B.Reset = omni_interface_P.InitialCondition_Value;
} else {
omni_interface_B.Reset = rt_MIN(omni_interface_B.Gain2[0],
omni_interface_DWork.FixPtUnitDelay1_DSTATE);
}
/* Switch: '<S9>/Reset' incorporates:
* Constant: '<S9>/Initial Condition'
* MinMax: '<S3>/MinMax'
* UnitDelay: '<S9>/FixPt Unit Delay1'
*/
if (0.0 != 0.0) {
omni_interface_B.Reset_c = omni_interface_P.InitialCondition_Value_d;
} else {
omni_interface_B.Reset_c = rt_MIN(omni_interface_B.Gain2[1],
omni_interface_DWork.FixPtUnitDelay1_DSTATE_i);
}
/* Switch: '<S10>/Reset' incorporates:
* Constant: '<S10>/Initial Condition'
* MinMax: '<S4>/MinMax'
* UnitDelay: '<S10>/FixPt Unit Delay1'
*/
if (0.0 != 0.0) {
omni_interface_B.Reset_n = omni_interface_P.InitialCondition_Value_g;
} else {
omni_interface_B.Reset_n = rt_MIN(omni_interface_B.Gain2[2],
omni_interface_DWork.FixPtUnitDelay1_DSTATE_a);
}
/* Switch: '<S11>/Reset' incorporates:
* Constant: '<S11>/Initial Condition'
* MinMax: '<S5>/MinMax'
* UnitDelay: '<S11>/FixPt Unit Delay1'
*/
if (0.0 != 0.0) {
omni_interface_B.Reset_c3 = omni_interface_P.InitialCondition_Value_gd;
} else {
omni_interface_B.Reset_c3 = rt_MAX(omni_interface_B.Gain2[0],
omni_interface_DWork.FixPtUnitDelay1_DSTATE_e);
}
/* Switch: '<S12>/Reset' incorporates:
* Constant: '<S12>/Initial Condition'
* MinMax: '<S6>/MinMax'
* UnitDelay: '<S12>/FixPt Unit Delay1'
*/
if (0.0 != 0.0) {
omni_interface_B.Reset_l = omni_interface_P.InitialCondition_Value_m;
} else {
omni_interface_B.Reset_l = rt_MAX(omni_interface_B.Gain2[1],
omni_interface_DWork.FixPtUnitDelay1_DSTATE_eh);
}
/* Switch: '<S13>/Reset' incorporates:
* Constant: '<S13>/Initial Condition'
* MinMax: '<S7>/MinMax'
* UnitDelay: '<S13>/FixPt Unit Delay1'
*/
if (0.0 != 0.0) {
omni_interface_B.Reset_i = omni_interface_P.InitialCondition_Value_f;
} else {
omni_interface_B.Reset_i = rt_MAX(omni_interface_B.Gain2[2],
omni_interface_DWork.FixPtUnitDelay1_DSTATE_d);
}
/* S-Function Block: omni_interface/Stream Answer (stream_answer_block) */
{
static const t_short endian_test = 0x0201;
t_error result = 0;
t_boolean close_listener = (FALSE != 0);
t_boolean close_client = (FALSE != 0);
rtb_StreamAnswer_o1 = NULL;
switch (omni_interface_DWork.StreamAnswer_State) {
case STREAM_ANSWER_STATE_NOT_LISTENING:
{
if (!close_listener) {
result = stream_listen("tcpip://localhost:18000", true,
&omni_interface_DWork.StreamAnswer_Listener);
if (result == 0) {
omni_interface_DWork.StreamAnswer_State =
STREAM_ANSWER_STATE_NOT_CONNECTED;
}
}
break;
}
case STREAM_ANSWER_STATE_NOT_CONNECTED:
{
if (!close_client) {
result = stream_accept(omni_interface_DWork.StreamAnswer_Listener,
omni_interface_P.StreamAnswer_SendBufferSize,
omni_interface_P.StreamAnswer_ReceiveBufferSize,
&omni_interface_DWork.StreamAnswer_Client);
if (result == 0) {
omni_interface_DWork.StreamAnswer_State =
STREAM_ANSWER_STATE_CONNECTED;
stream_set_swap_bytes(omni_interface_DWork.StreamAnswer_Client,
*(t_byte *) &endian_test !=
omni_interface_P.StreamAnswer_Endian);
rtb_StreamAnswer_o1 = &omni_interface_DWork.StreamAnswer_Client;
}
}
break;
}
case STREAM_ANSWER_STATE_CONNECTED:
{
rtb_StreamAnswer_o1 = &omni_interface_DWork.StreamAnswer_Client;
if (!close_client) {
break;
}
/* Fall through deliberately */
}
default:
{
t_error close_result = stream_close
(omni_interface_DWork.StreamAnswer_Client);
if (close_result == 0) {
omni_interface_DWork.StreamAnswer_Client = NULL;
omni_interface_DWork.StreamAnswer_State =
STREAM_ANSWER_STATE_NOT_CONNECTED;
rtb_StreamAnswer_o1 = NULL;
if (close_listener) {
close_result = stream_close
(omni_interface_DWork.StreamAnswer_Listener);
if (close_result == 0) {
omni_interface_DWork.StreamAnswer_Listener = NULL;
omni_interface_DWork.StreamAnswer_State =
STREAM_ANSWER_STATE_NOT_LISTENING;
} else if (result == 0) {
result = close_result;
}
}
} else if (result == 0) {
result = close_result;
}
break;
}
}
rtb_StreamAnswer1_o2 = omni_interface_DWork.StreamAnswer_State;
omni_interface_B.StreamAnswer_o3 = (int32_T) result;
}
/* Logic: '<Root>/Logical Operator' incorporates:
* Logic: '<Root>/Logical Operator1'
* RelationalOperator: '<S1>/Compare'
*/
omni_interface_B.LogicalOperator = ((rtb_StreamAnswer1_o2 != 0U) &&
(!((omni_interface_B.StreamAnswer_o3 != 0) != 0U)));
/* S-Function (stream_write_block): '<Root>/Stream Write' */
{
t_error result;
if (rtb_StreamAnswer_o1 != NULL) {
result = stream_send_unit_array(*rtb_StreamAnswer_o1,
omni_interface_B.Gain2, sizeof(real_T), 3);
if (result == 1) {
stream_flush(*rtb_StreamAnswer_o1);
}
if (result == -QERR_WOULD_BLOCK) {
result = 0;
}
}
rtb_StreamWrite_o1 = rtb_StreamAnswer_o1;
}
/* S-Function (stream_read_block): '<Root>/Stream Read' */
/* S-Function Block: omni_interface/Stream Read (stream_read_block) */
{
t_error result;
if (rtb_StreamWrite_o1 != NULL) {
result = stream_receive_unit_array(*rtb_StreamWrite_o1,
omni_interface_B.StreamRead_o2, sizeof(real_T), 3);
omni_interface_B.StreamRead_o3 = (result > 0);
if (result == -QERR_WOULD_BLOCK) {
result = 0;
}
/*
if (result <= 0 && result != -QERR_WOULD_BLOCK) {
memset(omni_interface_B.StreamRead_o2, 0, 3 * sizeof(real_T));
}
*/
} else {
memset(omni_interface_B.StreamRead_o2, 0, 3 * sizeof(real_T));
omni_interface_B.StreamRead_o3 = false;
result = 0;
}
rtb_StreamRead1_o1 = rtb_StreamWrite_o1;
}
/* S-Function Block: omni_interface/Stream Answer1 (stream_answer_block) */
{
static const t_short endian_test = 0x0201;
t_error result = 0;
t_boolean close_listener = (FALSE != 0);
t_boolean close_client = (FALSE != 0);
rtb_StreamAnswer1_o1 = NULL;
switch (omni_interface_DWork.StreamAnswer1_State) {
case STREAM_ANSWER_STATE_NOT_LISTENING:
{
if (!close_listener) {
result = stream_listen("tcpip://localhost:18002", true,
&omni_interface_DWork.StreamAnswer1_Listener);
if (result == 0) {
omni_interface_DWork.StreamAnswer1_State =
STREAM_ANSWER_STATE_NOT_CONNECTED;
}
}
break;
}
case STREAM_ANSWER_STATE_NOT_CONNECTED:
{
if (!close_client) {
result = stream_accept(omni_interface_DWork.StreamAnswer1_Listener,
omni_interface_P.StreamAnswer1_SendBufferSize,
omni_interface_P.StreamAnswer1_ReceiveBufferSize,
&omni_interface_DWork.StreamAnswer1_Client);
if (result == 0) {
omni_interface_DWork.StreamAnswer1_State =
STREAM_ANSWER_STATE_CONNECTED;
stream_set_swap_bytes(omni_interface_DWork.StreamAnswer1_Client,
*(t_byte *) &endian_test !=
omni_interface_P.StreamAnswer1_Endian);
rtb_StreamAnswer1_o1 = &omni_interface_DWork.StreamAnswer1_Client;
}
}
break;
}
case STREAM_ANSWER_STATE_CONNECTED:
{
rtb_StreamAnswer1_o1 = &omni_interface_DWork.StreamAnswer1_Client;
if (!close_client) {
break;
}
/* Fall through deliberately */
}
default:
{
t_error close_result = stream_close
(omni_interface_DWork.StreamAnswer1_Client);
if (close_result == 0) {
omni_interface_DWork.StreamAnswer1_Client = NULL;
omni_interface_DWork.StreamAnswer1_State =
STREAM_ANSWER_STATE_NOT_CONNECTED;
rtb_StreamAnswer1_o1 = NULL;
if (close_listener) {
close_result = stream_close
(omni_interface_DWork.StreamAnswer1_Listener);
if (close_result == 0) {
omni_interface_DWork.StreamAnswer1_Listener = NULL;
omni_interface_DWork.StreamAnswer1_State =
STREAM_ANSWER_STATE_NOT_LISTENING;
} else if (result == 0) {
result = close_result;
}
}
} else if (result == 0) {
result = close_result;
}
break;
}
}
omni_interface_B.StreamAnswer1_o3 = (int32_T) result;
}
/* Gain: '<Root>/Gain' */
rtb_StreamRead1_o2[0] = omni_interface_P.Gain_Gain[0] *
omni_interface_B.StreamRead_o2[0];
rtb_StreamRead1_o2[1] = omni_interface_P.Gain_Gain[1] *
omni_interface_B.StreamRead_o2[1];
rtb_StreamRead1_o2[2] = omni_interface_P.Gain_Gain[2] *
omni_interface_B.StreamRead_o2[2];
/* Product: '<Root>/Product' */
rtb_Saturation[0] = (int32_T)omni_interface_B.LogicalOperator ?
rtb_StreamRead1_o2[0] : 0.0;
rtb_Saturation[1] = (int32_T)omni_interface_B.LogicalOperator ?
rtb_StreamRead1_o2[1] : 0.0;
rtb_Saturation[2] = (int32_T)omni_interface_B.LogicalOperator ?
rtb_StreamRead1_o2[2] : 0.0;
/* Saturate: '<Root>/Saturation' */
rtb_Saturation[0] = rt_SATURATE(rtb_Saturation[0],
omni_interface_P.Saturation_LowerSat, omni_interface_P.Saturation_UpperSat);
rtb_Saturation[1] = rt_SATURATE(rtb_Saturation[1],
omni_interface_P.Saturation_LowerSat, omni_interface_P.Saturation_UpperSat);
rtb_Saturation[2] = rt_SATURATE(rtb_Saturation[2],
omni_interface_P.Saturation_LowerSat, omni_interface_P.Saturation_UpperSat);
/* S-Function Block: omni_interface/HIL Write (hil_write_block) */
{
t_error result;
result = hil_write(omni_interface_DWork.HILInitialize_Card,
NULL, 0U,
omni_interface_P.HILWrite_PWMChannels, 3U,
NULL, 0U,
NULL, 0U,
NULL,
rtb_Saturation,
NULL,
NULL
);
if (result < 0) {
msg_get_error_messageA(NULL, result, _rt_error_message, sizeof
(_rt_error_message));
rtmSetErrorStatus(omni_interface_M, _rt_error_message);
}
}
/* S-Function (stream_read_block): '<Root>/Stream Read1' */
/* S-Function Block: omni_interface/Stream Read1 (stream_read_block) */
{
t_error result;
if (rtb_StreamAnswer1_o1 != NULL) {
result = stream_receive_unit_array(*rtb_StreamAnswer1_o1,
rtb_StreamRead1_o2, sizeof(real_T), 3);
if (result == -QERR_WOULD_BLOCK) {
result = 0;
}
/*
if (result <= 0 && result != -QERR_WOULD_BLOCK) {
memset(rtb_StreamRead1_o2, 0, 3 * sizeof(real_T));
}
*/
} else {
memset(rtb_StreamRead1_o2, 0, 3 * sizeof(real_T));
result = 0;
}
rtb_StreamRead1_o1 = rtb_StreamAnswer1_o1;
}
/* Logic: '<Root>/Logical Operator2' */
rtb_LogicalOperator2[0] = ((rtb_HILRead_o3[0] != 0.0) || (0.0 != 0.0) ||
(rtb_StreamRead1_o2[1] != 0.0));
rtb_LogicalOperator2[1] = ((rtb_HILRead_o3[1] != 0.0) || (0.0 != 0.0) ||
(rtb_StreamRead1_o2[1] != 0.0));
/* Stop: '<Root>/Stop Simulation' */
if (rtb_LogicalOperator2[0] || rtb_LogicalOperator2[1]) {
rtmSetStopRequested(omni_interface_M, 1);
}
UNUSED_PARAMETER(tid);
}
int main(int argc, char **argv)
{
int ret;
baseRateInfo_t info;
pthread_attr_t attr;
pthread_t baseRateThread;
size_t stackSize;
unsigned long cpuMask = 0x1;
unsigned int len = sizeof(cpuMask);
printf("**starting the model**\n");
fflush(stdout);
rtmSetErrorStatus(raspberrypi_audioequalizer_M, 0);
rtExtModeParseArgs(argc, (const char_T **)argv, NULL);
/* All threads created by this process must run on a single CPU */
ret = sched_setaffinity(0, len, (cpu_set_t *) &cpuMask);
CHECK_STATUS(ret, "sched_setaffinity");
/* Initialize semaphore used for thread synchronization */
ret = sem_init(&stopSem, 0, 0);
CHECK_STATUS(ret, "sem_init:stopSem");
/* Create threads executing the Simulink model */
pthread_attr_init(&attr);
ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
CHECK_STATUS(ret, "pthread_attr_setinheritsched");
ret = pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
CHECK_STATUS(ret, "pthread_attr_setdetachstate");
/* PTHREAD_STACK_MIN is the minimum stack size required to start a thread */
stackSize = 64000 + PTHREAD_STACK_MIN;
ret = pthread_attr_setstacksize(&attr, stackSize);
CHECK_STATUS(ret, "pthread_attr_setstacksize");
/* Block signal used for timer notification */
info.period = 0.1;
info.signo = SIGRTMIN;
sigemptyset(&info.sigMask);
MW_blockSignal(info.signo, &info.sigMask);
signal(SIGTERM, MW_exitHandler); /* kill */
signal(SIGHUP, MW_exitHandler); /* kill -HUP */
signal(SIGINT, MW_exitHandler); /* Interrupt from keyboard */
signal(SIGQUIT, MW_exitHandler); /* Quit from keyboard */
raspberrypi_audioequalizer_initialize();
/* External mode */
rtSetTFinalForExtMode(&rtmGetTFinal(raspberrypi_audioequalizer_M));
rtExtModeCheckInit(1);
{
boolean_T rtmStopReq = FALSE;
rtExtModeWaitForStartPkt(raspberrypi_audioequalizer_M->extModeInfo, 1,
&rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(raspberrypi_audioequalizer_M, TRUE);
}
}
rtERTExtModeStartMsg();
/* Create base rate task */
ret = pthread_create(&baseRateThread, &attr, (void *) baseRateTask, (void *)
&info);
CHECK_STATUS(ret, "pthread_create");
pthread_attr_destroy(&attr);
/* Wait for a stopping condition. */
MW_sem_wait(&stopSem);
/* Received stop signal */
printf("**stopping the model**\n");
if (rtmGetErrorStatus(raspberrypi_audioequalizer_M) != NULL) {
printf("\n**%s**\n", rtmGetErrorStatus(raspberrypi_audioequalizer_M));
}
/* External mode shutdown */
rtExtModeShutdown(1);
/* Disable rt_OneStep() here */
/* Terminate model */
raspberrypi_audioequalizer_terminate();
return 0;
}
/* Model output function */
void Crane_output(int_T tid)
{
/* local block i/o variables */
real_T rtb_Falcon_o1[3];
real_T rtb_Gain4[3];
boolean_T rtb_Falcon_o2[4];
/* Update absolute time of base rate at minor time step */
if (rtmIsMinorTimeStep(Crane_M)) {
Crane_M->Timing.t[0] = rtsiGetT(&Crane_M->solverInfo);
}
if (rtmIsMajorTimeStep(Crane_M)) {
/* set solver stop time */
rtsiSetSolverStopTime(&Crane_M->solverInfo,
((Crane_M->Timing.clockTick0+1)*
Crane_M->Timing.stepSize0));
} /* end MajorTimeStep */
{
real_T rtb_Product_idx;
real_T rtb_Product_idx_0;
real_T rtb_Product_idx_1;
real_T rtb_Product1_idx;
real_T rtb_Product1_idx_0;
real_T rtb_Product1_idx_1;
/* S-Function Block: <S10>/Block#1 */
{
_rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block1_PWORK;
mechWork->genSimData.time = Crane_M->Timing.t[0];
mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M);
if (kinematicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
&(mechWork->outSimData))) {
{
const ErrorRecord *err = getErrorMsg();
static char_T errorMsg[1024];
sprintf(errorMsg,
err->errorMsg,
err->blocks[0],
err->blocks[1],
err->blocks[2],
err->blocks[3],
err->blocks[4]);
rtmSetErrorStatus(Crane_M, errorMsg);
return;
}
}
}
/* Gain: '<S2>/gain_1' */
Crane_B.gain_1[0] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[0];
Crane_B.gain_1[1] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[1];
Crane_B.gain_1[2] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[2];
if (rtmIsMajorTimeStep(Crane_M) &&
Crane_M->Timing.TaskCounters.TID[1] == 0) {
/* Memory: '<Root>/Memory' */
Crane_B.Memory[0] = Crane_DWork.Memory_PreviousInput[0];
Crane_B.Memory[1] = Crane_DWork.Memory_PreviousInput[1];
Crane_B.Memory[2] = Crane_DWork.Memory_PreviousInput[2];
}
/* Sum: '<S9>/Sum' */
Crane_B.Sum[0] = Crane_B.Memory[0] - Crane_B.gain_1[0];
Crane_B.Sum[1] = Crane_B.Memory[1] - Crane_B.gain_1[1];
Crane_B.Sum[2] = Crane_B.Memory[2] - Crane_B.gain_1[2];
/* Product: '<S9>/Product' incorporates:
* Constant: '<Root>/Constant2'
*/
rtb_Product_idx = Crane_P.Constant2_Value * Crane_B.Sum[0];
rtb_Product_idx_0 = Crane_P.Constant2_Value * Crane_B.Sum[1];
rtb_Product_idx_1 = Crane_P.Constant2_Value * Crane_B.Sum[2];
/* Integrator: '<S9>/Integrator' */
rtb_Gain4[0] = Crane_X.Integrator_CSTATE[0];
rtb_Gain4[1] = Crane_X.Integrator_CSTATE[1];
rtb_Gain4[2] = Crane_X.Integrator_CSTATE[2];
/* Product: '<S9>/Product1' incorporates:
* Constant: '<Root>/Constant5'
*/
rtb_Product1_idx = Crane_P.Constant5_Value * rtb_Gain4[0];
rtb_Product1_idx_0 = Crane_P.Constant5_Value * rtb_Gain4[1];
rtb_Product1_idx_1 = Crane_P.Constant5_Value * rtb_Gain4[2];
/* Derivative Block: '<S9>/Derivative' */
{
real_T t = Crane_M->Timing.t[0];
real_T timeStampA = Crane_DWork.Derivative_RWORK.TimeStampA;
real_T timeStampB = Crane_DWork.Derivative_RWORK.TimeStampB;
if (timeStampA >= t && timeStampB >= t) {
rtb_Gain4[0] = 0.0;
rtb_Gain4[1] = 0.0;
rtb_Gain4[2] = 0.0;
} else {
real_T deltaT;
real_T *lastBank = &Crane_DWork.Derivative_RWORK.TimeStampA;
if (timeStampA < timeStampB) {
if (timeStampB < t) {
lastBank += 4;
}
} else if (timeStampA >= t) {
lastBank += 4;
}
deltaT = t - *lastBank++;
rtb_Gain4[0] = (Crane_B.Sum[0] - *lastBank++) / deltaT;
rtb_Gain4[1] = (Crane_B.Sum[1] - *lastBank++) / deltaT;
rtb_Gain4[2] = (Crane_B.Sum[2] - *lastBank++) / deltaT;
}
}
/* Sum: '<S9>/Sum1' incorporates:
* Constant: '<Root>/Constant6'
* Product: '<S9>/Product2'
*/
Crane_B.Sum1[0] = (rtb_Product_idx + rtb_Product1_idx) +
Crane_P.Constant6_Value * rtb_Gain4[0];
Crane_B.Sum1[1] = (rtb_Product_idx_0 + rtb_Product1_idx_0) +
Crane_P.Constant6_Value * rtb_Gain4[1];
Crane_B.Sum1[2] = (rtb_Product_idx_1 + rtb_Product1_idx_1) +
Crane_P.Constant6_Value * rtb_Gain4[2];
/* Gain: '<Root>/Gain4' */
rtb_Gain4[0] = Crane_P.Gain4_Gain * Crane_B.Sum1[0];
rtb_Gain4[1] = Crane_P.Gain4_Gain * Crane_B.Sum1[1];
rtb_Gain4[2] = Crane_P.Gain4_Gain * Crane_B.Sum1[2];
/* Saturate: '<Root>/Saturation' */
Crane_B.Saturation[0] = rt_SATURATE(rtb_Gain4[2],
Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat);
Crane_B.Saturation[1] = rt_SATURATE(rtb_Gain4[0],
Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat);
Crane_B.Saturation[2] = rt_SATURATE(rtb_Gain4[1],
Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat);
if (rtmIsMajorTimeStep(Crane_M) &&
Crane_M->Timing.TaskCounters.TID[1] == 0) {
/* S-Function Block: Crane/Falcon (falcon_block) */
{
t_error result;
double force_vector[3];
double position[3];
t_int read_buttons;
force_vector[0] = Crane_B.Saturation[0];
force_vector[1] = Crane_B.Saturation[1];
force_vector[2] = Crane_B.Saturation[2];
/* read the position and buttons, and output the requested force to the falcon */
result = falcon_read_write(Crane_DWork.Falcon_Falcon, position,
&read_buttons, force_vector);
if (result < 0) {
msg_get_error_messageA(NULL, result, _rt_error_message, sizeof
(_rt_error_message));
rtmSetErrorStatus(Crane_M, _rt_error_message);
return;
}
rtb_Falcon_o1[0] = position[0];
rtb_Falcon_o1[1] = position[1];
rtb_Falcon_o1[2] = position[2];
rtb_Falcon_o2[0] = ((read_buttons & 0x01) != 0);
rtb_Falcon_o2[1] = ((read_buttons & 0x02) != 0);
rtb_Falcon_o2[2] = ((read_buttons & 0x04) != 0);
rtb_Falcon_o2[3] = ((read_buttons & 0x08) != 0);
}
/* Gain: '<Root>/Gain3' */
Crane_B.Gain3[0] = Crane_P.Gain3_Gain * rtb_Falcon_o1[0];
Crane_B.Gain3[1] = Crane_P.Gain3_Gain * rtb_Falcon_o1[1];
Crane_B.Gain3[2] = Crane_P.Gain3_Gain * rtb_Falcon_o1[2];
/* Constant: '<Root>/Constant' */
Crane_B.Constant = Crane_P.Constant_Value;
/* Stop: '<Root>/Stop Simulation' */
if (rtb_Falcon_o2[0] || rtb_Falcon_o2[1] || rtb_Falcon_o2[2] ||
rtb_Falcon_o2[3]) {
rtmSetStopRequested(Crane_M, 1);
}
/* Gain: '<Root>/Gain2' incorporates:
* Constant: '<Root>/Constant1'
*/
Crane_B.Gain2 = Crane_P.Gain2_Gain * Crane_P.Constant1_Value;
/* Gain: '<Root>/Gain1' incorporates:
* Constant: '<Root>/Constant3'
*/
Crane_B.Gain1 = Crane_P.Gain1_Gain * Crane_P.Constant3_Value;
}
/* Gain: '<Root>/Gain' incorporates:
* Sin: '<Root>/Sine Wave'
*/
Crane_B.Gain = (sin(Crane_P.SineWave_Freq * Crane_M->Timing.t[0] +
Crane_P.SineWave_Phase) * Crane_P.SineWave_Amp +
Crane_P.SineWave_Bias) * Crane_P.Gain_Gain;
if (rtmIsMajorTimeStep(Crane_M) &&
Crane_M->Timing.TaskCounters.TID[1] == 0) {
/* Gain: '<S10>/_gravity_conversion' incorporates:
* Constant: '<S8>/SOURCE_BLOCK'
*/
Crane_B._gravity_conversion[0] = Crane_P._gravity_conversion_Gain *
Crane_P.SOURCE_BLOCK_Value[0];
Crane_B._gravity_conversion[1] = Crane_P._gravity_conversion_Gain *
Crane_P.SOURCE_BLOCK_Value[1];
Crane_B._gravity_conversion[2] = Crane_P._gravity_conversion_Gain *
Crane_P.SOURCE_BLOCK_Value[2];
}
/* S-Function Block: <S10>/Block#2 */
{
_rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block2_PWORK;
mechWork->genSimData.time = Crane_M->Timing.t[0];
mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M);
if (dynamicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
&(mechWork->outSimData))) {
{
const ErrorRecord *err = getErrorMsg();
static char_T errorMsg[1024];
sprintf(errorMsg,
err->errorMsg,
err->blocks[0],
err->blocks[1],
err->blocks[2],
err->blocks[3],
err->blocks[4]);
rtmSetErrorStatus(Crane_M, errorMsg);
return;
}
}
}
/* S-Function Block: <S10>/Block#3 */
{
_rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block3_PWORK;
mechWork->genSimData.time = Crane_M->Timing.t[0];
mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M);
if (eventSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
&(mechWork->outSimData))) {
{
const ErrorRecord *err = getErrorMsg();
static char_T errorMsg[1024];
sprintf(errorMsg,
err->errorMsg,
err->blocks[0],
err->blocks[1],
err->blocks[2],
err->blocks[3],
err->blocks[4]);
rtmSetErrorStatus(Crane_M, errorMsg);
return;
}
}
}
}
UNUSED_PARAMETER(tid);
}
void MdlInitialize(void)
{
{
/* Signal Processing Blockset From Wave File (sdspwafi2) - '<Root>/From Wave File' - Initialize */
MWDSP_Wafi_FunctionPtrStruct* fcnPtrs = (MWDSP_Wafi_FunctionPtrStruct*)
&CelpSimulink_DWork.FromWaveFile_RuntimeFcnPtrs[0];
/* destroy previous runtime object, if there is one */
if (fcnPtrs != NULL && fcnPtrs->terminateFcn != NULL)
fcnPtrs->terminateFcn(CelpSimulink_DWork.FromWaveFile_FromWaveFileObj);
CelpSimulink_DWork.FromWaveFile_FromWaveFileObj = exMWDSP_Wafi_Create(
"C:\\Documents and Settings\\Maquina\\Meus documentos\\Arthur\\UFRGS\\DSP\\CELP\\audio.wav",
16U, 80, 1, 80, 8000.0, 1);
if (CelpSimulink_DWork.FromWaveFile_FromWaveFileObj == NULL) {
const char* errMsg = exMWDSP_Wafi_GetErrorMessage();
rtmSetErrorStatus(CelpSimulink_M, errMsg);
rtmSetStopRequested(CelpSimulink_M, 1);
} else {
exMWDSP_Wafi_GetFunctionPtrs(fcnPtrs);
}
}
/* Signal Processing Blockset Filter Implementation (sdspfilter2) - '<S2>/Pre-Emphasis' */
/* FIR, Direct-form */
{
real32_T *statePtr = (real32_T *)
&CelpSimulink_DWork.PreEmphasis_FILT_STATES[0];
/* Scalar expansion of ICs with extra zero element per channel */
*statePtr++ = *(const real32_T *)&CelpSimulink_ConstP.pooled1;
*statePtr++ = 0.0F;
}
/* Copy ICs into circular buffer */
{
const int_T bufLenBytes = 80 * sizeof(real32_T);
byte_T *circBufPtr = (byte_T *)
&CelpSimulink_DWork.OverlapAnalysisWindows_CircBuff[0];
const byte_T *icPtr = (const byte_T *)&CelpSimulink_ConstP.pooled1;
int_T i = 1;
while (i-- > 0) {
MWDSP_CopyScalarICs(circBufPtr, icPtr, 80, sizeof(real32_T));
circBufPtr += bufLenBytes;
}
}
/* Signal Processing Blockset Filter Implementation (sdspfilter2) - '<S3>/Time-Varying Synthesis Filter' */
/* All-pole, Direct-form (a0 ~= 1) */
{
real32_T *statePtr = (real32_T *)
&CelpSimulink_DWork.TimeVaryingSynthesisFilter_FILT[0];
/* Scalar expansion of ICs with extra zero element per channel */
int_T chanCount = 80;
while (chanCount--) {
int_T numElems= 10;
while (numElems--) {
*statePtr++ = *(const real32_T *)&CelpSimulink_ConstP.pooled1;
}
*statePtr++ = 0.0F;
}
}
/* Signal Processing Blockset Filter Implementation (sdspfilter2) - '<S3>/De-emphasis Filter' */
/* All-pole, Direct-form (a0 == 1) */
{
real32_T *statePtr = (real32_T *)
&CelpSimulink_DWork.DeemphasisFilter_FILT_STATES[0];
/* Scalar expansion of ICs with extra zero element per channel */
int_T chanCount = 80;
while (chanCount--) {
*statePtr++ = *(const real32_T *)&CelpSimulink_ConstP.pooled1;
*statePtr++ = 0.0F;
}
}
}
/* Model initialize function */
void udpRead_initialize(void)
{
/* Registration code */
/* initialize non-finites */
rt_InitInfAndNaN(sizeof(real_T));
/* initialize real-time model */
(void) memset((void *)udpRead_M, 0,
sizeof(RT_MODEL_udpRead_T));
rtmSetTFinal(udpRead_M, -1);
udpRead_M->Timing.stepSize0 = 0.001;
/* External mode info */
udpRead_M->Sizes.checksums[0] = (597430241U);
udpRead_M->Sizes.checksums[1] = (1019470990U);
udpRead_M->Sizes.checksums[2] = (4143940322U);
udpRead_M->Sizes.checksums[3] = (93090447U);
{
static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
static RTWExtModeInfo rt_ExtModeInfo;
static const sysRanDType *systemRan[5];
udpRead_M->extModeInfo = (&rt_ExtModeInfo);
rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
systemRan[0] = &rtAlwaysEnabled;
systemRan[1] = (sysRanDType *)&udpRead_DW.ForIteratorSubsystem_SubsysRanB;
systemRan[2] = (sysRanDType *)&udpRead_DW.ForIteratorSubsystem1_SubsysRan;
systemRan[3] = (sysRanDType *)&udpRead_DW.EnabledSubsystem_SubsysRanBC;
systemRan[4] = &rtAlwaysEnabled;
rteiSetModelMappingInfoPtr(udpRead_M->extModeInfo,
&udpRead_M->SpecialInfo.mappingInfo);
rteiSetChecksumsPtr(udpRead_M->extModeInfo, udpRead_M->Sizes.checksums);
rteiSetTPtr(udpRead_M->extModeInfo, rtmGetTPtr(udpRead_M));
}
/* block I/O */
(void) memset(((void *) &udpRead_B), 0,
sizeof(B_udpRead_T));
/* states (dwork) */
(void) memset((void *)&udpRead_DW, 0,
sizeof(DW_udpRead_T));
/* data type transition information */
{
static DataTypeTransInfo dtInfo;
(void) memset((char_T *) &dtInfo, 0,
sizeof(dtInfo));
udpRead_M->SpecialInfo.mappingInfo = (&dtInfo);
dtInfo.numDataTypes = 14;
dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
dtInfo.dataTypeNames = &rtDataTypeNames[0];
/* Block I/O transition table */
dtInfo.B = &rtBTransTable;
/* Parameters transition table */
dtInfo.P = &rtPTransTable;
}
{
char_T *sErr;
/* Start for S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
sErr = GetErrorBuffer(&udpRead_DW.UDPReceive_NetworkLib[0U]);
CreateUDPInterface(&udpRead_DW.UDPReceive_NetworkLib[0U]);
if (*sErr == 0) {
LibCreate_Network(&udpRead_DW.UDPReceive_NetworkLib[0U], 0, "0.0.0.0",
25000, "0.0.0.0", -1, 8192, 4, 0);
}
if (*sErr == 0) {
LibStart(&udpRead_DW.UDPReceive_NetworkLib[0U]);
}
if (*sErr != 0) {
DestroyUDPInterface(&udpRead_DW.UDPReceive_NetworkLib[0U]);
if (*sErr != 0) {
rtmSetErrorStatus(udpRead_M, sErr);
rtmSetStopRequested(udpRead_M, 1);
}
}
/* End of Start for S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
/* InitializeConditions for UnitDelay: '<S3>/FixPt Unit Delay2' */
udpRead_DW.FixPtUnitDelay2_DSTATE =
udpRead_P.FixPtUnitDelay2_InitialConditio;
/* InitializeConditions for UnitDelay: '<S3>/FixPt Unit Delay1' */
udpRead_DW.FixPtUnitDelay1_DSTATE = udpRead_P.UnitDelayResettable_vinit;
}
}
int_T main(void)
{
init();
#ifdef _RTT_USE_SERIAL1_
Serial_begin(1, 9600);
#endif
#ifdef _RTT_USE_SERIAL2_
Serial_begin(2, 9600);
#endif
#ifdef _RTT_USE_SERIAL3_
Serial_begin(3, 9600);
#endif
/* initialize external mode */
rtParseArgsForExtMode(0, NULL);
velo_id_gain_initialize();
/* External mode */
rtSetTFinalForExtMode(&rtmGetTFinal(velo_id_gain_M));
rtExtModeCheckInit(2);
{
boolean_T rtmStopReq = false;
rtExtModeWaitForStartPkt(velo_id_gain_M->extModeInfo, 2, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(velo_id_gain_M, true);
}
}
rtERTExtModeStartMsg();
arduino_Timer_Setup();
/* The main step loop */
while ((rtmGetErrorStatus(velo_id_gain_M) == (NULL)) && !rtmGetStopRequested
(velo_id_gain_M)) {
/* External mode */
{
boolean_T rtmStopReq = false;
rtExtModeOneStep(velo_id_gain_M->extModeInfo, 2, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(velo_id_gain_M, true);
}
}
rtExtModeCheckEndTrigger();
}
rtExtModeShutdown(2);
delay(1000);
velo_id_gain_terminate();
/* Disable Interrupts */
cli();
return 0;
}
/* Model step function */
void trajectoryModel_step(void)
{
/* local block i/o variables */
real_T rtb_Sqrt;
real_T rtb_Divide1;
int8_T rtAction;
real_T rtb_Divide;
real_T rtb_Add1;
if (rtmIsMajorTimeStep(trajectoryModel_M)) {
/* set solver stop time */
if (!(trajectoryModel_M->Timing.clockTick0+1)) {
rtsiSetSolverStopTime(&trajectoryModel_M->solverInfo,
((trajectoryModel_M->Timing.clockTickH0 + 1) *
trajectoryModel_M->Timing.stepSize0 * 4294967296.0));
} else {
rtsiSetSolverStopTime(&trajectoryModel_M->solverInfo,
((trajectoryModel_M->Timing.clockTick0 + 1) *
trajectoryModel_M->Timing.stepSize0 +
trajectoryModel_M->Timing.clockTickH0 *
trajectoryModel_M->Timing.stepSize0 * 4294967296.0));
}
} /* end MajorTimeStep */
/* Update absolute time of base rate at minor time step */
if (rtmIsMinorTimeStep(trajectoryModel_M)) {
trajectoryModel_M->Timing.t[0] = rtsiGetT(&trajectoryModel_M->solverInfo);
}
/* Integrator: '<Root>/x' */
trajectoryModel_B.x = trajectoryModel_X.x_CSTATE;
/* Sum: '<S3>/x-Planet_x' incorporates:
* Constant: '<Root>/0 '
*/
rtb_Divide1 = trajectoryModel_B.x - trajectoryModel_P._Value;
/* Integrator: '<Root>/y ' */
trajectoryModel_B.y = trajectoryModel_X.y_CSTATE;
/* Sqrt: '<S3>/Sqrt' incorporates:
* Product: '<S3>/(x-Planet_x)^2'
* Product: '<S3>/y^2'
* Sum: '<S3>/(x-Planet_x)^2+y^2'
*/
rtb_Sqrt = sqrt(rtb_Divide1 * rtb_Divide1 + trajectoryModel_B.y *
trajectoryModel_B.y);
/* If: '<Root>/If' incorporates:
* Constant: '<Root>/stopRadius'
* Constant: '<Root>/terminate'
*/
if (rtmIsMajorTimeStep(trajectoryModel_M)) {
if (rtb_Sqrt < trajectoryModel_P.stopRadius) {
rtAction = 0;
} else {
rtAction = 1;
}
trajectoryModel_DW.If_ActiveSubsystem = rtAction;
} else {
rtAction = trajectoryModel_DW.If_ActiveSubsystem;
}
switch (rtAction) {
case 0:
/* Outputs for IfAction SubSystem: '<Root>/If Action Subsystem' incorporates:
* ActionPort: '<S1>/Action Port'
*/
trajectoryMod_IfActionSubsystem(rtb_Sqrt,
&trajectoryModel_B.IfActionSubsystem);
/* End of Outputs for SubSystem: '<Root>/If Action Subsystem' */
break;
case 1:
/* Outputs for IfAction SubSystem: '<Root>/If Action Subsystem1' incorporates:
* ActionPort: '<S2>/Action Port'
*/
trajectoryMod_IfActionSubsystem(trajectoryModel_P.terminate_Value,
&trajectoryModel_B.IfActionSubsystem1);
/* End of Outputs for SubSystem: '<Root>/If Action Subsystem1' */
break;
}
/* End of If: '<Root>/If' */
if (rtmIsMajorTimeStep(trajectoryModel_M)) {
/* Stop: '<Root>/Stop Simulation' */
if (trajectoryModel_B.IfActionSubsystem1.In1 != 0.0) {
rtmSetStopRequested(trajectoryModel_M, 1);
}
/* End of Stop: '<Root>/Stop Simulation' */
}
/* Integrator: '<Root>/dx' */
trajectoryModel_B.x_dot = trajectoryModel_X.dx_CSTATE;
/* Integrator: '<Root>/dy' */
trajectoryModel_B.y_dot = trajectoryModel_X.dy_CSTATE;
if (rtmIsMajorTimeStep(trajectoryModel_M)) {
}
/* Sum: '<S4>/x-Planet_x' incorporates:
* Constant: '<Root>/Earth_x'
*/
rtb_Divide1 = trajectoryModel_B.x - (-trajectoryModel_P.mu);
/* Sqrt: '<S4>/Sqrt' incorporates:
* Product: '<S4>/(x-Planet_x)^2'
* Product: '<S4>/y^2'
* Sum: '<S4>/(x-Planet_x)^2+y^2'
*/
rtb_Divide1 = sqrt(rtb_Divide1 * rtb_Divide1 + trajectoryModel_B.y *
trajectoryModel_B.y);
/* Product: '<Root>/Divide1' incorporates:
* Constant: '<Root>/Moon_x Earth mass'
* Product: '<Root>/Divide2'
*/
rtb_Divide1 = (1.0 - trajectoryModel_P.mu) / (rtb_Divide1 * rtb_Divide1 *
rtb_Divide1);
/* Sum: '<S5>/x-Planet_x' incorporates:
* Constant: '<Root>/Moon_x Earth mass'
*/
rtb_Divide = trajectoryModel_B.x - (1.0 - trajectoryModel_P.mu);
/* Sqrt: '<S5>/Sqrt' incorporates:
* Product: '<S5>/(x-Planet_x)^2'
* Product: '<S5>/y^2'
* Sum: '<S5>/(x-Planet_x)^2+y^2'
*/
rtb_Divide = sqrt(rtb_Divide * rtb_Divide + trajectoryModel_B.y *
trajectoryModel_B.y);
/* Product: '<Root>/Divide' incorporates:
* Constant: '<Root>/Moon mass'
* Product: '<Root>/Divide3'
*/
rtb_Divide = trajectoryModel_P.mu / (rtb_Divide * rtb_Divide * rtb_Divide);
/* Sum: '<Root>/Add1' incorporates:
* Constant: '<Root>/2'
*/
rtb_Add1 = (trajectoryModel_P._Value_f - rtb_Divide1) - rtb_Divide;
/* Sum: '<Root>/Add' incorporates:
* Gain: '<Root>/Gain'
* Product: '<Root>/Product'
*/
trajectoryModel_B.Add = trajectoryModel_B.y * rtb_Add1 -
trajectoryModel_P.Gain_Gain * trajectoryModel_B.x_dot;
/* Sum: '<Root>/Add6' incorporates:
* Constant: '<Root>/Earth_x'
* Constant: '<Root>/Moon_x Earth mass'
* Gain: '<Root>/Gain1'
* Product: '<Root>/Product5'
* Product: '<Root>/Product6'
* Product: '<Root>/Product7'
* Sum: '<Root>/Add7'
*/
trajectoryModel_B.Add6 = (((1.0 - trajectoryModel_P.mu) * rtb_Divide +
-trajectoryModel_P.mu * rtb_Divide1) + trajectoryModel_B.x * rtb_Add1) +
trajectoryModel_P.Gain1_Gain * trajectoryModel_B.y_dot;
if (rtmIsMajorTimeStep(trajectoryModel_M)) {
/* Matfile logging */
rt_UpdateTXYLogVars(trajectoryModel_M->rtwLogInfo,
(trajectoryModel_M->Timing.t));
} /* end MajorTimeStep */
if (rtmIsMajorTimeStep(trajectoryModel_M)) {
/* signal main to stop simulation */
{ /* Sample time: [0.0s, 0.0s] */
if ((rtmGetTFinal(trajectoryModel_M)!=-1) &&
!((rtmGetTFinal(trajectoryModel_M)-
(((trajectoryModel_M->Timing.clockTick1+
trajectoryModel_M->Timing.clockTickH1* 4294967296.0)) * 0.01)) >
(((trajectoryModel_M->Timing.clockTick1+
trajectoryModel_M->Timing.clockTickH1* 4294967296.0)) * 0.01) *
(DBL_EPSILON))) {
rtmSetErrorStatus(trajectoryModel_M, "Simulation finished");
}
}
rt_ertODEUpdateContinuousStates(&trajectoryModel_M->solverInfo);
/* Update absolute time for base rate */
/* The "clockTick0" counts the number of times the code of this task has
* been executed. The absolute time is the multiplication of "clockTick0"
* and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
* overflow during the application lifespan selected.
* Timer of this task consists of two 32 bit unsigned integers.
* The two integers represent the low bits Timing.clockTick0 and the high bits
* Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
*/
if (!(++trajectoryModel_M->Timing.clockTick0)) {
++trajectoryModel_M->Timing.clockTickH0;
}
trajectoryModel_M->Timing.t[0] = rtsiGetSolverStopTime
(&trajectoryModel_M->solverInfo);
{
/* Update absolute timer for sample time: [0.01s, 0.0s] */
/* The "clockTick1" counts the number of times the code of this task has
* been executed. The resolution of this integer timer is 0.01, which is the step size
* of the task. Size of "clockTick1" ensures timer will not overflow during the
* application lifespan selected.
* Timer of this task consists of two 32 bit unsigned integers.
* The two integers represent the low bits Timing.clockTick1 and the high bits
* Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
*/
trajectoryModel_M->Timing.clockTick1++;
if (!trajectoryModel_M->Timing.clockTick1) {
trajectoryModel_M->Timing.clockTickH1++;
}
}
} /* end MajorTimeStep */
}
int main(void)
{
volatile boolean_T runModel = 1;
float modelBaseRate = 0.01;
float systemClock = 168;
#ifndef USE_RTX
__disable_irq();
#endif
;
stm32f4xx_init_board();
SystemCoreClockUpdate();
bootloaderInit();
rtmSetErrorStatus(BeschleunigungsAuswertung_M, 0);
/* initialize external mode */
rtParseArgsForExtMode(0, NULL);
BeschleunigungsAuswertung_initialize();
__enable_irq();
/* External mode */
rtSetTFinalForExtMode(&rtmGetTFinal(BeschleunigungsAuswertung_M));
rtExtModeCheckInit(1);
{
boolean_T rtmStopReq = false;
rtExtModeWaitForStartPkt(BeschleunigungsAuswertung_M->extModeInfo, 1,
&rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(BeschleunigungsAuswertung_M, true);
}
}
rtERTExtModeStartMsg();
__disable_irq();
ARMCM_SysTick_Config(modelBaseRate);
runModel =
(rtmGetErrorStatus(BeschleunigungsAuswertung_M) == (NULL)) &&
!rtmGetStopRequested(BeschleunigungsAuswertung_M);
__enable_irq();
__enable_irq();
while (runModel) {
/* External mode */
{
boolean_T rtmStopReq = false;
rtExtModeOneStep(BeschleunigungsAuswertung_M->extModeInfo, 1, &rtmStopReq);
if (rtmStopReq) {
rtmSetStopRequested(BeschleunigungsAuswertung_M, true);
}
}
runModel =
(rtmGetErrorStatus(BeschleunigungsAuswertung_M) == (NULL)) &&
!rtmGetStopRequested(BeschleunigungsAuswertung_M);
}
rtExtModeShutdown(1);
#ifndef USE_RTX
(void)systemClock;
#endif
;
/* Disable rt_OneStep() here */
/* Terminate model */
BeschleunigungsAuswertung_terminate();
__disable_irq();
return 0;
}
/* Model output function */
void udpRead_output(void)
{
char_T *sErr;
int32_T samplesRead;
boolean_T rtb_Compare;
int32_T s5_iter;
real_T tmp;
int32_T exitg1;
int32_T exitg2;
/* Reset subsysRan breadcrumbs */
srClearBC(udpRead_DW.ForIteratorSubsystem_SubsysRanB);
/* Reset subsysRan breadcrumbs */
srClearBC(udpRead_DW.ForIteratorSubsystem1_SubsysRan);
/* Reset subsysRan breadcrumbs */
srClearBC(udpRead_DW.EnabledSubsystem_SubsysRanBC);
/* S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
sErr = GetErrorBuffer(&udpRead_DW.UDPReceive_NetworkLib[0U]);
samplesRead = 31;
LibOutputs_Network(&udpRead_DW.UDPReceive_NetworkLib[0U],
&udpRead_B.UDPReceive_o1[0U], &samplesRead);
if (*sErr != 0) {
rtmSetErrorStatus(udpRead_M, sErr);
rtmSetStopRequested(udpRead_M, 1);
}
udpRead_B.UDPReceive_o2 = (uint16_T)samplesRead;
/* End of S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
/* RelationalOperator: '<S1>/Compare' incorporates:
* Constant: '<S1>/Constant'
*/
rtb_Compare = (udpRead_B.UDPReceive_o2 > udpRead_P.Constant_Value_p);
/* Outputs for Enabled SubSystem: '<Root>/Enabled Subsystem' incorporates:
* EnablePort: '<S2>/Enable'
*/
if (rtb_Compare) {
if (!udpRead_DW.EnabledSubsystem_MODE) {
udpRead_DW.EnabledSubsystem_MODE = true;
}
/* Outputs for Iterator SubSystem: '<S2>/For Iterator Subsystem1' incorporates:
* ForIterator: '<S5>/For Iterator'
*/
/* Selector: '<S5>/Selector' incorporates:
* Selector: '<S5>/Selector1'
* Selector: '<S5>/Selector2'
*/
s5_iter = 24;
do {
exitg2 = 0;
if (udpRead_P.Constant1_Value < 0.0) {
tmp = ceil(udpRead_P.Constant1_Value);
} else {
tmp = floor(udpRead_P.Constant1_Value);
}
if (rtIsNaN(tmp) || rtIsInf(tmp)) {
tmp = 0.0;
} else {
tmp = fmod(tmp, 4.294967296E+9);
}
if (s5_iter - 23 <= (tmp < 0.0 ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp)) {
udpRead_B.Selector = udpRead_B.UDPReceive_o1[s5_iter];
udpRead_B.Selector1 = udpRead_B.UDPReceive_o1[s5_iter + 2];
udpRead_B.Selector2 = udpRead_B.UDPReceive_o1[s5_iter + 4];
srUpdateBC(udpRead_DW.ForIteratorSubsystem1_SubsysRan);
s5_iter++;
} else {
exitg2 = 1;
}
} while (exitg2 == 0);
/* End of Outputs for SubSystem: '<S2>/For Iterator Subsystem1' */
/* Outputs for Iterator SubSystem: '<S2>/For Iterator Subsystem' incorporates:
* ForIterator: '<S4>/For Iterator'
*/
s5_iter = 1;
do {
exitg1 = 0;
if (udpRead_P.Constant_Value < 0.0) {
tmp = ceil(udpRead_P.Constant_Value);
} else {
tmp = floor(udpRead_P.Constant_Value);
}
if (rtIsNaN(tmp) || rtIsInf(tmp)) {
tmp = 0.0;
} else {
tmp = fmod(tmp, 4.294967296E+9);
}
if (s5_iter <= (tmp < 0.0 ? -(int32_T)(uint32_T)-tmp : (int32_T)(uint32_T)
tmp)) {
udpRead_B.Selector_d = udpRead_B.UDPReceive_o1[s5_iter - 1];
udpRead_B.Selector1_f = udpRead_B.UDPReceive_o1[s5_iter + 3];
udpRead_B.Selector2_f = udpRead_B.UDPReceive_o1[s5_iter + 7];
udpRead_B.Compare = (s5_iter == udpRead_P.CompareToConstant_const);
udpRead_B.Selector3 = udpRead_B.UDPReceive_o1[s5_iter + 11];
udpRead_B.Selector4 = udpRead_B.UDPReceive_o1[s5_iter + 15];
udpRead_B.Selector5 = udpRead_B.UDPReceive_o1[s5_iter + 19];
srUpdateBC(udpRead_DW.ForIteratorSubsystem_SubsysRanB);
s5_iter++;
} else {
exitg1 = 1;
}
} while (exitg1 == 0);
/* End of Outputs for SubSystem: '<S2>/For Iterator Subsystem' */
srUpdateBC(udpRead_DW.EnabledSubsystem_SubsysRanBC);
} else {
if (udpRead_DW.EnabledSubsystem_MODE) {
udpRead_DW.EnabledSubsystem_MODE = false;
}
}
/* End of Outputs for SubSystem: '<Root>/Enabled Subsystem' */
/* Switch: '<S3>/Init' incorporates:
* Constant: '<S3>/Initial Condition'
* Logic: '<S3>/FixPt Logical Operator'
* UnitDelay: '<S3>/FixPt Unit Delay1'
* UnitDelay: '<S3>/FixPt Unit Delay2'
*/
if (rtb_Compare || (udpRead_DW.FixPtUnitDelay2_DSTATE != 0)) {
udpRead_B.Init = udpRead_P.UnitDelayResettable_vinit;
} else {
udpRead_B.Init = udpRead_DW.FixPtUnitDelay1_DSTATE;
}
/* End of Switch: '<S3>/Init' */
/* Switch: '<S3>/Reset' incorporates:
* Constant: '<S3>/Initial Condition'
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Sum'
*/
if (rtb_Compare) {
udpRead_B.Xnew = udpRead_P.UnitDelayResettable_vinit;
} else {
udpRead_B.Xnew = 1.0F + udpRead_B.Init;
}
/* End of Switch: '<S3>/Reset' */
}