lin-epics-modules/sinqepicsapp
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- Many improvements to the MasterMACS driver

Browse Source 
- Slowing down the pmac driver
- Fixing a bug in the Nanotec driver which caused an IOC crash when the
  motor sends bad data
This commit is contained in:
koennecke
2023-08-28 15:01:48 +02:00
parent 7a81e2c5a0
commit e3ac2962f5
14 changed files with 928 additions and 698 deletions
Download Patch File Download Diff File Expand all files Collapse all files

245
sinqEPICSApp/src/MasterMACSDriver.cpp
View File

@ -3,7 +3,7 @@
For documentation see the standard SINQ place for hardware documentation or
Marcel Schildt
Mark Koennecke, March 2023
Mark Koennecke, March-August 2023
*/
@ -13,6 +13,7 @@
#include <math.h>
#include <ctype.h>
#include <time.h>
#include <unistd.h>
#include <iocsh.h>
#include <epicsThread.h>
@ -23,8 +24,6 @@
#include "MasterMACSDriver.h"
#include <epicsExport.h>
#define IDLEPOLL 60
#define CHECK_BIT(var,pos) ((var) & (1 << pos))
#define ABS(x) (x < 0 ? -(x) : (x))
@ -37,8 +36,8 @@
*/
MasterMACSController::MasterMACSController(const char *portName,
const char *MasterMACSPortName,
int
numAxes):SINQController
int numAxes,
int idlePoll, int busyPoll):SINQController
(portName, MasterMACSPortName, numAxes)
{
asynStatus status;
@ -66,8 +65,10 @@ MasterMACSController::MasterMACSController(const char *portName,
createParam(EnableAxisString, asynParamInt32, &enableAxis_);
createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
callParamCallbacks();
startPoller(1000. / 1000., IDLEPOLL, 2);
startPoller(busyPoll/1000., idlePoll/1000., 1);
setIdlePollPeriod(idlePoll/1000.);
setMovingPollPeriod(busyPoll/1000.);
}
@ -76,13 +77,15 @@ MasterMACSController::MasterMACSController(const char *portName,
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] MasterMACSPortName The name of the drvAsynIPPPort that was created previously to connect to the MasterMACS controller
* \param[in] numAxes The number of axes that this controller supports
* \param[in] idlePoll Poll interval when idle in microseconds
* \param[in] busyPoll Poll interval when moving in microSeconds
*/
extern "C" int
MasterMACSCreateController(const char *portName,
const char *MasterMACSPortName, int numAxes)
const char *MasterMACSPortName, int numAxes, int idlePoll, int busyPoll)
{
MasterMACSController *pMasterMACSController
= new MasterMACSController(portName, MasterMACSPortName, numAxes);
= new MasterMACSController(portName, MasterMACSPortName, numAxes, idlePoll, busyPoll);
pMasterMACSController = NULL;
return (asynSuccess);
}
@ -147,7 +150,7 @@ asynStatus
/* read with a short timeout in order to remove duplicate messages
* from the line. This also serves to slow down communication
*/
pasynOctetSyncIO->read(pasynUserController_, mmacsResponse, 35, .4, &in, &reason);
pasynOctetSyncIO->read(pasynUserController_, mmacsResponse, 35, .05, &in, &reason);
/* pack data for MasterMACS */
len = strlen(command) + 6;
@ -166,7 +169,9 @@ asynStatus
/* 0x03 is appended by asyn */
/* send the stuff away ... */
errlogSevPrintf(errlogMajor,"Sending command: %s\n", command);
if(debug) {
errlogSevPrintf(errlogMajor,"Sending command: %s\n", command);
}
status =
pasynOctetSyncIO->writeRead(pasynUserController_, mmacsData,
@ -175,8 +180,8 @@ asynStatus
if (status != asynSuccess) {
if (axis != NULL) {
errlogSevPrintf(errlogMajor,
"Lost connection to motor controller, reason %d",
reason);
"Lost connection to motor controller,a axis %d, reason %d",
axisNo, reason);
axis->updateMsgTxtFromDriver
("Lost connection to motor controller");
return status;
@ -252,6 +257,8 @@ asynStatus
char command[64] = { 0 };
char response[64] = { 0 };
int devStatus;
bool moving;
time_t startTime;
pAxis = (MasterMACSAxis *) this->getAxis(pasynUser);
if (!pAxis) {
@ -268,9 +275,10 @@ asynStatus
*/
devStatus = pAxis->readStatus();
if(devStatus < 900){
errlogPrintf("MMACS: Motor %d not ready to switch on\n", pAxis->axisNo_);
return asynError;
}
if (value == 1 && !pAxis->isOn(devStatus) ) {
if (value == 1 && !pAxis->isOn(devStatus) ) {
/* download parameters, does not work as of now */
/*
sprintf(command, "%dS85=1.", pAxis->axisNo_);
@ -280,64 +288,36 @@ asynStatus
/* actually enable */
sprintf(command, "%dS04=1", pAxis->axisNo_);
status = transactController(pAxis->axisNo_, command, response);
} else {
if(pAxis->isOn(devStatus)) {
// only send command when necessary
sprintf(command, "%dS04=0", pAxis->axisNo_);
status = transactController(pAxis->axisNo_, command, response);
}
} else if(pAxis->isOn(devStatus)) {
// only send command when necessary
sprintf(command, "%dS04=0", pAxis->axisNo_);
status = transactController(pAxis->axisNo_, command, response);
} else {
// nothing to do
return status;
}
if (status == asynSuccess) {
pAxis->updateMsgTxtFromDriver("");
} else {
errlogPrintf("Failure to enable or disable axis %d",
errlogPrintf("MMACS: Failure to enable or disable axis %d",
pAxis->axisNo_);
}
startTime = time(NULL);
while(time(NULL) < startTime + 2.){
// wait for the change to happen
devStatus = pAxis->readStatus();
if(pAxis->isOn(devStatus) == value){
pAxis->active = true;
pAxis->poll(&moving); // to update the Enable_RBV field
return asynSuccess;
}
usleep(200);
}
errlogPrintf("MMACS: Failed to enable motor %d within 2 seconds\n", pAxis->axisNo_);
}
return asynMotorController::writeInt32(pasynUser, value);
}
asynStatus
MasterMACSController::readInt32(asynUser * pasynUser,
epicsInt32 * value)
{
int function = pasynUser->reason;
MasterMACSAxis *pAxis = NULL;
char command[128] = { 0 }, reply[128] = {
0}, *pPtr;
float fval;
int devStatus, isOn, comStatus;
pAxis = (MasterMACSAxis *) (this->getAxis(pasynUser));
if (!pAxis) {
return asynError;
}
if (function == axisEnabled_) {
// Read the overall status of this motor */
sprintf(command, "%dR10", pAxis->axisNo_);
comStatus = transactController(pAxis->axisNo_, command, reply);
if (comStatus == asynError) {
return asynError;
}
pPtr = strstr(reply, "=");
if(pPtr) {
sscanf(pPtr + 1, "%f", &fval);
devStatus = (int) fval;
} else {
devStatus = 0;
errlogPrintf(" Bad reply %s when trying to read motor status %d\n", reply, pAxis->axisNo_);
}
isOn = pAxis->isOn(devStatus);
/* errlogPrintf("isOn in readInt32: %d, devStatus = %d\n", isOn, devStatus); */
setIntegerParam(axisEnabled_, isOn);
*value = isOn;
callParamCallbacks();
return asynSuccess;
}
return asynMotorController::readInt32(pasynUser, value);
}
// These are the MasterMACSAxis methods
@ -351,9 +331,8 @@ MasterMACSAxis::MasterMACSAxis(MasterMACSController * pC, int axisNo):SINQAxis(p
pC_
(pC)
{
next_poll = -1;
hasStarted = false;
errorReported = 1;
active = false;
}
/** Reports on status of the axis
@ -373,17 +352,27 @@ int MasterMACSAxis::readStatus()
{
char command[COMLEN], reply[COMLEN], *pPtr;
float fval;
int devStatus, status;
int status;
/*
* The MasterMACS sends invalid responses with a low frequency.
* Therefore I send cached status responses in such a case in order
* to help the logic evrywhere else in the code.
*/
sprintf(command, "%dR10", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
if (status == asynError) {
return -1000;
return oldStatus;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &fval);
devStatus = (int) fval;
return devStatus;
if(pPtr) {
sscanf(pPtr + 1, "%f", &fval);
oldStatus = (int) fval;
return oldStatus;
} else {
errlogPrintf("MMACS: invalid status reponse %s on axis %d\n", reply, axisNo_);
return oldStatus;
}
}
@ -463,9 +452,8 @@ asynStatus
return status;
}
hasStarted = true;
next_poll = -1;
homing = 0;
errorReported = 0;
active = true;
return status;
}
@ -496,14 +484,13 @@ asynStatus
setIntegerParam(pC_->motorStatusProblem_, false);
updateMsgTxtFromDriver("");
errorReported = 0;
sprintf(command, "%dS00=9", axisNo_);
homing = 1;
next_poll = -1;
status = pC_->transactController(axisNo_, command, reply);
hasStarted = true;
active = true;
return status;
}
@ -525,16 +512,13 @@ asynStatus MasterMACSAxis::stop(double acceleration)
memset(command, 0, COMLEN * sizeof(char));
if (errorReported == 0) {
devStatus = readStatus();
if(!CHECK_BIT(devStatus, 10)) {
// only try to stop when running ...
sprintf(command, "%dS00=8", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
updateMsgTxtFromDriver("Axis interrupted");
errorReported = 1;
}
devStatus = readStatus();
if(!CHECK_BIT(devStatus, 10)) {
// only try to stop when running ...
sprintf(command, "%dS00=8", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
updateMsgTxtFromDriver("Axis interrupted");
}
return status;
}
@ -567,21 +551,33 @@ asynStatus MasterMACSAxis::setClosedLoop(bool closedLoop)
asynStatus MasterMACSAxis::poll(bool * moving)
{
asynStatus comStatus = asynSuccess;
char command[COMLEN], reply[COMLEN], *pPtr;
char command[COMLEN], reply[COMLEN], *pPtr, buffer[80];
float errStatus;
unsigned int errCode, derCode, devStatus;
// protect against excessive polling
if (time(NULL) < next_poll) {
*moving = false;
return asynSuccess;
struct tm* tm_info;
time_t timer;
/*
* EPICS always polls all motors on a controller when one motor is active.
* In order to reduce load on the controller we check if we are active and
* if not return old stuff
*/
if(active == false && time(NULL) < lastPoll + pC_->idlePollPeriod_) {
*moving = false;
return asynSuccess;
}
timer = time(NULL);
tm_info = localtime(&timer);
strftime(buffer, 26, "%Y-%m-%d %H:%M:%S", tm_info);
errlogPrintf("poll called at %s on axis %d \n",
buffer, axisNo_ );
setIntegerParam(pC_->motorStatusProblem_, false);
memset(command, 0, COMLEN * sizeof(char));
// Read the current motor position
sprintf(command, "%dR12", axisNo_);
comStatus = pC_->transactController(axisNo_, command, reply);
@ -595,12 +591,20 @@ asynStatus MasterMACSAxis::poll(bool * moving)
} else {
errlogPrintf("Received malformed reply: Axis %d, reply %s\n",
axisNo_, reply + 4);
return asynError;
comStatus = asynError;
goto skip;
}
setDoubleParam(pC_->motorPosition_, position * 1000.);
setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);
/*
* keep track of position in order to check for a stuck motor later
*/
if(ABS(position - oldPosition) > 1){
oldPosition = position;
lastPositionUpdate = time(NULL);
}
// Read the overall status of this motor */
devStatus = readStatus();
@ -614,6 +618,7 @@ asynStatus MasterMACSAxis::poll(bool * moving)
position, devStatus);
}
setIntegerParam(pC_->axisEnabled_, isOn(devStatus));
if (!isOn(devStatus)) {
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Motor disabled");
@ -628,7 +633,6 @@ asynStatus MasterMACSAxis::poll(bool * moving)
if (!hasStarted) {
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
next_poll = time(NULL) + IDLEPOLL;
goto skip;
}
@ -636,18 +640,23 @@ asynStatus MasterMACSAxis::poll(bool * moving)
* We may have a valid status bit...
*/
if (!CHECK_BIT(devStatus, 10)) {
/* we are still creeping along .... */
*moving = true;
next_poll = -1;
setIntegerParam(pC_->motorStatusDone_, false);
goto skip;
/* we are still creeping along .... */
*moving = true;
setIntegerParam(pC_->motorStatusDone_, false);
if(time(NULL) > lastPositionUpdate + 60) {
// we are detecting a stuck motor
errlogPrintf("MMACS: axis %d is STUCK!!\n", axisNo_);
updateMsgTxtFromDriver("Axis STUCK!!");
setIntegerParam(pC_->motorStatusProblem_, true);
}
goto skip;
}
/*we are done moving */
*moving = false;
active = false;
setIntegerParam(pC_->motorStatusDone_, true);
next_poll = time(NULL) + IDLEPOLL;
/* when homing, set the proper flag */
if (homing) {
@ -655,7 +664,7 @@ asynStatus MasterMACSAxis::poll(bool * moving)
}
/* check for limit switches*/
setIntegerParam(pC_->motorStatusLowLimit_, false);
setIntegerParam(pC_->motorStatusLowLimit_, false);
setIntegerParam(pC_->motorStatusHighLimit_, false);
if (CHECK_BIT(devStatus, 11)) {
setIntegerParam(pC_->motorStatusProblem_, true);
@ -685,9 +694,15 @@ asynStatus MasterMACSAxis::poll(bool * moving)
goto skip;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &errStatus);
errCode = (unsigned int) errStatus;
if(pPtr) {
sscanf(pPtr + 1, "%f", &errStatus);
errCode = (unsigned int) errStatus;
} else {
errlogPrintf("MMACS: axis %d received invalid reply asking for error code \n", axisNo_);
updateMsgTxtFromDriver("Invalid reply asking error code R11");
errCode = 0;
goto skip;
}
sprintf(command, "%dR18", axisNo_);
comStatus = pC_->transactController(axisNo_, command, reply);
if (comStatus == asynError) {
@ -695,8 +710,14 @@ asynStatus MasterMACSAxis::poll(bool * moving)
goto skip;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &errStatus);
derCode = (unsigned int) errStatus;
if(pPtr) {
sscanf(pPtr + 1, "%f", &errStatus);
derCode = (unsigned int) errStatus;
} else {
errlogPrintf("MMACS: malformed reply for R18: %s, on axis %d\n", reply, axisNo_);
derCode = 0;
updateMsgTxtFromDriver("Invalid reply asking error code R18");
}
if(debug) {
errlogPrintf("Axis %d, errCode(R11) %d, derCode(R18) %d\n", axisNo_,
@ -784,18 +805,28 @@ static const iocshArg
static const iocshArg
MasterMACSCreateControllerArg2 = { "Number of axes", iocshArgInt };
static const iocshArg
MasterMACSCreateControllerArg3 = { "idlePoll", iocshArgInt };
static const iocshArg
MasterMACSCreateControllerArg4 = { "busyPoll", iocshArgInt };
static const iocshArg *const
MasterMACSCreateControllerArgs[] = { &MasterMACSCreateControllerArg0,
&MasterMACSCreateControllerArg1,
&MasterMACSCreateControllerArg2
&MasterMACSCreateControllerArg2,
&MasterMACSCreateControllerArg3,
&MasterMACSCreateControllerArg4
};
static const iocshFuncDef
MasterMACSCreateControllerDef =
{ "MasterMACSCreateController", 3, MasterMACSCreateControllerArgs };
{ "MasterMACSCreateController", 5, MasterMACSCreateControllerArgs };
static void MasterMACSCreateContollerCallFunc(const iocshArgBuf * args)
{
MasterMACSCreateController(args[0].sval, args[1].sval, args[2].ival);
MasterMACSCreateController(args[0].sval, args[1].sval, args[2].ival,
args[3].ival, args[4].ival);
}
/**

15
sinqEPICSApp/src/MasterMACSDriver.h
View File

@ -33,13 +33,18 @@ private:
MasterMACSController *pC_; /**< Pointer to the asynMotorController to which this axis belongs.
* Abbreviated because it is used very frequently */
double position;
double oldPosition;
time_t lastPositionUpdate;
int homing;
time_t next_poll;
int errorReported;
int hasStarted; /* The motor status is invalid if the thing has not run once */
int isOn(int axisStatus);
int readStatus();
int errorCodeFound;
int oldStatus;
bool active;
time_t lastPoll;
friend class MasterMACSController;
};
@ -49,7 +54,8 @@ friend class MasterMACSController;
class MasterMACSController : public SINQController {
public:
MasterMACSController(const char *portName, const char *MasterMACSPortName, int numAxes);
MasterMACSController(const char *portName, const char *MasterMACSPortName,
int numAxes, int idlePoll, int busyPoll);
void report(FILE *fp, int level);
MasterMACSAxis* getAxis(asynUser *pasynUser);
@ -58,9 +64,6 @@ public:
// overloaded because we want to enable/disable the motor
asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
// overloaded because we want to read the axis state
asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
friend class MasterMACSAxis;
private:
asynUser *pasynUserController_;

20
sinqEPICSApp/src/NanotecDriver.cpp
View File

@ -377,7 +377,7 @@ asynStatus NanotecAxis::poll(bool *moving)
asynStatus comStatus;
char command[COMLEN], reply[COMLEN];
char *pPtr;
int posVal, statVal;
int posVal, statVal, count = 0;
double lowLim, highLim;
@ -393,11 +393,12 @@ asynStatus NanotecAxis::poll(bool *moving)
if(comStatus) goto skip;
pPtr = strchr(reply,'C');
pPtr++;
if(pPtr){
posVal = atoi(pPtr);
} else {
errlogPrintf("Invalid response %s for #C received for axis %d\n", reply, axisNo_);
pPtr++;
count = sscanf(pPtr,"%d", &posVal);
}
if(pPtr == NULL || count < 1) {
errlogPrintf("Invalid response %s for #C received for axis %d, address %d\n", reply, axisNo_, busAddress);
return asynError;
}
@ -412,11 +413,12 @@ asynStatus NanotecAxis::poll(bool *moving)
if(comStatus) goto skip;
pPtr = strchr(reply,'$');
pPtr++;
if(pPtr) {
statVal = atoi(pPtr);
} else {
errlogPrintf("Invalid response %s for #$ received for axis %d\n", reply, axisNo_);
pPtr++;
count = sscanf(pPtr, "%d", &statVal);
}
if(pPtr == NULL || count < 1) {
errlogPrintf("Invalid response %s for #$ received for axis %d busAddress %d\n", reply, axisNo_, busAddress);
return asynError;
}
//errlogPrintf("Axis %d, reply %s, statVal = %d\n",

18
sinqEPICSApp/src/pmacAxis.cpp
View File

@ -185,7 +185,7 @@ asynStatus pmacAxis::getAxisInitialStatus(void) {
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
if (cmdStatus) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: Error: enaabling axis %d failed.\n", functionName,
"%s: Error: enabling axis %d failed.\n", functionName,
axisNo_);
return asynError;
}
@ -911,11 +911,6 @@ asynStatus pmacV3Axis::poll(bool *moving) {
static const char *functionName = "pmacV3Axis::poll";
char message[132];
// Protect against excessive polling
if (time(NULL) < next_poll) {
return asynSuccess;
}
sprintf(message, "%s: Polling axis: %d", functionName, this->axisNo_);
pC_->debugFlow(message);
@ -927,12 +922,6 @@ asynStatus pmacV3Axis::poll(bool *moving) {
functionName, pC_->portName, axisNo_);
}
if (*moving) {
next_poll = time(NULL) + BUSYPOLL;
} else {
next_poll = time(NULL) + IDLEPOLL;
}
callParamCallbacks();
return status ? asynError : asynSuccess;
}
@ -966,7 +955,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
pmacV3Controller *p3C_ = (pmacV3Controller *)pC_;
IsEnable = axStat != -3;
asynStatus st = setIntegerParam(p3C_->axisEnabled_, axStat > -1);
asynStatus st = setIntegerParam(p3C_->axisEnabled_, IsEnable);
cmdStatus = cmdStatus > st ? cmdStatus : st;
int direction = 0;
@ -993,7 +982,8 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
previous_direction_ = direction;
/* are we done? */
if ((axStat == 0 || axStat < 0) && starting == 0) {
/* if ((axStat == 0 || axStat < 0) && starting == 0) { */
if (axStat == 0 && starting == 0) {
done = 1;
} else {
starting = 0;

1138
sinqEPICSApp/src/pmacController.cpp
View File

File diff suppressed because it is too large Load Diff