277 lines
8.3 KiB
C++
277 lines
8.3 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 "ecmcSafetyGroup.h"
|
|
|
|
#define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64
|
|
#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.SAFETY"
|
|
|
|
static std::vector<ecmcSafetyGroup*> 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;
|
|
}
|
|
|
|
int createSafetyGroup(const char *name,
|
|
const char *ec_rampdown_cmd,
|
|
const char *ec_standstill_status,
|
|
int time_delay_ms) {
|
|
|
|
// create new ecmcSafetyGroup object
|
|
ecmcSafetyGroup* 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,
|
|
ec_rampdown_cmd,
|
|
ec_standstill_status,
|
|
time_delay_ms,
|
|
configString,
|
|
portNameBuffer);
|
|
}
|
|
catch(std::exception& e) {
|
|
if(safetyGroup) {
|
|
delete safetyGroup;
|
|
}
|
|
return ECMC_PLUGIN_SAFETY_ERROR_CODE;
|
|
}
|
|
|
|
safetyGroups.push_back(safetyGroup);
|
|
safetyGroupsCounter++;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int addAxisToSafetyGroup(const char *groupName,
|
|
int axisId,
|
|
double veloLimit,
|
|
int standStillTimeMs) {
|
|
// Find group by name
|
|
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
|
|
bool found = strcmp(groupName, (*psafetyGroup)->getName().c_str()) == 0;
|
|
if(found) {
|
|
(*psafetyGroup)->addAxis(axisId,veloLimit,standStillTimeMs);
|
|
return asynSuccess;
|
|
}
|
|
}
|
|
|
|
return asynError;
|
|
}
|
|
|
|
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) {
|
|
if(*psafetyGroup) {
|
|
delete (*psafetyGroup);
|
|
}
|
|
}
|
|
}
|
|
|
|
int validate() {
|
|
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
|
|
if(*psafetyGroup) {
|
|
try {
|
|
(*psafetyGroup)->validate();
|
|
}
|
|
catch(std::exception& e) {
|
|
printf("Exception: %s. Plugin will unload.\n",e.what());
|
|
return ECMC_PLUGIN_SAFETY_ERROR_CODE;
|
|
}
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int executeSafetyGroups() {
|
|
|
|
for(std::vector<ecmcSafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
|
|
if(*psafetyGroup) {
|
|
try {
|
|
(*psafetyGroup)->execute();
|
|
}
|
|
catch(std::exception& e) {
|
|
printf("Exception: %s. Plugin will unload.\n",e.what());
|
|
return ECMC_PLUGIN_SAFETY_ERROR_CODE;
|
|
}
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* EPICS iocsh shell command: ecmcAddSafetyGroup
|
|
*/
|
|
void ecmcAddSafetyGroupPrintHelp() {
|
|
printf("\n");
|
|
printf(" Use ecmcAddSafetyGroup(<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");
|
|
printf(" <time_delay_ms> : Time delay of STO [ms].\n");
|
|
printf("\n");
|
|
}
|
|
|
|
int ecmcAddSafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) {
|
|
if(!name) {
|
|
printf("Error: name.\n");
|
|
ecmcAddSafetyGroupPrintHelp();
|
|
return asynError;
|
|
}
|
|
|
|
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
|
|
ecmcAddSafetyGroupPrintHelp();
|
|
return asynSuccess;
|
|
}
|
|
|
|
if(!ec_rampdown_cmd) {
|
|
printf("Error: ec_rampdown_cmd ethercat entry not defined.\n");
|
|
ecmcAddSafetyGroupPrintHelp();
|
|
return asynError;
|
|
}
|
|
|
|
if(!ec_standstill_status) {
|
|
printf("Error: ec_standstill_status ethercat entry not defined.\n");
|
|
ecmcAddSafetyGroupPrintHelp();
|
|
return asynError;
|
|
}
|
|
if(time_delay_ms <= 0) {
|
|
printf("Error: time_delay invalid.\n");
|
|
exit (1);
|
|
}
|
|
|
|
try {
|
|
createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms);
|
|
}
|
|
catch(std::exception& e) {
|
|
printf("Exception: %s. Add safety group failed.\n",e.what());
|
|
exit (1);
|
|
}
|
|
|
|
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 =
|
|
{ "STO delay [ms]", iocshArgInt };
|
|
|
|
static const iocshArg *const initArgs_1[] = { &initArg0_1,
|
|
&initArg1_1,
|
|
&initArg2_1,
|
|
&initArg3_1};
|
|
|
|
static const iocshFuncDef initFuncDef_1 = { "ecmcAddSafetyGroup", 4, initArgs_1 };
|
|
static void initCallFunc_1(const iocshArgBuf *args) {
|
|
ecmcAddSafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].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(" <velo limit> : Axis standstill velo limit [unit of axis].\n");
|
|
printf(" <time> : Time for axis to be below velo limit [ms].\n");
|
|
printf("\n");
|
|
}
|
|
|
|
int ecmcAddAxisToSafetyGroup(const char* name, int axis_id, double velo_lim, int stand_still_time) {
|
|
if(!name) {
|
|
printf("Error: name.\n");
|
|
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 (1);
|
|
}
|
|
|
|
if(velo_lim < 0) {
|
|
printf("Error: Invalid velocity limit.\n");
|
|
exit (1);
|
|
}
|
|
|
|
if(stand_still_time < 0) {
|
|
printf("Error: Invalid stand still filter time.\n");
|
|
exit (1);
|
|
}
|
|
try {
|
|
return addAxisToSafetyGroup(name,axis_id, velo_lim, stand_still_time);
|
|
}
|
|
catch(std::exception& e) {
|
|
printf("Exception: %s. Add axis to safety group failed.\n",e.what());
|
|
exit (1);
|
|
}
|
|
|
|
return asynSuccess;
|
|
}
|
|
|
|
static const iocshArg initArg0_2 =
|
|
{ "Group name", iocshArgString };
|
|
static const iocshArg initArg1_2 =
|
|
{ "Axis id []", iocshArgInt };
|
|
static const iocshArg initArg2_2 =
|
|
{ "Velo limit [unit same as axis cfg]", iocshArgDouble };
|
|
static const iocshArg initArg3_2 =
|
|
{ "Velo stand still filter time [ms]", iocshArgInt };
|
|
|
|
static const iocshArg *const initArgs_2[] = { &initArg0_2,
|
|
&initArg1_2,
|
|
&initArg2_2,
|
|
&initArg3_2};
|
|
|
|
static const iocshFuncDef initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 4, initArgs_2};
|
|
static void initCallFunc_2(const iocshArgBuf *args) {
|
|
ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival, args[2].dval, args[3].ival);
|
|
}
|
|
|
|
void ecmcSafetyPlgRegister(void) {
|
|
iocshRegister(&initFuncDef_1, initCallFunc_1);
|
|
iocshRegister(&initFuncDef_2, initCallFunc_2);
|
|
}
|
|
|
|
epicsExportRegistrar(ecmcSafetyPlgRegister);
|