337 lines
11 KiB
C++
337 lines
11 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.
|
|
*
|
|
* ecmcSafetyPlgWrap.cpp
|
|
*
|
|
* Created on: jan 29, 2024
|
|
* Author: anderssandstrom
|
|
* Credits to https://github.com/sgreg/dynamic-loading
|
|
*
|
|
\*************************************************************************/
|
|
|
|
// Needed to get headers in ecmc right...
|
|
#define ECMC_IS_PLUGIN
|
|
|
|
#include <vector>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
#include <epicsExport.h>
|
|
|
|
#include <iocsh.h>
|
|
#include "ecmcSafetyPlgWrap.h"
|
|
#include "ecmcSS1SafetyGroup.h"
|
|
|
|
#define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64
|
|
#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.SAFETY"
|
|
|
|
static std::vector<ecmcSS1SafetyGroup*> safetyGroups;
|
|
static int safetyGroupsCounter = 0;
|
|
static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
|
|
|
|
static const char *configString = NULL;
|
|
|
|
int setCfgString(const char* cfgString) {
|
|
configString = cfgString;
|
|
return 0;
|
|
}
|
|
|
|
ecmcSS1SafetyGroup* getGroupFromName(const char *name) {
|
|
// Find group by name
|
|
for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
|
|
bool found = strcmp(name, (*psafetyGroup)->getName().c_str()) == 0;
|
|
if(found) {
|
|
return (*psafetyGroup);
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
int createSafetyGroup(const char *name,
|
|
const char *ec_rampdown_cmd,
|
|
const char *ec_standstill_status,
|
|
const char *ec_max_velo_cmd,
|
|
int time_delay_ms) {
|
|
|
|
// ensure group does not already exist
|
|
ecmcSS1SafetyGroup* safetyGroup = getGroupFromName(name);
|
|
if( safetyGroup ) {
|
|
printf("Safety: Error, group %s already defined. Plugin will unload.\n",name);
|
|
throw std::runtime_error( "Safety: Error, group already defined.");
|
|
}
|
|
|
|
// create new ecmcSS1SafetyGroup object
|
|
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 ".GROUP.%s",name );
|
|
try {
|
|
safetyGroup = new ecmcSS1SafetyGroup(name,
|
|
ec_rampdown_cmd,
|
|
ec_standstill_status,
|
|
ec_max_velo_cmd,
|
|
time_delay_ms,
|
|
configString,
|
|
portNameBuffer);
|
|
}
|
|
catch(std::exception& e) {
|
|
if(safetyGroup) {
|
|
delete safetyGroup;
|
|
}
|
|
}
|
|
|
|
safetyGroups.push_back(safetyGroup);
|
|
safetyGroupsCounter++;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int addAxisToSafetyGroup(const char *groupName,
|
|
int axisId,
|
|
double veloLimit,
|
|
int standStillTimeMs,
|
|
double maxVeloLimit) {
|
|
|
|
ecmcSS1SafetyGroup* grp = getGroupFromName(groupName);
|
|
|
|
if( !grp ) {
|
|
printf("Safety: Error, group %s not found. Plugin will unload.\n",groupName);
|
|
throw std::runtime_error( "Safety: Error, group not found.");
|
|
}
|
|
|
|
grp->addAxis(axisId,veloLimit,standStillTimeMs,maxVeloLimit);
|
|
|
|
return asynSuccess;
|
|
}
|
|
|
|
void deleteAllSafetyGroups() {
|
|
|
|
return; // The delete process results in seg fault.. need to investigate..
|
|
|
|
for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
|
|
if(*psafetyGroup) {
|
|
delete (*psafetyGroup);
|
|
}
|
|
}
|
|
}
|
|
|
|
int validate() {
|
|
int groupCount = 0;
|
|
for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
|
|
if(*psafetyGroup) {
|
|
try {
|
|
(*psafetyGroup)->validate();
|
|
groupCount++;
|
|
}
|
|
catch(std::exception& e) {
|
|
printf("Safety: %s. Plugin will unload.\n",e.what());
|
|
return ECMC_PLUGIN_SAFETY_ERROR_CODE;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Do not allow loaded plugin with no group configured
|
|
if(groupCount == 0) {
|
|
printf("Saftey: Error, group count 0. Plugin will unload.\n");
|
|
return ECMC_PLUGIN_SAFETY_ERROR_CODE;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int executeSafetyGroups() {
|
|
|
|
for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
|
|
if(*psafetyGroup) {
|
|
try {
|
|
(*psafetyGroup)->execute();
|
|
}
|
|
catch(std::exception& e) {
|
|
printf("Safety: %s. Plugin will unload.\n",e.what());
|
|
return ECMC_PLUGIN_SAFETY_ERROR_CODE;
|
|
}
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* EPICS iocsh shell command: ecmcAddSS1SafetyGroup
|
|
*/
|
|
void ecmcAddSS1SafetyGroupPrintHelp() {
|
|
printf("\n");
|
|
printf(" Use ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<ec_max_velo_cmd>,<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");
|
|
printf(" <ec_max_velo_cmd> : Ethercat entry input for activation of maximum velo limitation (set to \"empty\" to disable).\n");
|
|
printf(" <time_delay_ms> : Time delay of STO [ms].\n");
|
|
printf("\n");
|
|
}
|
|
|
|
int ecmcAddSS1SafetyGroup(const char* name,
|
|
const char* ec_rampdown_cmd,
|
|
const char* ec_standstill_status,
|
|
const char* ec_max_velo_cmd,
|
|
int time_delay_ms) {
|
|
if(!name) {
|
|
ecmcAddSS1SafetyGroupPrintHelp();
|
|
return asynError;
|
|
}
|
|
|
|
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
|
|
ecmcAddSS1SafetyGroupPrintHelp();
|
|
return asynSuccess;
|
|
}
|
|
|
|
if(!ec_rampdown_cmd) {
|
|
printf("Error: ec_rampdown_cmd ethercat entry not defined.\n");
|
|
ecmcAddSS1SafetyGroupPrintHelp();
|
|
return asynError;
|
|
}
|
|
|
|
if(!ec_standstill_status) {
|
|
printf("Error: ec_standstill_status ethercat entry not defined.\n");
|
|
ecmcAddSS1SafetyGroupPrintHelp();
|
|
return asynError;
|
|
}
|
|
|
|
if(!ec_max_velo_cmd) {
|
|
printf("Error: ec_max_velo_cmd ethercat entry not defined.\n");
|
|
ecmcAddSS1SafetyGroupPrintHelp();
|
|
return asynError;
|
|
}
|
|
|
|
if(time_delay_ms <= 0) {
|
|
printf("Error: time_delay invalid.\n");
|
|
exit (EXIT_FAILURE);
|
|
}
|
|
|
|
try {
|
|
createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status,ec_max_velo_cmd,time_delay_ms);
|
|
}
|
|
catch(std::exception& e) {
|
|
printf("Exception: %s. Add safety group failed.\n",e.what());
|
|
exit (EXIT_FAILURE);
|
|
}
|
|
|
|
return asynSuccess;
|
|
}
|
|
|
|
static const iocshArg initArg0_1 =
|
|
{ "Name", iocshArgString };
|
|
static const iocshArg initArg1_1 =
|
|
{ "ec entry input ramp down cmd", iocshArgString };
|
|
static const iocshArg initArg2_1 =
|
|
{ "ec entry output axes standstill status", iocshArgString };
|
|
static const iocshArg initArg3_1 =
|
|
{ "ec entry input activate max velo", iocshArgString };
|
|
static const iocshArg initArg4_1 =
|
|
{ "STO delay [ms]", iocshArgInt };
|
|
|
|
static const iocshArg *const initArgs_1[] = { &initArg0_1,
|
|
&initArg1_1,
|
|
&initArg2_1,
|
|
&initArg3_1,
|
|
&initArg4_1,};
|
|
|
|
static const iocshFuncDef initFuncDef_1 = { "ecmcAddSS1SafetyGroup", 5, initArgs_1 };
|
|
static void initCallFunc_1(const iocshArgBuf *args) {
|
|
ecmcAddSS1SafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].sval, args[4].ival);
|
|
}
|
|
|
|
/**
|
|
* EPICS iocsh shell command: ecmcAddAxisToSafetyGroup
|
|
*/
|
|
void ecmcAddAxisToSafetyGroupPrintHelp() {
|
|
printf("\n");
|
|
printf(" Use ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>)\n");
|
|
printf(" <name> : Name of safety group.\n");
|
|
printf(" <Axis id> : Axis index to add.\n");
|
|
printf(" <standstill_velo_limit> : Axis standstill velo limit [unit of axis].\n");
|
|
printf(" <standstill_time> : Time for axis to be below standstill_velo_limit [ms].\n");
|
|
printf(" <max_velo_limit> : Axis max velo limit [unit of axis] (0 to disable).\n");
|
|
printf("\n");
|
|
}
|
|
|
|
int ecmcAddAxisToSafetyGroup(const char* name,
|
|
int axis_id,
|
|
double standstill_velo_limit,
|
|
int stand_still_time,
|
|
double max_velo_limit) {
|
|
if(!name) {
|
|
ecmcAddAxisToSafetyGroupPrintHelp();
|
|
return asynError;
|
|
}
|
|
|
|
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
|
|
ecmcAddAxisToSafetyGroupPrintHelp();
|
|
return asynSuccess;
|
|
}
|
|
|
|
if(axis_id <= 0) {
|
|
printf("Error: Invalid axis id.\n");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
if(standstill_velo_limit < 0) {
|
|
printf("Error: Invalid standstill velocity limit.\n");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
if(stand_still_time < 0) {
|
|
printf("Error: Invalid standstill filter time.\n");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
if(max_velo_limit < 0) {
|
|
printf("Error: Invalid maximum velocity limit.\n");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
try {
|
|
return addAxisToSafetyGroup(name,axis_id,
|
|
standstill_velo_limit,
|
|
stand_still_time,
|
|
max_velo_limit);
|
|
}
|
|
catch(std::exception& e) {
|
|
printf("Exception: %s. Add axis to safety group failed.\n",e.what());
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
return asynSuccess;
|
|
}
|
|
|
|
static const iocshArg initArg0_2 =
|
|
{ "Group name", iocshArgString };
|
|
static const iocshArg initArg1_2 =
|
|
{ "Axis id []", iocshArgInt };
|
|
static const iocshArg initArg2_2 =
|
|
{ "Velo stand still limit [unit same as axis cfg]", iocshArgDouble };
|
|
static const iocshArg initArg3_2 =
|
|
{ "Velo stand still filter time [ms]", iocshArgInt };
|
|
static const iocshArg initArg4_2 =
|
|
{ "Velo max limit [unit same as axis cfg]", iocshArgDouble };
|
|
|
|
static const iocshArg *const initArgs_2[] = { &initArg0_2,
|
|
&initArg1_2,
|
|
&initArg2_2,
|
|
&initArg3_2,
|
|
&initArg4_2};
|
|
|
|
static const iocshFuncDef initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 5, initArgs_2};
|
|
static void initCallFunc_2(const iocshArgBuf *args) {
|
|
ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival, args[2].dval, args[3].ival, args[4].dval);
|
|
}
|
|
|
|
void ecmcSafetyPlgRegister(void) {
|
|
iocshRegister(&initFuncDef_1, initCallFunc_1);
|
|
iocshRegister(&initFuncDef_2, initCallFunc_2);
|
|
}
|
|
|
|
epicsExportRegistrar(ecmcSafetyPlgRegister);
|