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

Added adaptive polling

Browse Source 
See documentation in README.md for parameter ADAPTPOLL
This commit is contained in:
mathis_s
2025-04-16 13:05:48 +02:00
parent 4c3254687d
commit db03ffea0e
6 changed files with 126 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
src/sinqAxis.cpp
View File

@ -15,6 +15,8 @@ sinqAxis::sinqAxis(class sinqController *pC, int axisNo)
offsetMovTimeout_ = 30;
targetPosition_ = 0.0;
epicsTimeGetCurrent(&lastPollTime_);
// This check is also done in asynMotorAxis, but there the IOC continues
// running even though the configuration is incorrect. When failing this
// check, the IOC is stopped, since this is definitely a configuration
@ -115,6 +117,46 @@ asynStatus sinqAxis::poll(bool *moving) {
asynStatus poll_status = asynSuccess;
int homing = 0;
int homed = 0;
int adaptivePolling = 0;
/*
If adaptive polling is enabled:
- Check if the motor was moving during the last poll
- If yes, perform the poll
- If no, check if the last poll was at least an idlePollPeriod ago
- If yes, perform the poll
- If no, skip it
*/
pl_status =
pC_->getIntegerParam(axisNo_, pC_->adaptivePolling(), &adaptivePolling);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "adaptivePolling", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
};
// Using the EPICS timestamp here, see
// https://docs.epics-controls.org/projects/base/en/latest/epicsTime_h.html#_CPPv414epicsTimeStamp
// Get the current time
epicsTimeStamp ts;
epicsTimeGetCurrent(&ts);
if (adaptivePolling != 0) {
// Motor wasn't moving during the last poll
if (getWasMovingFlag() == 0) {
// Add the idle poll period
epicsTimeStamp earliestTimeNextPoll = lastPollTime_;
epicsTimeAddSeconds(&earliestTimeNextPoll, pC_->idlePollPeriod());
if (epicsTimeLessThan(&earliestTimeNextPoll, &ts) == 1) {
*moving = false;
return asynSuccess;
}
}
}
// Update the start time of the last poll
lastPollTime_ = ts;
/*
At the beginning of the poll, it is assumed that the axis has no status
@ -223,36 +265,42 @@ asynStatus sinqAxis::move(double position, int relative, double minVelocity,
double maxVelocity, double acceleration) {
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
asynStatus status = asynSuccess;
double motorRecResolution = 0.0;
// =========================================================================
// When a new move is done, the motor is not homed anymore
pl_status = setIntegerParam(pC_->motorStatusHomed(), 0);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusHomed_",
axisNo_, __PRETTY_FUNCTION__,
__LINE__);
}
pl_status = setIntegerParam(pC_->motorStatusAtHome(), 0);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorStatusAtHome_",
axisNo_, __PRETTY_FUNCTION__,
__LINE__);
}
pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
&motorRecResolution);
if (pl_status != asynSuccess) {
return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_",
axisNo_, __PRETTY_FUNCTION__,
__LINE__);
}
// Store the target position internally
targetPosition_ = position * motorRecResolution;
return doMove(position, relative, minVelocity, maxVelocity, acceleration);
status = doMove(position, relative, minVelocity, maxVelocity, acceleration);
if (status != asynSuccess) {
return status;
}
// Since the move command was successfull, we assume that the motor has
// started its movement.
status = setIntegerParam(pC_->motorStatusHomed(), 0);
if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorStatusHomed_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
status = setIntegerParam(pC_->motorStatusAtHome(), 0);
if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorStatusAtHome_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(),
&motorRecResolution);
if (status != asynSuccess) {
return pC_->paramLibAccessFailed(status, "motorRecResolution_", axisNo_,
__PRETTY_FUNCTION__, __LINE__);
}
// Needed for adaptive polling
setWasMovingFlag(1);
return pC_->callParamCallbacks();
}
asynStatus sinqAxis::doMove(double position, int relative, double minVelocity,

7
src/sinqAxis.h
View File

@ -7,6 +7,7 @@ Stefan Mathis, November 2024
#ifndef sinqAxis_H
#define sinqAxis_H
#include "asynMotorAxis.h"
#include <epicsTime.h>
class epicsShareClass sinqAxis : public asynMotorAxis {
public:
@ -351,11 +352,17 @@ class epicsShareClass sinqAxis : public asynMotorAxis {
// Internal variables used in the movement timeout watchdog
time_t expectedArrivalTime_;
time_t offsetMovTimeout_;
double scaleMovTimeout_;
bool watchdogMovActive_;
// Store the motor target position for the movement time calculation
double targetPosition_;
/*
Store the time since the last poll
*/
epicsTimeStamp lastPollTime_;
private:
sinqController *pC_;
};

24
src/sinqController.cpp
View File

@ -53,7 +53,7 @@ sinqController::sinqController(const char *portName,
asynStatus status = asynSuccess;
// Handle to the asynUser of the IP port asyn driver
ipPortAsynOctetSyncIO_ = nullptr;
pasynOctetSyncIOipPort_ = nullptr;
// Initial values for the average timeout mechanism, can be overwritten
// later by a FFI function
@ -81,9 +81,9 @@ sinqController::sinqController(const char *portName,
We try to connect to the port via the port name provided by the constructor.
If this fails, the function is terminated via exit.
*/
pasynOctetSyncIO->connect(ipPortConfigName, 0, &ipPortAsynOctetSyncIO_,
pasynOctetSyncIO->connect(ipPortConfigName, 0, &pasynOctetSyncIOipPort_,
NULL);
if (status != asynSuccess || ipPortAsynOctetSyncIO_ == nullptr) {
if (status != asynSuccess || pasynOctetSyncIOipPort_ == nullptr) {
errlogPrintf("Controller \"%s\" => %s, line %d:\nFATAL ERROR (cannot "
"connect to MCU controller).\n"
"Terminating IOC",
@ -252,6 +252,16 @@ sinqController::sinqController(const char *portName,
exit(-1);
}
status = createParam("ADAPTIVE_POLLING", asynParamInt32, &adaptivePolling_);
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a "
"parameter failed with %s).\nTerminating IOC",
portName, __PRETTY_FUNCTION__, __LINE__,
stringifyAsynStatus(status));
exit(-1);
}
status = createParam("ENCODER_TYPE", asynParamOctet, &encoderType_);
if (status != asynSuccess) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
@ -356,7 +366,7 @@ asynStatus sinqController::couldNotParseResponse(const char *command,
int line) {
asynStatus pl_status = asynSuccess;
asynPrint(ipPortAsynOctetSyncIO_, ASYN_TRACE_ERROR,
asynPrint(pasynOctetSyncIOipPort_, ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:\nCould not interpret "
"response \"%s\" for command \"%s\".\n",
portName, axisNo, functionName, line, response, command);
@ -385,7 +395,7 @@ asynStatus sinqController::paramLibAccessFailed(asynStatus status,
int line) {
if (status != asynSuccess) {
asynPrint(ipPortAsynOctetSyncIO_, ASYN_TRACE_ERROR,
asynPrint(pasynOctetSyncIOipPort_, ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:\n Accessing the "
"parameter library failed for parameter %s with error %s.\n",
portName, axisNo, functionName, line, parameter,
@ -654,9 +664,9 @@ asynStatus setMaxSubsequentTimeouts(const char *portName,
if (ptr == nullptr) {
/*
We can't use asynPrint here since this macro would require us
to get a ipPortAsynOctetSyncIO_ from a pointer to an asynPortDriver.
to get a pasynOctetSyncIOipPort_ from a pointer to an asynPortDriver.
However, the given pointer is a nullptr and therefore doesn't
have a ipPortAsynOctetSyncIO_! printf is an EPICS alternative which
have a pasynOctetSyncIOipPort_! printf is an EPICS alternative which
works w/o that, but doesn't offer the comfort provided
by the asynTrace-facility
*/

6
src/sinqController.h
View File

@ -291,6 +291,7 @@ class epicsShareClass sinqController : public asynMotorController {
int motorAcclFromDriver() { return motorAcclFromDriver_; }
int motorHighLimitFromDriver() { return motorHighLimitFromDriver_; }
int motorLowLimitFromDriver() { return motorLowLimitFromDriver_; }
int adaptivePolling() { return adaptivePolling_; }
int encoderType() { return encoderType_; }
// Additional members
@ -298,14 +299,14 @@ class epicsShareClass sinqController : public asynMotorController {
double idlePollPeriod() { return idlePollPeriod_; }
double movingPollPeriod() { return movingPollPeriod_; }
asynUser *asynUserSelf() { return pasynUserSelf; }
asynUser *ipPortAsynOctetSyncIO() { return ipPortAsynOctetSyncIO_; }
asynUser *pasynOctetSyncIOipPort() { return pasynOctetSyncIOipPort_; }
// =========================================================================
protected:
// Pointer to the port user which is specified by the char array
// `ipPortConfigName` in the constructor
asynUser *ipPortAsynOctetSyncIO_;
asynUser *pasynOctetSyncIOipPort_;
double movingPollPeriod_;
double idlePollPeriod_;
msgPrintControl msgPrintControl_;
@ -345,6 +346,7 @@ class epicsShareClass sinqController : public asynMotorController {
int motorAcclFromDriver_;
int motorHighLimitFromDriver_;
int motorLowLimitFromDriver_;
int adaptivePolling_;
int encoderType_;
#define LAST_SINQMOTOR_PARAM encoderType_