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

WIP

Browse Source
This commit is contained in:
sandst_a
2024-02-02 15:46:12 +01:00
parent 343d5dcc7e
commit 0d2c06a846
3 changed files with 114 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/ecmcPluginSafety.c
View File

@@ -64,7 +64,7 @@ void safetyDestruct(void)
**/
int safetyRealtime(int ecmcError)
{
if(destructs_) return;
if(destructs_) return 0;
executeSafetyGroups();
lastEcmcError = ecmcError;

129
src/ecmcSafetyGroup.cpp
View File

@@ -67,8 +67,11 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
axesCounter_ = 0;
ecmcSampleRateHz_ = getEcmcSampleRate();
dataSourcesLinked_ = 0;
dataItemRampDownCmd_ = NULL;
dataItemStandStillStat_ = NULL;
//dataItemRampDownCmd_ = NULL;
//dataItemStandStillStat_ = NULL;
ecMaster_ = NULL;
ecEntryRampDown_ = NULL;
ecEntryStandstill_ = NULL;
masterId_ = 0;
slaveIdRampDown_ = 0;
bitRampDown_ = 0;
@@ -76,6 +79,7 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
bitStandStill_ = 0;
aliasRampDown_[0] = 0;
aliasStandStill_[0] = 0;
rampDownCmd_ = 0;
// Config defaults
cfgDbgMode_ = 0;
@@ -247,27 +251,56 @@ void ecmcSafetyGroup::validateAxes() {
}
void ecmcSafetyGroup::connectToDataSources() {
if( dataSourcesLinked_ ) {
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_);
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() {
@@ -294,7 +327,50 @@ std::string ecmcSafetyGroup::to_string(int value) {
// Executed by ecmc rt thread.
void ecmcSafetyGroup::execute() {
// xxx
uint64_t data = 1;
if(!ecEntryRampDown_) {
return;
}
if(ecEntryRampDown_->readBit(bitRampDown_,
&data)) {
throw std::out_of_range("Safety: Read rampdown cmd failed");
}
rampDownCmd_ = data > 0;
if(rampDownCmd_) {
// set safety interlock in ecmc
setSafetyInterlocks();
}
/*
//Needed functions in motion.h
int getAxisBusy(int axisIndex,
int *value);
int getAxisEncVelo(int axisIndex,
double *velo);
int getAxisTrajVelo(int axisIndex,
double *velo);
int getAxisEnabled(int axisIndex,
int *value);
int setAxisEnable(int axisIndex,
int value);
int setAxisClearEmergencyInterlock(int axisIndex);
int setAxisEmergencyStopInterlock(int axisIndex,
double deceleration);
*/
}
// Set safety interlock
void ecmcSafetyGroup::setSafetyInterlocks() {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
if(!*saxis) {
throw std::runtime_error( "Safety: Axis object NULL.");
}
setAxisEmergencyStopInterlock((*saxis)->axisIndex_,1000);
}
}
asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
@@ -309,16 +385,13 @@ asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) {
ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId);
if(axis) {
safetyAxis* saxis = new safetyAxis(axis, axisId, veloLimit, standStillTimeMs);
axes_.push_back(saxis);
axesCounter_++;
} else {
throw std::out_of_range("Safety: Invalid axis id");
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: Added axis %d to safety group \"%s\"\n",axisId,sName_);
}

25
src/ecmcSafetyGroup.h
View File

@@ -18,23 +18,21 @@
#include "ecmcSafetyPlgDefs.h"
#include <string>
#include "ecmcAxisBase.h"
#include "ecmcEc.h"
#include <vector>
class safetyAxis {
public:
safetyAxis(ecmcAxisBase* axis,
int axisIndex,
safetyAxis(int axisIndex,
double veloLimit,
int standStillTimeMs) {
veloLimit_ = veloLimit;
axisIndex_ = axisIndex;
axis_ = axis;
axisIndex_ = axisIndex;
standStillTimeMs_ = standStillTimeMs;
}
double veloLimit_;
int axisIndex_;
int standStillTimeMs_;
ecmcAxisBase* axis_;
int standStillTimeMs_;
};
class ecmcSafetyGroup : public asynPortDriver {
@@ -66,13 +64,14 @@ class ecmcSafetyGroup : public asynPortDriver {
void validateAxes();
void stripBits();
void connectToDataSources();
void setSafetyInterlocks();
void parseConfigStr(const char *configStr);
void initAsyn();
double ecmcSampleRateHz_;
int dataSourcesLinked_; // To avoid link several times
int objectId_; // Unique object id
int cycleCounter_;
int rampDownCmd_;
// Config options
int cfgDbgMode_; // Config: allow dbg printouts
@@ -80,7 +79,7 @@ class ecmcSafetyGroup : public asynPortDriver {
int asynStatusId_;
int status_;
int axesCounter_;
std::vector<ecmcAxisBase*> axes_;
std::vector<safetyAxis*> axes_;
char* sName_;
char* sEcRampDownCmdNameOrg_;
@@ -88,8 +87,8 @@ class ecmcSafetyGroup : public asynPortDriver {
char* sEcRampDownCmdNameStrip_;
char* sEcAxesStandStillStatStrip_;
char* sConfig_;
ecmcDataItem *dataItemRampDownCmd_;
ecmcDataItem *dataItemStandStillStat_;
//ecmcDataItem *dataItemRampDownCmd_;
//ecmcDataItem *dataItemStandStillStat_;
int delayMs_;
int masterId_;
int slaveIdRampDown_;
@@ -98,9 +97,9 @@ class ecmcSafetyGroup : public asynPortDriver {
int bitStandStill_;
char aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
char aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
std::vector<safetyAxis*> axes_;
ecmcEc *ecMaster_;
ecmcEcEntry *ecEntryRampDown_;
ecmcEcEntry *ecEntryStandstill_;
// Some generic utility functions
static uint8_t getUint8(uint8_t* data);
static int8_t getInt8(uint8_t* data);