614 lines
16 KiB
C++
614 lines
16 KiB
C++
/*************************************************************************\
|
|
* Copyright (c) 2023 Paul Scherrer Institute
|
|
* ecmc is distributed subject to a Software License Agreement found
|
|
* in file LICENSE that is included with this distribution.
|
|
*
|
|
* ecmcSafetyGroup.cpp
|
|
*
|
|
* Created on: July 10, 2023
|
|
* Author: anderssandstrom
|
|
* Credits to https://github.com/sgreg/dynamic-loading
|
|
*
|
|
\*************************************************************************/
|
|
|
|
// Needed to get headers in ecmc right...
|
|
#define ECMC_IS_PLUGIN
|
|
|
|
#define ECMC_PLUGIN_ASYN_SAFETY_STAT "status"
|
|
|
|
#include <sstream>
|
|
#include "ecmcSafetyGroup.h"
|
|
#include "ecmcPluginClient.h"
|
|
#include "ecmcAsynPortDriver.h"
|
|
#include "ecmcAsynPortDriverUtils.h"
|
|
#include "ecmcMotion.h"
|
|
|
|
#ifdef BASE_VERSION
|
|
#define EPICS_3_13
|
|
extern DBBASE *pdbbase;
|
|
#endif
|
|
|
|
/** ecmc Safety interface class
|
|
* This object can throw:
|
|
* - bad_alloc
|
|
* - invalid_argument
|
|
* - runtime_error
|
|
*/
|
|
ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
|
|
const char *ec_rampdown_cmd,
|
|
const char *ec_standstill_status,
|
|
int time_delay_ms,
|
|
const char *cfg_string,
|
|
char* portName)
|
|
: asynPortDriver(portName,
|
|
1, /* maxAddr */
|
|
asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
|
|
asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
|
|
asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
|
|
asynInt32ArrayMask | asynUInt32DigitalMask, /* Interface mask */
|
|
asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
|
|
asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
|
|
asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
|
|
asynInt32ArrayMask | asynUInt32DigitalMask, /* Interrupt mask */
|
|
ASYN_CANBLOCK , /*NOT ASYN_MULTI_DEVICE*/
|
|
1, /* Autoconnect */
|
|
0, /* Default priority */
|
|
0) /* Default stack size */
|
|
{
|
|
sName_ = strdup(name);
|
|
sEcRampDownCmdNameOrg_ = strdup(ec_rampdown_cmd);
|
|
sEcAxesStandStillStatOrg_= strdup(ec_standstill_status);
|
|
sEcRampDownCmdNameStrip_ = strdup(ec_rampdown_cmd);
|
|
sEcAxesStandStillStatStrip_ = strdup(ec_standstill_status);
|
|
sConfig_ = strdup(cfg_string);
|
|
delayMs_ = time_delay_ms;
|
|
status_ = 0;
|
|
asynStatusId_ = -1;
|
|
axesCounter_ = 0;
|
|
ecmcSampleRateHz_ = getEcmcSampleRate();
|
|
dataSourcesLinked_ = 0;
|
|
ecMaster_ = NULL;
|
|
ecEntryRampDown_ = NULL;
|
|
ecEntryStandstill_ = NULL;
|
|
masterId_ = 0;
|
|
slaveIdRampDown_ = 0;
|
|
bitRampDown_ = 0;
|
|
slaveIdStandStill_ = 0;
|
|
bitStandStill_ = 0;
|
|
aliasRampDown_[0] = 0;
|
|
aliasStandStill_[0] = 0;
|
|
rampDownCmd_ = 0;
|
|
axesAreStandstill_ = 0;
|
|
rampDownCmdOld_ = 0;
|
|
axesAreStandstillOld_ = 0;
|
|
printEnableStatus_ = 1;
|
|
|
|
// Config defaults
|
|
cfgDbgMode_ = 0;
|
|
parseConfigStr(cfg_string);
|
|
initAsyn();
|
|
if(cfgDbgMode_) {
|
|
printf("Safety %s: Safety group created:\n"
|
|
" Name: %s\n"
|
|
" I/O for rampdown command from saftey PLC: %s\n"
|
|
" I/O for axes standstill status: %s\n"
|
|
" STO delay [ms]: %d\n"
|
|
" Configuration string: %s\n",
|
|
sName_,sName_,sEcRampDownCmdNameOrg_,
|
|
sEcAxesStandStillStatOrg_,delayMs_,sConfig_);
|
|
}
|
|
}
|
|
|
|
ecmcSafetyGroup::~ecmcSafetyGroup() {
|
|
if(cfgDbgMode_) {
|
|
printf("Safety %s: Cleanup\n",sName_);
|
|
}
|
|
|
|
free(sName_);
|
|
free(sEcRampDownCmdNameOrg_);
|
|
free(sEcAxesStandStillStatOrg_);
|
|
free(sEcRampDownCmdNameStrip_);
|
|
free(sEcAxesStandStillStatStrip_);
|
|
free(sConfig_);
|
|
}
|
|
|
|
void ecmcSafetyGroup::parseConfigStr(const char *configStr) {
|
|
|
|
// check config parameters
|
|
if (configStr && configStr[0]) {
|
|
char *pOptions = strdup(configStr);
|
|
char *pThisOption = pOptions;
|
|
char *pNextOption = pOptions;
|
|
|
|
while (pNextOption && pNextOption[0]) {
|
|
pNextOption = strchr(pNextOption, ';');
|
|
if (pNextOption) {
|
|
*pNextOption = '\0'; /* Terminate */
|
|
pNextOption++; /* Jump to (possible) next */
|
|
}
|
|
|
|
// ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0)
|
|
if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) {
|
|
pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
|
|
cfgDbgMode_ = atoi(pThisOption);
|
|
}
|
|
|
|
pThisOption = pNextOption;
|
|
}
|
|
free(pOptions);
|
|
}
|
|
}
|
|
|
|
void ecmcSafetyGroup::validate() {
|
|
validateCfgs();
|
|
connectToDataSources();
|
|
validateAxes();
|
|
}
|
|
|
|
void ecmcSafetyGroup::validateCfgs() {
|
|
|
|
// Validate data sources
|
|
int masterIdRampDown=-1;
|
|
if(parseEcPath(sEcRampDownCmdNameOrg_,
|
|
&masterIdRampDown,
|
|
&slaveIdRampDown_,
|
|
aliasRampDown_,
|
|
&bitRampDown_)) {
|
|
throw std::runtime_error( "Safety: Parse error: Data source for rampdown cmd.");
|
|
}
|
|
|
|
int masterIdStandStill=-1;
|
|
if(parseEcPath(sEcAxesStandStillStatOrg_,
|
|
&masterIdStandStill,
|
|
&slaveIdStandStill_,
|
|
aliasStandStill_,
|
|
&bitStandStill_)) {
|
|
throw std::runtime_error( "Safety: Parse error: Data source for standstill status.");
|
|
}
|
|
|
|
if(masterIdStandStill != masterIdRampDown ) {
|
|
throw std::runtime_error( "Safety: Parse error: Master id for datasources different.");
|
|
}
|
|
masterId_ = masterIdStandStill;
|
|
|
|
if(bitRampDown_ < 0) {
|
|
throw std::runtime_error( "Safety: Parse error: Rampdown cmd, bit invalid.");
|
|
}
|
|
|
|
if(bitStandStill_ < 0) {
|
|
throw std::runtime_error( "Safety: Parse error: Standstill status, bit invalid.");
|
|
}
|
|
|
|
if(cfgDbgMode_) {
|
|
printf("Safety %s: I/O link parsed:\n"
|
|
" name: %s\n"
|
|
" masterid: %d\n"
|
|
" slaveid: %d\n"
|
|
" alias: %s\n"
|
|
" bit: %d\n",
|
|
sName_,
|
|
sEcRampDownCmdNameOrg_,
|
|
masterIdRampDown,
|
|
slaveIdRampDown_,
|
|
aliasRampDown_,
|
|
bitRampDown_);
|
|
|
|
printf("Safety %s: I/O link parsed:\n"
|
|
" name: %s\n"
|
|
" masterid: %d\n"
|
|
" slaveid: %d\n"
|
|
" alias: %s\n"
|
|
" bit: %d\n",
|
|
sName_,
|
|
sEcAxesStandStillStatOrg_,
|
|
masterIdStandStill,
|
|
slaveIdStandStill_,
|
|
aliasStandStill_,
|
|
bitStandStill_);
|
|
}
|
|
|
|
// Now bits are idenfied. Now bits need to be removed in order to find data item.
|
|
stripBits();
|
|
}
|
|
|
|
void ecmcSafetyGroup::stripBits() {
|
|
char * lastdot = strrchr(sEcRampDownCmdNameStrip_,'.');
|
|
if(lastdot) {
|
|
lastdot[0] = 0;
|
|
}
|
|
else {
|
|
throw std::runtime_error( "Safety: Failed trim bit from ramp down I/O link.");
|
|
}
|
|
|
|
if(cfgDbgMode_) {
|
|
printf("Safety %s: I/O bit removed:\n"
|
|
" before: %s\n"
|
|
" after: %s\n",
|
|
sName_,
|
|
sEcRampDownCmdNameOrg_,
|
|
sEcRampDownCmdNameStrip_);
|
|
}
|
|
|
|
lastdot = strrchr(sEcAxesStandStillStatStrip_,'.');
|
|
if(lastdot) {
|
|
lastdot[0] = 0;
|
|
}
|
|
else {
|
|
throw std::runtime_error( "Safety: Failed trim bit from stand still I/O link.");
|
|
}
|
|
|
|
if(cfgDbgMode_) {
|
|
printf("Safety %s: I/O bit removed:\n"
|
|
" before: %s\n"
|
|
" after: %s\n",
|
|
sName_,
|
|
sEcAxesStandStillStatOrg_,
|
|
sEcAxesStandStillStatStrip_);
|
|
}
|
|
}
|
|
|
|
void ecmcSafetyGroup::validateAxes() {
|
|
|
|
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
|
|
if(!*saxis) {
|
|
throw std::runtime_error( "Safety: Axis object NULL.");
|
|
}
|
|
}
|
|
}
|
|
|
|
void ecmcSafetyGroup::connectToDataSources() {
|
|
|
|
ecMaster_ = (ecmcEc*)getEcMaster();
|
|
|
|
if(!ecMaster_) {
|
|
throw std::runtime_error( "Safety: EtherCAT master object NULL.");
|
|
}
|
|
|
|
// rampdown
|
|
ecmcEcSlave *slave = ecMaster_->findSlave(slaveIdRampDown_);
|
|
if(!slave) {
|
|
throw std::runtime_error( "Safety: EtherCAT slave for rampdown I/O NULL.");
|
|
}
|
|
|
|
ecEntryRampDown_ = slave->findEntry(aliasRampDown_);
|
|
if(!ecEntryRampDown_) {
|
|
throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL.");
|
|
}
|
|
|
|
// standstill
|
|
slave = ecMaster_->findSlave(slaveIdStandStill_);
|
|
if(!slave) {
|
|
throw std::runtime_error( "Safety: EtherCAT slave for standstill I/O NULL.");
|
|
}
|
|
|
|
ecEntryStandstill_ = slave->findEntry(aliasStandStill_);
|
|
if(!ecEntryStandstill_) {
|
|
throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL.");
|
|
};
|
|
|
|
dataSourcesLinked_ = 1;
|
|
|
|
return;
|
|
|
|
//// Get dataItem for rampdown command
|
|
//dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdNameStrip_);
|
|
//if(!dataItemRampDownCmd_) {
|
|
// throw std::runtime_error( "Safety: Data item for ramp down command NULL.");
|
|
//}
|
|
//
|
|
//// Get dataItem for axes standstill status
|
|
//dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStatStrip_);
|
|
//if(!dataItemStandStillStat_) {
|
|
// throw std::runtime_error( "Safety: Data item for axes standstill status NULL.");
|
|
//}
|
|
//
|
|
//if(cfgDbgMode_) {
|
|
// printf("Safety: Safety group \"%s\"\": Data sources linked.\n",sName_);
|
|
//}
|
|
//
|
|
//dataSourcesLinked_ = 1;
|
|
}
|
|
|
|
void ecmcSafetyGroup::initAsyn() {
|
|
|
|
// Add motion "plugin.motion%d.status"
|
|
std::string paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) +
|
|
"." + ECMC_PLUGIN_ASYN_SAFETY_STAT;
|
|
int *paramId = &asynStatusId_;
|
|
if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) {
|
|
throw std::runtime_error("Safety: Failed create asyn parameter mode");
|
|
}
|
|
setIntegerParam(*paramId, (epicsInt32)status_);
|
|
|
|
// Update integers
|
|
callParamCallbacks();
|
|
}
|
|
|
|
// Avoid issues with std:to_string()
|
|
std::string ecmcSafetyGroup::to_string(int value) {
|
|
std::ostringstream os;
|
|
os << value;
|
|
return os.str();
|
|
}
|
|
|
|
// Executed by ecmc rt thread.
|
|
void ecmcSafetyGroup::execute() {
|
|
|
|
uint64_t data = 0;
|
|
// Read ramp down command from safety plc
|
|
if(ecEntryRampDown_->readBit(bitRampDown_,
|
|
&data)) {
|
|
// Disable all axes
|
|
setAxesDisable(); // disable
|
|
setAxesSafetyInterlocks(0); // stop
|
|
setAxesStandstillStatus(0); // set output
|
|
throw std::out_of_range("Safety: Read rampdown cmd failed");
|
|
}
|
|
|
|
rampDownCmdOld_ = rampDownCmd_;
|
|
rampDownCmd_ = data == 0;
|
|
|
|
if(rampDownCmdOld_ != rampDownCmd_) {
|
|
resetPrintoutStatus();
|
|
if(cfgDbgMode_) {
|
|
if(rampDownCmd_) {
|
|
printf("Safety %s: Ramp down cmd active\n",sName_);
|
|
} else {
|
|
printf("Safety %s: Ramp down cmd not active\n",sName_);
|
|
}
|
|
}
|
|
}
|
|
|
|
// set safety interlock in ecmc
|
|
setAxesSafetyInterlocks(rampDownCmd_);
|
|
// check if axes are standstill to safety PLC
|
|
axesAreStandstill_ = checkAxesStandstillAndDisableIfNeeded();
|
|
setAxesStandstillStatus(axesAreStandstill_);
|
|
|
|
// Disable
|
|
if(axesAreStandstill_ && rampDownCmd_) {
|
|
setAxesDisable();
|
|
} else {
|
|
printEnableStatus_ = true;
|
|
}
|
|
}
|
|
|
|
void ecmcSafetyGroup::resetPrintoutStatus() {
|
|
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
|
|
(*saxis)->printEnableStat_ = 1;
|
|
}
|
|
}
|
|
|
|
void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
|
|
|
|
if(axesAreStandstillOld_ != standstill) {
|
|
if(standstill) {
|
|
printf("Safety %s: Axes are not moving\n",sName_);
|
|
} else {
|
|
printf("Safety %s: Axes are moving\n",sName_);
|
|
}
|
|
}
|
|
|
|
if(ecEntryStandstill_->writeBit(bitStandStill_,
|
|
standstill > 0)) {
|
|
throw std::out_of_range("Safety: Read rampdown cmd failed");
|
|
}
|
|
axesAreStandstillOld_ = standstill;
|
|
}
|
|
|
|
// Check standstill axes
|
|
bool ecmcSafetyGroup::checkAxesStandstill() {
|
|
bool standstill = 1;
|
|
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
|
|
standstill= standstill && checkAxisStandstill((*saxis));
|
|
}
|
|
return standstill;
|
|
}
|
|
|
|
// Check standstill axis
|
|
bool ecmcSafetyGroup::checkAxisStandstill(safetyAxis* axis) {
|
|
double velo = 1;
|
|
int err = getAxisTrajVelo(axis->axisId_, &velo);
|
|
if(err) {
|
|
return 0;
|
|
}
|
|
return std::abs(velo) <= axis->veloLimit_;
|
|
}
|
|
|
|
// Check standstill and disable
|
|
bool ecmcSafetyGroup::checkAxesStandstillAndDisableIfNeeded() {
|
|
bool standstill = 1;
|
|
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
|
|
standstill = standstill && checkAxisStandstillAndDisableIfNeeded((*saxis));
|
|
}
|
|
return standstill;
|
|
}
|
|
|
|
// Check standstill and disable
|
|
bool ecmcSafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) {
|
|
bool standstill = checkAxisStandstill(axis);
|
|
if( standstill && rampDownCmd_) {
|
|
setAxisEnable(axis->axisId_,0);
|
|
if(cfgDbgMode_ && axis->printEnableStat_) {
|
|
printf("Safety %s: Disabling axis %d\n",sName_,axis->axisId_);
|
|
axis->printEnableStat_ = 0;
|
|
}
|
|
}
|
|
return standstill;
|
|
}
|
|
|
|
// Disable all axes
|
|
void ecmcSafetyGroup::setAxesDisable() {
|
|
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
|
|
if(!*saxis) {
|
|
throw std::runtime_error("Safety: Axis object NULL.");
|
|
}
|
|
setAxisEnable((*saxis)->axisId_,0);
|
|
}
|
|
|
|
if(printEnableStatus_ && cfgDbgMode_) {
|
|
printf("Safety %s: All axes in group are beeing disabled\n",sName_);
|
|
}
|
|
printEnableStatus_ = 0;
|
|
}
|
|
|
|
// Set safety interlock
|
|
void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) {
|
|
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
|
|
if(!*saxis) {
|
|
throw std::runtime_error( "Safety: Axis object NULL.");
|
|
}
|
|
setAxisEmergencyStopInterlock((*saxis)->axisId_,stop);
|
|
}
|
|
}
|
|
|
|
asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
|
|
int function = pasynUser->reason;
|
|
if( function == asynStatusId_ ){
|
|
*value = (epicsInt32)status_;
|
|
return asynSuccess;
|
|
}
|
|
|
|
return asynError;
|
|
}
|
|
|
|
void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) {
|
|
|
|
if(!getAxisValid(axisId)) {
|
|
throw std::out_of_range("Safety: Invalid axis id");
|
|
}
|
|
|
|
axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs));
|
|
axesCounter_++;
|
|
|
|
|
|
if(cfgDbgMode_) {
|
|
printf("Safety %s: Added axis %d to safety group\n",sName_,axisId);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
std::string ecmcSafetyGroup::getName() {
|
|
return sName_;
|
|
}
|
|
|
|
uint8_t ecmcSafetyGroup::getUint8(uint8_t* data) {
|
|
return *data;
|
|
}
|
|
|
|
int8_t ecmcSafetyGroup::getInt8(uint8_t* data) {
|
|
int8_t* p=(int8_t*)data;
|
|
return *p;
|
|
}
|
|
|
|
uint16_t ecmcSafetyGroup::getUint16(uint8_t* data) {
|
|
uint16_t* p=(uint16_t*)data;
|
|
return *p;
|
|
}
|
|
|
|
int16_t ecmcSafetyGroup::getInt16(uint8_t* data) {
|
|
int16_t* p=(int16_t*)data;
|
|
return *p;
|
|
}
|
|
|
|
uint32_t ecmcSafetyGroup::getUint32(uint8_t* data) {
|
|
uint32_t* p=(uint32_t*)data;
|
|
return *p;
|
|
}
|
|
|
|
int32_t ecmcSafetyGroup::getInt32(uint8_t* data) {
|
|
int32_t* p=(int32_t*)data;
|
|
return *p;
|
|
}
|
|
|
|
uint64_t ecmcSafetyGroup::getUint64(uint8_t* data) {
|
|
uint64_t* p=(uint64_t*)data;
|
|
return *p;
|
|
}
|
|
|
|
int64_t ecmcSafetyGroup::getInt64(uint8_t* data) {
|
|
int64_t* p=(int64_t*)data;
|
|
return *p;
|
|
}
|
|
|
|
float ecmcSafetyGroup::getFloat32(uint8_t* data) {
|
|
float* p=(float*)data;
|
|
return *p;
|
|
}
|
|
|
|
double ecmcSafetyGroup::getFloat64(uint8_t* data) {
|
|
double* p=(double*)data;
|
|
return *p;
|
|
}
|
|
|
|
size_t ecmcSafetyGroup::getEcDataTypeByteSize(ecmcEcDataType dt){
|
|
switch(dt) {
|
|
case ECMC_EC_NONE:
|
|
return 0;
|
|
break;
|
|
|
|
case ECMC_EC_B1:
|
|
return 1;
|
|
break;
|
|
|
|
case ECMC_EC_B2:
|
|
return 1;
|
|
break;
|
|
|
|
case ECMC_EC_B3:
|
|
return 1;
|
|
break;
|
|
|
|
case ECMC_EC_B4:
|
|
return 1;
|
|
break;
|
|
|
|
case ECMC_EC_U8:
|
|
return 1;
|
|
break;
|
|
|
|
case ECMC_EC_S8:
|
|
return 1;
|
|
break;
|
|
|
|
case ECMC_EC_U16:
|
|
return 2;
|
|
break;
|
|
|
|
case ECMC_EC_S16:
|
|
return 2;
|
|
break;
|
|
|
|
case ECMC_EC_U32:
|
|
return 4;
|
|
break;
|
|
|
|
case ECMC_EC_S32:
|
|
return 4;
|
|
break;
|
|
|
|
case ECMC_EC_U64:
|
|
return 8;
|
|
break;
|
|
|
|
case ECMC_EC_S64:
|
|
return 8;
|
|
break;
|
|
|
|
case ECMC_EC_F32:
|
|
return 4;
|
|
break;
|
|
|
|
case ECMC_EC_F64:
|
|
return 8;
|
|
break;
|
|
|
|
default:
|
|
return 0;
|
|
break;
|
|
}
|
|
|
|
return 0;
|
|
}
|