#include <osal.h>
#include <ESL_os.h>
#include <ESL_BOARD_OS_config.h>
#include "ecSlvCiA402.h"
#include "ESL_cia402Demo.h"
#include "ESL_cia402Obd.h"
#include "ESL_gpioHelper.h"
#if !(defined BIT2BYTE)
#define BIT2BYTE(x) (((x)+7) >> 3)
#endif
#define ENABLE_DYNAMIC_POSITION_LIMITS 0
#define EC_SLV_APP_CIA_GETAXISVALUE(type_p, target_p, axisDesc_p) \
if (gotOffsets && (NULL != (axisDesc_p).pdoObject)) { \
(target_p) = ((type_p*)&(pApplication_p->pdRxBuffer[(axisDesc_p).pdoOffset]))[0]; } else { \
EC_SLV_APP_getCiA402ObjectValue(pApplication_p, (axisDesc_p).pSdo, sizeof(type_p), (uint16_t*)&(target_p)); }
#define EC_SLV_APP_CIA_SETAXISVALUE(type_p, axisDesc_p, value_p) \
if (gotOffsets && (NULL != (axisDesc_p).pdoObject)) { \
((type_p*)&(pApplication_p->pdTxBuffer[(axisDesc_p).pdoOffset]))[0] = (value_p); } else { \
EC_SLV_APP_setCiA402ObjectValue(pApplication_p, &(axisDesc_p), sizeof(type_p), (uint16_t*)&(value_p)); }
typedef struct
{
uint8_t id;
bool axisIsActive;
bool brakeApplied;
bool lowLevelPowerApplied;
bool highLevelPowerApplied;
bool axisFunctionEnabled;
bool configurationAllowed;
double positionActualValue;
uint32_t cycleTime;
}
#define SUPPORTED_DRIVE_MODE_CSP_BIT 7
#define SUPPORTED_DRIVE_MODE_CSV_BIT 8
#define SUPPORTED_DRIVE_MODE_CST_BIT 9
#define DRIVE_MODE_CSP (1 << SUPPORTED_DRIVE_MODE_CSP_BIT)
#define DRIVE_MODE_CSV (1 << SUPPORTED_DRIVE_MODE_CSV_BIT)
#define DRIVE_MODE_CST (1 << SUPPORTED_DRIVE_MODE_CST_BIT)
#define DRIVE_GEAR_RELATION 0.0010922
#define POSITION_MAX_LIMIT 0xFFFFFFFF
#define NON_DC_DEFAULT_CYCLE_TIME_USEC 4000
#define NSEC_TO_USEC 1000
#define ESC_DC_SYNC0_CYCLETIME_REG 0x09A0
{
if (NULL == pApplication_p || NULL == pObject_p)
{
goto Exit;
}
Exit:
return err;
}
{
uint32_t err;
{
pObject = pCiaObject_p->
pSdo;
}
else
{
if (err)
{
goto Exit;
}
}
Exit:
return err;
}
{
if (!pApplication || !pObjectEntry_p)
{
goto Exit;
}
Exit:
return error;
}
{
uint32_t err;
{
}
return err;
}
{
uint32_t err;
uint32_t driveMode = DRIVE_MODE_CSP | DRIVE_MODE_CSV | DRIVE_MODE_CST;
for(uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
{
sizeof (driveMode), (uint16_t*) &driveMode);
}
return err;
}
{
int32_t posMaxLimit = POSITION_MAX_LIMIT;
OSAL_printf("+%s\r\n", __func__);
for(uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
{
}
}
{
uint32_t sync0CycleTime = 0;
OSALUNREF_PARM(pIntMask_p);
if (!pApplicationInstance)
{
goto Exit;
}
sync0CycleTime = sync0CycleTime / NSEC_TO_USEC;
for(uint8_t axisNo = 0; 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;
}
}
Exit:
return retVal;
}
{
switch(characteristic_p)
{
break;
break;
break;
break;
default:
break;
}
return true;
}
{
int16_t targetTorque;
}
{
int32_t targetVelocity;
}
{
uint32_t targetPosition, actualPosition;
int32_t targetVelocity, actualVelocity;
int16_t targetTorque, actualTorque;
float incFactor = (float) (DRIVE_GEAR_RELATION * pCiA402Axis_p->
cycleTime);
if (incFactor != 0)
{
actualVelocity = (targetPosition - actualPosition) / incFactor;
}
}
{
uint16_t statusWord, controlWord;
uint32_t targetPosition;
uint32_t posMaxLimit, posMinLimit;
uint8_t operationModeDisplay;
if (!pApplication_p)
{
goto Exit;
}
#if (defined ENABLE_DYNAMIC_POSITION_LIMITS) && (1==ENABLE_DYNAMIC_POSITION_LIMITS)
#endif
{
&&(posMinLimit < pCiA402Axis_p->positionActualValue || pCiA402Axis_p->
positionActualValue < targetPosition))
{
switch(operationModeDisplay)
{
break;
break;
break;
default:
break;
}
}
else
{
}
}
Exit:
return;
}
{
static
bool gotOffsets = false;
uint16_t controlWord, statusWord, errorCode;
int16_t quickStopOptionCode, shutdownOptionCode, disableOperationCode, faultReactionCode;
uint8_t operationDisplayCode;
if (!pApplication_p)
{
goto Exit;
}
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_set(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
{
EC_SLV_APP_CiA_dropPDOffsets(pApplication_p);
gotOffsets = false;
}
{
EC_SLV_APP_CiA_fetchPDOffsets(pApplication_p);
gotOffsets = true;
}
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (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) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_set(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
{
}
{
}
for(uint8_t axisNo = 0; axisNo < AXES_NUMBER; axisNo++)
{
{
OSAL_printf("Axis %d Activated\n\r", axisNo);
}
if(errorCode &&
{
}
else
{
}
switch(errorCode)
{
{
{
{
}
{
}
{
}
{
}
}
{
{
}
}
}
break;
{
{
}
}
break;
{
{
}
}
break;
{
{
}
}
break;
default:
break;
}
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_set(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_1_MASK);
#endif
#endif
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_clear(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_1_MASK);
#endif
#endif
}
{
}
{
}
#if (defined GPIO_TEST_PINS) && (1==GPIO_TEST_PINS)
#if (defined GPIO_TEST_PROFILE_SEL) && (GPIO_TEST_PROFILE_1 == GPIO_TEST_PROFILE_SEL)
ESL_GPIO_testPins_clear(ESL_TESTPIN_STATE_REG_BANK, ESL_TESTPIN_2_MASK);
#endif
#endif
Exit:
return;
}
{
OSALUNREF_PARM(pCtxt_p);
OSAL_printf("Local error triggered: 0x%04x\r\n", errorCode_p);
}