ESL_cia402Demo.c

CiA 402 Callbacks Example.

CiA 402 Callbacks Example.

Author

Texas Instruments Incorporated

Copyright

Copyright (C) 2020 Texas Instruments Incorporated

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

Neither the name of Texas Instruments Incorporated nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 
#include <osal.h>
 
#include <ESL_os.h>
#include <ESL_BOARD_OS_config.h>
 
#include "ecSubDeviceCiA402.h"
 
#include "ESL_cia402Demo.h"
#include "ESL_cia402Obd.h"
#include "ESL_gpioHelper.h"
 
#if !(defined BIT2BYTE)
#define BIT2BYTE(x)                     (((x)+7) >> 3)
#endif
 
/* if dynamic value change while OP is required, this costs 15usec per cycle ! */
#define     ENABLE_DYNAMIC_POSITION_LIMITS      0
 
/* @cppcheck_justify{misra-c2012-20.7} brackets around type result in compile error */
/* cppcheck-suppress misra-c2012-20.7 */
#define EC_SLV_APP_CIA_GETAXISVALUE(gav_type, gav_target, gav_axisDesc) \
    { if (gotInOffset && (NULL != (gav_axisDesc).pdoObject)) { \
    (gav_target) = ((gav_type*)&(pApplication_p->pdRxBuffer[(gav_axisDesc).pdoOffset]))[0]; } else { \
    (void)EC_SLV_APP_getCiA402ObjectValue(pApplication_p, (gav_axisDesc).pSdo, sizeof(gav_type), (uint16_t*)&(gav_target)); } }
 
/* @cppcheck_justify{misra-c2012-20.7} brackets around type result in compile error */
/* cppcheck-suppress misra-c2012-20.7 */
#define EC_SLV_APP_CIA_SETAXISVALUE(sav_type, sav_axisDesc, sav_value) \
    { if (gotOutOffset && (NULL != (sav_axisDesc).pdoObject)) { \
    ((sav_type*)&(pApplication_p->pdTxBuffer[(sav_axisDesc).pdoOffset]))[0] = (sav_value); } else { \
    (void)EC_SLV_APP_setCiA402ObjectValue(pApplication_p, &(sav_axisDesc), sizeof(sav_type), (uint16_t*)&(sav_value)); } }
 
typedef struct EC_SLV_APP_CiA402_SAxis
{
    uint8_t     id; 
    bool        axisIsActive; 
    bool        brakeApplied; 
    bool        lowLevelPowerApplied; 
    bool        highLevelPowerApplied; 
    bool        axisFunctionEnabled; 
    bool        configurationAllowed; 
    double      positionActualValue; 
    uint32_t    cycleTime; 
} EC_SLV_APP_CiA402_SAxis_t;
 
/*-----------------------------------------------------------------------------------------
------
------    local variables and constants
------
-----------------------------------------------------------------------------------------*/
static EC_SLV_APP_CiA402_SAxis_t       localAxes_s[AXES_NUMBER];
 
//Supported drive modes: ETG6010 6.8.1
#define SUPPORTED_DRIVE_MODE_CSP_BIT    (7u)
#define SUPPORTED_DRIVE_MODE_CSV_BIT    (8u)
#define SUPPORTED_DRIVE_MODE_CST_BIT    (9u)
#define DRIVE_MODE_CSP                  ((uint32_t)(1u << SUPPORTED_DRIVE_MODE_CSP_BIT))
#define DRIVE_MODE_CSV                  ((uint32_t)(1u << SUPPORTED_DRIVE_MODE_CSV_BIT))
#define DRIVE_MODE_CST                  ((uint32_t)(1u << SUPPORTED_DRIVE_MODE_CST_BIT))
 
#define DRIVE_GEAR_RELATION             0.0010922
#define POSITION_MAX_LIMIT              (0xFFFFFFFFu)
 
#define NON_DC_DEFAULT_CYCLE_TIME_USEC  (4000u)
#define NSEC_TO_USEC                    (1000u)
#define ESC_DC_SYNC0_CYCLETIME_REG      (0x09A0u)
 
static uint32_t EC_SLV_APP_getCiA402ObjectValue(EC_SLV_APP_CIA_Application_t* pApplication_p, EC_API_SLV_SCoE_Object_t* pObject_p, uint16_t length_p, uint16_t* pValue_p)
{
    EC_API_SLV_SHandle_t*   pEcApiSlv   = NULL;
    uint32_t                err         = EC_API_eERR_INVALID;
 
    if((NULL == pApplication_p) || (NULL == pObject_p))
    {
        /* @cppcheck_justify{misra-c2012-15.1} goto is used to assure single point of exit */
        /* cppcheck-suppress misra-c2012-15.1 */
        goto Exit;
    }
 
    pEcApiSlv = pApplication_p->ptEcSlvApi;
 
    err = EC_API_SLV_CoE_getObjectData(pEcApiSlv, pObject_p, length_p, pValue_p);
 
Exit:
    return  err;
}
 
static uint32_t EC_SLV_APP_setCiA402ObjectValue(EC_SLV_APP_CIA_Application_t*  pApplication_p, EC_SLV_APP_sCIA_object_t* pCiaObject_p, uint16_t length_p, uint16_t* pValue_p)
{
    uint32_t err;
    EC_API_SLV_SCoE_Object_t* pObject = NULL;
 
    if (NULL != pCiaObject_p->pSdo)
    {
        pObject = pCiaObject_p->pSdo;
    }
    else
    {
        err = EC_API_SLV_CoE_getObject(pApplication_p->ptEcSlvApi, pCiaObject_p->objectIndex, &pObject);
        if (0u != err)
        {
            /* @cppcheck_justify{misra-c2012-15.1} goto is used to assure single point of exit */
            /* cppcheck-suppress misra-c2012-15.1 */
            goto Exit;
        }
    }
 
    err = EC_API_SLV_CoE_setObjectData(pApplication_p->ptEcSlvApi, pObject, 0, length_p, pValue_p);
 
Exit:
    return  err;
}
 
uint32_t EC_SLV_APP_getCiA402ObjectEntryValue(void* pAppCtxt_p, EC_API_SLV_SCoE_ObjEntry_t* pObjectEntry_p, uint16_t length_p, uint16_t* pValue_p)
{
    uint32_t                    error           = EC_API_eERR_INVALID;
    EC_API_SLV_SHandle_t*       pEcSlvApi       = NULL;
    /* @cppcheck_justify{misra-c2012-11.5} generic API requires cast */
    /* cppcheck-suppress misra-c2012-11.5 */
    EC_SLV_APP_CIA_Application_t*  pApplication    = (EC_SLV_APP_CIA_Application_t*)pAppCtxt_p;
 
    if (!pApplication || !pObjectEntry_p)
    {
        error = EC_API_eERR_NULLPTR;
        /* @cppcheck_justify{misra-c2012-15.1} goto is used to assure single point of exit */
        /* cppcheck-suppress misra-c2012-15.1 */
        goto Exit;
    }
 
    pEcSlvApi = pApplication->ptEcSlvApi;
 
    error = EC_API_SLV_CoE_getObjectEntryData(pEcSlvApi, pObjectEntry_p, length_p, pValue_p);
 
Exit:
    return  error;
}
 
static uint32_t EC_SLV_APP_setCiA402ObjectEntryValue(EC_API_SLV_SHandle_t* pEcApiSlv_p, uint16_t index_p, uint8_t subIndex_p, uint16_t length_p, uint16_t* pValue_p)
{
    EC_API_SLV_SCoE_ObjEntry_t* pObjEntry;
    uint32_t err;
    err = EC_API_SLV_CoE_getObjectEntry(pEcApiSlv_p,index_p, subIndex_p, &pObjEntry);
    if (err == EC_API_eERR_NONE)
    {
        err = EC_API_SLV_CoE_setObjectEntryData(pEcApiSlv_p, pObjEntry, length_p, pValue_p);
    }
    return  err;
}
 
static uint32_t EC_SLV_APP_setSupportedDriveModes(EC_SLV_APP_CIA_Application_t*  pApplication_p)
{
    uint32_t err;
    /* @cppcheck_justify{misra-c2012-10.8} false positive on type conversion */
    /* cppcheck-suppress misra-c2012-10.8 */
    /* @cppcheck_justify{misra-c2012-12.2} false positive, assignment correct */
    /* cppcheck-suppress misra-c2012-12.2 */
    uint32_t driveMode = (uint32_t)(DRIVE_MODE_CSP | DRIVE_MODE_CSV | DRIVE_MODE_CST);
    uint8_t axisNo;
 
    for(axisNo = 0u; axisNo < AXES_NUMBER; axisNo++)
    {
        err = EC_SLV_APP_setCiA402ObjectValue(pApplication_p,
                                              &pApplication_p->CiA402_axisData[axisNo].supportedDriveModesIndex,
                                              sizeof (driveMode),
                                              /* @cppcheck_justify{misra-c2012-11.3} cast required for geeric API */
                                              /* cppcheck-suppress misra-c2012-11.3 */
                                              (uint16_t*) &driveMode);
    }
    return err;
}
 
 
 
void EC_SLV_APP_setObdValues(void* ctxt)
{
    /* @cppcheck_justify{misra-c2012-11.5} generic API requires cast */
    /* cppcheck-suppress misra-c2012-11.5 */
    EC_SLV_APP_CIA_Application_t*  pApplicationInstance    = (EC_SLV_APP_CIA_Application_t*)ctxt;
    int32_t                     posMaxLimit             = POSITION_MAX_LIMIT;
    uint8_t                     axisNo;
 
    OSAL_printf("+%s\r\n", __func__);
 
    (void)EC_SLV_APP_setSupportedDriveModes(pApplicationInstance);
 
    for(axisNo = 0u; axisNo < AXES_NUMBER; axisNo++)
    {
        (void)EC_SLV_APP_setCiA402ObjectEntryValue(
            pApplicationInstance->ptEcSlvApi,
            OBD_SW_POSITION_LIMIT_INDEX(axisNo),
            2,
            sizeof(posMaxLimit),
            /* @cppcheck_justify{misra-c2012-11.3} generic API requires cast */
            /* cppcheck-suppress misra-c2012-11.3 */
            (uint16_t*) &posMaxLimit);
    }
}
 
uint16_t EC_SLV_APP_CIA_startInputHandler(void* ctxt, uint16_t* intMask)
{
    /* @cppcheck_justify{misra-c2012-11.5} generic API requires cast */
    /* cppcheck-suppress misra-c2012-11.5 */
    EC_SLV_APP_CIA_Application_t*  pApplicationInstance    = (EC_SLV_APP_CIA_Application_t*)ctxt;
    EC_API_SLV_SHandle_t*       pEcApiSlv               = NULL;
    uint32_t                    sync0CycleTime          = 0;
    EC_API_SLV_EUserRetCodes_t  retVal                  = EC_USR_eRET_ERROR;
    uint8_t                     axisNo;
 
    OSALUNREF_PARM(intMask);
 
    if (NULL == pApplicationInstance)
    {
        /* @cppcheck_justify{misra-c2012-15.1} goto is used to assure single point of exit */
        /* cppcheck-suppress misra-c2012-15.1 */
        goto Exit;
    }
 
    pEcApiSlv = pApplicationInstance->ptEcSlvApi;
 
    EC_API_SLV_readDoubleWordEscRegister(pEcApiSlv, ESC_DC_SYNC0_CYCLETIME_REG, &sync0CycleTime);
 
    sync0CycleTime = sync0CycleTime / NSEC_TO_USEC; //get cycle time in us
 
    for(axisNo = 0u; axisNo < AXES_NUMBER; axisNo++)
    {
        localAxes_s[axisNo].id = axisNo;
 
        if (localAxes_s[axisNo].axisIsActive)
        {
            localAxes_s[axisNo].cycleTime = sync0CycleTime;
        }
        if(!localAxes_s[axisNo].cycleTime)
        {
            localAxes_s[axisNo].cycleTime = NON_DC_DEFAULT_CYCLE_TIME_USEC;
        }
        OSALUNREF_PARM(localAxes_s[axisNo].brakeApplied);
        OSALUNREF_PARM(localAxes_s[axisNo].lowLevelPowerApplied);
        OSALUNREF_PARM(localAxes_s[axisNo].highLevelPowerApplied);
        OSALUNREF_PARM(localAxes_s[axisNo].axisFunctionEnabled);
        OSALUNREF_PARM(localAxes_s[axisNo].configurationAllowed);
    }
 
    retVal = EC_USR_eRET_OK;
Exit:
    return retVal;
}
 
static bool EC_SLV_APP_transitionAction(int16_t characteristic_p)
{
    switch(characteristic_p)
    {
        case SLOW_DOWN_RAMP:
            //do stuff
            break;
        case QUICKSTOP_RAMP:
            //do stuff
            break;
        case STOP_ON_CURRENT_LIMIT:
            //do stuff
            break;
        case STOP_ON_VOLTAGE_LIMIT:
            //do stuff
            break;
        default:
            break;
    }
 
    return true;
}
 
static void EC_SLV_APP_CST(
    EC_SLV_APP_CIA_Application_t *pApplication_p,
    EC_SLV_APP_CiA402_SAxis_t *pCiA402Axis_p,
    bool gotInOffset,
    bool gotOutOffset)
{
    int16_t targetTorque;
 
    //Read target torque value
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(int16_t, targetTorque, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].targetTorqueIndex);
 
    //Update torque value
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_SETAXISVALUE(int16_t, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].torqueActualValueIndex, targetTorque);
}
 
static void EC_SLV_APP_CSV(
    EC_SLV_APP_CIA_Application_t *pApplication_p,
    EC_SLV_APP_CiA402_SAxis_t *pCiA402Axis_p,
    bool gotInOffset,
    bool gotOutOffset)
{
    int32_t targetVelocity;
 
    //Read target velocity value
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(int32_t, targetVelocity, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].targetVelocityIndex);
 
    //Update velocity value
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_SETAXISVALUE(int32_t, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].velocityActualValueIndex, targetVelocity);
}
 
static void EC_SLV_APP_CSP(
    EC_SLV_APP_CIA_Application_t *pApplication_p,
    EC_SLV_APP_CiA402_SAxis_t *pCiA402Axis_p,
    bool gotInOffset,
    bool gotOutOffset)
{
    uint32_t targetPosition = 0;
    uint32_t actualPosition = 0;
    int32_t targetVelocity = 0;
    int32_t actualVelocity = 0;
    int16_t targetTorque = 0;
    int16_t actualTorque = 0;
 
    float incFactor = (float) (DRIVE_GEAR_RELATION * pCiA402Axis_p->cycleTime);
 
    //Read target position, velocity and torque values
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(uint32_t, targetPosition, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].targetPositionIndex);
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(int32_t,  targetVelocity, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].targetVelocityIndex);
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(int16_t,  targetTorque,   pApplication_p->CiA402_axisData[pCiA402Axis_p->id].targetTorqueIndex);
 
    OSALUNREF_PARM(targetVelocity);
    OSALUNREF_PARM(targetTorque);
 
    //Read actual position, velocity and torque values
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(uint32_t, actualPosition, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].positionActualValueIndex);
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(int32_t,  actualVelocity, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].velocityActualValueIndex);
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(int16_t,  actualTorque,   pApplication_p->CiA402_axisData[pCiA402Axis_p->id].torqueActualValueIndex);
 
    OSALUNREF_PARM(actualTorque);
 
    if(0.0 != incFactor)
    {
        actualVelocity = (int32_t)(((targetPosition - actualPosition) * 1.0) / incFactor);
        pCiA402Axis_p->positionActualValue = (double)(1.0*((int32_t)actualPosition + actualVelocity));
    }
 
    //Update position and velocity value
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_SETAXISVALUE(int32_t,  pApplication_p->CiA402_axisData[pCiA402Axis_p->id].velocityActualValueIndex, actualVelocity);
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_SETAXISVALUE(uint32_t, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].positionActualValueIndex, pCiA402Axis_p->positionActualValue);
}
 
static void EC_SLV_APP_motionControl(
    EC_SLV_APP_CIA_Application_t *pApplication_p,
    EC_SLV_APP_CiA402_SAxis_t *pCiA402Axis_p,
    bool gotInOffset,
    bool gotOutOffset)
{
    uint16_t statusWord = 0;
    uint16_t controlWord = 0;
    uint32_t targetPosition = 0;
    uint32_t posMaxLimit = 0;
    uint32_t posMinLimit = 0;
    uint8_t operationModeDisplay = 0;
 
    if (!pApplication_p)
    {
        /* @cppcheck_justify{misra-c2012-15.1} goto is used to assure single point of exit */
        /* cppcheck-suppress misra-c2012-15.1 */
        goto Exit;
    }
 
    //Read control, status and drive operation mode objects
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(uint16_t, controlWord, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].controlWordIndex);
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(uint16_t, statusWord, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].statusWordIndex);
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(uint8_t, operationModeDisplay, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].modesOfOperationDisplayIndex);
 
    //Read target position
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(uint32_t, targetPosition, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].targetPositionIndex);
 
    //Get software position limits
#if (defined ENABLE_DYNAMIC_POSITION_LIMITS) && (1==ENABLE_DYNAMIC_POSITION_LIMITS) /* if dynamic value change while OP is required, this costs 15usec per cycle ! */
    EC_SLV_APP_getCiA402ObjectEntryValue(pApplication_p,
                                         pApplication_p->CiA402_axisData[pCiA402Axis_p->id].positionLimitMin.pObjetEntry,
                                         sizeof(pApplication_p->CiA402_axisData[pCiA402Axis_p->id].posLimitMin),
                                         (uint16_t *) &pApplication_p->CiA402_axisData[pCiA402Axis_p->id].posLimitMin);
    EC_SLV_APP_getCiA402ObjectEntryValue(pApplication_p,
                                         pApplication_p->CiA402_axisData[pCiA402Axis_p->id].positionLimitMax.pObjetEntry,
                                         sizeof(pApplication_p->CiA402_axisData[pCiA402Axis_p->id].posLimitMax),
                                         (uint16_t *) &pApplication_p->CiA402_axisData[pCiA402Axis_p->id].posLimitMax);
#endif
    posMinLimit = pApplication_p->CiA402_axisData[pCiA402Axis_p->id].posLimitMin;
    posMaxLimit = pApplication_p->CiA402_axisData[pCiA402Axis_p->id].posLimitMax;
 
    //Calculate new targets for CSP, CSV or CST modes
    if(controlWord == CONTROLWORD_COMMAND_ENABLEOPERATION)
    {
        if(((posMaxLimit >= pCiA402Axis_p->positionActualValue) || (pCiA402Axis_p->positionActualValue >= targetPosition))
         &&((posMinLimit <= pCiA402Axis_p->positionActualValue) || (pCiA402Axis_p->positionActualValue <= targetPosition)))
        {
            statusWord &= ~STATUSWORD_INTERNAL_LIMIT;
 
            switch(operationModeDisplay)
            {
            case CYCLIC_SYNC_POSITION_MODE:
                EC_SLV_APP_CSP(pApplication_p, pCiA402Axis_p, gotInOffset, gotOutOffset);
                break;
            case CYCLIC_SYNC_VELOCITY_MODE:
                //nothing
                EC_SLV_APP_CSV(pApplication_p, pCiA402Axis_p, gotInOffset, gotOutOffset);
                break;
            case CYCLIC_SYNC_TORQUE_MODE:
                //nothing
                EC_SLV_APP_CST(pApplication_p, pCiA402Axis_p, gotInOffset, gotOutOffset);
                break;
            default:
                break;
            }
        }
        else
        {
            statusWord |= STATUSWORD_INTERNAL_LIMIT;
        }
    }
 
    //Update drive status
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_SETAXISVALUE(uint16_t,  pApplication_p->CiA402_axisData[pCiA402Axis_p->id].statusWordIndex, statusWord);
 
    //Accept new mode of operation
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_GETAXISVALUE(uint8_t, operationModeDisplay, pApplication_p->CiA402_axisData[pCiA402Axis_p->id].modesOfOperationIndex);
    /* @cppcheck_justify{misra-c2012-11.3} type cast required */
    /* cppcheck-suppress misra-c2012-11.3 */
    EC_SLV_APP_CIA_SETAXISVALUE(uint8_t,  pApplication_p->CiA402_axisData[pCiA402Axis_p->id].modesOfOperationDisplayIndex, operationModeDisplay);
 
Exit:
    return;
}
 
void EC_SLV_APP_cia402Application(void* ctxt)
{
    /* @cppcheck_justify{misra-c2012-11.5} type cast required */
    /* cppcheck-suppress misra-c2012-11.5 */
    EC_SLV_APP_CIA_Application_t*  pApplication_p    = (EC_SLV_APP_CIA_Application_t*)ctxt;
    EC_API_SLV_SHandle_t*       pEcApiSlv               = NULL;
    /* @cppcheck_justify{threadsafety-threadsafety} not called re-entrant */
    /* cppcheck-suppress threadsafety-threadsafety */
    static bool gotInOffset = false;
    static bool gotOutOffset = false;
    uint8_t                     axisNo;
 
    uint16_t controlWord = 0;
    uint16_t statusWord = 0;
    uint16_t errorCode = 0;
    int16_t quickStopOptionCode = 0;
    int16_t shutdownOptionCode = 0;
    int16_t disableOperationCode = 0;
    int16_t faultReactionCode = 0;
    uint8_t operationDisplayCode = 0;
 
    uint16_t driveRamp = DISABLE_DRIVE;
 
    EC_API_SLV_EEsmState_t  curState = EC_API_SLV_eESM_uninit;
    uint16_t                alErrorCode = ALSTATUSCODE_NOERROR;
 
    uint32_t    apiError;
 
    if (!pApplication_p)
    {
        /* @cppcheck_justify{misra-c2012-15.1} goto is used to assure single point of exit */
        /* cppcheck-suppress misra-c2012-15.1 */
        goto Exit;
    }
 
    pEcApiSlv = pApplication_p->ptEcSlvApi;
 
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
        ESL_GPIO_testPins_set(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
 
    EC_API_SLV_getState(pEcApiSlv, &curState, &alErrorCode);
 
    //OSAL_printf("0x%02x|0x%04x\r\n", curState, alErrorCode);
 
    curState &= ~EC_API_SLV_eESM_errState;
 
    if ((EC_API_SLV_eESM_op != curState) && (EC_API_SLV_eESM_safeop != curState) && (gotInOffset || gotOutOffset))
    {
        (void)EC_SLV_APP_CiA_dropPDOffsets(pApplication_p);
        pApplication_p->pdoOutLen = ~0;
        pApplication_p->pdoInLen  = ~0;
        gotInOffset = gotOutOffset = false;
    }
 
    if (((EC_API_SLV_eESM_op == curState) || (EC_API_SLV_eESM_safeop == curState)) && !gotInOffset)
    {
        (void)EC_SLV_APP_CiA_fetchPDOffsets(pApplication_p);
 
        EC_API_SLV_getInputProcDataLength(pApplication_p->ptEcSlvApi, &pApplication_p->pdoInLen);
        pApplication_p->pdoInLen = BIT2BYTE(pApplication_p->pdoInLen);
 
        OSAL_printf(
            "PDO size In:0x%x/0x%x\r\n",
            pApplication_p->pdoInLen,
            pApplication_p->realPdoInLen
            );
        gotInOffset = true;
    }
 
    if ((EC_API_SLV_eESM_op == curState) && !gotOutOffset)
    {
        (void)EC_SLV_APP_CiA_fetchPDOffsets(pApplication_p);
 
        EC_API_SLV_getOutputProcDataLength(pApplication_p->ptEcSlvApi, &pApplication_p->pdoOutLen);
        pApplication_p->pdoOutLen = BIT2BYTE(pApplication_p->pdoOutLen);
 
        OSAL_printf(
            "PDO size Out:0x%x/0x%x, In:0x%x/0x%x\r\n",
            pApplication_p->pdoOutLen,
            pApplication_p->realPdoOutLen,
            pApplication_p->pdoInLen,
            pApplication_p->realPdoInLen
            );
        gotOutOffset = true;
    }
 
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
        ESL_GPIO_testPins_clear(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
 
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
    ESL_GPIO_testPins_set(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
 
    if(((EC_API_SLV_eESM_op == curState) || (EC_API_SLV_eESM_safeop == curState)) && gotInOffset)
    {
        apiError = EC_API_SLV_preSeqInputPDBuffer(
            pApplication_p->ptEcSlvApi,
            pApplication_p->realPdoInLen,
            (void**)&(pApplication_p->pdTxBuffer)
        );
 
        if(EC_API_eERR_BUSY == apiError)
        {
            goto Exit;
        }
    }
 
    if((EC_API_SLV_eESM_op == curState) && gotOutOffset)
    {
        apiError = EC_API_SLV_preSeqOutputPDBuffer(
            pApplication_p->ptEcSlvApi,
            pApplication_p->realPdoOutLen,
            (void**)&(pApplication_p->pdRxBuffer)
        );
 
        if(EC_API_eERR_BUSY == apiError)
        {
            goto Exit;
        }
    }
 
    for(axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
    {
        //Read drive control and status objects
        /* @cppcheck_justify{misra-c2012-11.3} type cast required */
        /* cppcheck-suppress misra-c2012-11.3 */
        EC_SLV_APP_CIA_GETAXISVALUE(uint16_t, controlWord, pApplication_p->CiA402_axisData[axisNo].controlWordIndex);
        /* @cppcheck_justify{misra-c2012-11.3} type cast required */
        /* cppcheck-suppress misra-c2012-11.3 */
        EC_SLV_APP_CIA_GETAXISVALUE(uint16_t, statusWord, pApplication_p->CiA402_axisData[axisNo].statusWordIndex);
 
        //Enable high level power, no torque on the motor.
        if(controlWord == CONTROLWORD_COMMAND_SWITCHON)
        {
            OSAL_printf("Axis %d Activated\n\r", axisNo);
            EC_API_SLV_CiA402_activateAxis(pEcApiSlv, axisNo, true);
        }
 
        //Read drive's operation mode
        /* @cppcheck_justify{misra-c2012-11.3} type cast required */
        /* cppcheck-suppress misra-c2012-11.3 */
        EC_SLV_APP_CIA_GETAXISVALUE(uint8_t, operationDisplayCode, pApplication_p->CiA402_axisData[axisNo].modesOfOperationDisplayIndex);
 
        //Read supported error option codes (ETG6010 Chapter 4).
        EC_API_SLV_CiA402_SM_getErrorCode(pEcApiSlv, axisNo, &errorCode);
 
        if(errorCode &&
            ((operationDisplayCode == CYCLIC_SYNC_POSITION_MODE) ||
             (operationDisplayCode == CYCLIC_SYNC_VELOCITY_MODE) ||
             (operationDisplayCode == CYCLIC_SYNC_TORQUE_MODE)))
        {
            statusWord &= ~ STATUSWORD_DRIVE_FOLLOWS_COMMAND;
        }
        else
        {
            statusWord |= STATUSWORD_DRIVE_FOLLOWS_COMMAND;
        }
 
        //Analyse error codes
        switch(errorCode)
        {
        case OBD_QUICKSTOP_INDEX(0):
            /*State transition 11 is pending analyse shutdown option code (0x605A)*/
            {
                /* @cppcheck_justify{misra-c2012-11.3} type cast required */
                /* cppcheck-suppress misra-c2012-11.3 */
                EC_SLV_APP_CIA_GETAXISVALUE(uint16_t, quickStopOptionCode, pApplication_p->CiA402_axisData[axisNo].quickStopIndex);
 
                /*Masked and execute specified quick stop ramp characteristic */
                if((quickStopOptionCode >= SLOWDOWN_RAMP_NO_TRANSIT) && (quickStopOptionCode <= VOLTAGE_LIMIT_NO_TRANSIT))
                {
                    if (quickStopOptionCode == SLOWDOWN_RAMP_NO_TRANSIT)
                    {
                        driveRamp = SLOW_DOWN_RAMP;
                    }
                    if (quickStopOptionCode == QUICKSTOP_RAMP_NO_TRANSIT)
                    {
                        driveRamp = QUICKSTOP_RAMP;
                    }
                    if (quickStopOptionCode == CURRENT_LIMIT_NO_TRANSIT)
                    {
                        driveRamp = STOP_ON_CURRENT_LIMIT;
                    }
                    if (quickStopOptionCode == VOLTAGE_LIMIT_NO_TRANSIT)
                    {
                        driveRamp = STOP_ON_VOLTAGE_LIMIT;
                    }
                }
 
                if(EC_SLV_APP_transitionAction(driveRamp))
                {
                    /*Quick stop ramp is finished complete state transition*/
                    EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
                    if ((quickStopOptionCode >= SLOWDOWN_RAMP_NO_TRANSIT) && (quickStopOptionCode <= VOLTAGE_LIMIT_NO_TRANSIT))
                    {
                        statusWord |= STATUSWORD_TARGET_REACHED;
                    }
                }
            }
            break;
        case OBD_SHUTDOWN_INDEX(0):
            /*State transition 8 is pending analyse shutdown option code (0x605B)*/
            {
                /* @cppcheck_justify{misra-c2012-11.3} type cast required */
                /* cppcheck-suppress misra-c2012-11.3 */
                EC_SLV_APP_CIA_GETAXISVALUE(uint16_t, shutdownOptionCode, pApplication_p->CiA402_axisData[axisNo].shutdownIndex);
 
                if(EC_SLV_APP_transitionAction(shutdownOptionCode))
                {
                    /*shutdown ramp is finished complete state transition*/
                    EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
                }
            }
            break;
        case OBD_DISABLE_OPERATION_INDEX(0):
            /*State transition 5 is pending analyse Disable operation option code (0x605C)*/
            {
                /* @cppcheck_justify{misra-c2012-11.3} type cast required */
                /* cppcheck-suppress misra-c2012-11.3 */
                EC_SLV_APP_CIA_GETAXISVALUE(uint16_t, disableOperationCode, pApplication_p->CiA402_axisData[axisNo].disableOperationIndex);
 
                if(EC_SLV_APP_transitionAction(disableOperationCode))
                {
                    /*disable operation ramp is finished complete state transition*/
                    EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
                }
            }
            break;
 
        case OBD_FAULT_REACTION_INDEX(0):
            /*State transition 14 is pending analyse Fault reaction option code (0x605E)*/
            {
                /* @cppcheck_justify{misra-c2012-11.3} type cast required */
                /* cppcheck-suppress misra-c2012-11.3 */
                EC_SLV_APP_CIA_GETAXISVALUE(uint16_t, faultReactionCode, pApplication_p->CiA402_axisData[axisNo].faultReactionIndex);
 
                if(EC_SLV_APP_transitionAction(faultReactionCode))
                {
                    /*fault reaction ramp is finished complete state transition*/
                    EC_API_SLV_CiA402_SM_clearErrorCode(pEcApiSlv, axisNo);
                }
            }
            break;
        default:
            //Pending transition code is invalid => values from the master are used
            statusWord |= STATUSWORD_DRIVE_FOLLOWS_COMMAND;
            break;
        }
 
        /* @cppcheck_justify{misra-c2012-11.3} type cast required */
        /* cppcheck-suppress misra-c2012-11.3 */
        EC_SLV_APP_CIA_SETAXISVALUE(uint16_t, pApplication_p->CiA402_axisData[axisNo].statusWordIndex, statusWord);
 
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
        ESL_GPIO_testPins_set(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_1_MASK);
#endif
#endif
 
        EC_SLV_APP_motionControl(pApplication_p, &localAxes_s[axisNo], gotInOffset, gotOutOffset);
 
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
        ESL_GPIO_testPins_clear(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_1_MASK);
#endif
#endif
    }
 
Exit:
    if((EC_API_SLV_eESM_op == curState) && gotOutOffset)
    {
        EC_API_SLV_postSeqOutputPDBuffer(
            pApplication_p->ptEcSlvApi,
            pApplication_p->realPdoOutLen,
            pApplication_p->pdRxBuffer
        );
    }
    if(((EC_API_SLV_eESM_op == curState) || (EC_API_SLV_eESM_safeop == curState)) && gotInOffset)
    {
        EC_API_SLV_postSeqInputPDBuffer(
            pApplication_p->ptEcSlvApi,
            pApplication_p->realPdoInLen,
            pApplication_p->pdTxBuffer
        );
    }
 
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (defined GPIO_TEST_PROFILE_1) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
        ESL_GPIO_testPins_clear(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
    return;
}
 
void EC_SLV_APP_cia402LocalError(void* ctxt, uint16_t errorCode)
{
    OSALUNREF_PARM(ctxt);
    OSAL_printf("Local error triggered: 0x%04x\r\n", errorCode);
}
 
//*************************************************************************************************