diff --git a/include/mcc/mcc_coordinate.h b/include/mcc/mcc_coordinate.h index 69cacaf..fcaa8ac 100644 --- a/include/mcc/mcc_coordinate.h +++ b/include/mcc/mcc_coordinate.h @@ -712,7 +712,7 @@ public: 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}; + return {.dist = std::get<2>(d), .dx = std::get<0>(d), .dy = std::get<1>(d), .x2 = x, .y2 = y}; } protected: diff --git a/include/mcc/mcc_generic_movecontrols.h b/include/mcc/mcc_generic_movecontrols.h index 64dbfd5..cae0b01 100644 --- a/include/mcc/mcc_generic_movecontrols.h +++ b/include/mcc/mcc_generic_movecontrols.h @@ -16,9 +16,9 @@ #include #include +#include "mcc_coordinate.h" #include "mcc_error.h" - namespace mcc::impl { enum class MccGenericMovementControlsErrorCode : int { @@ -140,18 +140,18 @@ struct MccMovementPathFile { } // 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)); - } + // 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) + void addToPath(std::string_view fmt, ArgTs&... args) { - std::vformat_to(std::back_inserter(_buffer), fmt, std::make_format_args(std::forward(args)...)); + std::vformat_to(std::back_inserter(_buffer), fmt, std::make_format_args(args...)); std::ranges::copy(lineDelim, std::back_inserter(_buffer)); } @@ -169,11 +169,15 @@ struct MccMovementPathFile { 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); + + int state = (int)tdata.hwState.movementState; + auto tp_val = tp.count(); + + double mnt_x = MccAngle(tdata.mountPos.co_lon()).degrees(), mnt_y = MccAngle(tdata.mountPos.co_lon()).degrees(), + tag_x = dist.x2.degrees(), tag_y = dist.y2.degrees(), dx = dist.dy.degrees(), dy = dist.dy.degrees(), + dd = dist.dist.degrees(); + + addToPath(std::string_view(fmt.begin(), fmt.end()), tp_val, mnt_x, mnt_y, tag_x, tag_y, dx, dy, dd, state); } void clearPath() @@ -204,7 +208,7 @@ protected: enum class MccGenericMovementControlsPolicy : int { POLICY_ASYNC, POLICY_BLOCKING }; -template class MccGenericMovementControls { @@ -217,7 +221,21 @@ public: typedef PARAMS_T movement_params_t; - ~MccGenericMovementControls() + 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)) + { + } + + MccGenericMovementControls(const MccGenericMovementControls&) = delete; + MccGenericMovementControls(MccGenericMovementControls&& other) = default; + + MccGenericMovementControls& operator=(const MccGenericMovementControls&) = delete; + MccGenericMovementControls& operator=(MccGenericMovementControls&&) = default; + + virtual ~MccGenericMovementControls() { stopMount(); @@ -238,7 +256,7 @@ public: error_t slewToTarget(bool slew_and_stop) { - _stopMovementRequest = false; + *_stopMovementRequest = false; if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { _slewFuncFuture = std::async(std::launch::async, _slewFunc, slew_and_stop); @@ -253,7 +271,7 @@ public: error_t trackTarget() { - _stopMovementRequest = false; + *_stopMovementRequest = false; if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { _trackFuncFuture = std::async(std::launch::async, _trackFunc); @@ -268,7 +286,7 @@ public: error_t stopMount() { - _stopMovementRequest = true; + *_stopMovementRequest = true; if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) { _stopFuncFuture = std::async(std::launch::async, _stopFunc); @@ -283,30 +301,29 @@ public: error_t setMovementParams(movement_params_t const& pars) { - _currentParams->store(pars); + std::lock_guard lock{*_currentMovementParamsMutex}; + + _currentMovementParams = pars; return MccGenericMovementControlsErrorCode::ERROR_OK; } movement_params_t getMovementParams() const { - return _currentParams->load(); + std::lock_guard lock{*_currentMovementParamsMutex}; + + return _currentMovementParams; } protected: - 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 _currentMovementParamsMutex{new std::mutex{}}; + PARAMS_T _currentMovementParams{}; - std::unique_ptr> _currentParams{new std::atomic{}}; - - std::atomic_bool _stopMovementRequest{false}; + std::unique_ptr _stopMovementRequest{new std::atomic_bool{false}}; std::function _slewFunc{}; - std::function _trackFunc; - std::function _stopFunc; + std::function _trackFunc{}; + std::function _stopFunc{}; std::conditional_t, @@ -332,7 +349,7 @@ protected: // 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, + auto coordsAfterTime(mcc_coord_pair_c auto const& 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, @@ -360,7 +377,7 @@ protected: 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.setEpoch(cp.epoch() + time); cp_res.setX((double)cp.x() + dx); cp_res.setY((double)cp.y() + dy); @@ -373,6 +390,63 @@ protected: return cp_res; } + + auto coordsAfterTime(mcc_skypoint_c auto const& sp, + 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) + { + auto run_func = [&, this](auto& cp) { + sp.toAtSameEpoch(cp); + + auto new_cp = coordsAfterTime(cp, speedXY, braking_accelXY, time, dxy); + + std::remove_cvref_t sp_res{}; + + sp_res.from(cp); + + return sp_res; + }; + + switch (sp.pairKind()) { + case MccCoordPairKind::COORDS_KIND_RADEC_ICRS: { + MccSkyRADEC_ICRS rd; + return run_func(rd); + }; + case MccCoordPairKind::COORDS_KIND_RADEC_OBS: { + MccSkyRADEC_OBS rd; + return run_func(rd); + }; + case MccCoordPairKind::COORDS_KIND_RADEC_APP: { + MccSkyRADEC_APP rd; + return run_func(rd); + }; + case MccCoordPairKind::COORDS_KIND_HADEC_OBS: { + MccSkyHADEC_OBS hd; + return run_func(hd); + }; + case MccCoordPairKind::COORDS_KIND_HADEC_APP: { + MccSkyHADEC_APP hd; + return run_func(hd); + }; + case MccCoordPairKind::COORDS_KIND_AZZD: { + MccSkyAZZD azzd; + return run_func(azzd); + }; + case MccCoordPairKind::COORDS_KIND_AZALT: { + MccSkyAZALT azalt; + return run_func(azalt); + }; + case MccCoordPairKind::COORDS_KIND_GENERIC: + case MccCoordPairKind::COORDS_KIND_XY: { + MccGenXY xy; + return run_func(xy); + }; + default: + return sp; + } + } };