- Many improvements to the MasterMACS driver
- 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:
@@ -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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
Reference in New Issue
Block a user