diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index d97e5da3..c4ac2fb3 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -46,7 +46,7 @@ extern double DoubleTime(void); * with valid values. * \see getDMCSetting */ -enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration}; +typedef enum report_type {eReportMotion, eReportIdle, eReportFinal} ReportType; enum commandtype {CMD_RUN=1, CMD_HALT=2}; typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv; @@ -1153,28 +1153,52 @@ static void set_lastMotion(pDMC2280Driv self, int steps, int counts) { gettimeofday(&(self->time_lastPos_set), NULL); } -static bool report_motion(pDMC2280Driv self, bool isIdle) { - SConnection *pDumCon; - MotCallback sCall; - double current_time, skip_time; - long lDelta; +/* + * TODO: idle and final reporting + * when final, always report MOTDRIVE and MOTEND + * when idle, if time and motion > threshold, report MOTDRIVE and MOTEND + * when neither, if time and motion > threshold, report MOTDRIVE but not MOTEND + */ +static bool report_motion(pDMC2280Driv self, ReportType reportType) { + double current_time; + bool doMotDrive = false; + bool doMotEnd = false; + if (self->pMot == NULL) self->pMot = FindMotor(pServ->pSics, self->name); assert(self->pMot); 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; + if (reportType == eReportFinal) { + doMotDrive = true; + doMotEnd = true; + } else { + double skip_time; + skip_time = 0.001 * getMotorParam(self, "movecount"); + if (self->lastReportTime + skip_time <= current_time) { + long lDelta; + if (self->abs_encoder) + lDelta = fabs(self->currCounts - self->lastReportedCounts); + else + lDelta = fabs(self->currSteps - self->lastReportedSteps); + if (reportType == eReportIdle) { + if (lDelta > 10) { + doMotDrive = true; + doMotEnd = true; + } + } else + doMotDrive = true; + } + } + if (doMotDrive || doMotEnd) { + SConnection *pDumCon; + MotCallback sCall; + pDumCon = SCCreateDummyConnection(pServ->pSics); + MotorGetSoftPosition(self->pMot, pDumCon, &sCall.fVal); + SCDeleteConnection(pDumCon); + sCall.pName = self->pMot->name; + if (doMotDrive) { InvokeCallBack(self->pMot->pCall, MOTDRIVE, &sCall); - if (isIdle) + if (doMotEnd) InvokeCallBack(self->pMot->pCall, MOTEND, &sCall); self->lastReportedCounts = self->currCounts; self->lastReportedSteps = self->currSteps; @@ -2361,8 +2385,12 @@ static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) { return; } /* if the motor moved, update any observers */ - if (report_motion(self, true)) - set_lastMotion(self, self->currSteps, self->currCounts); + if (self->doReportMotion) { + (void) report_motion(self, eReportFinal); + self->doReportMotion = false; + } else { + (void) report_motion(self, eReportIdle); + } if (trace_switches || self->debug) DMC_SetTimer(self, self->motorPollFast); else @@ -3172,6 +3200,7 @@ static void DMCState_Oscillate(pDMC2280Driv self, pEvtEvent event) { if (self->oscillate_count > 0 && self->oscillate_counter >= self->oscillate_count) self->doOscillate = false; if (!self->doOscillate) { + (void) report_motion(self, eReportFinal); change_state(self, DMCState_OffTimer); /* TODO: check this is OK */ return; } @@ -3325,7 +3354,7 @@ static void DMCState_StepMove(pDMC2280Driv self, pEvtEvent event) { } } if (self->doReportMotion) - (void) report_motion(self, false); + (void) report_motion(self, eReportMotion); /* if the status response can be handled here, return */ if (motorHandleStatus(self)) return;