www.pudn.com > communicationmatlab.rar > CMPLXFLT.C

/*
 * SCHDINT   A SIMULINK scheduling integration
 *
 *           Syntax:  [sys, x0] = cmplxflt(t, x, u, flag, NUM, DEN, TS)
 *
 *      This block will reset the integration to be zero at K*TD(1) + TD(2).
 *      TS: sampling time of the integration. This is a discrete-time block.
 *      Modulo: Limitation of the integration. When State is larger than this
 *              value it will do x=rem(x,Modulo) computation.
 *
 * Wes Wang Dec. 8, 1994
 * Copyright (c) 1994-96 by The MathWorks, Inc.
 * All Rights Reserved
 * $Revision: 1.1 $  $Date: 1996/04/01 19:02:34 $
 */

#define S_FUNCTION_NAME cmplxflt

#include      /* needed for declaration of sprintf */

#ifdef MATLAB_MEX_FILE
#include     /* needed for declaration of sprintf */
#include "mex.h"      /* needed for declaration of mexErrMsgTxt */
#endif

/*
 * need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */

#include "simstruc.h"

/*
 * Defines for easy access of the input parameters
 */
#define NUM_ARGS   3
#define NUM        ssGetArg(S,0)
#define DEN        ssGetArg(S,1)
#define TS         ssGetArg(S,2)

/*
 * mdlInitializeSizes - called to initialize the sizes array stored in
 *                      the SimStruct.  The sizes array defines the
 *                      characteristics (number of inputs, outputs,
 *                      states, etc.) of the S-Function.
 */

static void mdlInitializeSizes(S)
    SimStruct *S;
{
    /*
     * Set-up size information.
     */ 
    
    if (ssGetNumArgs(S) == NUM_ARGS) {
       int  den_length, num_length;

       num_length = mxGetN(NUM) - 1;
       den_length = mxGetN(DEN) - 1;
/*
#ifdef MATLAB_MEX_FILE
    if (1) {
        char err_msg[256];
        sprintf(err_msg, "num_length= %d, den_length = %d\n",num_length, den_length);
        mexPrintf(err_msg);
    }
#endif
 */     
       if (mxGetM(NUM) > 2) {
#ifdef MATLAB_MEX_FILE
           mexErrMsgTxt("The row number of the numerator cannot be larger than 2.");
#endif
       }
       if (mxGetM(DEN) > 2) {
#ifdef MATLAB_MEX_FILE
           mexErrMsgTxt("The row number of the denominator cannot be larger than 2.");
#endif
       }
       if ((mxGetN(TS)*mxGetM(TS) != 1) &&
           (mxGetN(TS)*mxGetM(TS) != 2)) {
#ifdef MATLAB_MEX_FILE
           mexErrMsgTxt("The sample time must be a scalar or a vector of length 2");
#endif
       }
       if ((mxGetN(NUM)*mxGetM(NUM) <=0) ||
           (mxGetN(DEN)*mxGetM(DEN) <=0)) {
#ifdef MATLAB_MEX_FILE
           mexErrMsgTxt("The numerator or the denominator is empty.");
#endif
       }
       ssSetNumContStates(    S, 0);
       ssSetNumDiscStates(    S, num_length*2+den_length*2);
       ssSetNumInputs(        S, -1);
       ssSetNumOutputs(       S, 2);
       if (mxGetM(NUM) > 1) {
          if ((mxGetPr(NUM)[0] == 0.0) && (mxGetPr(NUM)[1] == 0.0))
            ssSetDirectFeedThrough(S, 0);
          else
            ssSetDirectFeedThrough(S, 1);
       } else {
          if (mxGetPr(NUM)[0] == 0.0)
            ssSetDirectFeedThrough(S, 0);
          else
            ssSetDirectFeedThrough(S, 1);
       }
       ssSetNumInputArgs(     S, NUM_ARGS);
       ssSetNumSampleTimes(   S, 1);
       ssSetNumRWork(         S, 4);
       ssSetNumIWork(         S, 1);
       ssSetNumPWork(         S, 0);
    } else {
#ifdef MATLAB_MEX_FILE
       char err_msg[256];
       sprintf(err_msg, "Wrong number of input arguments passed to S-function MEX-file.\n"
             "%d input arguments were passed in when expecting %d input arguments.\n", ssGetNumArgs(S) + 4, NUM_ARGS + 4);
       mexErrMsgTxt(err_msg);
#endif
    }
}

/*
 * mdlInitializeSampleTimes - initializes the array of sample times stored in
 *                            the SimStruct associated with this S-Function.
 */

static void mdlInitializeSampleTimes(S)
    SimStruct *S;
{
    double sampleTime, offsetTime;

    /*
     * Note, blocks that are continuous in nature should have a single
     * sample time of 0.0.
     */

    sampleTime = mxGetPr(TS)[0];
    if ((mxGetN(TS) * mxGetM(TS)) == 2)
       offsetTime = mxGetPr(TS)[1];
    else
       offsetTime = 0.;
    
    ssSetSampleTimeEvent(S, 0, sampleTime);
    ssSetOffsetTimeEvent(S, 0, offsetTime);
}

/*
 * mdlInitializeConditions - initializes the states for the S-Function
 */

static void mdlInitializeConditions(x0, S)
    double *x0;
    SimStruct *S;
{
    double *x2_y2  = ssGetRWork(S);
    double *lastTime        = ssGetRWork(S) + 1;
    double *lastY           = ssGetRWork(S) + 2;
    int    *lastCalculation = ssGetIWork(S);    

    int in_row = ssGetNumInputs(S);
    int num_length = mxGetN(NUM) - 1;
    int den_length = mxGetN(DEN) - 1;

    int i;

    if (in_row > 2) {
#ifdef MATLAB_MEX_FILE
        mexErrMsgTxt("The input vector length must be less than 2.");
#endif
    }    

    if (ssGetT(S) <= 0) {
        for (i = 0; i < 2*(num_length+den_length); i++)
            x0[i] = 0.0;
    }
    if (mxGetM(DEN) > 1) {
        if ((mxGetPr(DEN)[0] == 0.0) && (mxGetPr(DEN)[1] == 0.0)) {
#ifdef MATLAB_MEX_FILE
            mexErrMsgTxt("The first column of the denominator cannot be zero.");
#endif
		} else {
		    *x2_y2 = mxGetPr(DEN)[0] * mxGetPr(DEN)[0] + mxGetPr(DEN)[1] * mxGetPr(DEN)[1];
		}
	} else {
        if (mxGetPr(DEN)[0] == 0.0) {	    
#ifdef MATLAB_MEX_FILE
        mexErrMsgTxt("The first element of the denominator cannot be zero.");
#endif
		} else {
		    *x2_y2 = mxGetPr(DEN)[0] * mxGetPr(DEN)[0];		    
		}
    }
    lastTime[0] = -1;
    lastY[0] = 0;
    lastY[1] = 0;
    lastCalculation = 0;
}

/* routine called by both mdlOutputs and mdlUpdate */
static void ComplexFilterUpdate(y, x, u, S)
    double *y, *x, *u;
    SimStruct *S;
{
    double *x2_y2  = ssGetRWork(S);    
    double *num    = mxGetPr(NUM);
    double *den    = mxGetPr(DEN);

    int in_row     = ssGetNumInputs(S);
    int num_length = mxGetN(NUM) - 1;
    int num_row    = mxGetM(NUM);
    int den_length = mxGetN(DEN) - 1;
    int den_row    = mxGetM(DEN);

    
    int i, j;

    double tmp;

    if ((in_row > 1) && (num_row > 1)) {
        y[0] = u[0] * num[0] - u[1] * num[1];
        y[1] = u[0] * num[1] + u[1] * num[0];
    } else if ((in_row <= 1) && (num_row > 1)) {
        y[0] = u[0] * num[0];
        y[1] = u[0] * num[1];
    } else if ((in_row > 1) && (num_row <= 1)) {
        y[0] = u[0] * num[0];        
        y[1] = u[1] * num[0];        
    } else {
        y[0] = u[0] * num[0];
        y[1] = 0;
    }
    if (num_row > 1) {
        /* cmplex num */        
        i = 0;
        while (i < num_length) {
             y[0] = y[0]
                    + x[(den_length+i)*2] * num[i*2+2]
                    - x[(den_length+i)*2+1] * num[i*2+3];
             y[1] = y[1]
                    + x[(den_length+i)*2] * num[i*2+3]
                    + x[(den_length+i)*2+1] * num[i*2+2];
             i++;
        }
    } else {
        /* real num */
        i = 0;
        while (i < num_length) {
             y[0] = y[0]
                    + x[(den_length+i)*2] * num[i+1];
             y[1] = y[1]
                    + x[(den_length+i)*2+1] * num[i+1];
             i++;
		}
	}
    if (den_row > 1) {
        /* cmplex den */
        i = 0;
        while (i < den_length) {
             y[0] = y[0]
                    - x[i*2] * den[i*2+2]
                    + x[i*2+1] * den[i*2+3];
             y[1] = y[1]
                    - x[i*2] * den[i*2+3]
                    - x[i*2+1] * den[i*2+2];
             i++;
        }
    	tmp = y[0] * den[0] + y[1] * den[1];
		y[1] = - y[0] * den[1] + y[1] * den[0];
		y[0] = tmp / x2_y2[0];
		y[1] = y[1] / x2_y2[0];			
    } else {
        /* real den */
        i = 0;
        while (i < den_length) {
             y[0] = y[0]
                    - x[i*2] * den[i + 1];
             y[1] = y[1]
                    - x[i*2+1] * den[i + 1];
             i++;
		}
        y[0] = y[0] / den[0];
		y[1] = y[1] / den[0];
	}
}

/*
 * mdlOutputs - computes the outputs of the S-Function
 */
static void mdlOutputs(y, x, u, S, tid)
    double *y, *x, *u;
    SimStruct *S;
    int tid;
{
    double *lastTime        = ssGetRWork(S) + 1;
    double *lastY           = ssGetRWork(S) + 2;
    int    *lastCalculation = ssGetIWork(S);

    /*
#ifdef MATLAB_MEX_FILE
    if (1) {
        char err_msg[256];
        sprintf(err_msg, "In Yout Time= %f, lastTime= %f, lastCal = %d   ",ssGetT(S), lastTime[0], lastCalculation[0]);
        mexPrintf(err_msg);
    }
#endif
    */   
    if ((ssGetT(S) > lastTime[0]) || (lastCalculation[0] == 1)) {
        /*
#ifdef MATLAB_MEX_FILE
        if (1) {
            char err_msg[256];
            sprintf(err_msg, "Update\n");
            mexPrintf(err_msg);
        }
#endif
        */
        /* need new calculation */
        ComplexFilterUpdate(y, x, u, S);
        lastTime[0] = ssGetT(S);
        lastCalculation[0] = 1;
        lastY[0] = y[0];
        lastY[1] = y[1];
    } else {
        /*
#ifdef MATLAB_MEX_FILE
        if (1) {
            char err_msg[256];
            sprintf(err_msg, "No-Update\n");
            mexPrintf(err_msg);
        }
#endif
        */
        /* take the storage data */
        y[0] = lastY[0];
        y[1] = lastY[1];
    }
}

/*
 * mdlUpdate - computes the discrete states of the S-Function
 */

static void mdlUpdate(x, u, S, tid)
    double *x, *u;
    SimStruct *S;
    int tid;
{
    double *lastTime        = ssGetRWork(S) + 1;
    double *lastY           = ssGetRWork(S) + 2;
    int    *lastCalculation = ssGetIWork(S);    
    int in_row     = ssGetNumInputs(S);
    int num_length = mxGetN(NUM) - 1;
    int den_length = mxGetN(DEN) - 1;
    
    int i, j;

    double y[2];
    /*
#ifdef MATLAB_MEX_FILE
    if (1) {
        char err_msg[256];
        sprintf(err_msg, "In Xupdate Time= %f, lastTime= %f, lastCal = %d    ",ssGetT(S), lastTime[0], lastCalculation[0]);
        mexPrintf(err_msg);
    }
#endif
    */
    if ((ssGetT(S) > lastTime[0]) || (lastCalculation[0] == 0)) {
        /*
#ifdef MATLAB_MEX_FILE
        if (1) {
            char err_msg[256];
            sprintf(err_msg, "Update\n");
            mexPrintf(err_msg);
        }
#endif
        */
        /* need new calculation */
        ComplexFilterUpdate(y, x, u, S);
        lastTime[0] = ssGetT(S);
        lastCalculation[0] = 0;
        lastY[0] = y[0];
        lastY[1] = y[1];
    } else {
        /*
#ifdef MATLAB_MEX_FILE
        if (1) {
            char err_msg[256];
            sprintf(err_msg, "No-Update\n");
            mexPrintf(err_msg);
        }
#endif
        */
        /* take the storage data */
        y[0] = lastY[0];
        y[1] = lastY[1];
    }
    
    for (i = den_length+num_length-1; i > 0 ; i--) {
        x[i*2 + 1] = x[i*2 - 1];
        x[i*2] = x[i*2 - 2];
    }
    if (den_length > 0) {    
        x[0] = y[0];
        x[1] = y[1];
    }
    if (num_length > 0) {
        x[2*den_length] = u[0];
        if (in_row > 1)
           x[2*den_length+1] = u[1];
        else
           x[2*den_length+1] = 0;
    }
}

/*
 * mdlDerivatives - computes the derivatives of the S-Function
 */

static void mdlDerivatives(dx, x, u, S, tid)
    double *dx, *x, *u;
    SimStruct *S;
    int tid;
{
}

/*
 * mdlTerminate - called at termination of model execution.
 */

static void mdlTerminate(S)
    SimStruct *S;
{
}

#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