From 6fca94e571fcfeed8567fd8e18aec2bd0718f02f Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Fri, 12 Dec 2025 17:21:11 +0300 Subject: [PATCH] ... --- mcc/mcc_moving_controls.h | 346 ++++++++++++++++++++++++++++++++++++++ mcc/mcc_tracking_model.h | 140 ++++++++++----- 2 files changed, 447 insertions(+), 39 deletions(-) create mode 100644 mcc/mcc_moving_controls.h diff --git a/mcc/mcc_moving_controls.h b/mcc/mcc_moving_controls.h new file mode 100644 index 0000000..55f3b21 --- /dev/null +++ b/mcc/mcc_moving_controls.h @@ -0,0 +1,346 @@ +#pragma once + +#include + +#include "mcc_defaults.h" +#include "mcc_generics.h" +#include "mcc_moving_model_common.h" + +namespace mcc +{ + +enum class MccSimpleMovingControlsErrorCode : int { + ERROR_OK, + ERROR_HW_GETSTATE, + ERROR_HW_SETSTATE, + ERROR_PCM_COMP, + ERROR_GET_TELEMETRY, + ERROR_DIST_TELEMETRY, + ERROR_PZONE_CONTAINER_COMP, + ERROR_TARGET_IN_PZONE, + ERROR_NEAR_PZONE, + ERROR_TIMEOUT, + ERROR_ALREADY_SLEW, + ERROR_ALREADY_STOPPED, + ERROR_STOPPED +}; + +} // namespace mcc + + +namespace std +{ + +template <> +class is_error_code_enum : public true_type +{ +}; + +} // namespace std + + + +namespace mcc +{ + +// error category +struct MccSimpleMovingControlsCategory : public std::error_category { + MccSimpleMovingControlsCategory() : std::error_category() {} + + const char* name() const noexcept + { + return "SIMPLE-SLEWING-MODEL"; + } + + std::string message(int ec) const + { + MccSimpleMovingControlsErrorCode err = static_cast(ec); + + switch (err) { + case MccSimpleMovingControlsErrorCode::ERROR_OK: + return "OK"; + case MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE: + return "cannot get hardware state"; + case MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE: + return "cannot set hardware state"; + case MccSimpleMovingControlsErrorCode::ERROR_PCM_COMP: + return "PCM computation error"; + case MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY: + return "cannot get telemetry"; + case MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY: + return "cannot get target-to-mount-position distance"; + case MccSimpleMovingControlsErrorCode::ERROR_PZONE_CONTAINER_COMP: + return "pzone container computation error"; + case MccSimpleMovingControlsErrorCode::ERROR_TARGET_IN_PZONE: + return "target is in prohibited zone"; + case MccSimpleMovingControlsErrorCode::ERROR_NEAR_PZONE: + return "near prohibited zone"; + case MccSimpleMovingControlsErrorCode::ERROR_TIMEOUT: + return "a timeout occured while slewing"; + case MccSimpleMovingControlsErrorCode::ERROR_ALREADY_SLEW: + return "already slewing"; + case MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED: + return "slewing is already stopped"; + case MccSimpleMovingControlsErrorCode::ERROR_STOPPED: + return "slewing was stopped"; + default: + return "UNKNOWN"; + } + } + + static const MccSimpleMovingControlsCategory& get() + { + static const MccSimpleMovingControlsCategory constInst; + return constInst; + } +}; + + +inline std::error_code make_error_code(MccSimpleMovingControlsErrorCode ec) +{ + return std::error_code(static_cast(ec), MccSimpleMovingControlsCategory::get()); +} + + + +class MccSimpleMovingControls +{ +public: + typedef std::error_code error_t; + + enum Mode { MOVING_MODE_SLEW, MOVING_MODE_TRACK, MOVING_MODE_ERROR }; + + // typedef std::CallbackFuncTion mode_switch_callback_t; + + // protected: + // constexpr static auto defaultModeSwitchCallback = [](Mode) {}; + + // public: + + template CallbackFuncT = + decltype([](typename MountT::mount_status_t) {})> + MccSimpleMovingControls( + MountT* mount, + CallbackFuncT&& mode_switch_calback = [](typename MountT::mount_status_t) {}) + : _stopMoving(new std::atomic_bool), _currentParamsMutex(new std::mutex), _lastError(new std::atomic) + { + auto send_to_hardware = [mount](typename MountT::hardware_state_t const& hw_state, + MccTelemetryData const& tdata) { + mount->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", + mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees())); + + auto start_point = tdata.time_point; // needed for trajectory file + + if constexpr (mccIsEquatorialMount(MountT::mountType)) { + mount->logDebug(std::format(" entered target: HA = {}, DEC = {}", + mcc::MccAngle{tdata.target.HA}.sexagesimal(true), + mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal())); + mount->logDebug(std::format(" current mount: HA = {}, DEC = {}", + mcc::MccAngle{tdata.HA}.sexagesimal(true), + mcc::MccAngle{tdata.DEC_APP}.sexagesimal())); + + } else if constexpr (mccIsAltAzMount(MountT::mountType)) { + mount->logDebug(std::format(" entered target: AZ = {}, ZD = {}", + mcc::MccAngle{tdata.target.AZ}.sexagesimal(), + mcc::MccAngle{tdata.target.ZD}.sexagesimal())); + mount->logDebug(std::format(" current mount: AZ = {}, ZD = {}", + mcc::MccAngle{tdata.AZ}.sexagesimal(), + mcc::MccAngle{tdata.ZD}.sexagesimal())); + } + + auto hw_err = mount->hardwareSetState(hw_state); + if (hw_err) { + return mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE); + } + + mount->logDebug(" the 'hardwareSetState' method performed successfully!"); + + return MccSimpleMovingControlsErrorCode::ERROR_OK; + }; + + + *_stopMoving = true; + *_lastError = MccSimpleMovingControlsErrorCode::ERROR_OK; + + auto cb_sptr = std::make_shared(std::forward(mode_switch_calback)); + + _slewingFunc = [mount, cb_sptr, this](bool slew_and_stop) { + double braking_accelX, braking_accelY; + double min_time_to_pzone_in_secs; + + bool store_path = false; + std::ofstream fst; + using path_tp_t = std::chrono::duration; // seconds represented as double + + { + // std::lock_guard lock{*_currentParamsMutex}; + if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) { + braking_accelX = std::numeric_limits::min(); + } else { + braking_accelX = std::abs(_currentParams.brakingAccelX); + } + + if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) { + braking_accelY = std::numeric_limits::min(); + } else { + braking_accelY = std::abs(_currentParams.brakingAccelY); + } + + min_time_to_pzone_in_secs = + std::chrono::duration_cast>(_currentParams.minTimeToPZone).count(); + + if (!_currentParams.slewingPathFilename.empty()) { // open slewing trajectory file + fst.open(_currentParams.slewingPathFilename); + if (fst.is_open()) { + store_path = true; + } else { + mount->logError(std::format("Cannot open slewing path file: {}! Do not save it!", + _currentParams.slewingPathFilename)); + } + } + } + + mount->logInfo( + std::format("Start slewing in mode '{}'", (slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK"))); + mount->logInfo(std::format(" slewing process timeout: {} secs", _currentParams.slewTimeout.count())); + if (!slew_and_stop) { + mount->logInfo(std::format(" slewing tolerance radius: {} arcsecs", + mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs())); + } + mount->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)", + mcc::MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX)); + mount->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)", + mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY)); + mount->logInfo(std::format(" min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs)); + + + if (store_path) { + fst << "# \n"; + fst << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n"; + fst << "# Config:\n"; + fst << "# slewing tolerance radius: " << mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs() + << " arcsecs\n"; + fst << "# slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n"; + fst << "# \n"; + fst << "# Format (time is in seconds, coordinates are in radians): \n"; + fst << "# " + " \n"; + } + + + typename MountT::error_t t_err; + MccTelemetryData tdata; + + { + std::lock_guard lock{*_currentParamsMutex}; + t_err = mount->telemetryData(&tdata); + + if (t_err) { + return *_lastError = + mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY); + } + } + + auto last_hw_time = tdata.time_point; + + bool in_zone; + std::vector in_zone_vec; + + MccCelestialPoint cpt; + + if constexpr (mccIsEquatorialMount(MountT::mountType)) { + cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; + } else if constexpr (mccIsAltAzMount(MountT::mountType)) { + cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD; + } else { + static_assert(false, "UNKNOWN MOUNT TYPE!"); + } + + typename MountT::hardware_state_t hw_state; + + auto hw_err = mount->hardwareGetState(&hw_state); + if (hw_err) { + *_stopMoving = true; + return *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE); + } + + + hw_state.X = (double)tdata.target.X; + hw_state.Y = (double)tdata.target.Y; + + { + std::lock_guard lock{*_currentParamsMutex}; + + hw_state.speedX = _currentParams.slewRateX; + hw_state.speedY = _currentParams.slewRateY; + } + hw_state.moving_state = MountT::hardware_moving_state_t::HW_MOVE_SLEWING; + + if (*_stopMoving) { + mount->logDebug("slewing was stopped!"); + return *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED; + } + }; + + _trackingFunc = [mount, cb_sptr, this]() { + + }; + } + + error_t slewToTarget(bool slew_and_stop = false) + { + return *_lastError; + } + + error_t trackTarget() + { + return *_lastError; + } + + + error_t stopMountMoving() + { + if (*_stopMoving) { + *_lastError = MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED; + } else { + *_stopMoving = true; + } + + return *_lastError; + } + + error_t setMovingParams(MccSimpleMovingModelParams params) + { + std::lock_guard lock{*_currentParamsMutex}; + + _currentParams = std::move(params); + + return MccSimpleMovingControlsErrorCode::ERROR_OK; + } + + MccSimpleMovingModelParams getMovingParams() const + { + std::lock_guard lock{*_currentParamsMutex}; + + return _currentParams; + } + + error_t mountMovingLastError() const + { + return *_lastError; + } + +protected: + std::function _slewingFunc{}; + std::function _trackingFunc{}; + + std::unique_ptr _stopMoving; + + std::unique_ptr _currentParamsMutex; + MccSimpleMovingModelParams _currentParams{}; + + std::unique_ptr> _lastError; +}; + +} // namespace mcc diff --git a/mcc/mcc_tracking_model.h b/mcc/mcc_tracking_model.h index ee33f6a..308428f 100644 --- a/mcc/mcc_tracking_model.h +++ b/mcc/mcc_tracking_model.h @@ -6,6 +6,8 @@ /* SIMPLE Tracking MODEL IMPLEMENTATION */ +#include + #include "mcc_defaults.h" #include "mcc_moving_model_common.h" @@ -133,6 +135,10 @@ public: MccEqtHrzCoords intsc_coords; MccCelestialPoint target_in_future_pt; + bool store_path = false; + std::ofstream fst; + using path_tp_t = std::chrono::duration; // seconds represented as double + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP; } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { @@ -144,6 +150,7 @@ public: // double dist, dx, dy; logger.logInfo("Start tracking:"); + logger.logInfo(" timesift: {} millisecs", _currentParams.timeShiftToTargetPoint.count()); logger.logInfo(" min time to pzone: {} secs", _currentParams.minTimeToPZone.count()); @@ -153,6 +160,8 @@ public: return mcc_deduce_error_code(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY); } + auto start_point = tdata.time_point; // needed for trajectory file + bool in_zone; auto pz_err = controls->inPZone(tdata, &in_zone); if (pz_err) { @@ -257,7 +266,7 @@ public: return mcc_deduce_error_code(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP); } - mcc_tp2tp(tdata.time_point, hw_state.time_point); + mcc_tp2tp(tdata.time_point + tp_dt, hw_state.time_point); } return MccSimpleTrackingModelErrorCode::ERROR_OK; @@ -275,6 +284,8 @@ public: { std::lock_guard lock{*_currentParamsMutex}; + mcc_copy_eqt_hrz_coord(tdata, &tdata.target); + auto ccte_err = target_point(&target_in_future_pt); if (ccte_err) { *_stopTracking = true; @@ -296,6 +307,29 @@ public: } else { static_assert(false, "UNKNOWN MOUNT TYPE!"); } + + if (!_currentParams.trackingPathFilename.empty()) { // open tracking trajectory file + fst.open(_currentParams.trackingPathFilename); + if (fst.is_open()) { + store_path = true; + logger.logInfo(" timesift: {} millisecs", _currentParams.timeShiftToTargetPoint.count()); + logger.logInfo(" min time to pzone: {} secs", _currentParams.minTimeToPZone.count()); + + fst << "# \n"; + fst << "# Tracking trajectory, " << std::chrono::system_clock::now() << "\n"; + fst << "# Config:\n"; + fst << "# timeshift: " << _currentParams.timeShiftToTargetPoint.count() << " millisecs\n"; + fst << "# min time to pzone: " << _currentParams.minTimeToPZone.count() << " secs\n"; + fst << "# \n"; + fst << "# Format (time is in seconds, coordinates are in radians): \n"; + fst << "# " + " \n"; + + } else { + logger.logError(std::format("Cannot open tracking path file: {}! Do not save it!", + _currentParams.trackingPathFilename)); + } + } } // move mount @@ -328,6 +362,14 @@ public: } } + if (store_path) { + fst << std::chrono::duration_cast(tdata.time_point - start_point).count() << " " + << tdata.target.HA << " " << tdata.target.DEC_APP << " " << tdata.HA << " " << tdata.DEC_APP + << " " << (tdata.target.HA - tdata.HA) << " " << (tdata.target.DEC_APP - tdata.DEC_APP) << " " + << (int)hw_state.moving_state << "\n"; + } + + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { logger.logTrace(std::format(" current target: HA = {}, DEC = {}", mcc::MccAngle(tdata.target.HA).sexagesimal(true), @@ -353,52 +395,72 @@ public: } // control prohibited zones - if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) { - *_stopTracking = true; - return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE; - } - if (pz_err) { - *_stopTracking = true; - return mcc_deduce_error_code(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); - } + // if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) { + // logger.logError("Mount is near zone!"); + // *_stopTracking = true; + // return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE; + // } + // if (pz_err) { + // *_stopTracking = true; + // return mcc_deduce_error_code(pz_err, + // MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + // } if (*_stopTracking) { break; } - { - std::lock_guard lock{*_currentParamsMutex}; - auto now = std::chrono::steady_clock::now(); + // { + // std::lock_guard lock{*_currentParamsMutex}; + // auto now = std::chrono::steady_clock::now(); - if ((now - last_corr_tp) > _currentParams.trackingCycleInterval) { - // update prohibited zones intersection point - if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) { - pz_err = update_pzones_ipoint(); - if (pz_err) { - *_stopTracking = true; - return mcc_deduce_error_code( - pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); - } - } + // if ((now - last_corr_tp) > _currentParams.trackingCycleInterval) { + // // update prohibited zones intersection point + // if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) { + // pz_err = update_pzones_ipoint(); + // if (pz_err) { + // *_stopTracking = true; + // return mcc_deduce_error_code( + // pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + // } + // } - // compute new target-in-future point - auto ccte_err = target_point(&target_in_future_pt); - if (ccte_err) { - *_stopTracking = true; - return mcc_deduce_error_code(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE); - } + // // compute new target-in-future point + // auto ccte_err = target_point(&target_in_future_pt); + // if (ccte_err) { + // *_stopTracking = true; + // return mcc_deduce_error_code(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE); + // } - logger.logTrace("The updated target point:"); - if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { - logger.logTrace(" HA, DEC: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(true), - MccAngle(target_in_future_pt.Y).sexagesimal()); - } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { - logger.logTrace(" AZ, ZD: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(), - MccAngle(target_in_future_pt.Y).sexagesimal()); - } else { - static_assert(false, "UNKNOWN MOUNT TYPE!"); - } - } + // logger.logTrace("The updated target point:"); + // if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { + // logger.logTrace(" HA, DEC: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(true), + // MccAngle(target_in_future_pt.Y).sexagesimal()); + // } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { + // logger.logTrace(" AZ, ZD: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(), + // MccAngle(target_in_future_pt.Y).sexagesimal()); + // } else { + // static_assert(false, "UNKNOWN MOUNT TYPE!"); + // } + // } + // } + + // compute new target-in-future point + auto ccte_err = target_point(&target_in_future_pt); + if (ccte_err) { + *_stopTracking = true; + return mcc_deduce_error_code(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE); + } + + logger.logTrace("The updated target point:"); + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { + logger.logTrace(" HA, DEC: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(true), + MccAngle(target_in_future_pt.Y).sexagesimal()); + } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { + logger.logTrace(" AZ, ZD: {} {}", MccAngle(target_in_future_pt.X).sexagesimal(), + MccAngle(target_in_future_pt.Y).sexagesimal()); + } else { + static_assert(false, "UNKNOWN MOUNT TYPE!"); } // send corrections