1/*



2 *   xpcethercatpdotx.c - xPC Target Ethernet Receive



3 *



4 *   Copyright 2008-2014 The MathWorks, Inc.



5*/



6



7



8#define S_FUNCTION_LEVEL   2



9#undef  S_FUNCTION_NAME



10#define S_FUNCTION_NAME    xpcethercatpdotx



11



12#define VERBOSE 1



13



14#include <stddef.h>



15#include <stdlib.h>



16#include "simstruc.h"



17



18#ifndef MATLAB_MEX_FILE



19#include <windows.h>



20#include "xpctarget.h"



21#endif



22



23#ifdef MATLAB_MEX_FILE



24#include "mex.h"



25#else



26#include "xpcethercatutils.h"



27#endif



28



29#define BITS_PER_INT8      8



30#define BITS_PER_UINT8     8



31#define BITS_PER_INT16     16



32#define BITS_PER_UINT16    16



33#define BITS_PER_INT32     32



34#define BITS_PER_UINT32    32



35#define BITS_PER_REAL32    32



36#define BITS_PER_REALT     64



37



38



39// PARAMETERS



40



41typedef enum {



42   S_PDO_NAME = 0,



43   S_PDO_OFFSET,



44   S_SIGNAL_TYPE,



45   S_PDO_TYPE_BITSIZE,



46   S_SIGNAL_DIM,



47   S_DEVICE_ID,



48   S_SAMPLE_TIME,



49   NUM_S_PARAMS



50} s_params;



51



52#if 0



53// DEBUG, or used if we use the info lookup on the target



54typedef struct _EC_T_PROCESS_VAR_INFO



55{



56    uint8_T                   szName[72];   /**< [out] Name of the found process variable */



57    uint16_T                  wDataType;                          /**< [out] Data type of the found process variable */



58    uint32_T                  nBitSize;                           /**< [out] Size in bit of the found process variable */



59    uint32_T                  nBitOffs;                           /**< [out] Bit offset in the process data image */



60    uint16_T                  wFixedAddr;                         /**< [out] Fixed station address of the slave that is owner of this variable */



61    uint8_T                   bIsInputData;                       /**< [out] Determines whether the found process variable is an input variable or an output variable */



62    uint32_T  dummy[4];  // Extra to pad stack overwrite???



63}  EC_T_PROCESS_VAR_INFO, *EC_PT_PROCESS_VAR_INFO;



64



65// end DEBUG



66#endif



67



68#define ETH_PDO_NAME             ssGetSFcnParam(S,S_PDO_NAME)



69#define ETH_PDO_OFFSET           ssGetSFcnParam(S,S_PDO_OFFSET)



70#define ETH_PDO_TYPE_BITSIZE     ssGetSFcnParam(S,S_PDO_TYPE_BITSIZE)



71#define ETH_PDO_SIGNAL_TYPE      ssGetSFcnParam(S,S_SIGNAL_TYPE)



72#define ETH_PDO_SIGNAL_DIM       ssGetSFcnParam(S,S_SIGNAL_DIM)



73#define ETH_DEVICE_ID            ssGetSFcnParam(S,S_DEVICE_ID)



74#define ETH_PDO_SAMPLE_TIME      ssGetSFcnParam(S,S_SAMPLE_TIME)



75



76typedef enum {



77   I_DEVICE_ID = 0,



78   I_PDO_SIZE,



79   I_PDO_TYPE,



80   I_PDO_OFFSET,



81   I_PDO_TYPE_BIT_SIZE,



82   I_SIG_TYPE_BIT_SIZE,



83   I_SIG_WIDTH,



84   NUM_I_WORKS



85}  i_works;



86



87static char_T      msg[256];



88



89/*** Simulation Code



90#ifdef MATLAB_MEX_FILE



91



92#endif



93*******************/



94



95



96static int_T getDataTypeSize(SimStruct *S,DTypeId dtype)



97{



98    switch (dtype) {



99        case SS_DOUBLE:



100            return sizeof(real_T);



101        case SS_SINGLE:



102            return sizeof(real32_T);



103        case SS_BOOLEAN:



104            return sizeof(boolean_T);



105        case SS_INT8:



106            return sizeof (int8_T);



107        case SS_UINT8:



108            return sizeof (uint8_T);



109        case SS_INT16:



110            return sizeof (int16_T);



111        case SS_UINT16:



112            return sizeof (uint16_T);



113        case SS_INT32:



114            return sizeof (int32_T);



115        case SS_UINT32:



116            return sizeof(uint32_T);



117        default:



118            ssSetErrorStatus(S, "Unsupported data type specified for xpcethercat_tx_var");



119            return 0;



120    }



121   return 0;



122}



123



124/***************************************************************************************



125 * Function Name: bitCopy



126 * Purpose: This function copies the specified integer value into the bit array "bitPtr".



127 *



128 ****************************************************************************************/



129



130static void bitCopy(unsigned char *bitPtr, int bitoff, int numBits, unsigned int val)



131{



132    int   sByte;



133        int   sbit;



134        int   ebit;



135        int   i;



136        unsigned char   bitVal;



137        int             srcBitNum;



138        int             tgtBitNum;



139        unsigned char   *cValPtr;



140        int   tgtByte;



141



142        sbit   =  bitoff%8;



143        sByte  =  bitoff/8;



144        ebit   =  sbit + numBits - 1;



145



146



147        cValPtr = (unsigned char *) &val;



148        for (i=0;i<numBits;i++) {



149        // Get the bit value of the next bit to be copied.



150                srcBitNum = i%8;



151            bitVal = cValPtr[i/8];



152                bitVal = bitVal << (7-srcBitNum);



153                bitVal = bitVal>>7;



154        // Set the value of the bit in the target bit location in the bit array.



155                tgtBitNum = (sbit + i)%8;



156                tgtByte = sByte+(sbit+i)/8;



157        bitPtr[tgtByte] &= ~(1<<(tgtBitNum));    /* Clear the bit in the target */



158                bitPtr[tgtByte] |= (bitVal)<<tgtBitNum;  /* Set the bit in the target   */



159        }



160



161}



162



163static void mdlInitializeSizes(SimStruct *S)



164{



165   int_T                num_params;



166   int_T                portIndex;



167   int_T                sigWidth;



168   DTypeId              sigType;



169   int_T                j;



170



171



172   ssSetNumSFcnParams(S, NUM_S_PARAMS);



173   if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {



174      sprintf(msg, "%d input args expected, %d passed", NUM_S_PARAMS, ssGetSFcnParamsCount(S));



175      ssSetErrorStatus(S, msg);



176      return;



177   }



178



179   ssSetNumContStates(S, 0);



180   ssSetNumDiscStates(S, 0);



181   if ( !ssSetNumOutputPorts(S,0) ) return;



182   if ( !ssSetNumInputPorts(S, 1) ) return;



183   ssSetInputPortRequiredContiguous(S,0,1);



184   portIndex = 0;



185



186   sigWidth = (int_T)mxGetPr(ETH_PDO_SIGNAL_DIM)[0];



187



188    // The PDO parameter S_NUM_SIGNALS specifies the number of outputs.



189



190    num_params = (int32_T)ssGetSFcnParamsCount(S);



191



192    // Get signal data type



193



194    sigType = (DTypeId)((int_T)(mxGetPr(ETH_PDO_SIGNAL_TYPE)[0])-1);



195



196    // Get the number of cells in DataTypeArray



197



198



199#ifdef MATLAB_MEX_FILE



200



201    ssSetInputPortDirectFeedThrough(S,portIndex,1);



202        ssSetInputPortDataType(S, portIndex, sigType);



203        ssSetInputPortWidth(S, portIndex,sigWidth);



204



205#endif



206   ssSetNumSampleTimes(S, 1);



207   ssSetModelReferenceSampleTimeInheritanceRule(S, USE_DEFAULT_FOR_DISCRETE_INHERITANCE );



208   ssSetNumIWork(S, NUM_I_WORKS);



209   ssSetNumRWork(S, 0);



210   ssSetNumPWork(S, 0);



211   ssSetNumModes(S, 0);



212   ssSetNumNonsampledZCs(S, 0);



213



214   ssSetSimStateCompliance(S, HAS_NO_SIM_STATE);



215



216   for ( j = 0 ; j < NUM_S_PARAMS ; j++ ) {



217      ssSetSFcnParamTunable(S, j, SS_PRM_NOT_TUNABLE);



218   }



219   ssSetOptions(S, SS_OPTION_RUNTIME_EXCEPTION_FREE_CODE|



220                   SS_OPTION_DISALLOW_CONSTANT_SAMPLE_TIME);



221}



222



223static void mdlInitializeSampleTimes(SimStruct *S)



224{



225   real_T sampleTime;



226



227   sampleTime = mxGetPr(ETH_PDO_SAMPLE_TIME)[0];



228



229   // Since the sampletime comes from the config file, we should never inherit!



230   if ( sampleTime == -1.0 ) {



231        // We should never see this error message!



232        sprintf( msg, "%s\n", "No sampletime is set for the EtherCAT PDO read block");



233        ssSetErrorStatus(S, msg);



234        return;



235   } else {



236      ssSetSampleTime(S, 0, sampleTime);



237      ssSetOffsetTime(S, 0, 0.0);



238   }



239}



240



241#define MDL_START



242static void mdlStart(SimStruct *S)



243{



244   int_T                  deviceIndex;



245   DTypeId                sigType;



246   int_T                  pdoTypeBitSize;



247   int_T                  sigWidth;



248   int_T                  pdoOffset;



249   int_T                  sigTypeSize;



250



251   deviceIndex  = (int_T)mxGetPr(ETH_DEVICE_ID)[0];



252   deviceIndex=deviceIndex;



253



254#if 0



255   { // DEBUG



256        char *name = malloc(72); // arbitrary



257        int_T length = mxGetN(ETH_PDO_NAME);



258        int_T rtn = mxGetString(ETH_PDO_NAME, name, 255 );



259        EC_T_PROCESS_VAR_INFO InfoEntry;



260        int   ret;



261   



262        printf("rtn %d, lth %d\n", rtn, length );



263        printf("name: %s\n", name );



264        



265        ret = emFindOutpVarByName( deviceIndex, name, &InfoEntry );



266        if( ret == 0 )



267        {



268        printf("fname: %s\n", InfoEntry.szName );



269        printf("From lookup:\n");



270        printf("type %d, bitsize %d, bitoffs %d\n",



271                InfoEntry.wDataType,



272                InfoEntry.nBitSize,



273                InfoEntry.nBitOffs );



274        printf("fixedaddr %d, isinput %d\n",



275                InfoEntry.wFixedAddr,



276                InfoEntry.bIsInputData );



277        }



278        else



279            printf("ret = %d\n", ret );



280   }



281#endif



282



283   sigType = (DTypeId)((int_T)(mxGetPr(ETH_PDO_SIGNAL_TYPE)[0])-1);



284   pdoOffset = (int_T)mxGetPr(ETH_PDO_OFFSET)[0];



285   pdoTypeBitSize = (int_T)mxGetPr(ETH_PDO_TYPE_BITSIZE )[0];



286   sigTypeSize = getDataTypeSize(S,sigType);



287   sigWidth = (int_T)mxGetPr(ETH_PDO_SIGNAL_DIM)[0];



288



289   ssSetIWorkValue (S, I_DEVICE_ID, (uint32_T)deviceIndex);



290   ssSetIWorkValue (S, I_PDO_TYPE, (uint32_T)sigType);



291   ssSetIWorkValue (S, I_PDO_OFFSET, (uint32_T)pdoOffset);



292   ssSetIWorkValue (S, I_PDO_TYPE_BIT_SIZE, (uint32_T)pdoTypeBitSize);



293   ssSetIWorkValue (S, I_SIG_TYPE_BIT_SIZE, (uint32_T)sigTypeSize);



294   ssSetIWorkValue (S, I_SIG_WIDTH, sigWidth);



295}



296



297double txMaxTime=0.0;



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



299{



300



301#ifndef MATLAB_MEX_FILE



302    int8_T    *sigInputPtr;



303    int8_T    *ecatTxBufPtr;



304    int_T     deviceIndex;



305    static    int counter=0;



306    int_T     sigTypeSize;



307    int_T     pdoTypeBitSize;



308    int_T     pdoOffset;



309    int_T     sigWidth;



310    int_T     i;



311    DTypeId   sigType;



312    int_T     bitOffset;



313



314    pdoOffset = ssGetIWorkValue (S,I_PDO_OFFSET);



315    pdoTypeBitSize =ssGetIWorkValue (S, I_PDO_TYPE_BIT_SIZE);



316    deviceIndex = ssGetIWorkValue (S, I_DEVICE_ID);



317    sigInputPtr = (int8_T *)ssGetInputPortSignal(S,0);



318    sigTypeSize = ssGetIWorkValue(S, I_SIG_TYPE_BIT_SIZE);



319    sigType = ssGetIWorkValue(S, I_PDO_TYPE);



320



321    ecatTxBufPtr = xpcEtherCATgetPDout(deviceIndex);



322    



323    sigWidth = ssGetIWorkValue (S, I_SIG_WIDTH);



324



325    bitOffset = pdoOffset;



326    for (i=0; i<sigWidth; i++) {



327       switch (sigType){



328           case  SS_DOUBLE:



329              *((real_T *)(ecatTxBufPtr+bitOffset/BITS_PER_INT8)) = ((real_T *)sigInputPtr)[i];



330



331           break;



332



333           case  SS_SINGLE:



334               *((real32_T *)(ecatTxBufPtr+bitOffset/BITS_PER_INT8)) = ((real32_T *)sigInputPtr)[i];



335           break;



336



337           case SS_INT8:



338



339               if ((pdoTypeBitSize == BITS_PER_INT8) && (bitOffset%BITS_PER_INT8 == 0)) {



340                  *((int8_T *)(ecatTxBufPtr+bitOffset/BITS_PER_INT8)) = ((int8_T *)sigInputPtr)[i];



341               }



342               else {



343



344                   bitCopy((uint8_T *)ecatTxBufPtr, bitOffset, pdoTypeBitSize, (uint32_T)(*((int8_T *)sigInputPtr)));



345               }



346



347           break;



348



349           case SS_UINT8:



350               if ((pdoTypeBitSize == BITS_PER_UINT8) && (bitOffset%BITS_PER_UINT8 == 0)) {



351                   *((uint8_T *)(ecatTxBufPtr+bitOffset/BITS_PER_INT8)) = ((uint8_T *)sigInputPtr)[i];



352               }



353               else {



354                  bitCopy((uint8_T *)ecatTxBufPtr, bitOffset, pdoTypeBitSize, (uint32_T)(*((uint8_T *)sigInputPtr)));



355               }



356



357           break;



358



359           case SS_BOOLEAN:



360               if ((pdoTypeBitSize == BITS_PER_INT8) && (bitOffset%BITS_PER_INT8 == 0)) {



361                  *((int8_T *)(ecatTxBufPtr+bitOffset/BITS_PER_INT8)) = ((int8_T *)sigInputPtr)[i];



362               }



363               else {



364                  bitCopy((uint8_T *)ecatTxBufPtr, bitOffset, pdoTypeBitSize, (uint32_T)(*((int8_T *)sigInputPtr)));



365               }



366



367



368           break;



369



370           case SS_INT16:



371



372               if ((pdoTypeBitSize == BITS_PER_INT16) && (bitOffset%BITS_PER_INT16 == 0)) {



373                  *((int16_T *)(ecatTxBufPtr+bitOffset/BITS_PER_INT8)) = ((int16_T *)sigInputPtr)[i];



374



375               }



376               else {



377                   bitCopy((uint8_T *)ecatTxBufPtr, bitOffset, pdoTypeBitSize, (uint32_T)(*((int16_T *)sigInputPtr)));



378               }



379



380



381           break;



382



383           case SS_UINT16:



384               if ((pdoTypeBitSize == BITS_PER_UINT16) && (bitOffset%BITS_PER_UINT16 == 0)) {



385                   *((uint16_T *)(ecatTxBufPtr+bitOffset/BITS_PER_INT8)) =((uint16_T *)sigInputPtr)[i];



386               }



387               else {



388                    bitCopy((uint8_T *)ecatTxBufPtr, bitOffset, pdoTypeBitSize, (uint32_T)(*((uint16_T *)sigInputPtr)));



389               }



390



391



392           break;



393



394           case SS_INT32:



395               if ((pdoTypeBitSize == BITS_PER_INT32) && (bitOffset%BITS_PER_INT32 == 0)) {



396                   *((int32_T *)(ecatTxBufPtr+bitOffset/BITS_PER_INT8)) = ((int32_T *)sigInputPtr)[i];



397               }



398               else {



399                    bitCopy((uint8_T *)ecatTxBufPtr, bitOffset, pdoTypeBitSize, (uint32_T)(*((int32_T *)sigInputPtr)));



400               }



401



402



403



404           break;



405



406           case SS_UINT32:



407               if ((pdoTypeBitSize == BITS_PER_UINT32) && (bitOffset%BITS_PER_UINT32 == 0)) {



408                   *((uint32_T *)(ecatTxBufPtr+bitOffset/BITS_PER_INT8)) = ((uint32_T *)sigInputPtr)[i];



409               }



410               else {



411                    bitCopy((uint8_T *)ecatTxBufPtr, bitOffset, pdoTypeBitSize, (uint32_T)(*((uint32_T *)sigInputPtr)));



412               }



413



414



415           break;



416           default:



417               /* Fatal error. Should never happen as this is checked in mdlStart. */



418



419           break;



420         }



421



422         bitOffset += pdoTypeBitSize;



423     }



424 #else



425 #endif



426



427}



428



429static void mdlTerminate(SimStruct *S)



430{



431



432}



433



434



435



436



437#ifdef MATLAB_MEX_FILE



438#include "simulink.c"



439#else



440#include "cg_sfun.h"



441#endif



442