make tracking rate changes more robustHEADmaster

2 files changed, 44 insertions, 20 deletions

diff --git a/indi-celestronaux/celestronaux.cpp b/indi-celestronaux/celestronaux.cpp
index 4add9d2..d0a1932 100644
--- a/indi-celestronaux/celestronaux.cpp
+++ b/indi-celestronaux/celestronaux.cpp
@@ -1680,7 +1680,7 @@ void CelestronAUX::TimerHit()
 #endif
 
                     // Use PID to determine appropiate tracking rate
-                    trackByRate(AXIS_AZ, trackRates[AXIS_AZ]);
+                    trackByRate(AXIS_AZ, trackRates[AXIS_AZ], false);
                 }
 
                 // Only apply trackinf IF we're still on the same side of the curve
@@ -1700,7 +1700,7 @@ void CelestronAUX::TimerHit()
                                m_Controllers[AXIS_ALT]->integralTerm(),
                                m_Controllers[AXIS_ALT]->derivativeTerm());
 #endif
-                    trackByRate(AXIS_ALT, trackRates[AXIS_ALT]);
+                    trackByRate(AXIS_ALT, trackRates[AXIS_ALT], false);
                 }
             } else if (TrackMethodSP[TRACK_METHOD_DRIVER].getState() == ISS_ON) {
                 double r_ra, r_dec;
@@ -1722,7 +1722,7 @@ void CelestronAUX::TimerHit()
                 }
 #endif
 
-                CelestronAUX::SetTrackRate(r_ra, r_dec);
+                CelestronAUX::SetTrackRate(r_ra, r_dec, false);
             }
         }
         break;
@@ -1754,15 +1754,15 @@ bool CelestronAUX::track_method_changed(void)
 
     if (TrackMethodSP[TRACK_METHOD_MOUNT].getState() == ISS_ON) {
         if (IUFindOnSwitchIndex(&TrackModeSP) == TRACK_CUSTOM)
-            return SetTrackRate(TrackRateN[AXIS_AZ].value, TrackRateN[AXIS_ALT].value);
+            return SetTrackRate(TrackRateN[AXIS_AZ].value, TrackRateN[AXIS_ALT].value, true);
         else
             return SetTrackMode(IUFindOnSwitchIndex(&TrackModeSP));
     }
  
     if (TrackMethodSP[TRACK_METHOD_NONE].getState() == ISS_ON) {
         syslog_proxy("CelestronAUX::trackByRate 0 0");
-        trackByRate(AXIS_AZ, 0);
-        trackByRate(AXIS_ALT, 0);
+        trackByRate(AXIS_AZ, 0, true);
+        trackByRate(AXIS_ALT, 0, true);
         return true;
     }
 
@@ -2056,7 +2056,7 @@ bool CelestronAUX::isSlewing()
 bool CelestronAUX::slewTo(INDI_HO_AXIS axis, uint32_t steps, bool fast)
 {
     // Stop first.
-    trackByRate(axis, 0);
+    trackByRate(axis, 0, true);
     AUXCommand command(fast ? MC_GOTO_FAST : MC_GOTO_SLOW, APP, axis == AXIS_AZ ? AZM : ALT);
     m_AxisStatus[axis] = SLEWING;
     command.setData(steps, 3);
@@ -2071,7 +2071,7 @@ bool CelestronAUX::slewTo(INDI_HO_AXIS axis, uint32_t steps, bool fast)
 bool CelestronAUX::slewByRate(INDI_HO_AXIS axis, int8_t rate)
 {
     // Stop first.
-    trackByRate(axis, 0);
+    trackByRate(axis, 0, true);
     AUXCommand command(rate >= 0 ? MC_MOVE_POS : MC_MOVE_NEG, APP, axis == AXIS_AZ ? AZM : ALT);
     command.setData(std::abs(rate), 1);
     sendAUXCommand(command);
@@ -2202,7 +2202,7 @@ uint32_t CelestronAUX::getCordWrapPosition()
 bool CelestronAUX::stopAxis(INDI_HO_AXIS axis)
 {
     m_AxisStatus[axis] = STOPPED;
-    trackByRate(axis, 0);
+    trackByRate(axis, 0, true);
     AUXCommand command(MC_MOVE_POS, APP, (axis == AXIS_ALT) ? ALT : AZM);
     command.setData(0, 1);
     sendAUXCommand(command);
@@ -2240,9 +2240,10 @@ bool CelestronAUX::Abort()
 /// rate = 80 would cause the motor to spin at a rate of 1 step/s
 /// Have to check if 80 is specific to my Evolution 6" or not.
 /////////////////////////////////////////////////////////////////////////////////////
-bool CelestronAUX::trackByRate(INDI_HO_AXIS axis, int32_t rate)
+bool CelestronAUX::trackByRate(INDI_HO_AXIS axis, int32_t rate, bool force)
 {
-    if (std::abs(rate) > 0 && rate == m_LastTrackRate[axis])
+    //if (std::abs(rate) > 0 && rate == m_LastTrackRate[axis])
+    if (!force && (rate == m_LastTrackRate[axis]))
         return true;
 
     m_LastTrackRate[axis] = rate;
@@ -2260,6 +2261,7 @@ bool CelestronAUX::trackByRate(INDI_HO_AXIS axis, int32_t rate)
 bool CelestronAUX::trackByMode(INDI_HO_AXIS axis, uint8_t mode)
 {
     AUXCommand command(isNorthHemisphere() ? MC_SET_POS_GUIDERATE : MC_SET_NEG_GUIDERATE, APP, axis == AXIS_AZ ? AZM : ALT);
+    m_LastTrackRate[axis] =~0;
     switch (mode)
     {
         case TRACK_SOLAR:
@@ -2293,7 +2295,7 @@ bool CelestronAUX::SetTrackEnabled(bool enabled)
 
         if (TrackMethodSP[TRACK_METHOD_MOUNT].getState() == ISS_ON) {
             if (IUFindOnSwitchIndex(&TrackModeSP) == TRACK_CUSTOM)
-                return SetTrackRate(TrackRateN[AXIS_AZ].value, TrackRateN[AXIS_ALT].value);
+                return SetTrackRate(TrackRateN[AXIS_AZ].value, TrackRateN[AXIS_ALT].value, true);
             else
                 return SetTrackMode(IUFindOnSwitchIndex(&TrackModeSP));
         }
@@ -2302,8 +2304,8 @@ bool CelestronAUX::SetTrackEnabled(bool enabled)
     {
         TrackState = SCOPE_IDLE;
         syslog_proxy("CelestronAUX::trackByRate 0 0");
-        trackByRate(AXIS_AZ, 0);
-        trackByRate(AXIS_ALT, 0);
+        trackByRate(AXIS_AZ, 0, true);
+        trackByRate(AXIS_ALT, 0, true);
 
         if (HorizontalCoordsNP.getState() != IPS_IDLE)
         {
@@ -2320,19 +2322,39 @@ bool CelestronAUX::SetTrackEnabled(bool enabled)
 /////////////////////////////////////////////////////////////////////////////////////
 bool CelestronAUX::SetTrackRate(double raRate, double deRate)
 {
+    return CelestronAUX::SetTrackRate(raRate, deRate, false);
+}
+
+static int32_t rate_round(double d)
+{
+if (d>0.) return (int32_t) ( d+0.5);
+	else return - (int32_t) (-d +0.5);
+}
+
+
+bool CelestronAUX::SetTrackRate(double raRate, double deRate, bool force)
+{
     m_TrackRates[AXIS_AZ] = raRate;
     m_TrackRates[AXIS_ALT] = deRate;
 
     if (TrackState == SCOPE_TRACKING)
     {
-
         double steps[2] = {0, 0};
+	int32_t rates[2] = {0, 0};
+	unsigned i;
+	
         // rate = (steps) * gain
         steps[AXIS_AZ] = raRate * STEPS_PER_ARCSEC * GAIN_STEPS;
         steps[AXIS_ALT] = deRate * STEPS_PER_ARCSEC * GAIN_STEPS;
         syslog_proxy("CelestronAUX::trackByRate %.2f %.2f", steps[AXIS_AZ], steps[AXIS_ALT]);
-        trackByRate(AXIS_AZ, steps[AXIS_AZ]);
-        trackByRate(AXIS_ALT, steps[AXIS_ALT]);
+
+	for (i=0; i<2; ++i) {
+	    rates[i] = rate_round(steps[i]);
+	    if (rates[i] != m_LastTrackRate[i])
+                force = true;
+        }
+        trackByRate(AXIS_AZ, rates[AXIS_AZ], force);
+        trackByRate(AXIS_ALT, rates[AXIS_ALT], force);
     }
     return true;
 }
@@ -2364,12 +2386,13 @@ bool CelestronAUX::SetTrackMode(uint8_t mode)
                 steps[AXIS_AZ] = m_TrackRates[AXIS_AZ] * STEPS_PER_ARCSEC * GAIN_STEPS;
                 steps[AXIS_ALT] = m_TrackRates[AXIS_ALT] * STEPS_PER_ARCSEC * GAIN_STEPS;
                 syslog_proxy("CelestronAUX::trackByRate %.2f %.2f", steps[AXIS_AZ], steps[AXIS_ALT]);
-                trackByRate(AXIS_AZ, steps[AXIS_AZ]);
-                trackByRate(AXIS_ALT, steps[AXIS_ALT]);
+                trackByRate(AXIS_AZ, rate_round(steps[AXIS_AZ]), true);
+                trackByRate(AXIS_ALT, rate_round(steps[AXIS_ALT]), true);
             }
             else {
                 syslog_proxy("CelestronAUX::trackByMode %d", mode);
                 trackByMode(AXIS_AZ, mode);
+                trackByRate(AXIS_ALT, 0, true);
             }
         }
     }
diff --git a/indi-celestronaux/celestronaux.h b/indi-celestronaux/celestronaux.h
index 7c5dc29..eb5c81d 100644
--- a/indi-celestronaux/celestronaux.h
+++ b/indi-celestronaux/celestronaux.h
@@ -170,6 +170,7 @@ class CelestronAUX :
         bool SetTrackEnabled(bool enabled) override;
         bool SetTrackMode(uint8_t mode) override;
         bool SetTrackRate(double raRate, double deRate) override;
+        bool SetTrackRate(double raRate, double deRate, bool force);
         void resetTracking();
 
         /**
@@ -180,7 +181,7 @@ class CelestronAUX :
          * For Alt, negative is down while positive is up.
          * @return True if successful, false otherwise.
          */
-        bool trackByRate(INDI_HO_AXIS axis, int32_t rate);
+        bool trackByRate(INDI_HO_AXIS axis, int32_t rate, bool force);
 
         /**
          * @brief trackByRate Track using specific mode (sidereal, solar, or lunar)