From 6311d15b72109fa2be40d9d45c39ef9eb4b9ab79 Mon Sep 17 00:00:00 2001 From: Jasem Mutlaq Date: Tue, 27 Aug 2024 22:15:43 +0300 Subject: [PATCH] Update Tracking code (#2105) * Update tracking code to use Pawel predictive rate method * better logging * Fix offset steps * Fine tuned default values for AZ-GTi. Needs to be tested for other mounts * Stop when changing directions * Fix an issue where the client may be get disconnected from server * Add limit switch indicators * Fix warning --- drivers/dome/universal_ror.cpp | 13 + drivers/dome/universal_ror.h | 1 + drivers/dome/universal_ror_client.cpp | 1 + drivers/telescope/skywatcherAPIMount.cpp | 694 +++++++++++++++-------- drivers/telescope/skywatcherAPIMount.h | 42 +- libs/indicore/indicom.c | 10 + libs/indicore/indicom.h | 7 + 7 files changed, 537 insertions(+), 231 deletions(-) diff --git a/drivers/dome/universal_ror.cpp b/drivers/dome/universal_ror.cpp index 418a17f60d..a192ca8db5 100644 --- a/drivers/dome/universal_ror.cpp +++ b/drivers/dome/universal_ror.cpp @@ -45,6 +45,11 @@ bool UniversalROR::initProperties() InputTP.fill(getDeviceName(), "INPUT_INDEX", "Input Indexes", OPTIONS_TAB, IP_RW, 60, IPS_IDLE); InputTP.load(); + // Limit Switch Indicators + LimitSwitchLP[FullyOpened].fill("FULLY_OPENED", "Fully Opened", IPS_IDLE); + LimitSwitchLP[FullyClosed].fill("FULLY_CLOSED", "Fully Closed", IPS_IDLE); + LimitSwitchLP.fill(getDeviceName(), "LIMIT_SWITCHES", "Limit Switches", MAIN_CONTROL_TAB, IPS_IDLE); + // Output Indexes OutputTP[OpenRoof].fill("OPEN_ROOF", "Open Roof", "Comma separated indexes"); OutputTP[CloseRoof].fill("CLOSE_ROOF", "Close Roof", "Comma separated indexes"); @@ -117,6 +122,8 @@ bool UniversalROR::Connect() //////////////////////////////////////////////////////////////////////////////// bool UniversalROR::Disconnect() { + m_InputFullyOpened.clear(); + m_InputFullyClosed.clear(); return true; } @@ -182,11 +189,13 @@ bool UniversalROR::updateProperties() defineProperty(InputTP); defineProperty(OutputTP); + defineProperty(LimitSwitchLP); } else { deleteProperty(InputTP); deleteProperty(OutputTP); + deleteProperty(LimitSwitchLP); } return true; @@ -366,10 +375,14 @@ void UniversalROR::ActiveDevicesUpdated() m_Client->setFullyClosedCallback([&](bool on) { m_FullClosedLimitSwitch = on; + LimitSwitchLP[FullyClosed].setState(on ? IPS_OK : IPS_IDLE); + LimitSwitchLP.apply(); }); m_Client->setFullyOpenedCallback([&](bool on) { m_FullOpenLimitSwitch = on; + LimitSwitchLP[FullyOpened].setState(on ? IPS_OK : IPS_IDLE); + LimitSwitchLP.apply(); }); m_Client->watchDevice(input.c_str()); m_Client->watchDevice(output.c_str()); diff --git a/drivers/dome/universal_ror.h b/drivers/dome/universal_ror.h index 974b25ca41..5eedd7dcfc 100644 --- a/drivers/dome/universal_ror.h +++ b/drivers/dome/universal_ror.h @@ -56,6 +56,7 @@ class UniversalROR : public INDI::Dome bool m_FullOpenLimitSwitch {false}, m_FullClosedLimitSwitch {false}; INDI::PropertyText InputTP {2}; + INDI::PropertyLight LimitSwitchLP {2}; enum { FullyOpened, diff --git a/drivers/dome/universal_ror_client.cpp b/drivers/dome/universal_ror_client.cpp index 9bdb4cbdf9..4c8d2202e8 100644 --- a/drivers/dome/universal_ror_client.cpp +++ b/drivers/dome/universal_ror_client.cpp @@ -73,6 +73,7 @@ void UniversalRORClient::newProperty(INDI::Property property) /////////////////////////////////////////////////////////////////////////////////////////// void UniversalRORClient::serverDisconnected(int exitCode) { + INDI_UNUSED(exitCode); m_InputReady = m_OutputReady = false; } diff --git a/drivers/telescope/skywatcherAPIMount.cpp b/drivers/telescope/skywatcherAPIMount.cpp index 82271b2197..6b6d6d0eee 100644 --- a/drivers/telescope/skywatcherAPIMount.cpp +++ b/drivers/telescope/skywatcherAPIMount.cpp @@ -226,14 +226,14 @@ bool SkywatcherAPIMount::initProperties() SnapPortSP.fill(getDeviceName(), "SNAP_PORT", "Snap Port", MAIN_CONTROL_TAB, IP_RW, ISR_1OFMANY, 60, IPS_IDLE); // PID Control - Axis1PIDNP[Propotional].fill("Propotional", "Propotional", "%.2f", 0.1, 100, 1, 1.1); - Axis1PIDNP[Derivative].fill("Derivative", "Derivative", "%.2f", 0, 500, 10, 0.01); - Axis1PIDNP[Integral].fill("Integral", "Integral", "%.2f", 0, 500, 10, 0.65); + Axis1PIDNP[Propotional].fill("Propotional", "Propotional", "%.2f", 0.1, 100, 1, 0.1); + Axis1PIDNP[Derivative].fill("Derivative", "Derivative", "%.2f", 0, 500, 10, 0.05); + Axis1PIDNP[Integral].fill("Integral", "Integral", "%.2f", 0, 500, 10, 0.05); Axis1PIDNP.fill(getDeviceName(), "AXIS1_PID", "Axis1 PID", TRACKING_TAB, IP_RW, 60, IPS_IDLE); - Axis2PIDNP[Propotional].fill("Propotional", "Propotional", "%.2f", 0.1, 100, 1, 0.75); - Axis2PIDNP[Derivative].fill("Derivative", "Derivative", "%.2f", 0, 100, 10, 0.01); - Axis2PIDNP[Integral].fill("Integral", "Integral", "%.2f", 0, 100, 10, 0.13); + Axis2PIDNP[Propotional].fill("Propotional", "Propotional", "%.2f", 0.1, 100, 1, 0.2); + Axis2PIDNP[Derivative].fill("Derivative", "Derivative", "%.2f", 0, 100, 10, 0.1); + Axis2PIDNP[Integral].fill("Integral", "Integral", "%.2f", 0, 100, 10, 0.1); Axis2PIDNP.fill(getDeviceName(), "AXIS2_PID", "Axis2 PID", TRACKING_TAB, IP_RW, 60, IPS_IDLE); // Dead Zone @@ -1098,219 +1098,9 @@ void SkywatcherAPIMount::TimerHit() } else { - // Continue or start tracking - // Calculate where the mount needs to be in POLLMS time - // TODO may need to make this longer to get a meaningful result - //double JulianOffset = (getCurrentPollingPeriod() / 1000) / (24.0 * 60 * 60); - TelescopeDirectionVector TDV; - INDI::IHorizontalCoordinates AltAz { 0, 0 }; - - // We modify the SkyTrackingTarget for non-sidereal objects (Moon or Sun) - // FIXME: This was not tested. - if (TrackModeS[TRACK_LUNAR].s == ISS_ON) - { - // TRACKRATE_LUNAR how many arcsecs the Moon moved in one second. - // TRACKRATE_SIDEREAL how many arcsecs the Sky moved in one second. - double dRA = (TRACKRATE_LUNAR - TRACKRATE_SIDEREAL) * m_TrackingRateTimer.elapsed() / 1000.0; - m_SkyTrackingTarget.rightascension += (dRA / 3600.0) * 15.0; - m_TrackingRateTimer.restart(); - } - else if (TrackModeS[TRACK_SOLAR].s == ISS_ON) - { - double dRA = (TRACKRATE_SOLAR - TRACKRATE_SIDEREAL) * m_TrackingRateTimer.elapsed() / 1000.0; - m_SkyTrackingTarget.rightascension += (dRA / 3600.0) * 15.0; - m_TrackingRateTimer.restart(); - } - - auto ra = m_SkyTrackingTarget.rightascension + AxisOffsetNP[RAOffset].getValue() / 15.0; - auto de = m_SkyTrackingTarget.declination + AxisOffsetNP[DEOffset].getValue(); - auto JDOffset = AxisOffsetNP[JulianOffset].getValue() / 86400.0; - - if (TransformCelestialToTelescope(ra, de, JDOffset, TDV)) - { - DEBUGF(INDI::AlignmentSubsystem::DBG_ALIGNMENT, "TDV x %lf y %lf z %lf", TDV.x, TDV.y, TDV.z); - AltitudeAzimuthFromTelescopeDirectionVector(TDV, AltAz); - } - else - { - INDI::IEquatorialCoordinates EquatorialCoordinates { ra, de }; - INDI::EquatorialToHorizontal(&EquatorialCoordinates, &m_Location, ln_get_julian_from_sys() + JDOffset, &AltAz); - } - - DEBUGF(DBG_SCOPE, - "New Tracking Target AZ %lf° (%ld microsteps) AL %lf° (%ld microsteps) ", - AltAz.azimuth, - DegreesToMicrosteps(AXIS1, AltAz.azimuth), - AltAz.altitude, - DegreesToMicrosteps(AXIS2, AltAz.altitude)); - - // Calculate the auto-guiding delta degrees - double DeltaAlt = 0; - double DeltaAz = 0; - - for (auto Iter = GuidingPulses.begin(); Iter != GuidingPulses.end(); ) - { - // We treat the guide calibration specially - if (Iter->OriginalDuration == 1000) - { - DeltaAlt += Iter->DeltaAlt; - DeltaAz += Iter->DeltaAz; - } - else - { - DeltaAlt += Iter->DeltaAlt / 2; - DeltaAz += Iter->DeltaAz / 2; - } - Iter->Duration -= getCurrentPollingPeriod(); - - if (Iter->Duration < static_cast(getCurrentPollingPeriod())) - { - Iter = GuidingPulses.erase(Iter); - if (Iter == GuidingPulses.end()) - { - break; - } - continue; - } - ++Iter; - } - - GuideDeltaAlt += DeltaAlt; - GuideDeltaAz += DeltaAz; - - long SetPoint[2] = {0, 0}, Measurement[2] = {0, 0}, Error[2] = {0, 0}; - double TrackingRate[2] = {0, 0}; - - SetPoint[AXIS1] = DegreesToMicrosteps(AXIS1, AltAz.azimuth + GuideDeltaAz); - Measurement[AXIS1] = CurrentEncoders[AXIS1] - AxisOffsetNP[AZSteps].getValue() - ZeroPositionEncoders[AXIS1]; - - SetPoint[AXIS2] = DegreesToMicrosteps(AXIS2, AltAz.altitude + GuideDeltaAlt); - Measurement[AXIS2] = CurrentEncoders[AXIS2] - AxisOffsetNP[ALSteps].getValue() - ZeroPositionEncoders[AXIS2]; - - // Going the long way round - send it the other way - while (SetPoint[AXIS1] > MicrostepsPerRevolution[AXIS1] / 2) - SetPoint[AXIS1] -= MicrostepsPerRevolution[AXIS1]; - - while (SetPoint[AXIS2] > MicrostepsPerRevolution[AXIS2] / 2) - SetPoint[AXIS2] -= MicrostepsPerRevolution[AXIS2]; - - Error[AXIS1] = SetPoint[AXIS1] - Measurement[AXIS1]; - Error[AXIS2] = SetPoint[AXIS2] - Measurement[AXIS2]; - - auto Axis1CustomClockRate = Axis1TrackRateNP[TrackClockRate].getValue(); - - - if (!AxesStatus[AXIS1].FullStop && ( - (Axis1CustomClockRate == 0 && ((AxesStatus[AXIS1].SlewingForward && (Error[AXIS1] < -AxisDeadZoneNP[AXIS1].getValue())) || - (!AxesStatus[AXIS1].SlewingForward && (Error[AXIS1] > AxisDeadZoneNP[AXIS1].getValue())))) || - (Axis1CustomClockRate > 0 && Axis1TrackRateNP[TrackDirection].getValue() != m_LastCustomDirection[AXIS1]))) - { - m_LastCustomDirection[AXIS1] = Axis1TrackRateNP[TrackDirection].getValue(); - // Direction change whilst axis running - // Abandon tracking for this clock tick - LOG_DEBUG("Tracking -> AXIS1 direction change."); - LOGF_DEBUG("AXIS1 Setpoint %d Measurement %d Error %d Rate %f", - SetPoint[AXIS1], - Measurement[AXIS1], - Error[AXIS1], - TrackingRate[AXIS1]); - SlowStop(AXIS1); - } - else - { - TrackingRate[AXIS1] = m_Controllers[AXIS1]->calculate(SetPoint[AXIS1], Measurement[AXIS1]); - char Direction = TrackingRate[AXIS1] > 0 ? '0' : '1'; - TrackingRate[AXIS1] = std::fabs(TrackingRate[AXIS1]); - if (TrackingRate[AXIS1] != 0) - { - auto clockRate = (StepperClockFrequency[AXIS1] / TrackingRate[AXIS1]) * (AxisClockNP[AXIS1].getValue() / 100.0); - - if (Axis1CustomClockRate > 0) - { - clockRate = Axis1CustomClockRate; - Direction = Axis1TrackRateNP[TrackDirection].getValue() == 0 ? '0' : '1'; - } - - LOGF_DEBUG("AXIS1 Setpoint %d Measurement %d Error %d Rate %f Freq %f Dir %s", - SetPoint[AXIS1], - Measurement[AXIS1], - Error[AXIS1], - TrackingRate[AXIS1], - clockRate, - Direction == '0' ? "Forward" : "Backward"); -#ifdef DEBUG_PID - LOGF_DEBUG("Tracking AZ P: %f I: %f D: %f", - m_Controllers[AXIS1]->propotionalTerm(), - m_Controllers[AXIS1]->integralTerm(), - m_Controllers[AXIS1]->derivativeTerm()); -#endif - - SetClockTicksPerMicrostep(AXIS1, clockRate); - if (AxesStatus[AXIS1].FullStop) - { - LOG_DEBUG("Tracking -> AXIS1 restart."); - SetAxisMotionMode(AXIS1, '1', Direction); - StartAxisMotion(AXIS1); - } - } - } - - - auto Axis2CustomClockRate = Axis2TrackRateNP[TrackClockRate].getValue(); - - if (!AxesStatus[AXIS2].FullStop && ( - (Axis2CustomClockRate == 0 && ((AxesStatus[AXIS2].SlewingForward && (Error[AXIS2] < -AxisDeadZoneNP[AXIS2].getValue())) || - (!AxesStatus[AXIS2].SlewingForward && (Error[AXIS2] > AxisDeadZoneNP[AXIS2].getValue())))) || - (Axis2CustomClockRate > 0 && Axis2TrackRateNP[TrackDirection].getValue() != m_LastCustomDirection[AXIS2]))) - { - m_LastCustomDirection[AXIS2] = Axis2TrackRateNP[TrackDirection].getValue(); - - LOG_DEBUG("Tracking -> AXIS2 direction change."); - LOGF_DEBUG("AXIS2 Setpoint %d Measurement %d Error %d Rate %f", - SetPoint[AXIS2], - Measurement[AXIS2], - Error[AXIS2], - TrackingRate[AXIS2]); - SlowStop(AXIS2); - } - else - { - TrackingRate[AXIS2] = m_Controllers[AXIS2]->calculate(SetPoint[AXIS2], Measurement[AXIS2]); - char Direction = TrackingRate[AXIS2] > 0 ? '0' : '1'; - TrackingRate[AXIS2] = std::fabs(TrackingRate[AXIS2]); - if (TrackingRate[AXIS2] != 0) - { - auto clockRate = StepperClockFrequency[AXIS2] / TrackingRate[AXIS2] * (AxisClockNP[AXIS2].getValue() / 100.0); - - if (Axis2CustomClockRate > 0) - { - clockRate = Axis2CustomClockRate; - Direction = Axis2TrackRateNP[TrackDirection].getValue() == 0 ? '0' : '1'; - } - - LOGF_DEBUG("AXIS2 Setpoint %d Measurement %d Error %d Rate %f Freq %f Dir %s", - SetPoint[AXIS2], - Measurement[AXIS2], - Error[AXIS2], - TrackingRate[AXIS2], - clockRate, - Error[AXIS2] > 0 ? "Forward" : "Backward"); -#ifdef DEBUG_PID - LOGF_DEBUG("Tracking AZ P: %f I: %f D: %f", - m_Controllers[AXIS2]->propotionalTerm(), - m_Controllers[AXIS2]->integralTerm(), - m_Controllers[AXIS2]->derivativeTerm()); -#endif - - SetClockTicksPerMicrostep(AXIS2, clockRate); - if (AxesStatus[AXIS2].FullStop) - { - LOG_DEBUG("Tracking -> AXIS2 restart."); - SetAxisMotionMode(AXIS2, '1', Direction); - StartAxisMotion(AXIS2); - } - } - } + // TODO add switch to select between them + //trackUsingPID(); + trackUsingPredictiveRates(); } break; @@ -1789,15 +1579,473 @@ void SkywatcherAPIMount::resetTracking() m_TrackingRateTimer.restart(); GuideDeltaAlt = 0; GuideDeltaAz = 0; - m_Controllers[AXIS_AZ].reset(new PID(std::max(0.001, getPollingPeriod() / 1000.), 50, -50, + m_Controllers[AXIS_AZ].reset(new PID(std::max(0.001, getPollingPeriod() / 1000.), 1000, -1000, Axis1PIDNP[Propotional].getValue(), Axis1PIDNP[Derivative].getValue(), Axis1PIDNP[Integral].getValue())); - m_Controllers[AXIS_AZ]->setIntegratorLimits(-100, 100); - m_Controllers[AXIS_ALT].reset(new PID(std::max(0.001, getPollingPeriod() / 1000.), 50, -50, + m_Controllers[AXIS_AZ]->setIntegratorLimits(-1000, 1000); + m_Controllers[AXIS_ALT].reset(new PID(std::max(0.001, getPollingPeriod() / 1000.), 1000, -1000, Axis2PIDNP[Propotional].getValue(), Axis2PIDNP[Derivative].getValue(), Axis2PIDNP[Integral].getValue())); - m_Controllers[AXIS_ALT]->setIntegratorLimits(-100, 100); + m_Controllers[AXIS_ALT]->setIntegratorLimits(-1000, 1000); ResetGuidePulses(); } + +///////////////////////////////////////////////////////////////////////////////////// +/// Calculate and set T1 Preset from Clock Frequency and rate in arcsecs/s +///////////////////////////////////////////////////////////////////////////////////// +bool SkywatcherAPIMount::trackByRate(AXISID axis, double rate) +{ + if (std::abs(rate) > 0 && rate == m_LastTrackRate[axis]) + return true; + + m_LastTrackRate[axis] = rate; + + // If we are already stopped and rate is zero, we immediately return + if (AxesStatus[axis].FullStop && rate == 0) + return true; + // If rate is zero, or direction changed then we should stop. + else if (!AxesStatus[axis].FullStop && (rate == 0 || (AxesStatus[AXIS1].SlewingForward && rate < 0) + || (!AxesStatus[AXIS1].SlewingForward && rate > 0))) + { + SlowStop(axis); + LOGF_DEBUG("Tracking -> %s direction change.", (axis == AXIS1 ? "Axis 1" : "Axis 2")); + return true; + } + + char Direction = rate > 0 ? '0' : '1'; + auto stepsPerSecond = static_cast(std::abs(rate * (axis == AXIS1 ? + AxisOneEncoderValuesN[MICROSTEPS_PER_ARCSEC].value : AxisTwoEncoderValuesN[MICROSTEPS_PER_ARCSEC].value))); + auto clockRate = (StepperClockFrequency[axis] / std::max(1u, stepsPerSecond)); + + SetClockTicksPerMicrostep(axis, clockRate); + if (AxesStatus[axis].FullStop) + { + LOGF_DEBUG("Tracking -> %s restart.", (axis == AXIS1 ? "Axis 1" : "Axis 2")); + SetAxisMotionMode(axis, '1', Direction); + StartAxisMotion(axis); + } + + return true; +} + +///////////////////////////////////////////////////////////////////////////////////// +/// +///////////////////////////////////////////////////////////////////////////////////// +bool SkywatcherAPIMount::trackUsingPID() +{ + // Continue or start tracking + // Calculate where the mount needs to be in POLLMS time + // TODO may need to make this longer to get a meaningful result + //double JulianOffset = (getCurrentPollingPeriod() / 1000) / (24.0 * 60 * 60); + TelescopeDirectionVector TDV; + INDI::IHorizontalCoordinates AltAz { 0, 0 }; + + // We modify the SkyTrackingTarget for non-sidereal objects (Moon or Sun) + // FIXME: This was not tested. + if (TrackModeS[TRACK_LUNAR].s == ISS_ON) + { + // TRACKRATE_LUNAR how many arcsecs the Moon moved in one second. + // TRACKRATE_SIDEREAL how many arcsecs the Sky moved in one second. + double dRA = (TRACKRATE_LUNAR - TRACKRATE_SIDEREAL) * m_TrackingRateTimer.elapsed() / 1000.0; + m_SkyTrackingTarget.rightascension += (dRA / 3600.0) * 15.0; + m_TrackingRateTimer.restart(); + } + else if (TrackModeS[TRACK_SOLAR].s == ISS_ON) + { + double dRA = (TRACKRATE_SOLAR - TRACKRATE_SIDEREAL) * m_TrackingRateTimer.elapsed() / 1000.0; + m_SkyTrackingTarget.rightascension += (dRA / 3600.0) * 15.0; + m_TrackingRateTimer.restart(); + } + + auto ra = m_SkyTrackingTarget.rightascension + AxisOffsetNP[RAOffset].getValue() / 15.0; + auto de = m_SkyTrackingTarget.declination + AxisOffsetNP[DEOffset].getValue(); + auto JDOffset = AxisOffsetNP[JulianOffset].getValue() / 86400.0; + + if (TransformCelestialToTelescope(ra, de, JDOffset, TDV)) + { + DEBUGF(INDI::AlignmentSubsystem::DBG_ALIGNMENT, "TDV x %lf y %lf z %lf", TDV.x, TDV.y, TDV.z); + AltitudeAzimuthFromTelescopeDirectionVector(TDV, AltAz); + } + else + { + INDI::IEquatorialCoordinates EquatorialCoordinates { ra, de }; + INDI::EquatorialToHorizontal(&EquatorialCoordinates, &m_Location, ln_get_julian_from_sys() + JDOffset, &AltAz); + } + + DEBUGF(DBG_SCOPE, + "New Tracking Target AZ %lf° (%ld microsteps) AL %lf° (%ld microsteps) ", + AltAz.azimuth, + DegreesToMicrosteps(AXIS1, AltAz.azimuth), + AltAz.altitude, + DegreesToMicrosteps(AXIS2, AltAz.altitude)); + + // Calculate the auto-guiding delta degrees + double DeltaAlt = 0; + double DeltaAz = 0; + + getGuidePulses(DeltaAz, DeltaAlt); + + long SetPoint[2] = {0, 0}, Measurement[2] = {0, 0}, Error[2] = {0, 0}; + double TrackingRate[2] = {0, 0}; + + SetPoint[AXIS1] = DegreesToMicrosteps(AXIS1, AltAz.azimuth + GuideDeltaAz); + Measurement[AXIS1] = CurrentEncoders[AXIS1] - AxisOffsetNP[AZSteps].getValue() - ZeroPositionEncoders[AXIS1]; + + SetPoint[AXIS2] = DegreesToMicrosteps(AXIS2, AltAz.altitude + GuideDeltaAlt); + Measurement[AXIS2] = CurrentEncoders[AXIS2] - AxisOffsetNP[ALSteps].getValue() - ZeroPositionEncoders[AXIS2]; + + // Going the long way round - send it the other way + while (SetPoint[AXIS1] > MicrostepsPerRevolution[AXIS1] / 2) + SetPoint[AXIS1] -= MicrostepsPerRevolution[AXIS1]; + + while (SetPoint[AXIS2] > MicrostepsPerRevolution[AXIS2] / 2) + SetPoint[AXIS2] -= MicrostepsPerRevolution[AXIS2]; + + Error[AXIS1] = SetPoint[AXIS1] - Measurement[AXIS1]; + Error[AXIS2] = SetPoint[AXIS2] - Measurement[AXIS2]; + + auto Axis1CustomClockRate = Axis1TrackRateNP[TrackClockRate].getValue(); + + if (!AxesStatus[AXIS1].FullStop && ( + (Axis1CustomClockRate == 0 && ((AxesStatus[AXIS1].SlewingForward && (Error[AXIS1] < -AxisDeadZoneNP[AXIS1].getValue())) || + (!AxesStatus[AXIS1].SlewingForward && (Error[AXIS1] > AxisDeadZoneNP[AXIS1].getValue())))) || + (Axis1CustomClockRate > 0 && Axis1TrackRateNP[TrackDirection].getValue() != m_LastCustomDirection[AXIS1]))) + { + m_LastCustomDirection[AXIS1] = Axis1TrackRateNP[TrackDirection].getValue(); + // Direction change whilst axis running + // Abandon tracking for this clock tick + LOG_DEBUG("Tracking -> AXIS1 direction change."); + LOGF_DEBUG("AXIS1 Setpoint %d Measurement %d Error %d Rate %f", + SetPoint[AXIS1], + Measurement[AXIS1], + Error[AXIS1], + TrackingRate[AXIS1]); + SlowStop(AXIS1); + } + else + { + TrackingRate[AXIS1] = m_Controllers[AXIS1]->calculate(SetPoint[AXIS1], Measurement[AXIS1]); + char Direction = TrackingRate[AXIS1] > 0 ? '0' : '1'; + TrackingRate[AXIS1] = std::fabs(TrackingRate[AXIS1]); + if (TrackingRate[AXIS1] != 0) + { + auto clockRate = (StepperClockFrequency[AXIS1] / TrackingRate[AXIS1]) * (AxisClockNP[AXIS1].getValue() / 100.0); + + if (Axis1CustomClockRate > 0) + { + clockRate = Axis1CustomClockRate; + Direction = Axis1TrackRateNP[TrackDirection].getValue() == 0 ? '0' : '1'; + } + + LOGF_DEBUG("AXIS1 Setpoint %d Measurement %d Error %d Rate %f Freq %f Dir %s", + SetPoint[AXIS1], + Measurement[AXIS1], + Error[AXIS1], + TrackingRate[AXIS1], + clockRate, + Direction == '0' ? "Forward" : "Backward"); +#ifdef DEBUG_PID + LOGF_DEBUG("Tracking AZ P: %f I: %f D: %f", + m_Controllers[AXIS1]->propotionalTerm(), + m_Controllers[AXIS1]->integralTerm(), + m_Controllers[AXIS1]->derivativeTerm()); +#endif + + SetClockTicksPerMicrostep(AXIS1, clockRate); + if (AxesStatus[AXIS1].FullStop) + { + LOG_DEBUG("Tracking -> AXIS1 restart."); + SetAxisMotionMode(AXIS1, '1', Direction); + StartAxisMotion(AXIS1); + } + } + } + + + auto Axis2CustomClockRate = Axis2TrackRateNP[TrackClockRate].getValue(); + + if (!AxesStatus[AXIS2].FullStop && ( + (Axis2CustomClockRate == 0 && ((AxesStatus[AXIS2].SlewingForward && (Error[AXIS2] < -AxisDeadZoneNP[AXIS2].getValue())) || + (!AxesStatus[AXIS2].SlewingForward && (Error[AXIS2] > AxisDeadZoneNP[AXIS2].getValue())))) || + (Axis2CustomClockRate > 0 && Axis2TrackRateNP[TrackDirection].getValue() != m_LastCustomDirection[AXIS2]))) + { + m_LastCustomDirection[AXIS2] = Axis2TrackRateNP[TrackDirection].getValue(); + + LOG_DEBUG("Tracking -> AXIS2 direction change."); + LOGF_DEBUG("AXIS2 Setpoint %d Measurement %d Error %d Rate %f", + SetPoint[AXIS2], + Measurement[AXIS2], + Error[AXIS2], + TrackingRate[AXIS2]); + SlowStop(AXIS2); + } + else + { + TrackingRate[AXIS2] = m_Controllers[AXIS2]->calculate(SetPoint[AXIS2], Measurement[AXIS2]); + char Direction = TrackingRate[AXIS2] > 0 ? '0' : '1'; + TrackingRate[AXIS2] = std::fabs(TrackingRate[AXIS2]); + if (TrackingRate[AXIS2] != 0) + { + auto clockRate = StepperClockFrequency[AXIS2] / TrackingRate[AXIS2] * (AxisClockNP[AXIS2].getValue() / 100.0); + + if (Axis2CustomClockRate > 0) + { + clockRate = Axis2CustomClockRate; + Direction = Axis2TrackRateNP[TrackDirection].getValue() == 0 ? '0' : '1'; + } + + LOGF_DEBUG("AXIS2 Setpoint %d Measurement %d Error %d Rate %f Freq %f Dir %s", + SetPoint[AXIS2], + Measurement[AXIS2], + Error[AXIS2], + TrackingRate[AXIS2], + clockRate, + Error[AXIS2] > 0 ? "Forward" : "Backward"); +#ifdef DEBUG_PID + LOGF_DEBUG("Tracking AZ P: %f I: %f D: %f", + m_Controllers[AXIS2]->propotionalTerm(), + m_Controllers[AXIS2]->integralTerm(), + m_Controllers[AXIS2]->derivativeTerm()); +#endif + + SetClockTicksPerMicrostep(AXIS2, clockRate); + if (AxesStatus[AXIS2].FullStop) + { + LOG_DEBUG("Tracking -> AXIS2 restart."); + SetAxisMotionMode(AXIS2, '1', Direction); + StartAxisMotion(AXIS2); + } + } + } + + return true; +} + +///////////////////////////////////////////////////////////////////////////////////// +/// +///////////////////////////////////////////////////////////////////////////////////// +bool SkywatcherAPIMount::trackUsingPredictiveRates() +{ + TelescopeDirectionVector TDV; + TelescopeDirectionVector futureTDV; + TelescopeDirectionVector pastTDV; + INDI::IHorizontalCoordinates targetMountAxisCoordinates { 0, 0 }; + INDI::IHorizontalCoordinates pastMountAxisCoordinates { 0, 0 }; + INDI::IHorizontalCoordinates futureMountAxisCoordinates { 0, 0 }; + // time step for tracking rate estimation in seconds + double timeStep { 5.0 }; + // The same in days + double JDoffset { timeStep / (60 * 60 * 24) } ; + + // We modify the SkyTrackingTarget for non-sidereal objects (Moon or Sun) + // FIXME: This was not tested. + if (TrackModeS[TRACK_LUNAR].s == ISS_ON) + { + // TRACKRATE_LUNAR how many arcsecs the Moon moved in one second. + // TRACKRATE_SIDEREAL how many arcsecs the Sky moved in one second. + double dRA = (TRACKRATE_LUNAR - TRACKRATE_SIDEREAL) * m_TrackingRateTimer.elapsed() / 1000.0; + m_SkyTrackingTarget.rightascension += (dRA / 3600.0) * 15.0; + m_TrackingRateTimer.restart(); + } + else if (TrackModeS[TRACK_SOLAR].s == ISS_ON) + { + double dRA = (TRACKRATE_SOLAR - TRACKRATE_SIDEREAL) * m_TrackingRateTimer.elapsed() / 1000.0; + m_SkyTrackingTarget.rightascension += (dRA / 3600.0) * 15.0; + m_TrackingRateTimer.restart(); + } + + // Start by transforming tracking target celestial coordinates to telescope coordinates. + if (TransformCelestialToTelescope(m_SkyTrackingTarget.rightascension, m_SkyTrackingTarget.declination, + 0, TDV)) + { + // If mount is Alt-Az then that's all we need to do + AltitudeAzimuthFromTelescopeDirectionVector(TDV, targetMountAxisCoordinates); + TransformCelestialToTelescope(m_SkyTrackingTarget.rightascension, m_SkyTrackingTarget.declination, + JDoffset, futureTDV); + AltitudeAzimuthFromTelescopeDirectionVector(futureTDV, futureMountAxisCoordinates); + TransformCelestialToTelescope(m_SkyTrackingTarget.rightascension, m_SkyTrackingTarget.declination, + -JDoffset, pastTDV); + AltitudeAzimuthFromTelescopeDirectionVector(pastTDV, pastMountAxisCoordinates); + + } + // If transformation failed. + else + { + double JDnow {ln_get_julian_from_sys()}; + INDI::IEquatorialCoordinates EquatorialCoordinates { 0, 0 }; + EquatorialCoordinates.rightascension = m_SkyTrackingTarget.rightascension; + EquatorialCoordinates.declination = m_SkyTrackingTarget.declination; + INDI::EquatorialToHorizontal(&EquatorialCoordinates, &m_Location, JDnow, &targetMountAxisCoordinates); + INDI::EquatorialToHorizontal(&EquatorialCoordinates, &m_Location, JDnow + JDoffset, &futureMountAxisCoordinates); + INDI::EquatorialToHorizontal(&EquatorialCoordinates, &m_Location, JDnow - JDoffset, &pastMountAxisCoordinates); + } + + double azGuideOffset = 0, altGuideOffset = 0; + getGuidePulses(azGuideOffset, altGuideOffset); + + // Now add the guiding offsets, if any. + targetMountAxisCoordinates.azimuth += azGuideOffset; + pastMountAxisCoordinates.azimuth += azGuideOffset; + futureMountAxisCoordinates.azimuth += azGuideOffset; + + targetMountAxisCoordinates.altitude += altGuideOffset; + pastMountAxisCoordinates.altitude += altGuideOffset; + futureMountAxisCoordinates.altitude += altGuideOffset; + + // Calculate expected tracking rates + double predRate[2] = {0, 0}; + // Central difference, error quadratic in timestep + // Rates in deg/s + predRate[AXIS_AZ] = range180(AzimuthToDegrees(futureMountAxisCoordinates.azimuth - pastMountAxisCoordinates.azimuth)) / + timeStep / 2; + predRate[AXIS_ALT] = (futureMountAxisCoordinates.altitude - pastMountAxisCoordinates.altitude) / timeStep / 2; + + // Rates arcsec/s + predRate[AXIS_AZ] = 3600 * predRate[AXIS_AZ]; + predRate[AXIS_ALT] = 3600 * predRate[AXIS_ALT]; + + LOGF_DEBUG("Predicted positions (AZ): %9.4f %9.4f (now, future, degs)", + AzimuthToDegrees(targetMountAxisCoordinates.azimuth), + AzimuthToDegrees(futureMountAxisCoordinates.azimuth)) ; + LOGF_DEBUG("Predicted positions (AL): %9.4f %9.4f (now, future, degs)", targetMountAxisCoordinates.altitude, + futureMountAxisCoordinates.altitude); + LOGF_DEBUG("Predicted Rates (AZ, ALT): %9.4f %9.4f (arcsec/s)", predRate[AXIS_AZ], predRate[AXIS_ALT]); + + // If we had guiding pulses active, mark them as complete + if (GuideWENP.getState() == IPS_BUSY) + GuideComplete(AXIS_RA); + if (GuideNSNP.getState() == IPS_BUSY) + GuideComplete(AXIS_DE); + + // Next get current alt-az + INDI::IHorizontalCoordinates currentAltAz { 0, 0 }; + + // Current Azimuth + auto Axis1Steps = CurrentEncoders[AXIS1] - AxisOffsetNP[AZSteps].getValue() - ZeroPositionEncoders[AXIS1]; + currentAltAz.azimuth = DegreesToAzimuth(MicrostepsToDegrees(AXIS1, Axis1Steps)); + // Current Altitude + auto Axis2Steps = CurrentEncoders[AXIS2] - AxisOffsetNP[ALSteps].getValue() - ZeroPositionEncoders[AXIS2]; + currentAltAz.altitude = MicrostepsToDegrees(AXIS2, Axis2Steps); + + // Offset between target and current horizontal coordinates in arcsecs + double offsetAngle[2] = {0, 0}; + offsetAngle[AXIS_AZ] = range180(targetMountAxisCoordinates.azimuth - currentAltAz.azimuth) * 3600; + offsetAngle[AXIS_ALT] = (targetMountAxisCoordinates.altitude - currentAltAz.altitude) * 3600; + + int32_t targetSteps[2] = {0, 0}; + int32_t offsetSteps[2] = {0, 0}; + double trackRates[2] = {0, 0}; + + // Convert offsets from arcsecs to steps + offsetSteps[AXIS_AZ] = offsetAngle[AXIS_AZ] * AxisOneEncoderValuesN[MICROSTEPS_PER_ARCSEC].value; + offsetSteps[AXIS_ALT] = offsetAngle[AXIS_ALT] * AxisTwoEncoderValuesN[MICROSTEPS_PER_ARCSEC].value; + // Only apply tracking IF we're still on the same side of the curve + // If we switch over, let's settle for a bit + // This seems to not be required. To be removed after extensive testing + // if (m_LastOffset[AXIS_AZ] * offsetSteps[AXIS_AZ] >= 0 || m_OffsetSwitchSettle[AXIS_AZ]++ > 3) + { + m_OffsetSwitchSettle[AXIS_AZ] = 0; + m_LastOffset[AXIS_AZ] = offsetSteps[AXIS_AZ]; + targetSteps[AXIS_AZ] = DegreesToMicrosteps(AXIS1, AzimuthToDegrees(targetMountAxisCoordinates.azimuth)); + // Track rate: predicted + PID controlled correction based on tracking error: offsetSteps + trackRates[AXIS_AZ] = predRate[AXIS_AZ] + m_Controllers[AXIS_AZ]->calculate(0, -offsetAngle[AXIS_AZ]); + + LOGF_DEBUG("Tracking AZ Now: %8.f Target: %8d Offset: %8d Rate: %8.2f", Axis1Steps, targetSteps[AXIS_AZ], + offsetSteps[AXIS_AZ], trackRates[AXIS_AZ]); +#ifdef DEBUG_PID + LOGF_DEBUG("Tracking AZ P: %8.1f I: %8.1f D: %8.1f O: %8.1f", + m_Controllers[AXIS_AZ]->propotionalTerm(), + m_Controllers[AXIS_AZ]->integralTerm(), + m_Controllers[AXIS_AZ]->derivativeTerm(), + trackRates[AXIS_AZ] - predRate[AXIS_AZ]); +#endif + + // Set the tracking rate + trackByRate(AXIS1, trackRates[AXIS_AZ]); + } + + // Only apply tracking IF we're still on the same side of the curve + // If we switch over, let's settle for a bit + // This seems to not be required. To be removed after extensive testing + // if (m_LastOffset[AXIS_ALT] * offsetSteps[AXIS_ALT] >= 0 || m_OffsetSwitchSettle[AXIS_ALT]++ > 3) + { + m_OffsetSwitchSettle[AXIS_ALT] = 0; + m_LastOffset[AXIS_ALT] = offsetAngle[AXIS_ALT]; + targetSteps[AXIS_ALT] = DegreesToMicrosteps(AXIS2, targetMountAxisCoordinates.altitude); + // Track rate: predicted + PID controlled correction based on tracking error: offsetSteps + trackRates[AXIS_ALT] = predRate[AXIS_ALT] + m_Controllers[AXIS_ALT]->calculate(0, -offsetAngle[AXIS_ALT]); + + LOGF_DEBUG("Tracking AL Now: %8.f Target: %8d Offset: %8d Rate: %8.2f", Axis2Steps, + targetSteps[AXIS_ALT], + offsetSteps[AXIS_ALT], trackRates[AXIS_ALT]); +#ifdef DEBUG_PID + LOGF_DEBUG("Tracking AL P: %8.1f I: %8.1f D: %8.1f O: %8.1f", + m_Controllers[AXIS_ALT]->propotionalTerm(), + m_Controllers[AXIS_ALT]->integralTerm(), + m_Controllers[AXIS_ALT]->derivativeTerm(), + trackRates[AXIS_ALT] - predRate[AXIS_ALT]); +#endif + trackByRate(AXIS2, trackRates[AXIS_ALT]); + } + + return true; +} + +///////////////////////////////////////////////////////////////////////////////////// +/// +///////////////////////////////////////////////////////////////////////////////////// +double SkywatcherAPIMount::AzimuthToDegrees(double degree) +{ + if (isNorthHemisphere()) + return range360(degree); + else + return range360(degree + 180); +} + +///////////////////////////////////////////////////////////////////////////////////// +/// +///////////////////////////////////////////////////////////////////////////////////// +double SkywatcherAPIMount::DegreesToAzimuth(double degree) +{ + if (isNorthHemisphere()) + return range360(degree); + else + return range360(degree + 180); +} + +void SkywatcherAPIMount::getGuidePulses(double &az, double &alt) +{ + double DeltaAz = 0, DeltaAlt = 0; + + for (auto Iter = GuidingPulses.begin(); Iter != GuidingPulses.end(); ) + { + // We treat the guide calibration specially + if (Iter->OriginalDuration == 1000) + { + DeltaAlt += Iter->DeltaAlt; + DeltaAz += Iter->DeltaAz; + } + else + { + DeltaAlt += Iter->DeltaAlt / 2; + DeltaAz += Iter->DeltaAz / 2; + } + Iter->Duration -= getCurrentPollingPeriod(); + + if (Iter->Duration < static_cast(getCurrentPollingPeriod())) + { + Iter = GuidingPulses.erase(Iter); + if (Iter == GuidingPulses.end()) + { + break; + } + continue; + } + ++Iter; + } + + az = DeltaAlt; + alt = DeltaAz; +} \ No newline at end of file diff --git a/drivers/telescope/skywatcherAPIMount.h b/drivers/telescope/skywatcherAPIMount.h index bf1c61376b..29e376eceb 100644 --- a/drivers/telescope/skywatcherAPIMount.h +++ b/drivers/telescope/skywatcherAPIMount.h @@ -115,6 +115,12 @@ class SkywatcherAPIMount : long &Axis2Microsteps); const INDI::AlignmentSubsystem::TelescopeDirectionVector TelescopeDirectionVectorFromSkywatcherMicrosteps(long Axis1Microsteps, long Axis2Microsteps); + double AzimuthToDegrees(double degree); + double DegreesToAzimuth(double degree); + bool isNorthHemisphere() const + { + return m_Location.latitude >= 0; + } ///////////////////////////////////////////////////////////////////////////////////// /// Misc @@ -124,6 +130,23 @@ class SkywatcherAPIMount : bool getCurrentRADE(INDI::IHorizontalCoordinates altaz, INDI::IEquatorialCoordinates &rade); // Reset tracking timer to account for drift compensation void resetTracking(); + // Classical tracking via PID control loop + bool trackUsingPID(); + // Simpler tracking loop develped by Paweł T. Jochym + bool trackUsingPredictiveRates(); + // Get guide ticks + void getGuidePulses(double &az, double &alt); + + /** + * @brief TrackByRate Set axis tracking rate in arcsecs/sec. + * @param axis AXIS1 AZ or AXIS2 ALT + * @param rate arcsecs/s. Zero would stop tracking. + * For AZ, negative means left while positive means right. + * For Alt, negative is down while positive is up. + * @return True if successful, false otherwise. + */ + bool trackByRate(AXISID axis, double rate); + inline double average(const std::vector &values) { return values.empty() ? 0 : std::accumulate(values.begin(), values.end(), 0.0) / values.size(); @@ -233,16 +256,16 @@ class SkywatcherAPIMount : AZSteps, ALSteps, JulianOffset, - }; + }; - // Axis 1 Direct Track Control - INDI::PropertyNumber Axis1TrackRateNP {2}; - INDI::PropertyNumber Axis2TrackRateNP {2}; - enum - { + // Axis 1 Direct Track Control + INDI::PropertyNumber Axis1TrackRateNP {2}; + INDI::PropertyNumber Axis2TrackRateNP {2}; + enum + { TrackDirection, TrackClockRate, - }; + }; // AUX Encoders INDI::PropertySwitch AUXEncoderSP {2}; @@ -261,13 +284,16 @@ class SkywatcherAPIMount : std::unique_ptr m_Controllers[2]; // Maximum delta to track. If drift is above 5 degrees, we abort tracking. - static constexpr double MAX_TRACKING_DELTA {5}; + static constexpr double MAX_TRACKING_DELTA {5}; static constexpr const char *TRACKING_TAB = "Tracking"; INDI::ElapsedTimer m_TrackingRateTimer; + int32_t m_LastTrackRate[2] = {-1, -1}; uint8_t m_LastCustomDirection[2]; double GuideDeltaAlt { 0 }; double GuideDeltaAz { 0 }; + double m_LastOffset[2] = {0, 0}; + uint8_t m_OffsetSwitchSettle[2] = {0, 0}; GuidingPulse NorthPulse; GuidingPulse WestPulse; diff --git a/libs/indicore/indicom.c b/libs/indicore/indicom.c index c4ccdb85ac..bd0889ac61 100644 --- a/libs/indicore/indicom.c +++ b/libs/indicore/indicom.c @@ -1252,6 +1252,16 @@ double range360(double r) return res; } +double range180(double r) +{ + double res = r; + while (res < -180.0) + res += 360.0; + while (res > 180.0) + res -= 360.0; + return res; +} + double rangeDec(double decdegrees) { if ((decdegrees >= 270.0) && (decdegrees <= 360.0)) diff --git a/libs/indicore/indicom.h b/libs/indicore/indicom.h index 0ee56bc13a..2acd8c92b0 100644 --- a/libs/indicore/indicom.h +++ b/libs/indicore/indicom.h @@ -366,6 +366,13 @@ double range24(double r); */ double range360(double r); +/** + * \brief range180 Limits an angle to be between -180 to +180 degrees + * \param r angle in degrees + * \return Limited angle + */ +double range180(double r); + /** \brief rangeDec Limits declination value to be in -90 to 90 range. * \param r declination angle * \return limited declination