motion/ecmc_plugin_safety
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Renamed some stuff

Browse Source
This commit is contained in:
sandst_a
2024-02-16 12:20:58 +01:00
parent 30a767fe59
commit 4239ca071b
4 changed files with 95 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
GNUmakefile
View File

@@ -36,7 +36,7 @@ DB_DIR = $(BASE_DIR)/Db
SOURCES += $(SRC_DIR)/ecmcPluginSafety.c
SOURCES += $(SRC_DIR)/ecmcSafetyPlgWrap.cpp
SOURCES += $(SRC_DIR)/ecmcSafetyGroup.cpp
SOURCES += $(SRC_DIR)/ecmcSS1SafetyGroup.cpp
#SOURCES += $(foreach d,${SRC_DIR}, $(wildcard $d/*.c) $(wildcard $d/*.cpp))
HEADERS += $(foreach d,${SRC_DIR}, $(wildcard $d/*.h))

134
src/ecmcSafetyGroup.cpp → src/ecmcSS1SafetyGroup.cpp
View File

@@ -3,7 +3,7 @@
* ecmc is distributed subject to a Software License Agreement found
* in file LICENSE that is included with this distribution.
*
* ecmcSafetyGroup.cpp
* ecmcSS1SafetyGroup.cpp
*
* Created on: July 10, 2023
* Author: anderssandstrom
@@ -17,7 +17,7 @@
#define ECMC_PLUGIN_ASYN_SAFETY_STAT "status"
#include <sstream>
#include "ecmcSafetyGroup.h"
#include "ecmcSS1SafetyGroup.h"
#include "ecmcPluginClient.h"
#include "ecmcAsynPortDriver.h"
#include "ecmcAsynPortDriverUtils.h"
@@ -34,7 +34,7 @@ extern DBBASE *pdbbase;
* - invalid_argument
* - runtime_error
*/
ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
const char *ec_rampdown_cmd,
const char *ec_standstill_status,
int time_delay_ms,
@@ -89,6 +89,7 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
initAsyn();
if(cfgDbgMode_) {
printf("Safety %s: Safety group created:\n"
" Type: SS1\n"
" Name: %s\n"
" I/O for rampdown command from saftey PLC: %s\n"
" I/O for axes standstill status: %s\n"
@@ -99,7 +100,7 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
}
}
ecmcSafetyGroup::~ecmcSafetyGroup() {
ecmcSS1SafetyGroup::~ecmcSS1SafetyGroup() {
if(cfgDbgMode_) {
printf("Safety %s: Cleanup\n",sName_);
}
@@ -112,7 +113,7 @@ ecmcSafetyGroup::~ecmcSafetyGroup() {
free(sConfig_);
}
void ecmcSafetyGroup::parseConfigStr(const char *configStr) {
void ecmcSS1SafetyGroup::parseConfigStr(const char *configStr) {
// check config parameters
if (configStr && configStr[0]) {
@@ -139,13 +140,13 @@ void ecmcSafetyGroup::parseConfigStr(const char *configStr) {
}
}
void ecmcSafetyGroup::validate() {
void ecmcSS1SafetyGroup::validate() {
validateCfgs();
connectToDataSources();
validateAxes();
}
void ecmcSafetyGroup::validateCfgs() {
void ecmcSS1SafetyGroup::validateCfgs() {
// Validate data sources
int masterIdRampDown=-1;
@@ -179,39 +180,41 @@ void ecmcSafetyGroup::validateCfgs() {
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_);
}
//if(cfgDbgMode_) {
// printf("Safety %s: I/O link parsed:\n"
// " type: SS1\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"
// " type: SS1\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();
//stripBits();
}
void ecmcSafetyGroup::stripBits() {
void ecmcSS1SafetyGroup::stripBits() {
char * lastdot = strrchr(sEcRampDownCmdNameStrip_,'.');
if(lastdot) {
lastdot[0] = 0;
@@ -247,16 +250,19 @@ void ecmcSafetyGroup::stripBits() {
}
}
void ecmcSafetyGroup::validateAxes() {
void ecmcSS1SafetyGroup::validateAxes() {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
if(!*saxis) {
if(!(*saxis)) {
throw std::runtime_error( "Safety: Axis object NULL.");
}
if(!getAxisValid((*saxis)->axisId_)) {
throw std::runtime_error( "Safety: Ecmc Axis object not valid.");
}
}
}
void ecmcSafetyGroup::connectToDataSources() {
void ecmcSS1SafetyGroup::connectToDataSources() {
ecMaster_ = (ecmcEc*)getEcMaster();
@@ -309,7 +315,7 @@ void ecmcSafetyGroup::connectToDataSources() {
//dataSourcesLinked_ = 1;
}
void ecmcSafetyGroup::initAsyn() {
void ecmcSS1SafetyGroup::initAsyn() {
// Add motion "plugin.motion%d.status"
std::string paramName = ECMC_PLUGIN_ASYN_PREFIX "_" + to_string(objectId_) +
@@ -325,14 +331,14 @@ void ecmcSafetyGroup::initAsyn() {
}
// Avoid issues with std:to_string()
std::string ecmcSafetyGroup::to_string(int value) {
std::string ecmcSS1SafetyGroup::to_string(int value) {
std::ostringstream os;
os << value;
return os.str();
}
// Executed by ecmc rt thread.
void ecmcSafetyGroup::execute() {
void ecmcSS1SafetyGroup::execute() {
uint64_t data = 0;
// Read ramp down command from safety plc
@@ -373,13 +379,13 @@ void ecmcSafetyGroup::execute() {
}
}
void ecmcSafetyGroup::resetPrintoutStatus() {
void ecmcSS1SafetyGroup::resetPrintoutStatus() {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
(*saxis)->printEnableStat_ = 1;
}
}
void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
void ecmcSS1SafetyGroup::setAxesStandstillStatus(int standstill) {
if(axesAreStandstillOld_ != standstill) {
if(standstill) {
@@ -397,7 +403,7 @@ void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
}
// Check standstill axes
bool ecmcSafetyGroup::checkAxesStandstill() {
bool ecmcSS1SafetyGroup::checkAxesStandstill() {
bool standstill = 1;
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
standstill= standstill && checkAxisStandstill((*saxis));
@@ -406,7 +412,7 @@ bool ecmcSafetyGroup::checkAxesStandstill() {
}
// Check standstill axis
bool ecmcSafetyGroup::checkAxisStandstill(safetyAxis* axis) {
bool ecmcSS1SafetyGroup::checkAxisStandstill(safetyAxis* axis) {
double velo = 1;
int err = getAxisTrajVelo(axis->axisId_, &velo);
if(err) {
@@ -416,7 +422,7 @@ bool ecmcSafetyGroup::checkAxisStandstill(safetyAxis* axis) {
}
// Check standstill and disable
bool ecmcSafetyGroup::checkAxesStandstillAndDisableIfNeeded() {
bool ecmcSS1SafetyGroup::checkAxesStandstillAndDisableIfNeeded() {
bool standstill = 1;
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
standstill = standstill && checkAxisStandstillAndDisableIfNeeded((*saxis));
@@ -425,7 +431,7 @@ bool ecmcSafetyGroup::checkAxesStandstillAndDisableIfNeeded() {
}
// Check standstill and disable
bool ecmcSafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) {
bool ecmcSS1SafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) {
bool standstill = checkAxisStandstill(axis);
if( standstill && rampDownCmd_) {
setAxisEnable(axis->axisId_,0);
@@ -438,7 +444,7 @@ bool ecmcSafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) {
}
// Disable all axes
void ecmcSafetyGroup::setAxesDisable() {
void ecmcSS1SafetyGroup::setAxesDisable() {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
if(!*saxis) {
throw std::runtime_error("Safety: Axis object NULL.");
@@ -453,7 +459,7 @@ void ecmcSafetyGroup::setAxesDisable() {
}
// Set safety interlock
void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) {
void ecmcSS1SafetyGroup::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.");
@@ -462,7 +468,7 @@ void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) {
}
}
asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
asynStatus ecmcSS1SafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
int function = pasynUser->reason;
if( function == asynStatusId_ ){
*value = (epicsInt32)status_;
@@ -472,7 +478,7 @@ asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
return asynError;
}
void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) {
void ecmcSS1SafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) {
if(!getAxisValid(axisId)) {
throw std::out_of_range("Safety: Invalid axis id");
@@ -489,60 +495,60 @@ void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs)
return;
}
std::string ecmcSafetyGroup::getName() {
std::string ecmcSS1SafetyGroup::getName() {
return sName_;
}
uint8_t ecmcSafetyGroup::getUint8(uint8_t* data) {
uint8_t ecmcSS1SafetyGroup::getUint8(uint8_t* data) {
return *data;
}
int8_t ecmcSafetyGroup::getInt8(uint8_t* data) {
int8_t ecmcSS1SafetyGroup::getInt8(uint8_t* data) {
int8_t* p=(int8_t*)data;
return *p;
}
uint16_t ecmcSafetyGroup::getUint16(uint8_t* data) {
uint16_t ecmcSS1SafetyGroup::getUint16(uint8_t* data) {
uint16_t* p=(uint16_t*)data;
return *p;
}
int16_t ecmcSafetyGroup::getInt16(uint8_t* data) {
int16_t ecmcSS1SafetyGroup::getInt16(uint8_t* data) {
int16_t* p=(int16_t*)data;
return *p;
}
uint32_t ecmcSafetyGroup::getUint32(uint8_t* data) {
uint32_t ecmcSS1SafetyGroup::getUint32(uint8_t* data) {
uint32_t* p=(uint32_t*)data;
return *p;
}
int32_t ecmcSafetyGroup::getInt32(uint8_t* data) {
int32_t ecmcSS1SafetyGroup::getInt32(uint8_t* data) {
int32_t* p=(int32_t*)data;
return *p;
}
uint64_t ecmcSafetyGroup::getUint64(uint8_t* data) {
uint64_t ecmcSS1SafetyGroup::getUint64(uint8_t* data) {
uint64_t* p=(uint64_t*)data;
return *p;
}
int64_t ecmcSafetyGroup::getInt64(uint8_t* data) {
int64_t ecmcSS1SafetyGroup::getInt64(uint8_t* data) {
int64_t* p=(int64_t*)data;
return *p;
}
float ecmcSafetyGroup::getFloat32(uint8_t* data) {
float ecmcSS1SafetyGroup::getFloat32(uint8_t* data) {
float* p=(float*)data;
return *p;
}
double ecmcSafetyGroup::getFloat64(uint8_t* data) {
double ecmcSS1SafetyGroup::getFloat64(uint8_t* data) {
double* p=(double*)data;
return *p;
}
size_t ecmcSafetyGroup::getEcDataTypeByteSize(ecmcEcDataType dt){
size_t ecmcSS1SafetyGroup::getEcDataTypeByteSize(ecmcEcDataType dt){
switch(dt) {
case ECMC_EC_NONE:
return 0;

10
src/ecmcSafetyGroup.h → src/ecmcSS1SafetyGroup.h
View File

@@ -23,11 +23,11 @@
class safetyAxis {
public:
safetyAxis(int axisIndex,
safetyAxis(int axisId,
double veloLimit,
int standStillTimeMs) {
veloLimit_ = veloLimit;
axisId_ = axisIndex;
axisId_ = axisId;
standStillTimeMs_ = standStillTimeMs;
printEnableStat_ = 1;
}
@@ -38,7 +38,7 @@ class safetyAxis {
int printEnableStat_;
};
class ecmcSafetyGroup : public asynPortDriver {
class ecmcSS1SafetyGroup : public asynPortDriver {
public:
/** ecmc Safty Plg class
@@ -46,14 +46,14 @@ class ecmcSafetyGroup : public asynPortDriver {
* - bad_alloc
* - out_of_range
*/
ecmcSafetyGroup(const char *name,
ecmcSS1SafetyGroup(const char *name,
const char *ec_rampdown_cmd,
const char *ec_standstill_status,
int time_delay_ms,
const char *cfg_string,
char* portName);
~ecmcSafetyGroup();
~ecmcSS1SafetyGroup();
// Call just before realtime because then all data sources should be available
void validate();

38
src/ecmcSafetyPlgWrap.cpp
View File

@@ -21,12 +21,12 @@
#include <iocsh.h>
#include "ecmcSafetyPlgWrap.h"
#include "ecmcSafetyGroup.h"
#include "ecmcSS1SafetyGroup.h"
#define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64
#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.SAFETY"
static std::vector<ecmcSafetyGroup*> safetyGroups;
static std::vector<ecmcSS1SafetyGroup*> safetyGroups;
static int safetyGroupsCounter = 0;
static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
@@ -42,15 +42,15 @@ int createSafetyGroup(const char *name,
const char *ec_standstill_status,
int time_delay_ms) {
// create new ecmcSafetyGroup object
ecmcSafetyGroup* safetyGroup = NULL;
// create new ecmcSS1SafetyGroup object
ecmcSS1SafetyGroup* safetyGroup = NULL;
// create asynport name for new object ()
memset(portNameBuffer, 0, ECMC_PLUGIN_MAX_PORTNAME_CHARS);
snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS,
ECMC_PLUGIN_PORTNAME_PREFIX "_%d", safetyGroupsCounter);
try {
safetyGroup = new ecmcSafetyGroup(name,
safetyGroup = new ecmcSS1SafetyGroup(name,
ec_rampdown_cmd,
ec_standstill_status,
time_delay_ms,
@@ -75,7 +75,7 @@ int addAxisToSafetyGroup(const char *groupName,
double veloLimit,
int standStillTimeMs) {
// Find group by name
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
bool found = strcmp(groupName, (*psafetyGroup)->getName().c_str()) == 0;
if(found) {
(*psafetyGroup)->addAxis(axisId,veloLimit,standStillTimeMs);
@@ -90,7 +90,7 @@ void deleteAllSafetyGroups() {
return; // The delete process results in seg fault.. need to investigate..
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
if(*psafetyGroup) {
delete (*psafetyGroup);
}
@@ -98,7 +98,7 @@ void deleteAllSafetyGroups() {
}
int validate() {
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
if(*psafetyGroup) {
try {
(*psafetyGroup)->validate();
@@ -114,7 +114,7 @@ int validate() {
int executeSafetyGroups() {
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
if(*psafetyGroup) {
try {
(*psafetyGroup)->execute();
@@ -129,11 +129,11 @@ int executeSafetyGroups() {
}
/**
* EPICS iocsh shell command: ecmcAddSafetyGroup
* EPICS iocsh shell command: ecmcAddSS1SafetyGroup
*/
void ecmcAddSafetyGroupPrintHelp() {
void ecmcAddSS1SafetyGroupPrintHelp() {
printf("\n");
printf(" Use ecmcAddSafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)\n");
printf(" Use ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)\n");
printf(" <name> : Name of group.\n");
printf(" <ec_rampdown_cmd> : Ethercat entry input for rampdown cmd.\n");
printf(" <ec_standstill_status> : Ethercat entry output for group standstill status.\n");
@@ -141,27 +141,27 @@ void ecmcAddSafetyGroupPrintHelp() {
printf("\n");
}
int ecmcAddSafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) {
int ecmcAddSS1SafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) {
if(!name) {
printf("Error: name.\n");
ecmcAddSafetyGroupPrintHelp();
ecmcAddSS1SafetyGroupPrintHelp();
return asynError;
}
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
ecmcAddSafetyGroupPrintHelp();
ecmcAddSS1SafetyGroupPrintHelp();
return asynSuccess;
}
if(!ec_rampdown_cmd) {
printf("Error: ec_rampdown_cmd ethercat entry not defined.\n");
ecmcAddSafetyGroupPrintHelp();
ecmcAddSS1SafetyGroupPrintHelp();
return asynError;
}
if(!ec_standstill_status) {
printf("Error: ec_standstill_status ethercat entry not defined.\n");
ecmcAddSafetyGroupPrintHelp();
ecmcAddSS1SafetyGroupPrintHelp();
return asynError;
}
if(time_delay_ms <= 0) {
@@ -194,9 +194,9 @@ static const iocshArg *const initArgs_1[] = { &initArg0_1,
&initArg2_1,
&initArg3_1};
static const iocshFuncDef initFuncDef_1 = { "ecmcAddSafetyGroup", 4, initArgs_1 };
static const iocshFuncDef initFuncDef_1 = { "ecmcAddSS1SafetyGroup", 4, initArgs_1 };
static void initCallFunc_1(const iocshArgBuf *args) {
ecmcAddSafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].ival);
ecmcAddSS1SafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].ival);
}
/**