- Fixes to el734hp.c for handling of emergency stop

- Added waiting 5 sec after driving A6 to tas code

el734driv.c

@@ -8,7 +8,7 @@
 
 	Copyright:
 
-	Labor fuer Neutronenstreuung
+	Labor fuer Neutrnenstreuung
 	Paul Scherrer Institut
 	CH-5423 Villigen-PSI
 

el734hp.c

@@ -77,6 +77,66 @@
 #define POSFAULT  -13
 #define BADCUSHION -14
 #define BADCOUNT   -15
+/*-------------------------------------------------------------------
+  This reruns the command when an emergency stop has been detected.
+  This ougth to fix the problem that there is still another *ES in the
+  line even after releasing the emergency stop button
+  --------------------------------------------------------------------*/
+static int transactEL734(prs232 self, void *send, int sendLen,
+			 void *reply, int replylen){
+  int status, len, oldTimeout;
+  char *pReply = NULL;
+
+  /*
+    try to read away rubbish on the line first
+  */
+  oldTimeout = getRS232Timeout(self);
+  setRS232Timeout(self,0);
+  if(availableRS232(self)){
+    len = replylen;
+    readRS232(self,reply,&len); 
+  }
+  setRS232Timeout(self,oldTimeout);
+
+
+  /*
+    Actually do something. Some controllers send one erroneus *ES when the
+    emergency stop had been released. Therefore we believe an emergency stop
+    message only at the second try. This is the logic below.
+  */
+  status = transactRS232(self,send,sendLen,reply,replylen);
+  if(status == 1){
+    pReply = (char *)reply;
+    if(strstr(pReply,"*ES") != NULL){
+      if(availableRS232(self)){
+	len = replylen;
+	readRS232TillTerm(self,reply,&len);
+      }
+      return transactRS232(self,send,sendLen,reply,replylen);
+    } else {
+      return status;
+    }
+  } else {
+    return status;
+  }
+}
+/*-------------------------------------------------------------------
+  If we get an empty reply when we expect some response data, then, maybe,
+  the controller has sent us "\r*ES\r". Therefore this checks on
+  empty messages if there is some more, i.e: the *ES
+  --------------------------------------------------------------------*/
+static void checkEmpty(pEL734Driv self, char *pReply, int *replylen){
+  int oldTimeout;
+
+  if(strlen(pReply) < 1) {
+    oldTimeout = getRS232Timeout(self->controller);
+    setRS232Timeout(self->controller,0);
+    if(availableRS232(self->controller)){
+      readRS232(self->controller,pReply,replylen);
+    }
+    setRS232Timeout(self->controller,oldTimeout);
+  }
+}
 /*--------------------------------------------------------------------*/
 static int checkResponse(pEL734Driv self, char *pReply){
   /*
@@ -111,7 +171,7 @@ static int checkResponse(pEL734Driv self, char *pReply){
 /*---------------------------------------------------------------------*/
 static int EL734GetPos(void *pData, float *fPos){
   pEL734Driv self = NULL;
-  int status;
+  int status, replyLen;
   char pCommand[50],pReply[80];
 
   self = (pEL734Driv)pData;
@@ -123,12 +183,16 @@ static int EL734GetPos(void *pData, float *fPos){
   }
 
   snprintf(pCommand,79,"u %d\r",self->iMotor);
-  status = transactRS232(self->controller,pCommand,strlen(pCommand),
+  status = transactEL734(self->controller,pCommand,strlen(pCommand),
 			 pReply,79);
   if(status != 1){
     self->errorCode = status;
     return HWFault;
   }
+
+  replyLen = 79;
+  checkEmpty(self,pReply,&replyLen);
+
   if(!checkResponse(self,pReply)){
     return HWFault;
   }
@@ -148,7 +212,7 @@ static int EL734Run(void *pData,float fValue){
 
   self->oredMsr = 0;
   snprintf(pCommand,79,"p %d %.3f\r",self->iMotor,fValue);
-  status = transactRS232(self->controller,pCommand,strlen(pCommand),
+  status = transactEL734(self->controller,pCommand,strlen(pCommand),
 			 pReply,79);
   if(status != 1){
     self->errorCode = status;
@@ -189,9 +253,6 @@ static int decodeMSR(pEL734Driv self, int msr){
     } else if(self->oredMsr & 0x1000){
       self->errorCode = BADCUSHION;
       return HWFault;
-    } else if(self->oredMsr & 0x8){
-      self->errorCode = BADSTP;
-      return HWFault;
     } else if(self->oredMsr & 0x40) {
       self->errorCode = BADSTP;
       return HWFault;
@@ -225,19 +286,22 @@ static int decodeMSR(pEL734Driv self, int msr){
 /*------------------------------------------------------------------------*/
 static int EL734Status(void *pData){
   pEL734Driv self = NULL;
-  int status, msr;
+  int status, msr, replyLen = 79;
   char pCommand[50],pReply[80];
 
   self = (pEL734Driv)pData;
   assert(self);
 
   snprintf(pCommand,79,"msr %d\r",self->iMotor);
-  status = transactRS232(self->controller,pCommand,strlen(pCommand),
+  status = transactEL734(self->controller,pCommand,strlen(pCommand),
 			 pReply,79);
   if(status < 0){
     self->errorCode = status;
     return HWFault;
   }
+
+  checkEmpty(self,pReply,&replyLen);
+
   if(!checkResponse(self,pReply)){
     return HWFault;
   }
@@ -345,9 +409,17 @@ static int EL734Fix(void *pData, int iCode, float fValue){
       return MOTREDO;
     }
     break;
+  case BADUNKNOWN:
+    if(availableRS232(self->controller)){
+      len = 79;
+      readRS232TillTerm(self->controller,pReply,&len);
+      return MOTREDO;
+    }
+    return MOTFAIL;
+    break;
   case BADLOC:
     snprintf(pCommand,49,"RMT 1\r");
-    transactRS232(self->controller,pCommand,strlen(pCommand),pReply,79);
+    transactEL734(self->controller,pCommand,strlen(pCommand),pReply,79);
     return  MOTREDO;
   case NOTCONNECTED:
     initRS232(self->controller);
@@ -384,7 +456,7 @@ static int EL734Halt(void *pData){
   assert(self);
 
   snprintf(pCommand,79,"s %d\r",self->iMotor);
-  status = transactRS232(self->controller,pCommand,strlen(pCommand),
+  status = transactEL734(self->controller,pCommand,strlen(pCommand),
 			 pReply,79);
   if(status != 1){
     self->errorCode = status;
@@ -399,19 +471,22 @@ static int EL734Halt(void *pData){
 static int EL734GetPar(void *pData, char *name,
 		       float *fValue){
   pEL734Driv self = NULL;
-  int status;
+  int status, replyLen = 79;
   char pCommand[50],pReply[80];
 
   self = (pEL734Driv)pData;
   assert(self);
   if(strcmp(name,"speed") == 0){
     snprintf(pCommand,79,"J %d\r",self->iMotor);
-    status = transactRS232(self->controller,pCommand,strlen(pCommand),
+    status = transactEL734(self->controller,pCommand,strlen(pCommand),
 			 pReply,79);
     if(status != 1){
       self->errorCode = status;
       return 0;
     }
+
+    checkEmpty(self,pReply,&replyLen);
+
     if(!checkResponse(self,pReply)){
       return 0;
     }
@@ -429,15 +504,32 @@ static int EL734SetPar(void *pData, SConnection *pCon,
 
   self = (pEL734Driv)pData;
   assert(self);
+  pCommand[0] ='\0';
   if(strcmp(name,"speed") == 0){
     snprintf(pCommand,79,"J %d %d\r",self->iMotor,(int)newValue);
-    status = transactRS232(self->controller,pCommand,strlen(pCommand),
+  } else if(strcmp(name,"forceupper") == 0){
+    if(!SCMatchRights(pCon,usMugger)){
+      return 0;
+    }
+    self->fUpper = newValue;
+    snprintf(pCommand,79,"H %d %8.3f %8.3f\r",self->iMotor,self->fLower, self->fUpper);
+  } else if(strcmp(name,"forcelower") == 0){
+    if(!SCMatchRights(pCon,usMugger)){
+      return 0;
+    }
+    self->fLower = newValue;
+    snprintf(pCommand,79,"H %d %8.3f %8.3f\r",self->iMotor,self->fLower, self->fUpper);
+  }
+  if(strlen(pCommand) > 1){
+    status = transactEL734(self->controller,pCommand,strlen(pCommand),
 			 pReply,79);
     if(status != 1){
       self->errorCode = status;
       return 0;
     }
     if(!checkResponse(self,pReply)){
+      snprintf(pCommand,79,"ERROR %s while setting parameter",pReply);
+      SCWrite(pCon,pCommand,eError);
       return 0;
     }
     return 1;
@@ -512,14 +604,14 @@ MotorDriver *CreateEL734HP(SConnection *pCon, int argc, char *argv[]){
   pNew->KillPrivate = KillEL734;
   pNew->controller = controller;
   pNew->iMotor = motor;
-  
+
   /*
     connection will already have been set up, read limits
   */
   snprintf(pCommand,49,"h %d\r",pNew->iMotor);
   success = 0;
   for(i = 0; i < 3; i++){
-    status = transactRS232(pNew->controller, pCommand,strlen(pCommand),
+    status = transactEL734(pNew->controller, pCommand,strlen(pCommand),
 			 pReply,79);
     if(status != 1){
       getRS232Error(status,pReply,79);

tasdrive.c

@@ -301,6 +301,10 @@ int TASDrive(SConnection *pCon, SicsInterp *pSics, void *pData,
    */
    status = Wait4Success(GetExecutor());
    TASUpdate(self,pCon);
+
+   /*
+     handle interrupts
+   */
    if(status == DEVINT)
    {
       if(SCGetInterrupt(pCon) == eAbortOperation)

tasinit.c

@@ -298,7 +298,18 @@ static int RecalcAction(SConnection *pCon, SicsInterp *pSics, void *pData,
 
    return TASUpdate(self,pCon);
 }
-
+/*--------------------------------------------------------------------------
+ when A6 has been drive Bertrand wants to wait for some seconds in order
+ to allow for the analyzer shielding to settle down. This is done
+ through this callback function
+ ---------------------------------------------------------------------------*/
+static int A6WaitCallback(int iEvent, void *pEventData, void *pUserData)
+{
+  if(iEvent == MOTEND)
+  {
+    SicsWait(5);
+  }
+}
 /*-----------------------------------------------------------------------
   A function for killing the TAS data structure is needed
   -------------------------------------------------------------------------*/
@@ -321,6 +332,7 @@ int TASFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
    char pBueffel[132];
    pSicsVariable pVar = NULL;
    CommandList *pCom = NULL;
+   pMotor pMot = NULL;
 
    /* check arguments*/
    if(argc < 2)
@@ -395,6 +407,16 @@ int TASFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
      RegisterCallback(pVar->pCall,VALUECHANGE,TimerCallback,pNew,NULL);
    }
 
+   /*
+     Install the wait callback for A6
+   */
+   pMot = FindMotor(pSics,"a6");
+   if(pMot != NULL)
+   {
+     RegisterCallback(pMot->pCall,MOTEND,A6WaitCallback,NULL,NULL);
+   }
+
+
    /*
      initialize SRO value
    */