diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 0d438d52..d97e5da3 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -196,6 +196,9 @@ struct __MoDriv { double lastPosition; /**< Position at last position check */ int lastSteps; int lastCounts; + double lastReportTime; + int lastReportedCounts; + int lastReportedSteps; double origPosition; /**< Position at motor start */ double origTime; /**< Time at motor start */ int origSteps; @@ -271,6 +274,7 @@ struct __MoDriv { double S_r; bool doTracking; TrackEntryBuffer *trackHead, *trackTail; + bool doReportMotion; /**< flag for reporting motion when not driving */ bool doOscillate; /**< flag for motor oscillation */ bool oscillate_up; /**< next oscillation move is to high position */ bool oscillate_init; /**< initial move is not counted */ @@ -1149,18 +1153,36 @@ static void set_lastMotion(pDMC2280Driv self, int steps, int counts) { gettimeofday(&(self->time_lastPos_set), NULL); } -static void report_motion(pDMC2280Driv self) { +static bool report_motion(pDMC2280Driv self, bool isIdle) { SConnection *pDumCon; MotCallback sCall; + double current_time, skip_time; + long lDelta; if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); assert(self->pMot); - pDumCon = SCCreateDummyConnection(pServ->pSics); - MotorGetSoftPosition(self->pMot,pDumCon,&sCall.fVal); - SCDeleteConnection(pDumCon); - sCall.pName = self->pMot->name; - InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall); - InvokeCallBack(self->pMot->pCall, MOTEND, &sCall); + current_time = DoubleTime(); + skip_time = 0.001 * getMotorParam(self, "movecount"); + if (self->lastReportTime + skip_time <= current_time) { + if (self->abs_encoder) + lDelta = fabs(self->currCounts - self->lastReportedCounts); + else + lDelta = fabs(self->currSteps - self->lastReportedSteps); + if (lDelta > 1) { + pDumCon = SCCreateDummyConnection(pServ->pSics); + MotorGetSoftPosition(self->pMot, pDumCon, &sCall.fVal); + SCDeleteConnection(pDumCon); + sCall.pName = self->pMot->name; + InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall); + if (isIdle) + InvokeCallBack(self->pMot->pCall, MOTEND, &sCall); + self->lastReportedCounts = self->currCounts; + self->lastReportedSteps = self->currSteps; + self->lastReportTime = current_time; + return true; + } + } + return false; } /** \brief Reads absolute encoder. @@ -2329,7 +2351,6 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { pAsyncTxn pCmd = event->event.msg.cmd; if (self->subState == 1) { /* Status Response */ int iRet; - long lDelta; iRet = rspStatus(self, pCmd->inp_buf); if (iRet == 0) break; @@ -2340,14 +2361,8 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { return; } /* if the motor moved, update any observers */ - if (self->abs_encoder) - lDelta = fabs(self->currCounts - self->lastCounts); - else - lDelta = fabs(self->currSteps - self->lastSteps); - if (lDelta > 10) { + if (report_motion(self, true)) set_lastMotion(self, self->currSteps, self->currCounts); - report_motion(self); - } if (trace_switches || self->debug) DMC_SetTimer(self, self->motorPollFast); else @@ -3309,6 +3324,8 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) { ent->Counts = self->currCounts; } } + if (self->doReportMotion) + (void) report_motion(self, false); /* if the status response can be handled here, return */ if (motorHandleStatus(self)) return; @@ -3972,6 +3989,7 @@ static int DMC2280Run(void *pData,float fValue){ if (fValue >= self->fLower && fValue <= self->fUpper) { self->fTarget = fValue; self->doOscillate = false; + self->doReportMotion = false; state_cmd_execute(self, CMD_RUN); return 1; } @@ -5492,6 +5510,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, else self->oscillate_up = false; self->doOscillate = true; + self->doReportMotion = true; self->oscillate_init = true; self->oscillate_counter = 0; state_cmd_execute(self, CMD_RUN); @@ -5561,6 +5580,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, self->fTarget = fHard; SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard); self->doOscillate = false; + self->doReportMotion = true; state_cmd_execute(self, CMD_RUN); } }