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

Reduce polling, se MsgTxt if move when disabled

Browse Source
This commit is contained in:
brambilla_m
2022-03-22 11:01:06 +01:00
parent ad05433602
commit f5da0d54bd
3 changed files with 100 additions and 259 deletions
Download Patch File Download Diff File Expand all files Collapse all files

339
sinqEPICSApp/src/pmacAxis.cpp
View File

@ -43,13 +43,7 @@
#include <string.h>
#include <time.h>
#include <typeinfo>
#include "pmacController.h"
template <typename T>
const char *getClassName(T) {
return typeid(T).name();
}
#define MULT 1000.
@ -907,212 +901,16 @@ asynStatus pmacV3Axis::poll(bool *moving) {
return status ? asynError : asynSuccess;
}
// asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
// char command[pC_->PMAC_MAXBUF_];
// char response[pC_->PMAC_MAXBUF_];
// int cmdStatus = 0;
// int done = 0, posChanging = 0;
// double position = 0;
// int nvals = 0;
// int axisProblemFlag = 0;
// epicsUInt32 axErr = 0, highLim = 0, lowLim = 0, axDone = 0;
// int axStat=0;
// char message[132], *axMessage;
// static const char *functionName = "pmacV3Axis::getAxisStatus";
// pmacV3Controller* p3C_ = (pmacV3Controller*)pC_;
// sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_);
// cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
// nvals = sscanf(response, "%lf %d %d", &position, &axStat, &axDone);
// if (cmdStatus || nvals != 3) {
// asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
// "drvPmacAxisGetStatus: Failed to read position and status, "
// "Status: %d\nCommand :%s\nResponse:%s\n",
// cmdStatus, command, response);
// updateMsgTxtFromDriver("Cannot read Axis position and status");
// }
// int st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0);
// // st = callParamCallbacks();
// // printf("axStat: callParamCallbacks -> ok: %d\n", st == asynSuccess);
// setDoubleParam(pC_->motorPosition_, position * MULT);
// setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
// /* Use previous position and current position to calculate direction.*/
// int direction = previous_direction_;
// if ((position - previous_position_) > 0) {
// direction = 1;
// }
// if ((position - previous_position_) < 0) {
// direction = 0;
// }
// setIntegerParam(pC_->motorStatusDirection_, direction);
// /*Store position to calculate direction for next poll.*/
// previous_position_ = position;
// previous_direction_ = direction;
// // errlogPrintf("Polling, axStat = %d, position = %f\n", axStat, position);
// /* are we done? */
// if ( axDone == 0 && starting == 0) {
// done = 1;
// } else {
// starting = 0;
// done = 0;
// }
// if (starting && time(NULL) > startTime + 10) {
// /*
// did not start in 10 seconds: messed up: cause an alarm
// */
// done = 1;
// *moving = false;
// setIntegerParam(pC_->motorStatusMoving_, false);
// setIntegerParam(pC_->motorStatusDone_, true);
// setIntegerParam(pC_->motorStatusProblem_, true);
// errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n",
// axisNo_); updateMsgTxtFromDriver("Axis did not start within 10 seconds");
// starting = 0;
// callParamCallbacks();
// return asynSuccess;
// }
// /*
// code to test against too long status 5 or 6
// */
// if (axStat == 5 || axStat == 6) {
// if (status6Time == 0) {
// status6Time = time(NULL);
// statusPos = position;
// } else {
// if (time(NULL) > status6Time + 60) {
// /* trigger error only when not moving */
// if (abs(position - statusPos) < .1) {
// done = 1;
// *moving = false;
// setIntegerParam(pC_->motorStatusMoving_, false);
// setIntegerParam(pC_->motorStatusDone_, true);
// setIntegerParam(pC_->motorStatusProblem_, true);
// errlogPrintf(
// "Axis %d stayed in 5,6 for more then 60 seconds BROKEN\n",
// axisNo_);
// updateMsgTxtFromDriver(
// "Axis stayed in 5,6 for more then 60 seconds: BROKEN");
// status6Time = 0;
// callParamCallbacks();
// return asynSuccess;
// } else {
// status6Time = time(NULL);
// statusPos = position;
// }
// }
// }
// }
// if (!done) {
// *moving = true;
// setIntegerParam(pC_->motorStatusMoving_, true);
// setIntegerParam(pC_->motorStatusDone_, false);
// } else {
// *moving = false;
// setIntegerParam(pC_->motorStatusMoving_, false);
// setIntegerParam(pC_->motorStatusDone_, true);
// if (homing) {
// setIntegerParam(pC_->motorStatusHomed_, done);
// homing = 0;
// }
// }
// sprintf(message, "poll results: axis %d, status %d, done = %d",
// axisNo_, axStat, axDone);
// pC_->debugFlow(message);
// if (*moving == false) {
// sprintf(command, "P%2.2d01 P%2.2d37 P%2.2d38", axisNo_, axisNo_,
// axisNo_); cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
// nvals = sscanf(response, "%d %d %d", &axErr, &highLim, &lowLim);
// if (cmdStatus || nvals != 1) {
// asynPrint(
// pC_->pasynUserSelf, ASYN_TRACE_ERROR,
// "drvPmacAxisGetStatus: Failed to read axis Error, high and low
// limit " "flags Status: %d\nCommand :%s\nResponse:%s\n",
// cmdStatus, command, response);
// updateMsgTxtFromDriver(
// "Cannot read Axis Error, high and low limit flag Status");
// }
// // errlogPrintf("Polling, axErr = %d, highLim = %d, lowLim = %d\n",
// axStat,
// // axErr, highLim, lowLim);
// sprintf(message, "poll results: axis %d, axErr %d", axisNo_, axErr);
// pC_->debugFlow(message);
// /* search for trouble */
// if (highLim) {
// setIntegerParam(pC_->motorStatusHighLimit_, true);
// updateMsgTxtFromDriver("High Limit Hit");
// } else {
// setIntegerParam(pC_->motorStatusHighLimit_, false);
// }
// if (lowLim) {
// setIntegerParam(pC_->motorStatusLowLimit_, true);
// updateMsgTxtFromDriver("Low Limit Hit");
// } else {
// setIntegerParam(pC_->motorStatusLowLimit_, false);
// }
// if (axErr == 11) {
// setIntegerParam(pC_->motorStatusFollowingError_, true);
// updateMsgTxtFromDriver("Following error triggered");
// } else {
// setIntegerParam(pC_->motorStatusFollowingError_, false);
// }
// /* Set any axis specific general problem bits. */
// }
// if ((axStat < 0 && axStat != -3) || axErr != 0) {
// axisProblemFlag = 1;
// if (axisErrorCount < 10) {
// axMessage = translateAxisError(axErr);
// printf("axErr: %d\taxStat: %d\taxMessage: %s\n", axErr, axStat,
// axMessage); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
// "drvPmacAxisGetStatus: Axis %d is in deep trouble: axis error
// " "code %d, translated: %s:, status code = %d\n", axisNo_,
// axErr, axMessage, axStat);
// snprintf(message, sizeof(message), "PMAC Axis error: %s", axMessage);
// updateMsgTxtFromDriver(message);
// if (axMessage != NULL) {
// free(axMessage);
// }
// axisErrorCount++;
// } else if (axisErrorCount == 10) {
// asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
// "Suppressing further axis error messages\n");
// axisErrorCount++;
// }
// } else {
// axisProblemFlag = 0;
// axisErrorCount = 0;
// }
// setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
// callParamCallbacks();
// return asynSuccess;
// }
asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
char command[pC_->PMAC_MAXBUF_];
char response[pC_->PMAC_MAXBUF_];
int cmdStatus = 0;
asynStatus cmdStatus = asynSuccess;
int done = 0, posChanging = 0;
double position = 0;
int nvals = 0;
int axisProblemFlag = 0, axStat = 0, axDone;
int axisProblemFlag = 0, axStat = 0;
int acknowledge = 0;
epicsUInt32 axErr = 0, highLim = 0, lowLim = 0;
char message[132], *axMessage;
@ -1120,7 +918,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%lf %d %d", &position, &axStat, &axDone);
nvals = sscanf(response, "%lf %d %d", &position, &axStat, &acknowledge);
if (cmdStatus || nvals != 3) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read position and status, "
@ -1130,25 +928,22 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
}
pmacV3Controller *p3C_ = (pmacV3Controller *)pC_;
int st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0);
sprintf(command, "P%2.2d01 P%2.2d37 P%2.2d38", axisNo_, axisNo_, axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d %d %d", &axErr, &highLim, &lowLim);
if (cmdStatus || nvals != 3) {
asynPrint(
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read axis Error, high and low limit "
"flags Status: %d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
updateMsgTxtFromDriver(
"Cannot read Axis Error, high and low limit flag Status");
}
IsEnable = axStat != -3;
asynStatus st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0);
cmdStatus = cmdStatus > st ? cmdStatus : st;
int direction = 0;
setDoubleParam(pC_->motorPosition_, position * MULT);
setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
st = setDoubleParam(pC_->motorPosition_, position * MULT);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
cmdStatus = cmdStatus > st ? cmdStatus : st;
if(axisNo_ == 2) {
printf("position(%d): %lf\n", axisNo_, position);
printf("ack(%d): %d\n", axisNo_, acknowledge);
}
/* Use previous position and current position to calculate direction.*/
if ((position - previous_position_) > 0) {
@ -1158,15 +953,13 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
} else {
direction = 0;
}
setIntegerParam(pC_->motorStatusDirection_, direction);
st = setIntegerParam(pC_->motorStatusDirection_, direction);
cmdStatus = cmdStatus > st ? cmdStatus : st;
/*Store position to calculate direction for next poll.*/
previous_position_ = position;
previous_direction_ = direction;
// errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat,
// axErr, position);
/* are we done? */
if ((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0) {
done = 1;
@ -1181,13 +974,16 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
*/
done = 1;
*moving = false;
setIntegerParam(pC_->motorStatusMoving_, false);
setIntegerParam(pC_->motorStatusDone_, true);
setIntegerParam(pC_->motorStatusProblem_, true);
st = setIntegerParam(pC_->motorStatusMoving_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setIntegerParam(pC_->motorStatusDone_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setIntegerParam(pC_->motorStatusProblem_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n", axisNo_);
updateMsgTxtFromDriver("Axis did not start within 10 seconds");
starting = 0;
return asynSuccess;
return st;
}
/*
@ -1203,16 +999,20 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
if (abs(position - statusPos) < .1) {
done = 1;
*moving = false;
setIntegerParam(pC_->motorStatusMoving_, false);
setIntegerParam(pC_->motorStatusDone_, true);
setIntegerParam(pC_->motorStatusProblem_, true);
st = setIntegerParam(pC_->motorStatusMoving_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setIntegerParam(pC_->motorStatusDone_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setIntegerParam(pC_->motorStatusProblem_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
errlogPrintf(
"Axis %d stayed in 5,6 for more then 60 seconds BROKEN\n",
axisNo_);
updateMsgTxtFromDriver(
"Axis stayed in 5,6 for more then 60 seconds: BROKEN");
status6Time = 0;
return asynSuccess;
return cmdStatus;
} else {
status6Time = time(NULL);
statusPos = position;
@ -1223,40 +1023,73 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
if (!done) {
*moving = true;
setIntegerParam(pC_->motorStatusMoving_, true);
setIntegerParam(pC_->motorStatusDone_, false);
st = setIntegerParam(pC_->motorStatusMoving_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setIntegerParam(pC_->motorStatusDone_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
} else {
*moving = false;
setIntegerParam(pC_->motorStatusMoving_, false);
setIntegerParam(pC_->motorStatusDone_, true);
st = setIntegerParam(pC_->motorStatusMoving_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setIntegerParam(pC_->motorStatusDone_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
if (homing) {
setIntegerParam(pC_->motorStatusHomed_, done);
st = setIntegerParam(pC_->motorStatusHomed_, done);
cmdStatus = cmdStatus > st ? cmdStatus : st;
homing = 0;
}
}
if (*moving) {
return cmdStatus;
}
sprintf(command, "P%2.2d01 P%2.2d37 P%2.2d38", axisNo_, axisNo_, axisNo_);
st = pC_->lowLevelWriteRead(axisNo_, command, response);
cmdStatus = cmdStatus > st ? cmdStatus : st;
nvals = sscanf(response, "%d %d %d", &axErr, &highLim, &lowLim);
if (st || nvals != 3) {
asynPrint(
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read axis Error, high and low limit "
"flags Status: %d\nCommand :%s\nResponse:%s\n",
st, command, response);
updateMsgTxtFromDriver(
"Cannot read Axis Error, high and low limit flag Status");
}
// errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat,
// axErr, position);
sprintf(message, "poll results: axis %d, status %d, axErr %d, done = %d",
axisNo_, axStat, axErr, done);
pC_->debugFlow(message);
/* search for trouble */
if (highLim) {
setIntegerParam(pC_->motorStatusHighLimit_, true);
st = setIntegerParam(pC_->motorStatusHighLimit_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
updateMsgTxtFromDriver("High Limit Hit");
} else {
setIntegerParam(pC_->motorStatusHighLimit_, false);
st = setIntegerParam(pC_->motorStatusHighLimit_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
}
if (lowLim) {
setIntegerParam(pC_->motorStatusLowLimit_, true);
st = setIntegerParam(pC_->motorStatusLowLimit_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
updateMsgTxtFromDriver("Low Limit Hit");
} else {
setIntegerParam(pC_->motorStatusLowLimit_, false);
st = setIntegerParam(pC_->motorStatusLowLimit_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
}
if (axErr == 11) {
setIntegerParam(pC_->motorStatusFollowingError_, true);
st = setIntegerParam(pC_->motorStatusFollowingError_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
updateMsgTxtFromDriver("Following error triggered");
} else {
setIntegerParam(pC_->motorStatusFollowingError_, false);
st = setIntegerParam(pC_->motorStatusFollowingError_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
}
/* Set any axis specific general problem bits. */
if (axErr != 0) {
@ -1282,7 +1115,19 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
axisProblemFlag = 0;
axisErrorCount = 0;
}
setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
st = setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
cmdStatus = cmdStatus > st ? cmdStatus : st;
return asynSuccess;
return cmdStatus;
}
asynStatus pmacV3Axis::move(double position, int relative, double min_velocity,
double max_velocity, double acceleration) {
if(!IsEnable) {
updateMsgTxtFromDriver("Error: axis disabled");
return asynSuccess;
}
updateMsgTxtFromDriver("");
return pmacAxis::move(position, relative, min_velocity, max_velocity, acceleration);
}

5
sinqEPICSApp/src/pmacAxis.h
View File

@ -144,9 +144,12 @@ public:
pmacV3Axis(pmacController *pController, int axisNo);
asynStatus move(double position, int relative, double min_velocity,
double max_velocity, double acceleration);
asynStatus poll(bool *moving);
protected:
int IsEnable = 0;
asynStatus getAxisStatus(bool *moving);
friend class pmacController;

15
sinqEPICSApp/src/pmacController.cpp
View File

@ -364,11 +364,8 @@ asynStatus pmacController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
/* Set the parameter and readback in the parameter library. */
status = pAxis->setDoubleParam(function, value);
printf("%s: pAxis->setDoubleParam(%d, %lf)\n", functionName, function, value);
if (function == motorPosition_) {
printf("%s: pAxis->setDoubleParam(%d, %lf)\n", functionName, function, value);
}
// if (function == motorPosition_) {
// /*Set position on motor axis.*/
// epicsInt32 position = (epicsInt32) floor(value*32/pAxis->scale_ + 0.5);
@ -841,19 +838,16 @@ asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
"%s: Enable axis on controller %s, axis %d enable=%d\n",
functionName, portName, pAxis->axisNo_, value);
//errlogPrintf("Enable axis %d: %d, command = %s\n", pAxis->axisNo_, value, command);
if(value){
pAxis->updateMsgTxtFromDriver("");
}
}
// Execute the command.
if (command[0] != 0 && status == asynSuccess) {
status = lowLevelWriteRead(pAxis->axisNo_, command, response);
printf("%s: lowLevelWriteRead: success=%d\n (function == enableAxis_? %d)\n",functionName, status==asynSuccess, function == enableAxis_);
}
// Call base class method
// This will handle callCallbacks even if the function was handled here.
// status = asynMotorController::writeInt32(pasynUser, value);
return pmacController::writeInt32(pasynUser, value);
}
@ -878,7 +872,6 @@ asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) {
*value = strtol(response, NULL, 10);
int st = setIntegerParam(axisEnabled_, *value);
// printf("%s: setIntegerParam(%d, %d) -> success=%d\n", functionName,axisEnabled_, *value, st==asynSuccess);
callParamCallbacks();
}
return pmacController::readInt32(pasynUser, value);