Added 'interest' method

confvirtualmot.c

@@ -8,6 +8,7 @@
   COPYRIGHT: see file COPYRIGHT
 
   Mark Koennecke, August 2004
+  interest added: Ferdi Franceschini, April 2006
 -------------------------------------------------------------------------*/
 #include <stdlib.h>
 #include <assert.h>
@@ -31,6 +32,20 @@ typedef struct {
   float value;
   int running;
 } RealMotor, *pRealMotor;
+
+/* Data passed by event generating object */
+typedef struct {
+  float fVal;
+  char *pName;
+} EventInfo; 
+
+/* Data available when registering interest */
+typedef struct {
+  char *pName;
+  SConnection *pCon;
+  float lastValue; 
+} RegisteredInfo, *pRegisteredInfo;   
+
 /*---------------------------------------------------------------------*/
 static int HaltMotors(void *pData){
   pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
@@ -172,6 +187,7 @@ static long ConfSetValue(void *pData, SConnection *pCon, float newValue){
   }
   self->targetValue = newValue;
   self->targetReached = 0;
+  self->posCount = 0;
 
   status = startMotorList(self,pCon);
   if(status != OKOK){
@@ -214,11 +230,51 @@ static int ConfCheckLimits(void *pData, float fVal, char *error, int errLen){
   }
   return 1;
 }
+
+  static void KillInfo(void *pData)
+  {
+    pRegisteredInfo self = NULL;
+    
+    assert(pData);
+    self = (pRegisteredInfo)pData;
+    if(self->pName)
+    {
+      free(self->pName);
+    }
+    free(self);
+  }                 
+/*------------------- The CallBack function for interest ------------------
+ * iEvent: Event ID, see event.h for SICS events
+ * pEvent: May contain data from event generating object
+ * pUser: Data available when registering interest, see RegisteredInfo struct
+ *        defined above for available info */
+  static int InterestCallback(int iEvent, void *pEvent, void *pUser,
+			      commandContext cc)
+  {
+     pRegisteredInfo pInfo = NULL;
+     char pBueffel[80];
+     EventInfo *pEventData;
+     
+     assert(pEvent);
+     assert(pUser);
+  
+     pEventData = (EventInfo *)pEvent;
+     pInfo = (RegisteredInfo *)pUser;
+
+     if (pInfo->lastValue != pEventData->fVal) {
+       pInfo->lastValue = pEventData->fVal;    
+       (pInfo->pCon)->conEventType=POSITION;
+       sprintf(pBueffel,"%s.position = %f ", pInfo->pName, pInfo->lastValue);
+       SCWriteInContext(pInfo->pCon,pBueffel,eEvent,cc);
+     }
+     return 1;
+  }  
 /*------------------------------------------------------------------------*/
 static int ConfCheckStatus(void *pData, SConnection *pCon){
   pConfigurableVirtualMotor self = (pConfigurableVirtualMotor)pData;
   RealMotor tuktuk;
   int iRet, status, result;
+  EventInfo event;
 
   result = HWIdle;
   iRet = LLDnodePtr2First(self->motorList);
@@ -248,6 +304,21 @@ static int ConfCheckStatus(void *pData, SConnection *pCon){
     }
     iRet = LLDnodePtr2Next(self->motorList);
   }
+
+  if (result == HWIdle) {
+      event.pName = self->name;
+      event.fVal = self->pDriv->GetValue(self,pCon);
+      InvokeCallBack(self->pCall, MOTDRIVE, &event);
+  } else if (result == HWBusy) {
+    self->posCount++;
+    if(self->posCount >= 10/*ObVal(self->ParArray,MOVECOUNT)*/)
+    {
+      event.pName = self->name;
+      event.fVal = self->pDriv->GetValue(self,pCon);
+      InvokeCallBack(self->pCall, MOTDRIVE, &event);
+      self->posCount = 0;
+    }
+  }
   return result;
 }
 /*---------------------------------------------------------------------*/
@@ -321,6 +392,9 @@ static void *GetConfigurableVirtualMotorInterface(void *pData, int iID){
   assert(self);
   if(iID == DRIVEID){
     return self->pDriv;
+  } else if(iID == CALLBACKINTERFACE)
+  {
+    return self->pCall;
   }
   return NULL;
 }
@@ -335,6 +409,10 @@ static void KillConfigurableVirtualMotor(void *data){
       self->pDes = NULL;
     }
     LLDdelete(self->motorList);
+    if (self->name != NULL) {
+      free(self->name);
+      self->name = NULL;
+    }
     if(self->scriptName != NULL){
       free(self->scriptName);
       self->scriptName = NULL;
@@ -366,6 +444,8 @@ int MakeConfigurableVirtualMotor(SConnection *pCon, SicsInterp *pSics,
   }
   memset(pNew,0,sizeof(ConfigurableVirtualMotor));
 
+  pNew->name = strdup(argv[1]);
+  pNew->posCount = 0;
   pNew->pDes = CreateDescriptor("ConfigurableVirtualMotor");
   pNew->pDriv = CreateDrivableInterface();
   pNew->motorList = LLDcreate(sizeof(RealMotor));
@@ -385,6 +465,13 @@ int MakeConfigurableVirtualMotor(SConnection *pCon, SicsInterp *pSics,
   pNew->pDriv->CheckStatus = ConfCheckStatus;
   pNew->pDriv->GetValue = ConfGetValue;
 
+   /* initialise callback interface */
+   pNew->pCall = CreateCallBackInterface();
+   if(!pNew->pCall)
+   {
+     KillConfigurableVirtualMotor(pNew);
+     return 0;
+   }
   /*
     install command
   */
@@ -406,6 +493,9 @@ int ConfigurableVirtualMotorAction(SConnection *pCon, SicsInterp *pSics,
   pConfigurableVirtualMotor self = NULL;
   char pBueffel[512];
   float value;
+  int iRet;
+  long lID;
+  pRegisteredInfo pRegInfo = NULL;
 
   self = (pConfigurableVirtualMotor)data;
   assert(self != NULL);
@@ -452,6 +542,29 @@ int ConfigurableVirtualMotorAction(SConnection *pCon, SicsInterp *pSics,
 	  return 1;
 	}
       }
+    } else if(strcmp(argv[1],"interest") == 0)
+     {
+        pRegInfo = (pRegisteredInfo)malloc(sizeof(RegisteredInfo));
+        if(!pRegInfo)
+        {
+          SCWrite(pCon,"ERROR: out of memory in confvirtualmot.c",eError);
+          return 0;
+        }
+        pRegInfo->pName = strdup(argv[0]);
+        pRegInfo->pCon = pCon;
+        value = ConfGetValue(self,pCon);
+        if(!iRet)
+        {
+              sprintf(pBueffel,"Failed to register interest, Reason:Error obtaining current position for %s",argv[0]);
+              SCWrite(pCon,pBueffel,eError);
+              return 0;
+         }
+        pRegInfo->lastValue = value;
+
+        lID = RegisterCallback(self->pCall, SCGetContext(pCon),MOTDRIVE, InterestCallback, pRegInfo, KillInfo);
+        SCRegister(pCon,pSics, self->pCall,lID);
+        SCSendOK(pCon);
+        return 1;                       
      } else {
       snprintf(pBueffel,5120,"ERROR: subcommand %s to %s unknown",
 	       argv[1],argv[0]);