diff --git a/mcc/mcc_guiding_model.h b/mcc/mcc_guiding_model.h new file mode 100644 index 0000000..8dc3720 --- /dev/null +++ b/mcc/mcc_guiding_model.h @@ -0,0 +1,152 @@ +#pragma once + +/* MOUNT CONTROL COMPONENTS LIBRARY */ + + +/* SIMPLE GUIDING MODEL IMPLEMENTATION */ + + +#include "mcc_defaults.h" +#include "mcc_moving_model_common.h" + +namespace mcc +{ + +enum class MccSimpleGuidingModelErrorCode : int { + ERROR_OK, + ERROR_HW_GETSTATE, + ERROR_HW_SETSTATE, + ERROR_PCM_COMP, + ERROR_GET_TELEMETRY, + ERROR_DIST_TELEMETRY, + ERROR_DIFF_TELEMETRY, + ERROR_PZONE_CONTAINER_COMP, + ERROR_IN_PZONE, + ERROR_NEAR_PZONE, + ERROR_TIMEOUT, + ERROR_UNEXPECTED_AXIS_RATES, + ERROR_STOPPED +}; + +} // namespace mcc + + +namespace std +{ + +template <> +class is_error_code_enum : public true_type +{ +}; + +} // namespace std + + + +namespace mcc +{ + +class MccSimpleGuidingModel +{ +public: + typedef std::error_code error_t; + + typedef MccSimpleMovingModelParams guiding_params_t; + + + template + MccSimpleGuidingModel(TelemetryT* telemetry, HardwareT* hardware, PZoneContT* pz_cont) + : _stopGuiding(new std::atomic_bool()), _currentParamsMutex(new std::mutex()) + { + _guidingFunc = [telemetry, hardware, pz_cont, this]() -> error_t { + typename TelemetryT::error_t t_err; + + MccCelestialPoint cpt; + typename HardwareT::hardware_state_t hw_state; + + if constexpr (mccIsEquatorialMount(HardwareT::mountType)) { + cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) { + cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT; + static_assert(false, "NOT IMPLEMENTED!"); + } else { + static_assert(false, "UNKNOW MOUNT TYPE!"); + } + + + + MccTelemetryData tdata; + + std::vector> pz_timeto; // in seconds + std::chrono::duration min_time{0.0}; + std::vector intsc_pt(pz_cont->sizePZones(), cpt); + + // compute intersection points with the prohibited zones + auto pz_err = pz_cont->intersectPZone(tdata, &intsc_pt); + if (pz_err) { + return mcc_deduce_error(pz_err, MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + } + + while (*_stopGuiding) { + // wait for updated telemetry data + { + std::lock_guard lock{*_currentParamsMutex}; + + t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); + + if (t_err) { + return mcc_deduce_error(t_err, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY); + } + } + + // control prohibited zones + pz_err = pz_cont->timeToPZone(tdata, &pz_timeto); + if (pz_err) { + return mcc_deduce_error(pz_err, + MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + } + + min_time = std::chrono::duration{0}; + for (size_t i = 0; i < pz_cont->sizePZones(); ++i) { + if (pz_timeto[i] < _currentParams.minTimeToPZone) { + return MccSimpleGuidingModelErrorCode::ERROR_NEAR_PZONE; + } + if (pz_timeto[i] < min_time) { + min_time = pz_timeto[i]; + } + } + } + + return MccSimpleGuidingModelErrorCode::ERROR_OK; + }; + } + + MccSimpleGuidingModel(MccSimpleGuidingModel&&) = default; + MccSimpleGuidingModel& operator=(MccSimpleGuidingModel&&) = default; + + MccSimpleGuidingModel(const MccSimpleGuidingModel&) = delete; + MccSimpleGuidingModel& operator=(const MccSimpleGuidingModel&) = delete; + + error_t startGuidingTarget() + { + *_stopGuiding = false; + + return _guidingFunc(); + } + + error_t stoptGuidingTarget() + { + *_stopGuiding = true; + + return MccSimpleGuidingModelErrorCode::ERROR_OK; + } + +protected: + std::function _guidingFunc{}; + std::unique_ptr _stopGuiding; + + guiding_params_t _currentParams{}; + std::unique_ptr _currentParamsMutex{}; +}; + +} // namespace mcc diff --git a/mcc/mcc_moving_model_common.h b/mcc/mcc_moving_model_common.h new file mode 100644 index 0000000..eaceb20 --- /dev/null +++ b/mcc/mcc_moving_model_common.h @@ -0,0 +1,63 @@ +#pragma once + +/* MOUNT CONTROL COMPONENTS LIBRARY */ + + +/* COMMON DEFINITIONS FOR SIMPLE SLEWING, TRACKING AND GUIDING MODEL IMPLEMENTATIONS */ + + +#include + +#include "mcc_angle.h" + +namespace mcc +{ + +struct MccSimpleMovingModelParams { + // ******* common for all modes ******* + + // mean celestial rate + static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second + + // timeout to telemetry updating + std::chrono::seconds telemetryTimeout{3}; + + // minimal time to prohibited zone (at current speed in slewing mode). if it is lesser then exit with error + std::chrono::seconds minTimeToPZone{10}; + + + // ******* slewing mode ******* + + bool slewAndStop{false}; // slew to target and stop mount + + // coordinates difference to stop slewing (in radians) + double slewToleranceRadius{5.0_arcsecs}; + + // target-mount coordinate difference to start adjusting of slewing (in radians) + double adjustCoordDiff{slewToleranceRadius * 10.0}; + + // slew process timeout + std::chrono::seconds slewTimeout{3600}; + + double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) + double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) + + std::chrono::milliseconds adjustCycleInterval{500}; // minimum time between two successive adjustments + + double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) + double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) + + + // ******* tracking mode ******* + + double trackSpeedX{}; + double trackSpeedY{}; + + + // ******* guiding mode ******* + + double correctionRange[2]{0.3_arcsecs, 3.0_arcsecs}; + bool dualAxisGuiding{true}; // mount must be of an equatorial type: false means guiding along only HA-axis +}; + +} // namespace mcc diff --git a/mcc/mcc_slewing_model.h b/mcc/mcc_slewing_model.h index ae10ddc..fbbcc1a 100644 --- a/mcc/mcc_slewing_model.h +++ b/mcc/mcc_slewing_model.h @@ -8,7 +8,7 @@ #include "mcc_defaults.h" #include "mcc_generics.h" - +#include "mcc_moving_model_common.h" namespace mcc { @@ -24,7 +24,9 @@ enum class MccSimpleSlewingModelErrorCode : int { ERROR_PZONE_CONTAINER_COMP, ERROR_IN_PZONE, ERROR_NEAR_PZONE, - ERROR_UNEXPECTED_AXIS_RATES + ERROR_TIMEOUT, + ERROR_UNEXPECTED_AXIS_RATES, + ERROR_STOPPED }; } // namespace mcc @@ -54,38 +56,37 @@ class MccSimpleSlewingModel public: typedef std::error_code error_t; - struct slewing_params_t { - bool slewAndStop{false}; // slew to target and stop mount + typedef MccSimpleMovingModelParams slewing_params_t; - std::chrono::seconds telemetryTimeout{3}; + // struct slewing_params_t { + // bool slewAndStop{false}; // slew to target and stop mount - // minimal time to prohibited zone at current speed. if it is lesser then exit with error - std::chrono::seconds minTimeToPZone{10}; + // std::chrono::seconds telemetryTimeout{3}; - // target-mount coordinate difference to start adjusting of slewing (in radians) - double adjustCoordDiff{10.0_degs}; + // // minimal time to prohibited zone at current speed. if it is lesser then exit with error + // std::chrono::seconds minTimeToPZone{10}; - // coordinates difference to stop slewing (in radians) - double slewToleranceRadius{5.0_arcsecs}; + // // target-mount coordinate difference to start adjusting of slewing (in radians) + // double adjustCoordDiff{10.0_degs}; - // slew process timeout - std::chrono::seconds slewTimeout{3600}; + // // coordinates difference to stop slewing (in radians) + // double slewToleranceRadius{5.0_arcsecs}; - double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) - double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) + // // slew process timeout + // std::chrono::seconds slewTimeout{3600}; - double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) - double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) - }; + // double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) + // double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate) - template - MccSimpleSlewingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont) + // double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) + // double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) + // }; + + template + MccSimpleSlewingModel(TelemetryT* telemetry, HardwareT* hardware, PZoneContT* pz_cont) : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex) { - _slewingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t { + _slewingFunc = [telemetry, hardware, pz_cont, this]() -> error_t { *_stopSlewing = false; // first, check target coordinates @@ -111,6 +112,10 @@ public: return MccSimpleSlewingModelErrorCode::ERROR_IN_PZONE; } + if (*_stopSlewing) { + return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + } + MccCelestialPoint cpt; mcc_tp2tp(tdata.time_point, cpt.time_point); @@ -128,6 +133,10 @@ public: return mcc_deduce_error(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); } + if (*_stopSlewing) { + return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + } + typename HardwareT::hardware_state_t hw_state; auto hw_err = hardware->hardwareGetState(&hw_state); @@ -145,18 +154,30 @@ public: } hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_SLEWING; + if (*_stopSlewing) { + return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + } + // start slewing hw_err = hardware->hardwareSetState(hw_state); if (hw_err) { return mcc_deduce_error(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); } - double dist, dx, dy, sinY; + std::chrono::system_clock::time_point start_slewing_tp, last_adjust_tp; + mcc_tp2tp(hw_state.time_point, start_slewing_tp); + + double dist, dx, dy, sinY, rate2, xrate; std::chrono::duration dtx, dty; // seconds in double - while (!*_stopSlewing) { + + bool adjust_mode = false; + static constexpr auto sideral_rate2 = slewing_params_t::sideralRate * slewing_params_t::sideralRate; + + while (true) { // wait for updated telemetry data { std::lock_guard lock{*_currentParamsMutex}; + t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); if (t_err) { @@ -164,34 +185,57 @@ public: } } + if (*_stopSlewing) { + return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + } // compute time to prohibited zones at current speed for (auto const& pt : isct_pt) { if (std::isfinite(pt.X) && std::isfinite(pt.Y)) { if constexpr (mccIsEquatorialMount(HardwareT::mountType)) { - sinY = sin(std::numbers::pi / 2.0 - tdata.DEC_APP); + // sinY = sin(std::numbers::pi / 2.0 - tdata.DEC_APP); dx = pt.X - tdata.HA; dy = pt.Y - tdata.DEC_APP; } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) { - sinY = sin(tdata.ZD); + // sinY = sin(tdata.ZD); dx = pt.X - tdata.AZ; dy = pt.Y - tdata.ZD; } - if (utils::isEqual(sinY, 0.0)) { - dtx = decltype(dtx){std::numeric_limits::infinity()}; - } else { - dtx = decltype(dtx){std::abs(dx / tdata.speedX / sinY)}; - } + // if (utils::isEqual(sinY, 0.0)) { + // dtx = decltype(dtx){std::numeric_limits::infinity()}; + // rate2 = std::numeric_limits::infinity(); + // } else { + // xrate = tdata.speedX * sinY; + // dtx = decltype(dtx){std::abs(dx / xrate)}; + // } + dtx = decltype(dtx){std::abs(dx / tdata.speedX)}; dty = decltype(dty){std::abs(dy / tdata.speedY)}; - if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) { - return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE; + + { + std::lock_guard lock{*_currentParamsMutex}; + + if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) { + return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE; + } } } + + if (*_stopSlewing) { + return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + } } - auto hw_err = hardware->hardwareGetState(&hw_state); + { + std::lock_guard lock{*_currentParamsMutex}; + + if ((std::chrono::system_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) { + return MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT; + } + } + + hw_err = hardware->hardwareGetState(&hw_state); if (hw_err) { return mcc_deduce_error(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); } @@ -201,17 +245,72 @@ public: return mcc_deduce_error(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY); } - if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle - break; + if (*_stopSlewing) { + return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } - if (dist <= _currentParams.adjustCoordDiff) { + { + std::lock_guard lock{*_currentParamsMutex}; + + if (adjust_mode && !_currentParams.slewAndStop) { + // do not allow mount speed fall below sideral + if constexpr (mccIsEquatorialMount(HardwareT::mountType)) { + if (tdata.speedX < slewing_params_t::sideralRate) { + hw_state.X = (double)tdata.target.X; + hw_state.Y = (double)tdata.target.Y; + + hw_state.speedX = slewing_params_t::sideralRate; + + hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING; + + hw_err = hardware->hardwareSetState(hw_state); + if (hw_err) { + return mcc_deduce_error(hw_err, + MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); + } + } + } else if constexpr (mccIsAltAzMount(HardwareT::mountType)) { + } else { + static_assert(false, "UNKNOWN MOUNT TYPE!!"); + } + } + + if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle + if (hw_state.moving_type == + HardwareT::hardware_moving_state_t::HW_MOVE_STOPPED) { // mount was stopped + break; + } + } + + if (dist <= _currentParams.adjustCoordDiff) { // adjust mount pointing + if ((std::chrono::system_clock::now() - last_adjust_tp) < _currentParams.adjustCycleInterval) { + continue; + } + + hw_state.X = (double)tdata.target.X; + hw_state.Y = (double)tdata.target.Y; + + hw_state.speedX = _currentParams.adjustXRate; + hw_state.speedY = _currentParams.adjustYRate; + + hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_ADJUSTING; + + hw_err = hardware->hardwareSetState(hw_state); + if (hw_err) { + return mcc_deduce_error(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); + } + + last_adjust_tp = std::chrono::system_clock::now(); + + adjust_mode = true; + + } else { + adjust_mode = false; + } } - // check for current axis speed - if (utils::isEqual(tdata.speedX, 0.0) && utils::isEqual(tdata.speedY, 0.0)) { - // unhandled stop state?!!! - return MccSimpleSlewingModelErrorCode::ERROR_UNEXPECTED_AXIS_RATES; + if (*_stopSlewing) { + return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } } diff --git a/mcc/mcc_tracking_model.h b/mcc/mcc_tracking_model.h index 0034cea..3de9e9f 100644 --- a/mcc/mcc_tracking_model.h +++ b/mcc/mcc_tracking_model.h @@ -8,7 +8,7 @@ #include "mcc_defaults.h" #include "mcc_generics.h" - +#include "mcc_moving_model_common.h" namespace mcc { @@ -48,16 +48,18 @@ class MccSimpleTrackingModel public: typedef std::error_code error_t; - struct tracking_params_t { - static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second + typedef MccSimpleMovingModelParams tracking_params_t; - double trackSpeedX{}; - double trackSpeedY{}; + // struct tracking_params_t { + // static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second - std::chrono::seconds telemetryTimeout{3}; - // minimal time to prohibited zone. if it is lesser then exit with error - std::chrono::seconds minTimeToPZone{10}; - }; + // double trackSpeedX{}; + // double trackSpeedY{}; + + // std::chrono::seconds telemetryTimeout{3}; + // // minimal time to prohibited zone. if it is lesser then exit with error + // std::chrono::seconds minTimeToPZone{10}; + // }; template > pz_timeto; // in seconds std::chrono::duration min_time{0.0}; @@ -132,14 +135,15 @@ public: } if (utils::isEqual(dha_min, 0.0)) { // no intersections - cpt.X = tdata.HA - 1.0_mins; + no_intersects = true; + cpt.X = tdata.HA + 710.0_mins; // 12h - 10min cpt.Y = tdata.DEC_APP; } // compute position in future - auto err = hardware->hardwareGetState(&hw_state); - if (err) { - return mcc_deduce_error(err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE); + auto hw_err = hardware->hardwareGetState(&hw_state); + if (hw_err) { + return mcc_deduce_error(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE); } MccPCMResult pcm_inv_res; @@ -161,9 +165,9 @@ public: hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING; // start tracking - err = hardware->hardwareSetState(std::move(hw_state)); - if (err) { - return mcc_deduce_error(err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE); + hw_err = hardware->hardwareSetState(std::move(hw_state)); + if (hw_err) { + return mcc_deduce_error(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE); } @@ -194,14 +198,44 @@ public: break; } - // check for current axis speed - if (utils::isEqual(tdata.speedX, 0.0) && utils::isEqual(tdata.speedY, 0.0)) { - // unhandled stop state?!!! - return MccSimpleTrackingModelErrorCode::ERROR_UNEXPECTED_AXIS_RATES; + if (no_intersects) { + if ((cpt.X - tdata.HA) < 10.0_mins) { // recompute target point + cpt.X += 11.0_hours; + + hw_err = hardware->hardwareGetState(&hw_state); + if (hw_err) { + return mcc_deduce_error(hw_err, + MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE); + } + + pcm_err = pcm->computeInversePCM(cpt, &pcm_inv_res, &hw_state); + if (pcm_err) { + return mcc_deduce_error(pcm_err, + MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP); + } + + // just set sideral rate once + mcc_tp2tp(tdata.time_point, hw_state.time_point); + { + std::lock_guard lock{*_currentTrackParamsMutex}; + + hw_state.speedX = _currentTrackParams.trackSpeedX; + hw_state.speedY = _currentTrackParams.trackSpeedY; + } + hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING; + + // start tracking + hw_err = hardware->hardwareSetState(std::move(hw_state)); + if (hw_err) { + return mcc_deduce_error(hw_err, + MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE); + } + } } } return MccSimpleTrackingModelErrorCode::ERROR_OK; + } else if constexpr (mccIsAltAzMount(PcmT::mountType)) { static_assert(false, "NOT IMPLEMENTED!"); } else {