Mark Koennecke
722fab935e
There is a rare condition when SICS has frehsly stared up and on the first call to a coordinated driving, like in TAS, something fails. Then the group has ID 0, the default and that one is always active. This has been fixed by starting valid groups at 7, defining 0 ans IDUNDEFINED and checking in isTaskGroupRunning for IDUNDEFINED Then there is an issue in tasdrive. I added a call to CheckStatus when starting the mono. This is necessary for the eiger monochromator as with that one starting is deferred to the CheckSttaus function. The reason is that in other use cases, the looser might want to drive a2 and a2w at the same time. This can only correctly be accomodated by deferring the caclulations to the CheckStatus phase.
435 lines
12 KiB
C
435 lines
12 KiB
C
/*--------------------------------------------------------------------------
|
|
I N T E R F A C E
|
|
|
|
|
|
Just a few utility function for creating interfaces.
|
|
|
|
Mark Koennecke, June 1997
|
|
|
|
Copyright:
|
|
|
|
Labor fuer Neutronenstreuung
|
|
Paul Scherrer Institut
|
|
CH-5423 Villigen-PSI
|
|
|
|
Extended over time with utility functions
|
|
|
|
Added Drivable and Countable task functions
|
|
|
|
Mark Koennecke, February 2013
|
|
|
|
The authors hereby grant permission to use, copy, modify, distribute,
|
|
and license this software and its documentation for any purpose, provided
|
|
that existing copyright notices are retained in all copies and that this
|
|
notice is included verbatim in any distributions. No written agreement,
|
|
license, or royalty fee is required for any of the authorized uses.
|
|
Modifications to this software may be copyrighted by their authors
|
|
and need not follow the licensing terms described here, provided that
|
|
the new terms are clearly indicated on the first page of each file where
|
|
they apply.
|
|
|
|
IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
|
|
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
|
ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
|
|
DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
|
|
POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
|
|
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
|
|
FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT. THIS SOFTWARE
|
|
IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
|
|
NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
|
|
MODIFICATIONS.
|
|
----------------------------------------------------------------------------*/
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include "fortify.h"
|
|
#include "sics.h"
|
|
#include "motor.h"
|
|
#include <status.h>
|
|
/*=========================================================================
|
|
Empty driveable interface functions
|
|
==========================================================================*/
|
|
static int EmptyHalt(void *self)
|
|
{
|
|
return OKOK;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static int EmptyLimits(void *self, float fVal, char *error, int errLen)
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static long EmptyValue(void *self, SConnection * pCon, float fVal)
|
|
{
|
|
SCWrite(pCon, "WARNING: empty SetValue", eWarning);
|
|
return OKOK;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static int EmptyStatus(void *self, SConnection * pCon)
|
|
{
|
|
return HWIdle;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*/
|
|
static float EmptyGet(void *self, SConnection * pCon)
|
|
{
|
|
SCWrite(pCon, "WARNING: empty GetValue", eWarning);
|
|
return 555.55;
|
|
}
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
pIDrivable CreateDrivableInterface(void)
|
|
{
|
|
pIDrivable pRes = NULL;
|
|
|
|
pRes = (pIDrivable) malloc(sizeof(IDrivable));
|
|
if (!pRes) {
|
|
return NULL;
|
|
}
|
|
memset(pRes, 0, sizeof(IDrivable));
|
|
pRes->ID = DRIVEID;
|
|
pRes->Halt = EmptyHalt;
|
|
pRes->CheckLimits = EmptyLimits;
|
|
pRes->SetValue = EmptyValue;
|
|
pRes->CheckStatus = EmptyStatus;
|
|
pRes->GetValue = EmptyGet;
|
|
pRes->drivableStatus = HWIdle;
|
|
return pRes;
|
|
}
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
pICountable CreateCountableInterface(void)
|
|
{
|
|
pICountable pRes = NULL;
|
|
|
|
pRes = (pICountable) malloc(sizeof(ICountable));
|
|
if (!pRes) {
|
|
return NULL;
|
|
}
|
|
memset(pRes, 0, sizeof(ICountable));
|
|
pRes->ID = COUNTID;
|
|
return pRes;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
pEVInterface CreateEVInterface(void)
|
|
{
|
|
pEVInterface pRes = NULL;
|
|
|
|
pRes = (pEVInterface) malloc(sizeof(EVInterface));
|
|
if (!pRes) {
|
|
return NULL;
|
|
}
|
|
memset(pRes, 0, sizeof(EVInterface));
|
|
pRes->iID = ENVIRINTERFACE;
|
|
return pRes;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
static void *FindInterface(void *pObject, int iID)
|
|
{
|
|
pDummy pDum = NULL;
|
|
|
|
pDum = (pDummy) pObject;
|
|
if (!pDum) {
|
|
return NULL;
|
|
}
|
|
if (!pDum->pDescriptor) {
|
|
return NULL;
|
|
}
|
|
|
|
return pDum->pDescriptor->GetInterface(pDum, iID);
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
pIDrivable GetDrivableInterface(void *pObject)
|
|
{
|
|
return (pIDrivable) FindInterface(pObject, DRIVEID);
|
|
}
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
int GetDrivablePosition(void *pObject, SConnection * pCon, float *fPos)
|
|
{
|
|
pIDrivable pDriv = NULL;
|
|
pMotor pMot = NULL;
|
|
float value;
|
|
|
|
pDriv = GetDrivableInterface(pObject);
|
|
if (pDriv == NULL) {
|
|
return 0;
|
|
}
|
|
if (iHasType(pObject, "Motor")) {
|
|
pMot = (pMotor) pObject;
|
|
return MotorGetSoftPosition(pMot, pCon, fPos);
|
|
}
|
|
value = pDriv->GetValue(pObject, pCon);
|
|
if (value < -9999.99) {
|
|
return 0;
|
|
}
|
|
*fPos = value;
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
typedef struct {
|
|
void *obj;
|
|
pIDrivable pDriv;
|
|
SConnection *pCon;
|
|
char *name;
|
|
}DriveTaskData;
|
|
/*-------------------------------------------------------------------------*/
|
|
static void KillDriveTaskData(void *data)
|
|
{
|
|
DriveTaskData *taskData = (DriveTaskData *)data;
|
|
|
|
if(taskData == NULL){
|
|
return;
|
|
}
|
|
if(taskData->name != NULL){
|
|
free(taskData->name);
|
|
}
|
|
if(taskData->pCon != NULL){
|
|
SCDeleteConnection(taskData->pCon);
|
|
}
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void DriveTaskSignal(void *data, int iSignal, void *pSigData)
|
|
{
|
|
DriveTaskData *taskData = (DriveTaskData *)data;
|
|
int *interrupt;
|
|
|
|
assert(taskData != NULL);
|
|
|
|
if(iSignal == SICSINT){
|
|
interrupt = (int *)pSigData;
|
|
if(*interrupt != eContinue){
|
|
SCPrintf(taskData->pCon,eLogError,"ERROR: Interrupting %s", taskData->name);
|
|
taskData->pDriv->Halt(taskData->obj);
|
|
SCSetInterrupt(taskData->pCon,*interrupt);
|
|
}
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int DriveTaskFunc(void *data)
|
|
{
|
|
DriveTaskData *taskData = (DriveTaskData *)data;
|
|
int status;
|
|
|
|
assert(taskData != NULL);
|
|
|
|
|
|
status = taskData->pDriv->CheckStatus(taskData->obj,taskData->pCon);
|
|
if(status == HWBusy){
|
|
return 1;
|
|
}
|
|
if(status == HWFault){
|
|
taskData->pDriv->iErrorCount++;
|
|
}
|
|
if(status == HWFault || status == HWPosFault){
|
|
SetDevexecStatus(pServ->pExecutor,DEVERROR);
|
|
}
|
|
DevexecLog("STOP",taskData->name);
|
|
if(status == HWIdle || status == OKOK){
|
|
ExeInterest(pServ->pExecutor,taskData->name, "finished");
|
|
} else {
|
|
ExeInterest(pServ->pExecutor,taskData->name, "finished with problem");
|
|
}
|
|
return 0;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
long StartDriveTask(void *obj, SConnection *pCon, char *name, float fTarget)
|
|
{
|
|
pIDrivable pDriv = NULL;
|
|
char error[132], buffer[132];
|
|
DriveTaskData *taskData = NULL;
|
|
|
|
pDriv = GetDrivableInterface(obj);
|
|
if(pDriv == NULL){
|
|
SCPrintf(pCon,eError,"ERROR: %s is not drivable", name);
|
|
return -1;
|
|
}
|
|
if(pDriv->CheckLimits(obj,fTarget,error,sizeof(error)) != OKOK){
|
|
SCPrintf(pCon,eError,"ERROR: %s cannot reach %f, reason %s", name,
|
|
fTarget, error);
|
|
return -1;
|
|
}
|
|
taskData = calloc(1,sizeof(DriveTaskData));
|
|
if(taskData == NULL){
|
|
SCPrintf(pCon,eError,"ERROR: out of memory starting %s", name);
|
|
return -1;
|
|
}
|
|
if(pDriv->SetValue(obj,pCon,fTarget) != OKOK){
|
|
return -1;
|
|
}
|
|
ExeInterest(pServ->pExecutor,name,"started");
|
|
DevexecLog("START",name);
|
|
InvokeNewTarget(pServ->pExecutor,name,fTarget);
|
|
|
|
taskData->obj = obj;
|
|
taskData->pDriv = pDriv;
|
|
taskData->pCon = SCCopyConnection(pCon);
|
|
taskData->name = strdup(name);
|
|
|
|
return TaskRegisterN(pServ->pTasker,
|
|
name,
|
|
DriveTaskFunc,
|
|
DriveTaskSignal,
|
|
KillDriveTaskData,
|
|
taskData,0);
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
pICountable GetCountableInterface(void *pObject)
|
|
{
|
|
return (pICountable) FindInterface(pObject, COUNTID);
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
int GetCountLock(pICountable self, SConnection * pCon)
|
|
{
|
|
if (self->running == 1) {
|
|
SCWrite(pCon, "ERROR: someone else is already counting!", eError);
|
|
/* printf("Countlock denied\n"); */
|
|
return 0;
|
|
} else {
|
|
/* printf("Countlock aquired\n");*/
|
|
self->running = 1;
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
void ReleaseCountLock(pICountable self)
|
|
{
|
|
/* printf("Countlock released\n"); */
|
|
self->running = 0;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
int isRunning(pICountable self)
|
|
{
|
|
return self->running;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
typedef struct {
|
|
void *obj;
|
|
pICountable pCount;
|
|
SConnection *pCon;
|
|
char *name;
|
|
}CountTaskData;
|
|
/*-------------------------------------------------------------------------*/
|
|
static void KillCountTaskData(void *data)
|
|
{
|
|
CountTaskData *taskData = (CountTaskData *)data;
|
|
|
|
if(taskData == NULL){
|
|
return;
|
|
}
|
|
if(taskData->name != NULL){
|
|
free(taskData->name);
|
|
}
|
|
if(taskData->pCon != NULL){
|
|
SCDeleteConnection(taskData->pCon);
|
|
}
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void CountTaskSignal(void *data, int iSignal, void *pSigData)
|
|
{
|
|
CountTaskData *taskData = (CountTaskData *)data;
|
|
int *interrupt;
|
|
|
|
assert(taskData != NULL);
|
|
|
|
if(iSignal == SICSINT){
|
|
interrupt = (int *)pSigData;
|
|
if(*interrupt != eContinue){
|
|
SCPrintf(taskData->pCon,eLogError,"ERROR: Interrupting %s", taskData->name);
|
|
taskData->pCount->Halt(taskData->obj);
|
|
SCSetInterrupt(taskData->pCon,*interrupt);
|
|
}
|
|
} else if(iSignal == IPAUSE){
|
|
taskData->pCount->Pause(taskData->obj,taskData->pCon);
|
|
} else if(iSignal == CONTINUE){
|
|
taskData->pCount->Continue(taskData->obj,taskData->pCon);
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int CountTaskFunc(void *data)
|
|
{
|
|
CountTaskData *taskData = (CountTaskData *)data;
|
|
int status;
|
|
|
|
assert(taskData != NULL);
|
|
|
|
status = taskData->pCount->CheckCountStatus(taskData->obj,taskData->pCon);
|
|
if(status == HWBusy) {
|
|
SetStatus(eCounting);
|
|
return 1;
|
|
} else if(status == HWNoBeam){
|
|
SetStatus(eOutOfBeam);
|
|
return 1;
|
|
} else if(status == HWPause){
|
|
SetStatus(ePaused);
|
|
return 1;
|
|
}
|
|
|
|
taskData->pCount->TransferData(taskData->obj, taskData->pCon);
|
|
|
|
if(status == HWFault){
|
|
SetDevexecStatus(pServ->pExecutor,DEVERROR);
|
|
}
|
|
|
|
DevexecLog("STOP",taskData->name);
|
|
if(status == HWIdle || status == OKOK){
|
|
ExeInterest(pServ->pExecutor,taskData->name, "finished");
|
|
} else {
|
|
ExeInterest(pServ->pExecutor,taskData->name, "finished with problem");
|
|
}
|
|
return 0;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
long StartCountTask(void *obj, SConnection *pCon, char *name)
|
|
{
|
|
pICountable pCount = NULL;
|
|
char error[132], buffer[132];
|
|
CountTaskData *taskData = NULL;
|
|
|
|
pCount = FindInterface(obj,COUNTID);
|
|
if(pCount == NULL){
|
|
SCPrintf(pCon,eError,"ERROR: %s is not countable", name);
|
|
return 1;
|
|
}
|
|
taskData = calloc(1,sizeof(CountTaskData));
|
|
if(taskData == NULL){
|
|
SCPrintf(pCon,eError,"ERROR: out of memory starting %s", name);
|
|
return -1;
|
|
}
|
|
if(pCount->StartCount(obj,pCon) != OKOK){
|
|
pCount->running = 0;
|
|
return -1;
|
|
}
|
|
ExeInterest(pServ->pExecutor,name,"started");
|
|
DevexecLog("START",name);
|
|
|
|
taskData->obj = obj;
|
|
taskData->pCount = pCount;
|
|
taskData->pCon = SCCopyConnection(pCon);
|
|
taskData->name = strdup(name);
|
|
|
|
return TaskRegisterN(pServ->pTasker,
|
|
name,
|
|
CountTaskFunc,
|
|
CountTaskSignal,
|
|
KillCountTaskData,
|
|
taskData,0);
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
pICallBack GetCallbackInterface(void *pObject)
|
|
{
|
|
return (pICallBack) FindInterface(pObject, CALLBACKINTERFACE);
|
|
}
|