diff --git a/asibfm700/asibfm700_common.h b/asibfm700/asibfm700_common.h index 97b7181..7140d9b 100644 --- a/asibfm700/asibfm700_common.h +++ b/asibfm700/asibfm700_common.h @@ -13,6 +13,7 @@ #include "mcc_ccte_erfa.h" #include "mcc_slewing_model.h" +#include "mcc_tracking_model.h" namespace asibfm700 { @@ -24,5 +25,6 @@ typedef mcc::MccDefaultPCM Asibfm700PCM; typedef mcc::MccPZoneContainer Asibfm700PZoneContainer; typedef mcc::utils::MccSpdlogLogger Asibfm700Logger; typedef mcc::MccSimpleSlewingModel Asibfm700SlewingModel; +typedef mcc::MccSimpleTrackingModel Asibfm700TrackingModel; } // namespace asibfm700 diff --git a/asibfm700/asibfm700_mount.cpp b/asibfm700/asibfm700_mount.cpp index fcb474a..7d3d79d 100644 --- a/asibfm700/asibfm700_mount.cpp +++ b/asibfm700/asibfm700_mount.cpp @@ -19,7 +19,7 @@ Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_p std::make_tuple(this), std::make_tuple(), std::make_tuple(this, Asibfm700Logger{logger}), - std::make_tuple(this), + std::make_tuple(this, Asibfm700Logger{logger}), std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)), // base_gm_class_t(Asibfm700StartState{}, // std::make_tuple(config.servoControllerConfig()), diff --git a/asibfm700/asibfm700_mount.h b/asibfm700/asibfm700_mount.h index 76855ce..f08b89c 100644 --- a/asibfm700/asibfm700_mount.h +++ b/asibfm700/asibfm700_mount.h @@ -22,14 +22,14 @@ class Asibfm700Mount : public Asibfm700CCTE, mcc::MccTelemetry, Asibfm700PZoneContainer, Asibfm700SlewingModel, - mcc::MccSimpleTrackingModel, + Asibfm700TrackingModel, Asibfm700Logger> { typedef mcc::MccGenericMount gm_class_t; diff --git a/mcc/mcc_ccte_erfa.h b/mcc/mcc_ccte_erfa.h index 245707d..8da070f 100644 --- a/mcc/mcc_ccte_erfa.h +++ b/mcc/mcc_ccte_erfa.h @@ -506,7 +506,8 @@ public: lst_eo(); if (!ret) { // ha = MccAngle(lst - from_pt.X + eo).normalize(); - ha = MccAngle(lst - from_pt.X - eo).normalize(); + ha = MccAngle(lst - from_pt.X - eo).normalize(); + // ha = MccAngle(lst - from_pt.X - eo).normalize(); } else { return ret; } diff --git a/mcc/mcc_slewing_model.h b/mcc/mcc_slewing_model.h index 26c6f91..84dd657 100644 --- a/mcc/mcc_slewing_model.h +++ b/mcc/mcc_slewing_model.h @@ -122,7 +122,10 @@ public: MccSimpleSlewingModel(CONTROLS_T* controls, LoggerT logger) : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex) { - logger.logDebug("Create MccSimpleSlewingModel class instance"); + std::ostringstream os; + os << std::this_thread::get_id(); + + logger.logDebug(std::format("Create MccSimpleSlewingModel class instance (thread: {})", os.str())); *_stopSlewing = true; diff --git a/mcc/mcc_tracking_model.h b/mcc/mcc_tracking_model.h index 58f01b3..8009e73 100644 --- a/mcc/mcc_tracking_model.h +++ b/mcc/mcc_tracking_model.h @@ -110,13 +110,18 @@ public: typedef MccSimpleMovingModelParams tracking_params_t; - template - MccSimpleTrackingModel(CONTROLS_T* controls) + template + MccSimpleTrackingModel(CONTROLS_T* controls, LoggerT logger) : _stopTracking(new std::atomic_bool()), _currentParamsMutex(new std::mutex()) { + std::ostringstream os; + os << std::this_thread::get_id(); + + logger.logDebug(std::format("Create MccSimpleTrackingModel class instance (thread: {})", os.str())); + *_stopTracking = true; - _trackingFunc = [controls, this]() -> error_t { + _trackingFunc = [logger = std::move(logger), controls, this]() -> error_t { typename CONTROLS_T::hardware_state_t hw_state; MccTelemetryData tdata; @@ -133,6 +138,10 @@ public: // double dist, dx, dy; + logger.logInfo("Start tracking:"); + logger.logInfo(" min time to pzone: {} secs", _currentParams.minTimeToPZone.count()); + + auto t_err = controls->telemetryData(&tdata); if (t_err) { *_stopTracking = true; @@ -183,10 +192,12 @@ public: _currentParams.timeShiftToTargetPoint); // point in +time_dist future - MccCelestialPoint pt{ - .pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP, - .X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0).normalize(), - .Y = tdata.DEC_APP}; + MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP, + // .X = MccAngle(dt.count() * std::numbers::pi / 3600.0 + // / 15.0).normalize(), + .X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0) + .normalize(), + .Y = tdata.DEC_APP}; mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point); point->time_point = pt.time_point; @@ -292,25 +303,23 @@ public: std::lock_guard lock{*_currentParamsMutex}; auto now = std::chrono::steady_clock::now(); - if ((now - last_corr_tp) < _currentParams.trackingCycleInterval) { - continue; - } - - // 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); + } } } @@ -321,6 +330,12 @@ public: *_stopTracking = true; return mcc_deduce_error_code(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE); } + + // sleep here + { + std::lock_guard lock{*_currentParamsMutex}; + std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval); + } } return MccSimpleTrackingModelErrorCode::ERROR_OK;