/* admultiq3.c - xPC Target, non-inlined S-function driver for: */
/*               A/D section of MultiQ3 ISA board (Quanser)     */
/* Written by R.A. de Callafon, Feb. 2004, Dept. of MAE, UCSD   */
/* xPC is Copyright 1996-2002 The MathWorks, Inc.               */

#define     S_FUNCTION_LEVEL    2
#undef      S_FUNCTION_NAME
#define     S_FUNCTION_NAME     admultiq3

#include    <stddef.h>
#include    <stdlib.h>

#include    "simstruc.h"

#ifdef      MATLAB_MEX_FILE
#include    "mex.h"
#else
#include    <windows.h>
#include    "io_xpcimport.h"
#endif

/* Input Arguments */

#define NUM_PARAMS             (3)
#define BASE_ADDRESS_ARG       (ssGetSFcnParam(S,0))
#define CHANNEL_ARG            (ssGetSFcnParam(S,1))
#define SAMPLE_TIME_PARAM      (ssGetSFcnParam(S,2))


/* Convert S Function Parameters to Variables of Unsigned Integer Type */

#define BASE                   ((uint_T) mxGetPr(BASE_ADDRESS_ARG)[0])
#define SAMPLE_TIME            ((real_T) mxGetPr(SAMPLE_TIME_PARAM)[0])
#define SAMPLE_OFFSET          ((real_T) mxGetPr(SAMPLE_TIME_PARAM)[1])

/* Definition of Quanser Register Adresses for AD conversion */

#define AD_CS           (BASE + 0x04)   /* a write to this address initiates conversion, provided the CONTROL REGISTER has been set up properly */
#define AD_DATA         (BASE + 0x04)   /* a double read from this 8 bit address first yields the high then the low 8 bits of the AD conversion */
#define CONTROL_REG     (BASE + 0x06)   /* the address of the CONTROL REGISTER */
#define STATUS_REG      (BASE + 0x06)   /* the address of the CONTROL REGISTER (or better STATUS REGISTER in case of reading) */

/* Definition of 16 bit CONTROL REGISTER values for the Quanser card */

#define AD_SH           0x200       /* (S/H) = disable sample and hold on the A/D (keep this 1 all the time) */
#define AD_AUTOCAL      0x100       /* (CAL) = enable autocalibartion on the A/D */
#define AD_AUTOZ        0x80        /* (AZ)  = enable auto Zero on the A/D */
#define AD_MUX_EN       0x40        /* (MX)  = enable the 8 channel multiplexer */
#define AD_CLOCK_4M     0x400       /* (CLK) = choose clock frequency of the A/D, (1 = 4MHz, 0 = 2 MHz) */
#define CONTROL_MUST    (AD_SH | AD_CLOCK_4M)  /* bit wise OR, so we keep both S/H and CLK high all the time */

/* Definition of msg string in case we need to report errors */

static char_T msg[256];


/*====================*
 * S-function methods *
 *====================*/

static void mdlCheckParameters(SimStruct *S)
{
}

static void mdlInitializeSizes(SimStruct *S)
{
    uint_T i;

#ifndef MATLAB_MEX_FILE
#include "io_xpcimport.c"
#endif


    ssSetNumSFcnParams(S, NUM_PARAMS);
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Parameter mismatch will be reported by Simulink */
    }

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);



    if (!ssSetNumInputPorts(S, 0)) return;

    if (!ssSetNumOutputPorts(S, mxGetNumberOfElements(CHANNEL_ARG))) return;

    for (i=0;i<mxGetNumberOfElements(CHANNEL_ARG);i++) {
        ssSetOutputPortWidth(S, i, 1);
    }

    ssSetNumSampleTimes(S, 1);

    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    ssSetSFcnParamNotTunable(S,0);
    ssSetSFcnParamNotTunable(S,1);
    ssSetSFcnParamNotTunable(S,2);

    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_PLACE_ASAP);

}



static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, SAMPLE_TIME);
    ssSetOffsetTime(S, 0, SAMPLE_OFFSET);
}



#define MDL_START
static void mdlStart(SimStruct *S)
{

#ifndef MATLAB_MEX_FILE


    uint_T i, channel, control_word, w, maxcnt;

    /* Do a calibration */

    rl32eOutpW(CONTROL_REG, (AD_AUTOCAL | CONTROL_MUST));  /* write 1 to CAL, S/H and CLK of CONTROL REGISTER */
    rl32eOutpW(CONTROL_REG, (CONTROL_MUST));               /* write 1 to S/H and CLK of CONTROL REGISTER */

    /* Wait till EOC (End of Conversion = 3rd bit on STATUS REGISTER) goes high */

    w=0;
    maxcnt=0x10000;
    while (((rl32eInpW(STATUS_REG) &0x08) == 0x00 ) && (w < maxcnt)) w++;
    if (w==0x10000) {           /* if no EOC was detected after all these loops, report an error */
        sprintf(msg,"%s (0x%x): AD calibration failed", "MultiQ3 ASI", BASE);
        ssSetErrorStatus(S,msg);
        return;
    }


#endif

}

static void mdlOutputs(SimStruct *S, int_T tid)
{

#ifndef MATLAB_MEX_FILE

    uint_T i, channel, control_word, hb, lb;
    real_T *y;
    int_T int_data;

    for (i=0;i<mxGetNumberOfElements(CHANNEL_ARG);i++) {
        channel=(uint_T)mxGetPr(CHANNEL_ARG)[i]-1;

        /* enable the multiplexer and select channel */
        /* channel selection is done with shift left 3 operator */
        /* to write the A0 (4th), A1 (5th) and A2 (6th) bits of the CONTROL REGISTER */

        control_word = CONTROL_MUST | AD_MUX_EN | (channel<<3);
        rl32eOutpW(CONTROL_REG, control_word);

        /* wait till EOC (End of Conversion = 3rd bit on STATUS REGISTER) goes high */

        while( ( rl32eInpW(STATUS_REG) &0x08) == 0x00 );

        /* do an actual conversion - any write to the AD_CS register will start the conversion */

        rl32eOutpW(AD_CS, 0);

        /* wait until data ready by observing the EOC_I = 4th bit on the STATUS REGISTER goes high */
        /* in the mean time, select the output in the Simulink model */

        y=ssGetOutputPortSignal(S,i);

        while ((rl32eInpW(STATUS_REG) & 0x10) == 0x00);

        /* Get the data */

        hb = rl32eInpW(AD_DATA)&0xFF; /* the first read is the high byte */
        lb = rl32eInpW(AD_DATA)&0xFF; /* the second read is the low byte */

        /* Reset the CONTROl REGISTER */
        /* this is not really necessary and makes things go faster */

        /* rl32eOutpW(CONTROL_REG, CONTROL_MUST); */

        /* compute the actual value of the measured signal */
        /* using the fixed range of -5/+5 Volts using 12 bits + 1 SGN bit */

        int_data = (hb<<8)|(lb);
        if (hb&0xF0)
            /* the 4 SGN bits on the high bit (hb) are 1, so the signal is negative */
            y[0]=5.0*(int_data-0xF000)/4095.0-5.0;
        else
            /* signal is positive */
            y[0]=5.0*(int_data)/4095.0;

    }

#endif

}

static void mdlTerminate(SimStruct *S)
{
}


#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