Add a disconnected state to the motor state machine, rename Unknown to Init

site_ansto/motor_dmc2280.c

@@ -273,6 +273,7 @@ static bool trace_switches = false;
 #define THREADZERO -21 /* Thread zero not running */
 //#define AMPERROR   -22 /* Amplifier OK not asserted */
 #define RUNERROR -23 /* Error from RUN variable */
+#define DISCONNECTED -24
 /*--------------------------------------------------------------------*/
 #define STATUSMOVING 128 /* Motor is moving */
 #define STATUSERRORLIMIT 64 /* Number of errors exceed limit */
@@ -1584,7 +1585,8 @@ static void state_trace_prn(pDMC2280Driv self) {
 
 /* State Functions */
 
-static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event);
+static void DMCState_Disconnected(pDMC2280Driv self, pEvtEvent event);
+static void DMCState_Init(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_MotorStart(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event);
@@ -1595,7 +1597,8 @@ static void DMCState_MotorStop(pDMC2280Driv self, pEvtEvent event);
 static void DMCState_Error(pDMC2280Driv self, pEvtEvent event);
 
 static char* state_name(StateFunc func) {
-  if (func == DMCState_Unknown) return "DMCState_Unknown";
+  if (func == DMCState_Disconnected) return "DMCState_Disconnected";
+  if (func == DMCState_Init) return "DMCState_Init";
   if (func == DMCState_Idle) return "DMCState_Idle";
   if (func == DMCState_MotorStart) return "DMCState_MotorStart";
   if (func == DMCState_MotorOn) return "DMCState_MotorOn";
@@ -1830,7 +1833,36 @@ static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd) {
   return 0;
 }
 
-static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
+static void DMCState_Disconnected(pDMC2280Driv self, pEvtEvent event) {
+  switch (event->event_type) {
+    case eStateEvent:
+      self->run_flag = 0;
+      self->driver_status = HWIdle;
+      if (AsyncUnitIsQueueConnected(self->asyncUnit) != 1)
+        self->errorCode = DISCONNECTED;
+      /* set a timer and wait until the device is connected */
+      DMC_SetTimer(self, self->motorPollSlow);
+      return;
+    case eTimerEvent:
+      if (self->pMot == NULL)
+        self->pMot = FindMotor(pServ->pSics, self->name);
+      if (self->pMot && AsyncUnitIsQueueConnected(self->asyncUnit) == 1) {
+        /* reconnected but state is unknown */
+        change_state(self, DMCState_Init);
+        return;
+      }
+      DMC_SetTimer(self, self->motorPollSlow);
+      return;
+    case eMessageEvent:
+      return;
+    case eCommandEvent:
+      return;
+    case eTimeoutEvent:
+      return;
+  }
+}
+
+static void DMCState_Init(pDMC2280Driv self, pEvtEvent event) {
   char cmd[CMDLEN];
   int value;
 
@@ -1884,7 +1916,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
       do {
         pAsyncTxn pCmd = event->event.msg.cmd;
         if (self->subState == 0) { /* waitResponse */
-          change_state(self, DMCState_Unknown);
+          change_state(self, DMCState_Init);
           return;
         }
         if (self->subState == 1) { /* Vars */
@@ -1921,7 +1953,7 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
       break;
     case eTimeoutEvent:
       /* handle timeout */
-      change_state(self, DMCState_Unknown);
+      change_state(self, DMCState_Init);
       return;
   }
   unhandled_event(self, event);
@@ -3048,6 +3080,9 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
     case RUNERROR:
       strncpy(error, self->dmc2280Error, (size_t)errLen);
       break;
+    case DISCONNECTED:
+      strncpy(error, "MOTION CONTROL DEVICE DISCONNECTED", (size_t)errLen);
+      break;
     default:
       /* What's the default */
       snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode);
@@ -3095,6 +3130,7 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
     case AMPERROR:
 #endif
     case RUNERROR:
+    case DISCONNECTED:
       return MOTFAIL;
     case POSFAULT:
       self->faultPending = false;
@@ -3102,7 +3138,7 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
     case STATEERROR:
       /* recover state error */
       DMC_ClearTimer(self);
-      change_state(self, DMCState_Unknown);
+      change_state(self, DMCState_Init);
       return MOTFAIL;
     default:
       return MOTFAIL;
@@ -3780,7 +3816,7 @@ static void DMC_Notify(void* context, int event) {
       snprintf(line, 132, "Disconnect on Motor '%s'", self->name);
       SICSLogWrite(line, eStatus);
       /* TODO: disconnect */
-      change_state(self, DMCState_Error);
+      change_state(self, DMCState_Disconnected);
       break;
     case AQU_RECONNECT:
       snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
@@ -3788,7 +3824,7 @@ static void DMC_Notify(void* context, int event) {
       /* TODO: reconnect */
       /* Reset the state machine */
       DMC_ClearTimer(self);
-      change_state(self, DMCState_Unknown);
+      change_state(self, DMCState_Disconnected);
       break;
   }
   return;
@@ -4160,7 +4196,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
     }
   }
 
-  change_state(pNew, DMCState_Unknown);
+  change_state(pNew, DMCState_Disconnected);
 
   return (MotorDriver *)pNew;
 }
@@ -4318,8 +4354,8 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
       /* Reset the state machine */
       DMC_ClearTimer(self);
       self->driver_status = HWIdle;
-      change_state(self, DMCState_Unknown);
-      while (self->myState == DMCState_Unknown) {
+      change_state(self, DMCState_Init);
+      while (self->myState == DMCState_Init) {
         pTaskMan pTasker = GetTasker();
         TaskYield(pTasker);
       }