sfAnalogInput.c

   1 /*
   2  * S-function to support analog inputs on Humusoft MF624 card
   3  *
   4  * Copyright (C) 2013 Michal Kreč <krecmich@fel.cvut.cz>
   5  * Copyright (C) 2013 Michal Sojka <sojkam1@fel.cvut.cz>
   6  *
   7  * Department of Control Engineering
   8  * Faculty of Electrical Engineering
   9  * Czech Technical University in Prague (CTU)
  10  *
  11  * The S-Function for ERT Linux can be distributed in compliance
  12  * with GNU General Public License (GPL) version 2 or later.
  13  * Other licence can negotiated with CTU.
  14  *
  15  * Next exception is granted in addition to GPL.
  16  * Instantiating or linking compiled version of this code
  17  * to produce an application image/executable, does not
  18  * by itself cause the resulting application image/executable
  19  * to be covered by the GNU General Public License.
  20  * This exception does not however invalidate any other reasons
  21  * why the executable file might be covered by the GNU Public License.
  22  * Publication of enhanced or derived S-function files is required
  23  * although.
  24  *
  25  * Linux ERT code is available from
  26  *    http://rtime.felk.cvut.cz/gitweb/ert_linux.git
  27  * More CTU Linux target for Simulink components are available at
  28  *    http://lintarget.sourceforge.net/
  29  *
  30  * sfuntmpl_basic.c by The MathWorks, Inc. has been used to accomplish
  31  * required S-function structure.
  32  */
  33
  34 /*
  35  * You must specify the S_FUNCTION_NAME as the name of your S-function
  36  * (i.e. replace sfuntmpl_basic with the name of your S-function).
  37  */
  38
  39 #define S_FUNCTION_NAME  sfAnalogInput
  40 #define S_FUNCTION_LEVEL 2
  41 #define MASK_PRM(S)     (mxGetScalar(ssGetSFcnParam(S, 0)))
  42
  43 /*
  44  * Need to include simstruc.h for the definition of the SimStruct and
  45  * its associated macro definitions.
  46  */
  47 #include "simstruc.h"
  48
  49 #include <stdint.h>
  50
  51 #ifndef WITHOUT_HW
  52 #include "mf624_SIMULINK.h"
  53 #endif /*WITHOUT_HW*/
  54
  55 #ifdef __GNUC__
  56   #include <alloca.h>
  57   #define my_popcount __builtin_popcount
  58 #else
  59   #include <malloc.h>
  60   int my_popcount(uint32_t patt)
  61   {
  62     int pops = 0;
  63     while (patt) {
  64       if (patt & 1)
  65         pops++;
  66       patt >>= 1;
  67     }
  68     return pops;
  69   }
  70 #endif
  71
  72
  73 /* Error handling
  74  * --------------
  75  *
  76  * You should use the following technique to report errors encountered within
  77  * an S-function:
  78  *
  79  *       ssSetErrorStatus(S,"Error encountered due to ...");
  80  *       return;
  81  *
  82  * Note that the 2nd argument to ssSetErrorStatus must be persistent memory.
  83  * It cannot be a local variable. For example the following will cause
  84  * unpredictable errors:
  85  *
  86  *      mdlOutputs()
  87  *      {
  88  *         char msg[256];         {ILLEGAL: to fix use "static char msg[256];"}
  89  *         sprintf(msg,"Error due to %s", string);
  90  *         ssSetErrorStatus(S,msg);
  91  *         return;
  92  *      }
  93  *
  94  * See matlabroot/simulink/src/sfuntmpl_doc.c for more details.
  95  */
  96
  97 /*====================*
  98  * S-function methods *
  99  *====================*/
 100
 101 /* Function: mdlInitializeSizes ===============================================
 102  * Abstract:
 103  *    The sizes information is used by Simulink to determine the S-function
 104  *    block's characteristics (number of inputs, outputs, states, etc.).
 105  */
 106 static void mdlInitializeSizes(SimStruct *S)
 107 {
 108     int ADCCMask;
 109     int i;
 110     int ADCChannels;
 111     /* See sfuntmpl_doc.c for more details on the macros below */
 112
 113     ssSetNumSFcnParams(S, 1);  /* Number of expected parameters */
 114     if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
 115         /* Return if number of expected != number of actual parameters */
 116         ssSetErrorStatus(S,"Parameter mismatch");
 117         return;
 118     }
 119
 120     ssSetNumContStates(S, 0);
 121     ssSetNumDiscStates(S, 0);
 122
 123     ADCCMask = (int)MASK_PRM(S);
 124
 125     if(ADCCMask > 255 || ADCCMask < 0) {
 126         ssSetErrorStatus(S,"Invalid parameter mask, set to 0-255");
 127     }
 128
 129     ADCChannels = my_popcount((uint32_t)ADCCMask);           //Counts number of set bits in ADCCMask
 130
 131     if (!ssSetNumInputPorts(S, 0)) return;
 132
 133
 134
 135     if (!ssSetNumOutputPorts(S, ADCChannels)) return;
 136     for(i=0; i < ADCChannels;i++){
 137         ssSetOutputPortWidth(S, i, 1);
 138     }
 139     ssSetNumSampleTimes(S, 1);
 140     ssSetNumRWork(S, 0);
 141     ssSetNumIWork(S, 1);
 142     ssSetNumPWork(S, 0);
 143     ssSetNumModes(S, 0);
 144     ssSetNumNonsampledZCs(S, 0);
 145
 146     /* Specify the sim state compliance to be same as a built-in block */
 147     ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
 148
 149     ssSetOptions(S, 0);
 150
 151
 152 }
 153
 154
 155
 156 /* Function: mdlInitializeSampleTimes =========================================
 157  * Abstract:
 158  *    This function is used to specify the sample time(s) for your
 159  *    S-function. You must register the same number of sample times as
 160  *    specified in ssSetNumSampleTimes.
 161  */
 162 static void mdlInitializeSampleTimes(SimStruct *S)
 163 {
 164     ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
 165     ssSetOffsetTime(S, 0, FIXED_IN_MINOR_STEP_OFFSET);
 166
 167 }
 168
 169
 170
 171 #undef MDL_INITIALIZE_CONDITIONS   /* Change to #undef to remove function */
 172 #if defined(MDL_INITIALIZE_CONDITIONS)
 173   /* Function: mdlInitializeConditions ========================================
 174    * Abstract:
 175    *    In this function, you should initialize the continuous and discrete
 176    *    states for your S-function block.  The initial states are placed
 177    *    in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
 178    *    You can also perform any other initialization activities that your
 179    *    S-function may require. Note, this routine will be called at the
 180    *    start of simulation and if it is present in an enabled subsystem
 181    *    configured to reset states, it will be call when the enabled subsystem
 182    *    restarts execution to reset the states.
 183    */
 184   static void mdlInitializeConditions(SimStruct *S)
 185   {
 186   }
 187 #endif /* MDL_INITIALIZE_CONDITIONS */
 188
 189
 190
 191 #define MDL_START  /* Change to #undef to remove function */
 192 #if defined(MDL_START)
 193   /* Function: mdlStart =======================================================
 194    * Abstract:
 195    *    This function is called once at start of model execution. If you
 196    *    have states that should be initialized once, this is the place
 197    *    to do it.
 198    */
 199   static void mdlStart(SimStruct *S)
 200   {
 201     int ADCChannels;
 202
 203   #ifndef WITHOUT_HW
 204     if (mf624_init(NULL) != 0)
 205         return;
 206   #endif /*WITHOUT_HW*/
 207
 208     int ADCCMask = (int)MASK_PRM(S);
 209
 210     if(ADCCMask > 255 || ADCCMask < 0) {
 211         ssSetErrorStatus(S,"Invalid parameter mask, set to 0-255");
 212     }
 213
 214     ADCChannels = my_popcount((uint32_t)ADCCMask);           //Counts number of set bits in ADCCMask
 215
 216     ssSetIWorkValue(S, 0, ADCChannels);
 217   #ifndef WITHOUT_HW
 218     mfst->ADC_enabled = ADCCMask;
 219     mfst->ADC_enabled &= 0xFF;
 220
 221     mf624_write16(mfst->ADC_enabled, MFST2REG(mfst, 2, ADCTRL_reg));
 222   #endif /*WITHOUT_HW*/
 223   }
 224 #endif /*  MDL_START */
 225
 226
 227
 228 /* Function: mdlOutputs =======================================================
 229  * Abstract:
 230  *    In this function, you compute the outputs of your S-function
 231  *    block.
 232  */
 233 static void mdlOutputs(SimStruct *S, int_T tid)
 234 {
 235
 236     int ADCChannels = ssGetIWorkValue(S, 0);
 237     real_T**       y;
 238     int i;
 239     int res,res1;
 240   #ifndef WITHOUT_HW
 241     if (mf624_check(S) != 0)
 242         return;
 243
 244     // Activate trigger to start conversion
 245     mf624_read16(MFST2REG(mfst, 2, ADSTART_reg));
 246   #endif /*WITHOUT_HW*/
 247
 248     y = alloca(ADCChannels * sizeof(*y));
 249
 250     for(i=0; i < ADCChannels;i++){
 251         y[i]=ssGetOutputPortSignal(S,i);
 252     }
 253
 254   #ifndef WITHOUT_HW
 255     // Check if conversion has finished
 256     while((mf624_read32(MFST2REG(mfst, 0, GPIOC_reg)) & GPIOC_EOLC_mask)) {
 257         for (i = 0; i < 1000; i++) {} // small wait
 258     }
 259
 260     for(i=1; i < ADCChannels;i+=2){
 261         res = mf624_read32(MFST2REG(mfst, 2, ADDATA0_reg));
 262         res1= res >> 16;
 263         res = res & 0xFFFF;
 264         *y[i-1] = (real_T) (10.0 * ((int16_t) (res << 2)) / (double) 0x8000);
 265         *y[i] = (real_T) (10.0 * ((int16_t) (res1 << 2)) / (double) 0x8000);
 266     }
 267
 268     if(i == ADCChannels){
 269         res = mf624_read16(MFST2REG(mfst, 2, ADDATA0_reg));
 270         *y[ADCChannels-1]=(real_T)(10.0 * ((int16_t) (res << 2)) / (double) 0x8000);
 271     }
 272   #else /*WITHOUT_HW*/
 273     for(i=0; i < ADCChannels; i++){
 274         *y[i] = 0;
 275     }
 276   #endif /*WITHOUT_HW*/
 277 }
 278
 279
 280
 281 #undef MDL_UPDATE  /* Change to #undef to remove function */
 282 #if defined(MDL_UPDATE)
 283   /* Function: mdlUpdate ======================================================
 284    * Abstract:
 285    *    This function is called once for every major integration time step.
 286    *    Discrete states are typically updated here, but this function is useful
 287    *    for performing any tasks that should only take place once per
 288    *    integration step.
 289    */
 290   static void mdlUpdate(SimStruct *S, int_T tid)
 291   {
 292   }
 293 #endif /* MDL_UPDATE */
 294
 295
 296
 297 #undef MDL_DERIVATIVES  /* Change to #undef to remove function */
 298 #if defined(MDL_DERIVATIVES)
 299   /* Function: mdlDerivatives =================================================
 300    * Abstract:
 301    *    In this function, you compute the S-function block's derivatives.
 302    *    The derivatives are placed in the derivative vector, ssGetdX(S).
 303    */
 304   static void mdlDerivatives(SimStruct *S)
 305   {
 306   }
 307 #endif /* MDL_DERIVATIVES */
 308
 309
 310
 311 /* Function: mdlTerminate =====================================================
 312  * Abstract:
 313  *    In this function, you should perform any actions that are necessary
 314  *    at the termination of a simulation.  For example, if memory was
 315  *    allocated in mdlStart, this is the place to free it.
 316  */
 317 static void mdlTerminate(SimStruct *S)
 318 {
 319   #ifndef WITHOUT_HW
 320     mf624_done(S);
 321   #endif /*WITHOUT_HW*/
 322 }
 323
 324
 325 /*======================================================*
 326  * See sfuntmpl_doc.c for the optional S-function methods *
 327  *======================================================*/
 328
 329 /*=============================*
 330  * Required S-function trailer *
 331  *=============================*/
 332
 333 #ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
 334 #include "simulink.c"      /* MEX-file interface mechanism */
 335 #else
 336 #include "cg_sfun.h"       /* Code generation registration function */
 337 #endif