probem with smulink and matlab crash

I run a simulink model which have to measure the position of a device and print this information, using two s-function. At the first time the simulink model run without problem, at the second time when I try to run again the model Matlab crash and suddenly stopped. the two function are:

rt_clock_hi

#undef S_FUNCTION_NAME #define S_FUNCTION_NAME rt_clock_hi

#include "simstruc.h" /* Where simulation structure, S, is defined */ #include windows.h

#ifdef MATLAB_MEX_FILE #include "mex.h" #endif

/* Input Arguments */ #define MAX_ERROR_ARG ssGetArg(S,0) #define NUMBER_OF_ARGS (1)

#define NSAMPLE_TIMES (1)

/* IWORK Storage Allocation */ #define START_TIME (0) #define MAX_ERROR (1) #define NUMBER_OF_IWORKS (2)

/* RWORK Storage Allocation */ #define NUMBER_OF_RWORKS (0)

__int64 errorsum=0; __int64 ciclo=0;

#define READINGS_PER_SECOND 100000 // N° reading per second (e.g. 1000 = 1 millisecond)

typedef union ms_un { LARGE_INTEGER lin; __int64 i64; } ms_un; // ------ static double ms_cps = 0; // ticks per millisecond ms_un HiRes_time; // Hi resolution time

// -- Set the high resolution clock static void Init_HiRes_Clock(void) { //##### ms_un HiRes_time; HiRes_time.i64 = 0; QueryPerformanceFrequency(&HiRes_time.lin); if(HiRes_time.i64 == 0) { ms_cps = -1; OutputDebugString("Error. No QPF.\n"); _exit(1); } else ms_cps = ((double)HiRes_time.i64/READINGS_PER_SECOND); }

// -- Read the high resolution clock __int64 HiRes_Clock() { QueryPerformanceCounter(&HiRes_time.lin); return (__int64) HiRes_time.i64/(__int64) ms_cps; //##### }

static void mdlInitializeSizes(S) SimStruct S; { if (ssGetNumArgs(S) != NUMBER_OF_ARGS) { #ifdef MATLAB_MEX_FILE mexErrMsgTxt("Numero errato di parametri passati.\nE' richiesto un solo argomento.\n"); #endif } else { / Set-up size information */ ssSetNumContStates(S, 0); ssSetNumDiscStates(S, 0); ssSetNumInputs(S, 0); ssSetNumOutputs(S, 0); ssSetDirectFeedThrough(S, 0); ssSetNumInputArgs(S, NUMBER_OF_ARGS); ssSetNumSampleTimes(S, NSAMPLE_TIMES); ssSetNumIWork(S, NUMBER_OF_IWORKS); ssSetNumRWork(S, NUMBER_OF_RWORKS); } }

/* Inizializzazione del tempo di campionamento */ static void mdlInitializeSampleTimes(S) SimStruct *S; { ssSetSampleTimeEvent(S, 0, INHERITED_SAMPLE_TIME); ssSetOffsetTimeEvent(S, 0, 0.0); }

static void mdlInitializeConditions(x0, S) double *x0; SimStruct *S; { unsigned long error; __int64 initial_time; // Time when simulation starts

    Init_HiRes_Clock();

   SetPriorityClass(GetCurrentProcess(),HIGH_PRIORITY_CLASS);
   SetThreadPriority(GetCurrentThread(),THREAD_PRIORITY_HIGHEST);

        error=(unsigned long) (READINGS_PER_SECOND*mxGetPr(MAX_ERROR_ARG)[0]);
  ssSetIWorkValue(S, MAX_ERROR, error);
        errorsum=0;
        ciclo=0;
        initial_time=HiRes_Clock();
  ssSetIWorkValue(S, START_TIME, initial_time);

}

/* Function to compute outputs */ static void mdlOutputs(y, x, u, S, tid) double *y; double *x; double *u; SimStruct *S; int tid; { }

/* Function to compute model update */ static void mdlUpdate(x, u, S, tid) double *x; double *u; SimStruct *S; int tid; { __int64 start_time; __int64 target_time; __int64 max_error; __int64 error;

start_time = ssGetIWorkValue(S, START_TIME); target_time = (unsigned long) (ssGetT(S)*READINGS_PER_SECOND)+start_time; ciclo++;

        error=HiRes_Clock()-target_time;
        if (error > 0)            // Simulazione in ritardo
          {
          max_error = ssGetIWorkValue(S, MAX_ERROR);
          if (error>max_error)
            {
#ifdef MATLAB_MEX_FILE
            mexPrintf("Massimo errore superato.   Simulazione bloccata.\n");
            mexPrintf("Errore assoluto = %lu millisecondi \n",error);
            mexPrintf("MAX Errore assoluto = %lu millisecondi \n",max_error);
#endif
            ssSetStopRequested(S,1);
            }
          else
            {
#ifdef MATLAB_MEX_FILE
            errorsum+=error;
            //mexPrintf("Attenzione: Momentanea perdita del real-time!    ");
            //mexPrintf("Errore = %lu millisecondi\n",error);
#endif
            }
          }
        else   // Temporizzazione OK
          {
          while (HiRes_Clock() < target_time) ;
          }
}

/* Function to compute derivatives */ static void mdlDerivatives(dx, x, u, S, tid) double *dx; double *x; double *u; SimStruct *S; int tid; { }

/* Function to perform housekeeping at execution termination */ static void mdlTerminate(S) SimStruct *S; {

double error_sec=(double) ((double)errorsum/READINGS_PER_SECOND)*1000/ciclo; mexPrintf("Numero cicli = %i \n",ciclo); mexPrintf("Media errore per ciclo (msec) = %f \n",error_sec);

 SetPriorityClass(GetCurrentProcess(),NORMAL_PRIORITY_CLASS);
 SetThreadPriority(GetCurrentThread(),THREAD_PRIORITY_NORMAL);

}

#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? / #include "simulink.c" / Mex glue / #else #include "cg_sfun.h" / Code generation glue */ #endif

haptik_simulink -LICENSE // This file is part of the Haptik Library Source Distribution. // Copyright (C) 2003-2006 by Maurizio de Pascale. All rights reserved. // // This library is free software; you can redistribute it and/or modify // it under the terms of the GNU General Public License("GPL") version 2 // as published by the Free Software Foundation. //---------------------------------------------------------------------->

//---HEADER // Name: Haptik.Simulink // Desc: S-Function interface to Haptik Library for simulink // Auto: Maurizio de Pascale // Date: 10 July 2005 // Tabs: tabbed at 3 // Http: www.haptiklibrary.org // Svns: $Revision: 87 $ // $Date: 2006-11-05 18:50:34 +0100 (dom, 05 nov 2006) $ //---------------------------------------------------------->

//?are we compiling from the VC? #ifdef _MSC_VER

//---DEFINES //--------------------> #define MATLAB_MEX_FILE

//---LIBS //------------------------------> #pragma comment(lib,"libmex.lib") #pragma comment(lib,"libmx.lib")

#endif

#include RSLib/Haptik.hpp using namespace RSLib;

#ifdef __cplusplus extern "C" { // use the C fcn-call standard for all functions #endif // defined within this scope

#define S_FUNCTION_NAME haptik_simulink #define S_FUNCTION_LEVEL 2

#include "simstruc.h"

static void mdlInitializeSizes(SimStruct S) { ssSetNumSFcnParams(S, 0); / Number of expected parameters / if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { / * If the the number of expected input parameters is not equal * to the number of parameters entered in the dialog box return. * Simulink will generate an error indicating that there is a * parameter mismatch. */ return; }

/* Register the number and type of states the S-Function uses */

ssSetNumContStates( S, 0); /* number of continuous states / ssSetNumDiscStates( S, 0); / number of discrete states */

if (!ssSetNumInputPorts(S, 1)) return; ssSetInputPortWidth(S,0,3); ssSetInputPortDirectFeedThrough(S, 0, 0);

if (!ssSetNumOutputPorts(S, 1)) return; ssSetOutputPortWidth(S,0,4); ssSetNumSampleTimes( S, 1); /* number of sample times */

ssSetNumRWork( S, 0); /* number of real work vector elements / ssSetNumIWork( S, 0); / number of integer work vector elements*/ ssSetNumPWork( S, 1); /* number of pointer work vector elements*/ ssSetNumModes( S, 0); /* number of mode work vector elements / ssSetNumNonsampledZCs( S, 0); / number of nonsampled zero crossings */

ssSetOptions( S, 0); /* general options (SS_OPTION_xx) / } static void mdlInitializeSampleTimes(SimStruct *S) { ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME); ssSetOffsetTime(S, 0, 0.0); } #define MDL_START static void mdlStart(SimStruct *S) { Haptik haptik; IHaptikDeviceInterface device = (IHaptikDeviceInterface)haptik.GetDeviceInterface(); if(device == NULL) return; device->Init(); device->Start(); void* ptrs = ssGetPWork(S); ptrs[0] = (void*)device; } #define MDL_UPDATE void mdlUpdate(SimStruct *S, int_T tid) { InputRealPtrsType x = ssGetInputPortRealSignalPtrs(S,0); IHaptikDeviceInterface device = (IHaptikDeviceInterface) ssGetPWork(S)[0]; if (device == NULL) return; HaptikData data; data.forceFeedback = float3( (flt32)*x[0], (flt32)*x[1], (flt32)*x[2]); data.torqueFeedback = float3(0,0,0); device->Write(data); } static void mdlOutputs(SimStruct *S, int_T tid) { real_T *y = ssGetOutputPortRealSignal(S,0); IHaptikDeviceInterface device = (IHaptikDeviceInterface) ssGetPWork(S)[0]; if (device == NULL) return; HaptikData data; device->Read(data); y[0] = data.position.x; y[1] = data.position.y; y[2] = data.position.z;

} static void mdlTerminate(SimStruct *S) { IHaptikDeviceInterface device = (IHaptikDeviceInterface) ssGetPWork(S)[0]; RELEASE_HAPTIK_INTERFACE(device); }

#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

#ifdef __cplusplus } // end of extern "C" scope #endif