diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a2e2e9..bcfa1dc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -215,6 +215,7 @@ set(MCC_SRC include/mcc/mcc_pcm.h include/mcc/mcc_telemetry.h include/mcc/mcc_movement_controls.h + include/mcc/mcc_generic_movecontrols.h include/mcc/mcc_serialization_common.h include/mcc/mcc_deserializer.h include/mcc/mcc_serializer.h @@ -265,7 +266,10 @@ endif() if(USE_BSPLINE_PCM) target_compile_definitions(${PROJECT_NAME} INTERFACE USE_BSPLINE_PCM) target_link_libraries(${PROJECT_NAME} INTERFACE fitpack) - target_link_directories(${PROJECT_NAME} INTERFACE "$") + target_link_directories( + ${PROJECT_NAME} + INTERFACE "$" + ) endif() if(USE_ASIO) @@ -275,7 +279,10 @@ endif() if(USE_SPDLOG) target_link_libraries(${PROJECT_NAME} INTERFACE spdlog::spdlog_header_only) # target_compile_definitions(${PROJECT_NAME} INTERFACE SPDLOG_USE_STD_FORMAT=1 SPDLOG_FMT_EXTERNAL=0) - target_compile_definitions(${PROJECT_NAME} INTERFACE SPDLOG_USE_STD_FORMAT=1) + target_compile_definitions( + ${PROJECT_NAME} + INTERFACE SPDLOG_USE_STD_FORMAT=1 + ) endif() if(BUILD_TESTS) diff --git a/include/mcc/mcc_concepts.h b/include/mcc/mcc_concepts.h index c1f83be..5565f24 100644 --- a/include/mcc/mcc_concepts.h +++ b/include/mcc/mcc_concepts.h @@ -499,21 +499,55 @@ struct mcc_skypoint_interface_t { { return std::forward(self).EO(eo); } + + // template SelfT> + // typename std::remove_cvref_t::dist_result_t distance(this SelfT&& self, auto const& sp) + // { + // return std::forward(self).distance(sp); + // } + // template SelfT> + // SelfT& operator+=(this SelfT& self, mcc_coord_pair_c auto const& dxy) + // { + // return self.operator+=(dxy); + // } + + // template SelfT> + // SelfT& operator-=(this SelfT& self, mcc_coord_pair_c auto const& dxy) + // { + // return self.operator-=(dxy); + // } }; template -concept mcc_skypoint_c = std::derived_from && requires(const T t_const) { - { t_const.epoch() } -> mcc_coord_epoch_c; +concept mcc_skypoint_c = + std::derived_from && requires(const T t_const, const T t_other_const, T t) { + { t_const.epoch() } -> mcc_coord_epoch_c; - // currently stored coordinates pair kind - { t_const.pairKind() } -> std::same_as; + // currently stored coordinates pair kind + { t_const.pairKind() } -> std::same_as; - // currently stored co-longitude coordinate - { t_const.co_lon() } -> std::convertible_to; + // currently stored co-longitude coordinate + { t_const.co_lon() } -> std::convertible_to; - // currently stored co-latitude coordinate - { t_const.co_lat() } -> std::convertible_to; -}; + // currently stored co-latitude coordinate + { t_const.co_lat() } -> std::convertible_to; + + requires requires(typename T::dist_result_t res) { + requires mcc_angle_c; // distance on sphere + requires mcc_angle_c; // defference on co-longitude coordinates + requires mcc_angle_c; // defference on co-latitude coordinates + requires mcc_angle_c; // co-longitude coordinates of target sky point (in the same + // coordinate system as 'this') + requires mcc_angle_c; // co-latitude coordinates of target sky point (in the same + // coordinate system as 'this') + }; + + // distance on sphere between two sky points + { t_const.distance(std::declval()) } -> std::same_as; + + // { t_const - t_other_const } -> mcc_coord_pair_c; + // { t_const + t_other_const } -> mcc_coord_pair_c; + }; diff --git a/include/mcc/mcc_coordinate.h b/include/mcc/mcc_coordinate.h index 646d0ee..69cacaf 100644 --- a/include/mcc/mcc_coordinate.h +++ b/include/mcc/mcc_coordinate.h @@ -380,6 +380,14 @@ public: using error_t = typename CCTE_T::error_t; + struct dist_result_t { + MccAngle dist{}; + MccAngle dx{}; + MccAngle dy{}; + MccAngle x2{}; + MccAngle y2{}; + }; + MccGenericSkyPoint() {} template @@ -685,11 +693,34 @@ public: return cctEngine.equationOrigins(_epoch, eo); } + + dist_result_t distance(MccGenericSkyPoint const& sp) const + // dist_result_t distance(mcc_skypoint_c auto const& sp) + { + double x, y; + + if (_pairKind == sp.pairKind() && utils::isEqual(_epoch.MJD(), sp.epoch().MJD())) { + x = sp.co_lon(); + y = sp.co_lat(); + } else { // convert to the same coordinates kind + MccGenericSkyPoint p{sp}; + p.to(_pairKind, _epoch); + + x = p.co_lon(); + y = p.co_lat(); + } + + auto d = utils::distanceOnSphere(_x, _y, x, y); + + return {.dist = std::get<2>(d), .dx = std::get<0>(d), .dx = std::get<1>(d), .x2 = x, .y2 = y}; + } + protected: double _x{0.0}, _y{0.0}; MccCoordPairKind _pairKind{MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; MccCelestialCoordEpoch _epoch{}; // J2000.0 + template void fromOtherSkyPoint(T&& other) { diff --git a/include/mcc/mcc_generic_movecontrols.h b/include/mcc/mcc_generic_movecontrols.h index e16397f..5283c60 100644 --- a/include/mcc/mcc_generic_movecontrols.h +++ b/include/mcc/mcc_generic_movecontrols.h @@ -1,5 +1,21 @@ #pragma once +/**************************************************************************************** + * * + * MOUNT CONTROL COMPONENTS LIBRARY * + * * + * * + * GENERIC IMPLEMENTATION OF MOUNT MOVEMENT CONTROLS * + * * + ****************************************************************************************/ + + + +#include +#include +#include +#include + #include "mcc_error.h" @@ -26,7 +42,10 @@ namespace mcc::impl // error category struct MccGenericMovementControlsErrorCategory : std::error_category { - const char* name() const noexcept { return "MCC-GENERIC-MOVECONTRL"; } + const char* name() const noexcept + { + return "MCC-GENERIC-MOVECONTRL"; + } std::string message(int ec) const { @@ -44,12 +63,12 @@ struct MccGenericMovementControlsErrorCategory : std::error_category { default: return "unknown"; } + } - static const MccGenericMovementControlsErrorCategory& get() - { - static const MccGenericMovementControlsErrorCategory constInst; - return constInst; - } + static const MccGenericMovementControlsErrorCategory& get() + { + static const MccGenericMovementControlsErrorCategory constInst; + return constInst; } }; @@ -89,28 +108,280 @@ struct MccGenericMovementControlsParams { }; -template +/* UTILITY CLASS TO HOLD AND SAVE MOUNT MOVING TRAJECTORY */ + +struct MccMovementPathFile { + static constexpr std::string_view commentSeq{"# "}; + static constexpr std::string_view lineDelim{"\n"}; + + void setCommentSeq(traits::mcc_input_char_range auto const& s) + { + _commentSeq.clear(); + std::ranges::copy(s, std::back_inserter(_commentSeq)); + } + + void setLineDelim(traits::mcc_input_char_range auto const& s) + { + _lineDelim.clear(); + std::ranges::copy(s, std::back_inserter(_lineDelim)); + } + + // add comment string/strings + template + void addComment(RT const& r, RTs const&... rs) + { + std::ranges::copy(_commentSeq, std::back_inserter(_buffer)); + std::ranges::copy(r, std::back_inserter(_buffer)); + std::ranges::copy(lineDelim, std::back_inserter(_buffer)); + + if constexpr (sizeof...(RTs)) { + addComment(rs...); + } + } + + // general purpose method + template ... ArgTs> + void addToPath(std::format_string fmt, ArgTs&&... args) + { + std::format_to(std::back_inserter(_buffer), fmt, std::forward(args)...); + std::ranges::copy(lineDelim, std::back_inserter(_buffer)); + } + + // general purpose method + template ... ArgTs> + void addToPath(std::string_view fmt, ArgTs&&... args) + { + std::vformat_to(std::back_inserter(_buffer), fmt, std::make_format_args(std::forward(args)...)); + std::ranges::copy(lineDelim, std::back_inserter(_buffer)); + } + + // default-implemented method + void addToPath(mcc_telemetry_data_c auto const& tdata) + { + // UNIX-time millisecs, mount X, mount Y, target X, target Y, dX(mount-target), dY(mount-target), dist, state + auto dist = tdata.mountPos.distance(tdata.targetPos); + + using d_t = std::chrono::milliseconds; + + auto tp = std::chrono::duration_cast(tdata.mountPos.epoch().UTC().time_since_epoch()); + + const std::string_view d_fmt = "{:f14.8}"; + const auto v = std::views::repeat(d_fmt, 7) | std::views::join_with(' '); + std::string fmt = "{} " + std::string(v.begin(), v.end()) + " {}"; + + addToPath(fmt, tp.count(), (double)tdata.mountPos.co_lon() * MCC_RADS_TO_DEGRESS, + (double)tdata.mountPos.co_lat() * MCC_RADS_TO_DEGRESS, (double)dist.x2 * MCC_RADS_TO_DEGRESS, + (double)dist.y2 * MCC_RADS_TO_DEGRESS, (double)dist.dx * MCC_RADS_TO_DEGRESS, + (double)dist.dy * MCC_RADS_TO_DEGRESS, (double)dist.dist * MCC_RADS_TO_DEGRESS, + (int)tdata.hwState.movementState); + } + + void clearPath() + { + _buffer.clear(); + } + + bool saveToFile(std::string const& filename, std::ios_base::openmode mode = std::ios_base::trunc) + { + std::ofstream fst(filename, mode); + + if (fst.is_open()) { + fst << _buffer; + + return true; + } else { + return false; + } + } + +protected: + std::string _buffer{}; + + std::string _commentSeq{commentSeq}; + std::string _lineDelim{lineDelim}; +}; + + +enum class MccGenericMovementControlsPolicy : int { POLICY_ASYNC, POLICY_BLOCKING }; + +template class MccGenericMovementControls { public: + static constexpr MccGenericMovementControlsPolicy executePolicy = EXEC_POLICY; + + static constexpr std::chrono::seconds defaultWaitTimeout{3}; + typedef MccError error_t; typedef PARAMS_T movement_params_t; - error_t slewToTarget(bool slew_and_stop) {} + ~MccGenericMovementControls() + { + stopMount(); - error_t trackTarget() {} + if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { + if (_slewFuncFuture.valid()) { + auto status = _slewFuncFuture.wait_for(_waitTimeout->load()); + } - error_t stopMount() {} + if (_trackFuncFuture.valid()) { + auto status = _trackFuncFuture.wait_for(_waitTimeout->load()); + } - error_t setMovementParams(movement_params_t const& pars) { _currentParams->store(pars); } + if (_stopFuncFuture.valid()) { + auto status = _stopFuncFuture.wait_for(_waitTimeout->load()); + } + } + } - movement_params_t getMovementParams() const { return _currentParams->load(); } + error_t slewToTarget(bool slew_and_stop) + { + _stopMovementRequest = false; + + if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { + _slewFuncFuture = std::async(std::launch::async, _slewFunc, slew_and_stop); + + return MccGenericMovementControlsErrorCode::ERROR_OK; + } else if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_BLOCKING) { + return _slewFunc(slew_and_stop); + } else { + static_assert(false, "UNKNOWN EXECUTION POLICY!"); + } + } + + error_t trackTarget() + { + _stopMovementRequest = false; + + if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { + _trackFuncFuture = std::async(std::launch::async, _trackFunc); + + return MccGenericMovementControlsErrorCode::ERROR_OK; + } else if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_BLOCKING) { + return _trackFunc(); + } else { + static_assert(false, "UNKNOWN EXECUTION POLICY!"); + } + } + + error_t stopMount() + { + _stopMovementRequest = true; + + if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { + _stopFuncFuture = std::async(std::launch::async, _stopFunc); + + return MccGenericMovementControlsErrorCode::ERROR_OK; + } else if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_BLOCKING) { + return _stopFunc(); + } else { + static_assert(false, "UNKNOWN EXECUTION POLICY!"); + } + } + + error_t setMovementParams(movement_params_t const& pars) + { + _currentParams->store(pars); + + return MccGenericMovementControlsErrorCode::ERROR_OK; + } + + movement_params_t getMovementParams() const + { + return _currentParams->load(); + } protected: - MccGenericMovementControls() = default; + template SLEW_FUNC_T, std::invocable<> TRACK_FUNC_T, std::invocable<> STOP_FUNC_T> + MccGenericMovementControls(SLEW_FUNC_T&& slew_func, TRACK_FUNC_T&& track_func, STOP_FUNC_T&& stop_func) + : _slewFunc(std::forward(slew_func)), + _trackFunc(std::forward(track_func)), + _stopFunc(std::forward(stop_func)){}; std::unique_ptr> _currentParams{new std::atomic{}}; + + std::atomic_bool _stopMovementRequest{false}; + + std::function _slewFunc{}; + std::function _trackFunc; + std::function _stopFunc; + + std::conditional_t, + std::nullptr_t> + _slewFuncFuture{}; + + std::conditional_t, + std::nullptr_t> + _trackFuncFuture{}; + + std::conditional_t, + std::nullptr_t> + _stopFuncFuture{}; + + + std::unique_ptr> _waitTimeout{ + new std::atomic{defaultWaitTimeout}}; + + + // utility methods + + // the method calculates the change in coordinates of a point over a given time given the current speed and braking + // acceleration. a position after given 'time' interval is returned + auto coordsAfterTime(mcc_coord_pair_c auto& cp, + mcc_coord_pair_c auto const& speedXY, // in radians per seconds + mcc_coord_pair_c auto const& braking_accelXY, // in radians per seconds in square + traits::mcc_time_duration_c auto const& time, + mcc_coord_pair_c auto* dxy = nullptr) + { + // time to stop mount with given current speed and constant braking acceleration + double tx_stop = std::abs(speedXY.x()) / braking_accelXY.x(); + double ty_stop = std::abs(speedXY.y()) / braking_accelXY.y(); + + using secs_t = std::chrono::duration; // seconds as double + + double tx = std::chrono::duration_cast(time).count(); + double ty = std::chrono::duration_cast(time).count(); + + if (std::isfinite(tx_stop) && (tx > tx_stop)) { + tx = tx_stop; + } + if (std::isfinite(ty_stop) && (ty > ty_stop)) { + ty = ty_stop; + } + + // the distance: + // here, one must take into account the sign of the speed!!! + double dx = speedXY.x() * tx - std::copysign(braking_accelXY.x(), speedXY.x()) * tx * tx / 2.0; + double dy = speedXY.y() * ty - std::copysign(braking_accelXY.y(), speedXY.y()) * ty * ty / 2.0; + + std::remove_cvref_t cp_res{}; + cp_res.setEpoch(cp.epoch().UTC() + time); + + cp_res.setX((double)cp.x() + dx); + cp_res.setY((double)cp.y() + dy); + + if (dxy) { + dxy->setX(dx); + dxy->setY(dy); + dxy->setEpoch(cp_res.epoch()); + } + + return cp_res; + } }; + +template +using MccGenericBlockingMovementControls = + MccGenericMovementControls; + +template +using MccGenericAsyncMovementControls = + MccGenericMovementControls; + } // namespace mcc::impl