328 lines
11 KiB
C
328 lines
11 KiB
C
/**
|
||
**************************************************************************
|
||
* @file MCU.c
|
||
* @brief Исходный код S-Function.
|
||
**************************************************************************
|
||
@details
|
||
Данный файл содержит функции S-Function, который вызывает MATLAB.
|
||
**************************************************************************
|
||
@note
|
||
Описание функций по большей части сгенерировано MATLAB'ом, поэтому на английском
|
||
**************************************************************************/
|
||
|
||
/**
|
||
* @addtogroup WRAPPER_SFUNC S-Function funtions
|
||
* @ingroup MCU_WRAPPER
|
||
* @brief Дефайны и функции блока S-Function
|
||
* @details Здесь собраны функции, с которыми непосредственно работает S-Function
|
||
* @note Описание функций по большей части сгенерировано MATLAB'ом, поэтому на английском
|
||
* @{
|
||
*/
|
||
|
||
#define S_FUNCTION_NAME MCU
|
||
#define S_FUNCTION_LEVEL 2
|
||
|
||
#include "mcu_wrapper_conf.h"
|
||
|
||
#include <windows.h>
|
||
#include <process.h>
|
||
#include <intrin.h>
|
||
#include "mex.h"
|
||
|
||
|
||
static unsigned long long get_timer_frequency(void)
|
||
{
|
||
#ifdef USE_CPU_TIMER
|
||
HKEY hKey;
|
||
DWORD frequency = 0;
|
||
DWORD size = sizeof(DWORD);
|
||
|
||
if (RegOpenKeyExA(HKEY_LOCAL_MACHINE, "HARDWARE\\DESCRIPTION\\System\\CentralProcessor\\0",
|
||
0, KEY_READ, &hKey) == ERROR_SUCCESS) {
|
||
if (RegQueryValueExA(hKey, "~MHz", NULL, NULL, (LPBYTE)&frequency, &size) == ERROR_SUCCESS) {
|
||
RegCloseKey(hKey);
|
||
return (unsigned long long)frequency * 1000000ULL; // MHz -> Hz
|
||
}
|
||
RegCloseKey(hKey);
|
||
}
|
||
#elif defined(USE_QPF_TIMER)
|
||
LARGE_INTEGER frequency;
|
||
QueryPerformanceFrequency(&frequency);
|
||
return frequency.QuadPart;
|
||
#endif
|
||
}
|
||
|
||
static double ticksToNanoseconds(unsigned long long start, unsigned long long end)
|
||
{
|
||
#if defined(USE_CPU_TIMER) || defined(USE_QPF_TIMER)
|
||
unsigned long long elapsed = end - start;
|
||
unsigned long long frequency = get_timer_frequency();
|
||
return (double)elapsed * 1e9 / (double)frequency;
|
||
#endif
|
||
}
|
||
|
||
static void InitializeHighPrecisionTimer(void)
|
||
{
|
||
#ifdef USE_QPF_TIMER
|
||
QueryPerformanceFrequency(&hmcu.dTimer.Frequency);
|
||
hmcu.dTimer.TimerResolutionNs = 1e9 / (double)(hmcu.dTimer.Frequency);
|
||
#endif
|
||
}
|
||
|
||
static unsigned long long read_timer(void)
|
||
{
|
||
#ifdef USE_CPU_TIMER
|
||
return __rdtsc();
|
||
#elif defined(USE_QPF_TIMER)
|
||
LARGE_INTEGER counter;
|
||
QueryPerformanceCounter(&counter);
|
||
return counter.QuadPart;
|
||
#endif
|
||
}
|
||
|
||
|
||
#define MDL_UPDATE ///< для подключения mdlUpdate()
|
||
/**
|
||
* @brief Update S-Function at every step of simulation
|
||
* @param S - pointer to S-Function (library struct from "simstruc.h")
|
||
* @details Abstract:
|
||
* This function is called once for every major integration time step.
|
||
* Discrete states are typically updated here, but this function is useful
|
||
* for performing any tasks that should only take place once per
|
||
* integration step.
|
||
*/
|
||
static void mdlUpdate(SimStruct* S, int_T tid)
|
||
{
|
||
// Расчет периода (время между вызовами)
|
||
if (hmcu.dTimer.call_count > 0 && hmcu.dTimer.SFuncPrevTime != 0) {
|
||
hmcu.dSFuncPeriod = ticksToNanoseconds(
|
||
hmcu.dTimer.SFuncPrevTime,
|
||
hmcu.dTimer.SFuncStartTime) / 1000.0; // в микросекундах
|
||
}
|
||
|
||
// Сохраняем текущее время для следующего расчета периода
|
||
hmcu.dTimer.SFuncPrevTime = hmcu.dTimer.SFuncStartTime;
|
||
// Текущий вызов становится началом S-Function
|
||
hmcu.dTimer.SFuncStartTime = read_timer();
|
||
|
||
// get time of simulation
|
||
time_T TIME = ssGetT(S);
|
||
|
||
//---------------SIMULATE MCU---------------
|
||
// Измерение времени выполнения MCU_Step_Simulation
|
||
hmcu.dTimer.MCUStepStartTime = read_timer();
|
||
MCU_Step_Simulation(S, TIME); // SIMULATE MCU
|
||
hmcu.dTimer.MCUStepEndTime = read_timer();
|
||
//------------------------------------------
|
||
|
||
// Расчет времени выполнения в микросекундах
|
||
hmcu.dMCUStepTime = ticksToNanoseconds(
|
||
hmcu.dTimer.MCUStepStartTime,
|
||
hmcu.dTimer.MCUStepEndTime) / 1000.0;
|
||
}//end mdlUpdate
|
||
|
||
/**
|
||
* @brief Writting outputs of S-Function
|
||
* @param S - pointer to S-Function (library struct from "simstruc.h")
|
||
* @details Abstract:
|
||
* In this function, you compute the outputs of your S-function
|
||
* block. Generally outputs are placed in the output vector(s),
|
||
* ssGetOutputPortSignal.
|
||
*/
|
||
static void mdlOutputs(SimStruct* S, int_T tid)
|
||
{
|
||
SIM_writeOutputs(S);
|
||
|
||
// Измерение времени окончания выполнения mdlOutputs
|
||
hmcu.dTimer.SFuncEndTime = read_timer();
|
||
|
||
// Общее время выполнения S-Function в микросекундах
|
||
if (hmcu.dTimer.SFuncStartTime)
|
||
{
|
||
hmcu.dSFuncTime = ticksToNanoseconds(
|
||
hmcu.dTimer.SFuncStartTime,
|
||
hmcu.dTimer.SFuncEndTime) / 1000.0;
|
||
}
|
||
|
||
// Накопление статистики
|
||
hmcu.dTimer.call_count++;
|
||
}//end mdlOutputs
|
||
|
||
#define MDL_CHECK_PARAMETERS /* Change to #undef to remove function */
|
||
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
|
||
static void mdlCheckParameters(SimStruct* S)
|
||
{
|
||
int i;
|
||
|
||
// Проверяем и принимаем параметры и разрешаем или запрещаем их менять
|
||
// в процессе моделирования
|
||
for (i = 0; i < 1; i++)
|
||
{
|
||
// Input parameter must be scalar or vector of type double
|
||
if (!mxIsDouble(ssGetSFcnParam(S, i)) || mxIsComplex(ssGetSFcnParam(S, i)) ||
|
||
mxIsEmpty(ssGetSFcnParam(S, i)))
|
||
{
|
||
ssSetErrorStatus(S, "Input parameter must be of type double");
|
||
return;
|
||
}
|
||
// Параметр м.б. только скаляром, вектором или матрицей
|
||
if (mxGetNumberOfDimensions(ssGetSFcnParam(S, i)) > 2)
|
||
{
|
||
ssSetErrorStatus(S, "Параметр м.б. только скаляром, вектором или матрицей");
|
||
return;
|
||
}
|
||
// sim_dt = mxGetPr(ssGetSFcnParam(S,0))[0];
|
||
// Parameter not tunable
|
||
// ssSetSFcnParamTunable(S, i, SS_PRM_NOT_TUNABLE);
|
||
// Parameter tunable (we must create a corresponding run-time parameter)
|
||
ssSetSFcnParamTunable(S, i, SS_PRM_TUNABLE);
|
||
// Parameter tunable only during simulation
|
||
// ssSetSFcnParamTunable(S, i, SS_PRM_SIM_ONLY_TUNABLE);
|
||
|
||
}//for (i=0; i<NPARAMS; i++)
|
||
|
||
}//end mdlCheckParameters
|
||
#endif //MDL_CHECK_PARAMETERS
|
||
static void mdlInitializeSizes(SimStruct* S)
|
||
{
|
||
ssSetNumSFcnParams(S, 1);
|
||
// Кол-во ожидаемых и фактических параметров должно совпадать
|
||
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S))
|
||
{
|
||
// Проверяем и принимаем параметры
|
||
mdlCheckParameters(S);
|
||
}
|
||
else
|
||
{
|
||
return;// Parameter mismatch will be reported by Simulink
|
||
}
|
||
|
||
// set up discrete states
|
||
ssSetNumContStates(S, 0); // number of continuous states
|
||
ssSetNumDiscStates(S, DISC_STATES_WIDTH); // number of discrete states
|
||
|
||
// set up input port
|
||
if (!ssSetNumInputPorts(S, IN_PORT_NUMB)) return;
|
||
for (int i = 0; i < IN_PORT_NUMB; i++)
|
||
{
|
||
ssSetInputPortWidth(S, i, inLengths[i]);
|
||
ssSetInputPortDirectFeedThrough(S, i, 0);
|
||
ssSetInputPortRequiredContiguous(S, i, 1);
|
||
}
|
||
|
||
// set up output port
|
||
if (!ssSetNumOutputPorts(S, OUT_PORT_NUMB)) return;
|
||
for (int i = 0; i < OUT_PORT_NUMB; i++)
|
||
ssSetOutputPortWidth(S, i, outLengths[i]);
|
||
|
||
|
||
ssSetNumSampleTimes(S, 1);
|
||
|
||
|
||
ssSetNumRWork(S, 5); // number of real work vector elements
|
||
ssSetNumIWork(S, 5); // number of integer work vector elements
|
||
ssSetNumPWork(S, 0); // number of pointer work vector elements
|
||
ssSetNumModes(S, 0); // number of mode work vector elements
|
||
ssSetNumNonsampledZCs(S, 0); // number of nonsampled zero crossings
|
||
|
||
|
||
ssSetRuntimeThreadSafetyCompliance(S, RUNTIME_THREAD_SAFETY_COMPLIANCE_TRUE);
|
||
/* Take care when specifying exception free code - see sfuntmpl.doc */
|
||
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
|
||
|
||
|
||
}
|
||
|
||
#define MDL_START /* Change to #undef to remove function */
|
||
#if defined(MDL_START)
|
||
/**
|
||
* @brief Initialize S-Function at start of simulation
|
||
* @param S - pointer to S-Function (library struct from "simstruc.h")
|
||
* @details Abstract:
|
||
* This function is called once at start of model execution. If you
|
||
* have states that should be initialized once, this is the place
|
||
* to do it.
|
||
*/
|
||
static void mdlStart(SimStruct* S)
|
||
{
|
||
// Инициализация высокоточного таймера
|
||
InitializeHighPrecisionTimer();
|
||
|
||
SIM_Initialize_Simulation(S);
|
||
}
|
||
#endif // MDL_START
|
||
|
||
/**
|
||
* @brief Initialize Sample Time of Simulation
|
||
* @param S - pointer to S-Function (library struct from "simstruc.h")
|
||
* @details Abstract:
|
||
* This function is used to specify the sample time(s) for your
|
||
* S-function. You must register the same number of sample times as
|
||
* specified in ssSetNumSampleTimes.
|
||
*/
|
||
static void mdlInitializeSampleTimes(SimStruct* S)
|
||
{
|
||
// Шаг дискретизации
|
||
hmcu.sSimSampleTime = mxGetPr(ssGetSFcnParam(S, NPARAMS - 1))[0];
|
||
|
||
// Register one pair for each sample time
|
||
ssSetSampleTime(S, 0, hmcu.sSimSampleTime);
|
||
ssSetOffsetTime(S, 0, 0.0);
|
||
}
|
||
|
||
/**
|
||
* @brief Terminate S-Function at the end of simulation
|
||
* @param S - pointer to S-Function (library struct from "simstruc.h")
|
||
* @details Abstract:
|
||
* In this function, you should perform any actions that are necessary
|
||
* at the termination of a simulation. For example, if memory was
|
||
* allocated in mdlStart, this is the place to free it.
|
||
*/
|
||
static void mdlTerminate(SimStruct* S)
|
||
{
|
||
hmcu.fMCU_Stop = 1;
|
||
#ifdef RUN_APP_MAIN_FUNC_THREAD
|
||
if (hmcu.hMCUThread != NULL) {
|
||
ResumeThread(hmcu.hMCUThread);
|
||
|
||
//// Простая версия - ждем завершения потока
|
||
|
||
//// Ждем до 5 секунд
|
||
//for (int i = 0; i < 50; i++) {
|
||
// // Проверяем, завершился ли поток (упрощенная проверка)
|
||
// DWORD exitCode;
|
||
// if (GetExitCodeThread(hmcu.hMCUThread, &exitCode) && exitCode != STILL_ACTIVE) {
|
||
// break; // поток завершился
|
||
// }
|
||
// Sleep(100); // ждем 100ms
|
||
//}
|
||
|
||
|
||
// Даем потоку шанс завершиться нормально
|
||
DWORD waitResult = WaitForSingleObject(hmcu.hMCUThread, 10000);
|
||
if (waitResult == WAIT_TIMEOUT) {
|
||
// Поток не ответил - завершаем принудительно
|
||
TerminateThread(hmcu.hMCUThread, 0);
|
||
}
|
||
|
||
CloseHandle(hmcu.hMCUThread);
|
||
hmcu.hMCUThread = NULL;
|
||
}
|
||
#endif
|
||
SIM_deInitialize_Simulation(S);
|
||
mexUnlock();
|
||
}
|
||
|
||
|
||
/** WRAPPER_SFUNC
|
||
* @}
|
||
*/
|
||
|
||
#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a
|
||
MEX-file? */
|
||
#include "simulink.c" /* MEX-file interface mechanism */
|
||
#else
|
||
#include "cg_sfun.h" /* Code generation registration
|
||
function */
|
||
#endif
|