lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity

merged r17178, r17179, r17277, r17278, r17279, r17417

Browse Source
This commit is contained in:
Jens Eden
2014-05-28 12:52:35 +00:00
parent f76d13660e
commit e52990e6e7
6 changed files with 155 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
documentation/motor_release.html
+7
View File
@@ -48,6 +48,13 @@
set the limit mode to "Hard".
</p>
<p><b>OMS Asyn</b></p>
<p>
Flush OMS MAXv response buffer more than once (Thanks to Mitch D'Ewart).
Allow up to 10 axes for current MAXnet cards. Increased read buffer length for
MAXnet cards.
</p>
<p><b>Micos MoCo</b></p>
<p>
Bug fix for protection against NULL *parms pointer from motor record.
motorApp/OmsAsynSrc/omsBaseAxis.cpp
+6 -2
View File
@@ -48,7 +48,11 @@ asynStatus omsBaseAxis::move(double position, int relative, double min_velocity,
else
rela = 0;
pos = (epicsInt32) (position + 0.5);
if ( position < 0.0)
pos = (epicsInt32) (position - 0.5);
else
pos = (epicsInt32) (position + 0.5);
if (abs(pos) > 67000000){
asynPrint(pasynUser_, ASYN_TRACE_ERROR,
"%s:%s:%s axis %d position out of range %f\n",
@@ -167,7 +171,7 @@ asynStatus omsBaseAxis::stop(double acceleration )
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
"%s:%s: port %s, set axis %d to stop with accel=%f\n",
driverName, functionName, pC_->portName, axisNo_, axisNo_, acceleration );
driverName, functionName, pC_->portName, axisNo_, acceleration );
return status;
}
motorApp/OmsAsynSrc/omsBaseController.cpp
+61 -48
View File
@@ -37,7 +37,7 @@ static const char *driverName = "omsBaseDriver";
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= motorOMSBASEdebug) \
#define Debug(l, f, args...) {if (l & motorOMSBASEdebug) \
errlogPrintf(f, ## args);}
#else
#define Debug(l, f, args...)
@@ -77,6 +77,9 @@ omsBaseController::omsBaseController(const char *portName, int maxAxes, int prio
++omsTotalControllerNumber;
sanityCounter = 0;
fwMajor = 0;
fwMinor = 0;
fwRevision = 0;
useWatchdog = false;
enabled = true;
numAxes = maxAxes;
@@ -134,8 +137,8 @@ void omsBaseController::report(FILE *fp, int level)
double velocity, position, encoderPosition;
int haveEncoder=0;
fprintf(fp, "Oms %s motor driver %s, numAxes=%d\n",
controllerType, portName, numAxes);
fprintf(fp, "Oms %s motor driver %s, numAxes=%d; Firmware: %d.%d.%d\n",
controllerType, portName, numAxes, fwMajor, fwMinor, fwRevision);
for (axis=0; axis < numAxes; axis++) {
omsBaseAxis *pAxis = pAxes[axis];
@@ -228,7 +231,7 @@ asynStatus omsBaseController::readInt32(asynUser *pasynUser, epicsInt32 *value)
int function = pasynUser->reason;
asynStatus status = asynSuccess;
omsBaseAxis *pAxis = getAxis(pasynUser);
static const char *functionName = "readInt32";
// static const char *functionName = "readInt32";
static char outputBuffer[8];
if (!pAxis) return asynError;
@@ -268,7 +271,7 @@ asynStatus omsBaseController::writeInt32(asynUser *pasynUser, epicsInt32 value)
if (function == motorDeferMoves_)
{
asynPrint(pasynUser, ASYN_TRACE_ERROR,
"%s:%s:%s Deferred Move: not yet implemented %s\n",
"%s:%s:%s Deferred Move: not yet implemented\n",
driverName, functionName, portName);
}
else if (function == motorClosedLoop_)
@@ -449,7 +452,7 @@ asynStatus omsBaseController::Init(const char* initString, int multiple){
char *p, *tokSave;
int totalAxes;
char axisChrArr[OMS_MAX_AXES] = {'X','Y','Z','T','U','V','R','S'};
char axisChrArr[OMS_MAX_AXES] = {'X','Y','Z','T','U','V','R','S','W','K'};
char outputBuffer[10];
epicsInt32 axisPosArr[OMS_MAX_AXES];
@@ -459,6 +462,9 @@ asynStatus omsBaseController::Init(const char* initString, int multiple){
/* Stop all axes */
sendOnlyLock("AM SA;");
/* wait before sending init commands */
epicsThreadSleep(0.5);
/* send InitString */
if ((initString != NULL) && (strlen(initString) > 0)) {
if (multiple){
@@ -473,9 +479,10 @@ asynStatus omsBaseController::Init(const char* initString, int multiple){
sendOnlyLock(initString);
}
}
/* Some init commands (like "LT") need some time to process */
epicsThreadSleep(0.5);
/* get Positions of all axes */
inputBuffer[0] = '\0';
sendReceiveLock((char*) "AA RP;", inputBuffer, sizeof(inputBuffer));
if (numAxes > OMS_MAX_AXES) {
@@ -508,7 +515,7 @@ asynStatus omsBaseController::Init(const char* initString, int multiple){
pAxis->setIntegerParam(motorStatusCommsError_, 0);
/* Determine if encoder is present and if mode is stepper or servo. */
if (firmwareMin(1,1,30))
if (firmwareMin(1,30,0))
strcpy(outputBuffer,"A? PS?");
else
strcpy(outputBuffer,"A? ?PS");
@@ -532,7 +539,7 @@ asynStatus omsBaseController::Init(const char* initString, int multiple){
/* Determine limit true state high or low */
/* CAUTION you need firmware version 1.30 or higher to do this */
if (firmwareMin(1,1,30))
if (firmwareMin(1,30,0))
strcpy(outputBuffer,"A? LT?");
else
strcpy(outputBuffer,"A? ?LS");
@@ -611,14 +618,14 @@ void omsBaseController::omsPoller()
if (loopBreakCount > 10){
errlogPrintf("%s:%s:%s: Error: %d consecutive unsuccessful attempts to read from motor card\n"
, driverName, functionName, this->portName, loopBreakCount);
loopBreakCount = 0;
break;
}
epicsTimeGetCurrent(&loopStart);
retry_count = 0;
while ((getAxesPositions(axisPosArr) != asynSuccess) && (retry_count < 5)){
epicsThreadSleepQuantum();
epicsThreadSleep(0.1);
++retry_count;
}
if (retry_count > 4){
@@ -635,9 +642,9 @@ void omsBaseController::omsPoller()
}
/* read all axis status values and reset done-field
* MDNN,MDNN,PNLN,PNNN,PNLN,PNNN,PNNN,PNNN*/
* MDNN,MDNN,PNLN,PNNN,PNLN,PNNN,PNNN,PNNN */
if (getAxesStatus(statusBuffer, sizeof(statusBuffer), &moveDone) != asynSuccess){
Debug(1, "%s:%s:%s: error reading axes status\n", driverName, functionName, this->portName);
Debug(1, "%s:%s:%s: error reading axes status\n", driverName, functionName, this->portName);
++loopBreakCount;
continue;
}
@@ -668,24 +675,18 @@ void omsBaseController::omsPoller()
Debug(1,"%s:%s:%s: Error reading encoder status buffer >%s<\n", driverName, functionName, this->portName, encStatusBuffer);
}
/* read all limits */
if (moveDone || (anyMoving == 0))
haveLimits = true;
else
haveLimits = false;
haveLimits = true;
limitFlags =0;
if (haveLimits){
if ((sendReceiveLock((char*) "AM;QL;", pollInputBuffer, sizeof(pollInputBuffer)) == asynSuccess)){
if (1 != sscanf(pollInputBuffer, "%x", &limitFlags)){
Debug(1,"%s:%s:%s: error converting limits: %s\n", driverName, functionName, this->portName, pollInputBuffer);
haveLimits = false;
}
}
else {
haveLimits = false;
Debug(1,"%s:%s:%s: error reading limits %s\n", driverName, functionName, this->portName, pollInputBuffer);
}
}
if ((sendReceiveLock((char*) "AM;QL;", pollInputBuffer, sizeof(pollInputBuffer)) == asynSuccess)){
if (1 != sscanf(pollInputBuffer, "%x", &limitFlags)){
Debug(1,"%s:%s:%s: error converting limits: %s\n", driverName, functionName, this->portName, pollInputBuffer);
haveLimits = false;
}
}
else {
haveLimits = false;
Debug(1,"%s:%s:%s: error reading limits %s\n", driverName, functionName, this->portName, pollInputBuffer);
}
if (enabled) watchdogOK();
anyMoving = 0;
@@ -715,33 +716,42 @@ void omsBaseController::omsPoller()
/* check the done flag or current velocity */
if (statusBuffer[i*STATUSSTRINGLEN + 1] == 'D'){
Debug(3, "%s:%s:%s: found Done Flag axis %d\n", driverName, functionName, portName, i);
Debug(8, "%s:%s:%s: found Done Flag axis %d\n", driverName, functionName, portName, i);
pAxis->setIntegerParam(motorStatusProblem_, 0);
pAxis->moveDelay=0;
pAxis->setIntegerParam(motorStatusDone_, 1);
pAxis->setIntegerParam(motorStatusMoving_, 0);
if (pAxis->homing) pAxis->homing = 0;
Debug(1, "%s:%s:%s: done axis %d \n", driverName, functionName,
this->portName, i );
}
else if (haveVeloArray && (veloArr[i] == 0)){
/* keep a small delay here, to make sure that a motorAxisDone cycle is performed
even if the motor is on hard limits */
getIntegerParam(pAxis->axisNo_, motorStatusMoving_, &axisMoving);
if (axisMoving){
pAxis->moveDelay++ ;
if (pAxis->moveDelay >= 5) {
Debug(4, "%s:%s:%s: setting Problem Flag axis %d\n", driverName, functionName, portName, i);
if (statusBuffer[i*STATUSSTRINGLEN + 2] == 'L'){
pAxis->setIntegerParam(motorStatusProblem_, 0);
pAxis->moveDelay=0;
pAxis->setIntegerParam(motorStatusDone_, 1);
pAxis->setIntegerParam(motorStatusMoving_, 0);
pAxis->setIntegerParam(motorStatusProblem_, 1);
pAxis->moveDelay = 0;
if (pAxis->homing) pAxis->homing = 0;
Debug(1, "%s:%s:%s: stop axis %d, moveDelay count %d\n", driverName, functionName,
this->portName, i, pAxis->moveDelay );
if (statusBuffer[i*STATUSSTRINGLEN + 2] == 'P')
pAxis->setIntegerParam(motorStatusHighLimit_, 1);
else
pAxis->setIntegerParam(motorStatusLowLimit_, 1);
}
else {
pAxis->moveDelay++ ;
if (pAxis->moveDelay >= 5) {
Debug(4, "%s:%s:%s: setting Problem Flag axis %d\n", driverName, functionName, portName, i);
pAxis->setIntegerParam(motorStatusDone_, 1);
pAxis->setIntegerParam(motorStatusMoving_, 0);
pAxis->setIntegerParam(motorStatusProblem_, 1);
pAxis->moveDelay = 0;
if (pAxis->homing) pAxis->homing = 0;
Debug(1, "%s:%s:%s: stop axis %d, moveDelay count %d\n", driverName, functionName,
this->portName, i, pAxis->moveDelay );
}
Debug(2, "%s:%s:%s: moveDelay axis %d, count %d\n", driverName, functionName,
this->portName, i, pAxis->moveDelay );
}
Debug(2, "%s:%s:%s: moveDelay axis %d, count %d\n", driverName, functionName,
this->portName, i, pAxis->moveDelay );
}
}
else {
@@ -756,11 +766,13 @@ void omsBaseController::omsPoller()
/* check limits */
if (haveLimits){
if (((limitFlags & (1 << i)) > 0) ^ (pAxis->getLimitInvert()))
pAxis->setIntegerParam(motorStatusLowLimit_, 1);
int limitOffset = 0;
if (i > 7) limitOffset = 8; // same as limitOffset = 16 ; i -= 8
if (((limitFlags & (1 << (i+limitOffset))) > 0) ^ (pAxis->getLimitInvert()))
pAxis->setIntegerParam(motorStatusLowLimit_, 1);
else
pAxis->setIntegerParam(motorStatusLowLimit_, 0);
if (((limitFlags & (1 << (i+8))) > 0) ^ (pAxis->getLimitInvert()))
if (((limitFlags & (1 << (i+limitOffset+8))) > 0) ^ (pAxis->getLimitInvert()))
pAxis->setIntegerParam(motorStatusHighLimit_, 1);
else
pAxis->setIntegerParam(motorStatusHighLimit_, 0);
@@ -810,7 +822,7 @@ void omsBaseController::omsPoller()
waiting may be interrupted by pollEvent or interrupt messages*/
epicsTimeGetCurrent(&now);
timeToWait = timeout - epicsTimeDiffInSeconds(&now, &loopStart);
Debug(5, "%s:%s:%s: poller loop: waiting %f s\n", driverName, functionName, this->portName, timeToWait);
Debug(16, "%s:%s:%s: poller loop: waiting %f s\n", driverName, functionName, this->portName, timeToWait);
if (waitInterruptible(timeToWait) == epicsEventWaitOK) {
fastPolls = forcedFastPolls;
}
@@ -959,6 +971,7 @@ asynStatus omsBaseController::sendOnlyLock(const char *outputBuff)
asynStatus omsBaseController::sendReceiveLock(const char *outputBuff, char *inputBuff, unsigned int inputSize)
{
asynStatus status;
if (inputSize > 0) inputBuff[0] = '\0';
baseMutex->lock();
status = sendReceive(outputBuff, inputBuff, inputSize);
baseMutex->unlock();
motorApp/OmsAsynSrc/omsBaseController.h
+1 -1
View File
@@ -27,7 +27,7 @@ HeadURL: $URL$
#include "omsBaseAxis.h"
#include <epicsExport.h>
#define OMS_MAX_AXES 8
#define OMS_MAX_AXES 10
#define OMSBASE_MAXNUMBERLEN 12
#define OMSINPUTBUFFERLEN OMSBASE_MAXNUMBERLEN * OMS_MAX_AXES + 2
motorApp/OmsAsynSrc/omsMAXnet.cpp
+53 -50
View File
@@ -20,7 +20,7 @@ HeadURL: $URL$
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= motorMAXnetdebug) \
#define Debug(l, f, args...) {if (l & motorMAXnetdebug) \
errlogPrintf(f, ## args);}
#else
#define Debug(l, f, args...)
@@ -28,11 +28,12 @@ HeadURL: $URL$
#else
#define Debug
#endif
static const char *driverName = "omsMAXnetDriver";
volatile int motorMAXnetdebug = 0;
extern "C" {epicsExportAddress(int, motorMAXnetdebug);}
#define MAXnet_MAX_BUFFERLENGTH 80
#define MAXnet_MAX_BUFFERLENGTH 250
static void connectCallback(asynUser *pasynUser, asynException exception)
{
@@ -43,12 +44,12 @@ static void connectCallback(asynUser *pasynUser, asynException exception)
if (exception == asynExceptionConnect) {
status = pasynManager->isConnected(pasynUser, &connected);
if (connected){
if (motorMAXnetdebug > 4) asynPrint(pasynUser, ASYN_TRACE_FLOW,
if (motorMAXnetdebug & 8) asynPrint(pasynUser, ASYN_TRACE_FLOW,
"MAXnet connectCallback: TCP-Port connected\n");
pController->portConnected = 1;
}
else {
if (motorMAXnetdebug > 3) asynPrint(pasynUser, ASYN_TRACE_FLOW,
if (motorMAXnetdebug & 4) asynPrint(pasynUser, ASYN_TRACE_FLOW,
"MAXnet connectCallback: TCP-Port disconnected\n");
pController->portConnected = 0;
}
@@ -93,14 +94,12 @@ omsMAXnet::omsMAXnet(const char* portName, int numAxes, const char* serialPortNa
pasynUserSerial = pasynManager->createAsynUser(0,0);
pasynUserSerial->userPvt = this;
Debug(9, "omsMAXnet connect to %s \n", serialPortName);
status = pasynManager->connectDevice(pasynUserSerial,serialPortName,0);
if(status != asynSuccess){
printf("MAXnetConfig: can't connect to port %s: %s\n",serialPortName,pasynUserSerial->errorMessage);
return;
}
Debug(9, "omsMAXnet add Callback \n");
status = pasynManager->exceptionCallbackAdd(pasynUserSerial, connectCallback);
if(status != asynSuccess){
printf("MAXnetConfig: can't set exceptionCallback for %s: %s\n",serialPortName,pasynUserSerial->errorMessage);
@@ -109,7 +108,6 @@ omsMAXnet::omsMAXnet(const char* portName, int numAxes, const char* serialPortNa
/* set the connect flag */
pasynManager->isConnected(pasynUserSerial, &portConnected);
Debug(9, "omsMAXnet findInterface \n");
pasynInterface = pasynManager->findInterface(pasynUserSerial,asynOctetType,1);
if( pasynInterface == NULL) {
printf("MAXnetConfig: %s driver not supported\n", asynOctetType);
@@ -118,7 +116,6 @@ omsMAXnet::omsMAXnet(const char* portName, int numAxes, const char* serialPortNa
pasynOctetSerial = (asynOctet*)pasynInterface->pinterface;
octetPvtSerial = pasynInterface->drvPvt;
Debug(9, "omsMAXnet SyncIO->connect \n");
status = pasynOctetSyncIO->connect(serialPortName, 0, &pasynUserSyncIOSerial, NULL);
if(status != asynSuccess){
printf("MAXnetConfig: can't connect pasynOctetSyncIO %s: %s\n",serialPortName,pasynUserSyncIOSerial->errorMessage);
@@ -126,13 +123,11 @@ omsMAXnet::omsMAXnet(const char* portName, int numAxes, const char* serialPortNa
}
/* flush any junk at input port - should be no data available */
Debug(9, "omsMAXnet flush \n");
pasynOctetSyncIO->flush(pasynUserSyncIOSerial);
timeout = 2.0;
pasynUserSerial->timeout = 0.0;
Debug(9, "omsMAXnet setInputEos \n");
// CAUTION firmware versions before 1.33.4 use '\n' for serial port and '\n\r' for IP port as InputEOS
// Set inputEOS in st.cmd for old firmware versions
status = pasynOctetSyncIO->setInputEos(pasynUserSyncIOSerial, "\n\r", 2);
@@ -141,14 +136,12 @@ omsMAXnet::omsMAXnet(const char* portName, int numAxes, const char* serialPortNa
return ;
}
Debug(9, "omsMAXnet setOutputEos \n");
status = pasynOctetSyncIO->setOutputEos(pasynUserSyncIOSerial, "\n", 1);
if(status != asynSuccess){
printf("MAXnetConfig: unable to set OutputEOS %s: %s\n",serialPortName,pasynUserSyncIOSerial->errorMessage);
return ;
}
Debug(9, "omsMAXnet registerInterruptUser \n");
void* registrarPvt= NULL;
status = pasynOctetSerial->registerInterruptUser(octetPvtSerial, pasynUserSerial, omsMAXnet::asynCallback, this, &registrarPvt);
if(status != asynSuccess) {
@@ -156,7 +149,6 @@ omsMAXnet::omsMAXnet(const char* portName, int numAxes, const char* serialPortNa
return;
}
Debug(9, "omsMAXnet get FirmwareVersion \n");
/* get FirmwareVersion */
if(getFirmwareVersion() != asynSuccess) {
printf("MAXnetConfig: unable to talk to controller at %s: %s\n",serialPortName,pasynUserSyncIOSerial->errorMessage);
@@ -184,7 +176,6 @@ epicsEventWaitStatus omsMAXnet::waitInterruptible(double timeout)
epicsEventWaitStatus waitStatus = epicsEventWaitTimeout;
epicsTimeGetCurrent(&starttime);
// TODO use local poll Periods or lock()
if (timeout == idlePollPeriod_)
pollWait = idlePollPeriod_ / 5.0;
else
@@ -195,7 +186,7 @@ epicsEventWaitStatus omsMAXnet::waitInterruptible(double timeout)
pasynManager->unlockPort(pasynUserSerial);
while ( timeToWait > 0){
/* reading with bufferlength 0 and timeout 0.0 triggers a callback and
* poll event, if a notification is outstanding */
* poll event, if a notification is outstanding. One character will be read. */
if (enabled) {
pasynManager->lockPort(pasynUserSerial);
status = pasynOctetSerial->read(octetPvtSerial, pasynUserSerial, inputBuff,
@@ -218,14 +209,13 @@ asynStatus omsMAXnet::sendOnly(const char *outputBuff)
asynStatus status;
if (!enabled) return asynError;
Debug(9, "omsMAXnet::sendOnly: write: %s \n", outputBuff);
status = pasynOctetSyncIO->write(pasynUserSyncIOSerial, outputBuff,
strlen(outputBuff), timeout, &nActual);
if (status != asynSuccess) {
asynPrint(pasynUserSyncIOSerial, ASYN_TRACE_ERROR,
"drvMAXnetAsyn:sendOnly: error sending command %d, sent=%d, status=%d\n",
"drvMAXnetAsyn:sendOnly: error sending command %s, sent=%d, status=%d\n",
outputBuff, nActual, status);
}
Debug(4, "omsMAXnet::sendOnly: wrote: %s \n", outputBuff);
@@ -235,28 +225,30 @@ asynStatus omsMAXnet::sendOnly(const char *outputBuff)
asynStatus omsMAXnet::sendReceive(const char *outputBuff, char *inputBuff, unsigned int inputSize)
{
char localBuffer[MAXnet_MAX_BUFFERLENGTH + 1] = "";
size_t nRead, nWrite, bufferSize = inputSize;
size_t nRead=0, nReadnext=0, nWrite=0;
size_t bufferSize = MAXnet_MAX_BUFFERLENGTH;
int eomReason = 0;
asynStatus status = asynSuccess;
char *outString;
char *outString = localBuffer;
int errorCount = 10;
if (!enabled) return asynError;
Debug(4, "omsMAXnet::sendReceive: write: %s \n", outputBuff);
if (bufferSize > MAXnet_MAX_BUFFERLENGTH) bufferSize = MAXnet_MAX_BUFFERLENGTH;
Debug(9, "omsMAXnet::sendReceive: read the notification \n");
/*
* read the notification from input buffer
*/
while ((notificationCounter > 0) && errorCount){
status = pasynOctetSyncIO->read(pasynUserSyncIOSerial, localBuffer, sizeof(localBuffer), 0.1, &nRead, &eomReason);
status = pasynOctetSyncIO->read(pasynUserSyncIOSerial, localBuffer, bufferSize, 0.001, &nRead, &eomReason);
while ((status == asynSuccess) && !(eomReason & ASYN_EOM_EOS)) {
status = pasynOctetSyncIO->read(pasynUserSyncIOSerial, localBuffer+nRead,
bufferSize-nRead, timeout, &nReadnext, &eomReason);
nRead += nReadnext;
}
localBuffer[nRead] = '\0';
outString = localBuffer;
while (*outString == 6) ++outString;
if (status == asynSuccess) {
localBuffer[nRead] = '\0';
if (isNotification(localBuffer) && (notificationCounter > 0)) {
Debug(2, "omsMAXnet::sendReceive: decrement notificationCounter: %s, len: %d, reason: %d\n", localBuffer, nRead, eomReason);
if (isNotification(outString) && (notificationCounter > 0)) {
--notificationCounter;
}
}
@@ -268,33 +260,43 @@ asynStatus omsMAXnet::sendReceive(const char *outputBuff, char *inputBuff, unsig
}
}
Debug(9, "omsMAXnet::sendReceive: write \n");
Debug(4, "omsMAXnet::sendReceive: write: %s \n", outputBuff);
nRead=0;
nReadnext=0;
status = pasynOctetSyncIO->writeRead(pasynUserSyncIOSerial, outputBuff, strlen(outputBuff), localBuffer,
bufferSize, timeout, &nWrite, &nRead, &eomReason);
if (status == asynSuccess) localBuffer[nRead] = '\0';
// if input data is a notification read until expected data arrived
while ((status == asynSuccess) && isNotification(localBuffer)) {
Debug(2, "omsMAXnet::sendReceive: notification while reading: %s\n", localBuffer);
while ((status == asynSuccess) && !(eomReason & ASYN_EOM_EOS)) {
status = pasynOctetSyncIO->read(pasynUserSyncIOSerial, localBuffer+nRead,
bufferSize-nRead, timeout, &nReadnext, &eomReason);
nRead += nReadnext;
}
localBuffer[nRead] = '\0';
// cut off a leading /006
outString = localBuffer;
while (*outString == 6) ++outString;
// if input data is a notification read until expected data arrived
while ((status == asynSuccess) && isNotification(outString)) {
nRead=0;
nReadnext=0;
status = pasynOctetSyncIO->read(pasynUserSyncIOSerial, localBuffer,
bufferSize, timeout, &nRead, &eomReason);
if (status == asynSuccess) localBuffer[nRead] = '\0';
while ((status == asynSuccess) && !(eomReason & ASYN_EOM_EOS)) {
status = pasynOctetSyncIO->read(pasynUserSyncIOSerial, localBuffer+nRead,
bufferSize-nRead, timeout, &nReadnext, &eomReason);
nRead += nReadnext;
}
localBuffer[nRead] = '\0';
// cut off a leading /006
outString = localBuffer;
while (*outString == 6) ++outString;
if (notificationCounter > 0) --notificationCounter;
}
if ((status == asynSuccess) && (eomReason == ASYN_EOM_EOS)) {
// cut off a leading /006
outString = strrchr(localBuffer, 6);
if (outString == NULL) {
outString = localBuffer;
}
else {
++outString;
}
// copy into inputBuffer
strncpy(inputBuff, outString, inputSize);
inputBuff[inputSize-1] = '\0';
}
// copy into inputBuffer
strncpy(inputBuff, outString, inputSize);
inputBuff[inputSize-1] = '\0';
Debug(4, "omsMAXnet::sendReceive: read: %s \n", inputBuff);
@@ -303,6 +305,7 @@ asynStatus omsMAXnet::sendReceive(const char *outputBuff, char *inputBuff, unsig
/*
* check if buffer is a notification messages with 13 chars ("%000 SSSSSSSS")
* (first character may miss
*/
int omsMAXnet::isNotification (char *buffer) {
@@ -313,8 +316,8 @@ int omsMAXnet::isNotification (char *buffer) {
printf("%s:%s:%s: CMD_ERR_FLAG received\n", driverName, functionName, portName);
}
else {
if (motorMAXnetdebug > 1) printf("%s:%s:%s: Interrupt notification: %s\n",
driverName, functionName, portName, buffer);
Debug(2,"%s:%s:%s: Interrupt notification: %s\n",
driverName, functionName, portName, buffer);
epicsEventSignal(pollEventId_);
}
return 1;
motorApp/OmsAsynSrc/omsMAXv.cpp
+27 -19
View File
@@ -23,7 +23,7 @@ HeadURL: $URL$
#include <epicsExit.h>
#include "omsMAXv.h"
static const char *driverName = "omsMAXvDriver";
static const char *driverName = "omsMAXvAsyn";
#define MIN(a,b) ((a)<(b)? (a): (b))
@@ -66,10 +66,11 @@ void omsMAXv::InterruptHandler( void * param )
epicsInterruptContextMessage(errmsg);
}
// not clearing this may corrupt the next read/write operation
/* unset this bit to not clear the text_response bit */
if (status1_flag.Bits.text_response != 0) status1_flag.Bits.text_response = 0;
pmotor->status1_flag.All = status1_flag.All; /* Release IRQ's. */
/* Release IRQ's. Clear bits by writing a 1 */
pmotor->status1_flag.All = status1_flag.All;
/* do a dummy read to ensure that all previous writes, which may
* have been queued in the VME bridge chip get processed
@@ -248,7 +249,7 @@ void omsMAXv::initialize(const char* portName, int numAxes, int cardNo, const ch
Debug(64, "motor_init: send init string\n");
if( Init(initString, 1) != asynSuccess) {
if( Init(initString, 0) != asynSuccess) {
errlogPrintf("%s:%s:%s: unable to send initstring to controller card %d\n",
driverName, functionName, portName, cardNo);
return;
@@ -282,6 +283,7 @@ asynStatus omsMAXv::sendOnly(const char *outputBuff)
STATUS1 flag1;
const char* functionName = "sendOnly";
int len = strlen(outputBuff);
double timeout = 0.01;
epicsUInt16 getIndex, putIndex;
if (!enabled) return asynError;
@@ -295,7 +297,8 @@ asynStatus omsMAXv::sendOnly(const char *outputBuff)
}
/* see if junk at input port - should not be any data available */
if ((epicsUInt16) pmotor->inGetIndex != (epicsUInt16) pmotor->inPutIndex)
int flushTime = 0;
while (((epicsUInt16) pmotor->inGetIndex != (epicsUInt16) pmotor->inPutIndex) && (flushTime < 100))
{
// flush cards response Buffer
#ifdef DEBUG
@@ -310,7 +313,12 @@ asynStatus omsMAXv::sendOnly(const char *outputBuff)
flag1.All = pmotor->status1_flag.All;
pmotor->status1_flag.All = flag1.All;
pmotor->msg_semaphore=0;
epicsThreadSleep(timeout);
flushTime++;
if (flushTime == 100 ) {
Debug(1, "%s:%s:%s: unable to flush more than 100 strings\n", driverName, functionName, portName);
return asynError;
}
}
putIndex = (epicsUInt16) pmotor->outPutIndex;
@@ -323,10 +331,12 @@ asynStatus omsMAXv::sendOnly(const char *outputBuff)
pmotor->outPutIndex = putIndex; /* Message Sent */
int count=0, prevdeltaIndex =0;
int count = 0, prevdeltaIndex = 0, index = 0;
int maxcount = (int)(0.1 / epicsThreadSleepQuantum());
// skip busy-waiting for small epicsThreadSleepQuantum
if (epicsThreadSleepQuantum() <= 0.01) index = 100;
int deltaIndex = ((epicsUInt16)pmotor->outPutIndex) - ((epicsUInt16)pmotor->outGetIndex);
int index = 0;
while (deltaIndex != 0)
while ((deltaIndex != 0) && (count <= maxcount))
{
deltaIndex = ((epicsUInt16)pmotor->outPutIndex) - ((epicsUInt16)pmotor->outGetIndex);
// do busy-waiting but not more than 100 times
@@ -334,16 +344,13 @@ asynStatus omsMAXv::sendOnly(const char *outputBuff)
deltaIndex = ((epicsUInt16)pmotor->outPutIndex) - ((epicsUInt16)pmotor->outGetIndex);
++index;
}
// epicsThreadSleepQuantum => 0.02s for RTEMS
if ((index >= 100) && (deltaIndex != 0)) epicsThreadSleep(epicsThreadSleepQuantum());
if ((index >= 100) && (deltaIndex != 0)) epicsThreadSleep(timeout);
if (deltaIndex == prevdeltaIndex)
++count;
else
count =0;
if (count > 10) break;
count = 0;
prevdeltaIndex = deltaIndex;
};
Debug(32, "%s:%s:%s: Waited %d loops\n", driverName, functionName, portName, index);
if (deltaIndex != 0) {
Debug(1, "%s:%s:%s: Timeout\n", driverName, functionName, portName);
@@ -366,7 +373,7 @@ asynStatus omsMAXv::sendReceive(const char *outputBuff, char *inputBuff, unsigne
size_t bufsize;
size_t usedSpace = 0;
char *start, *end;
int itera;
int itera = 0;
asynStatus status;
if (!enabled) return asynError;
@@ -378,10 +385,10 @@ asynStatus omsMAXv::sendReceive(const char *outputBuff, char *inputBuff, unsigne
*inputBuff = '\0';
Debug(64, "omsMAXv::sendReceive: receiving\n");
itera = 0;
double time = 0.0;
double timeout = epicsThreadSleepQuantum() + 0.001;
double timeout = 0.1;
// skip busy-waiting for small epicsThreadSleepQuantum
if (epicsThreadSleepQuantum() <= 0.01) itera = 2001;
while ((pmotor->status1_flag.Bits.text_response == 0) && (time < timeout)){
Debug(32, "%s:%s:%s: Waiting for reponse, itera:%d\n",
driverName, functionName, portName, itera);
@@ -395,7 +402,8 @@ asynStatus omsMAXv::sendReceive(const char *outputBuff, char *inputBuff, unsigne
if (pmotor->status1_flag.Bits.text_response == 0)
{
Debug(1, "Timeout occurred in recv_mess, %s\n", outputBuff);
Debug(1, "%s:%s:%s: Timeout occurred , %s\n",
driverName, functionName, portName, outputBuff);
return asynTimeout;
}