diff --git a/include/mcc/mcc_concepts.h b/include/mcc/mcc_concepts.h index 9cbc513..5501589 100644 --- a/include/mcc/mcc_concepts.h +++ b/include/mcc/mcc_concepts.h @@ -803,7 +803,7 @@ struct mcc_telemetry_interface_t { // get entered target position template SelfT> - RetT setTarget(this SelfT&& self, mcc_skypoint_c auto* pt) + RetT getTarget(this SelfT&& self, mcc_skypoint_c auto* pt) { return std::forward(self).getTarget(pt); } @@ -927,10 +927,15 @@ struct mcc_pzone_container_interface_t { template -concept mcc_pzone_container_c = std::derived_from> && requires { - // error type - requires mcc_error_c; -}; +concept mcc_pzone_container_c = + std::derived_from> && requires(const T t_const) { + // error type + requires mcc_error_c; + + [](R const&) { + return requires { requires std::formattable, char>; }; + }(t_const.pzoneNames()); + }; diff --git a/include/mcc/mcc_movement_controls.h b/include/mcc/mcc_movement_controls.h new file mode 100644 index 0000000..18aa311 --- /dev/null +++ b/include/mcc/mcc_movement_controls.h @@ -0,0 +1,550 @@ +#pragma once + + + +/**************************************************************************************** + * * + * MOUNT CONTROL COMPONENTS LIBRARY * + * * + * * + * IMPLEMENTATION OF VERY SIMPLE MOUNT MOVEMENT CONTROLS CLASS * + * * + ****************************************************************************************/ + +#include +#include + +#include "mcc/mcc_coordinate.h" +#include "mcc_concepts.h" +#include "mcc_error.h" + +namespace mcc::impl +{ + +namespace details +{ +// just auxiliary class instance +static MccNullLogger NullLogger{}; +} // namespace details + + +enum class MccSimpleMovementControlsErrorCode : int { + ERROR_OK, + ERROR_HW_GETSTATE, + ERROR_HW_SETSTATE, + ERROR_PCM_COMP, + ERROR_CCTE_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::impl + + +namespace std +{ + +template <> +class is_error_code_enum : public true_type +{ +}; + +} // namespace std + + + +namespace mcc::impl +{ + +// error category +struct MccSimpleMovementControlsCategory : public std::error_category { + MccSimpleMovementControlsCategory() : std::error_category() {} + + const char* name() const noexcept + { + return "SIMPLE-SLEWING-MODEL"; + } + + std::string message(int ec) const + { + MccSimpleMovementControlsErrorCode err = static_cast(ec); + + switch (err) { + case MccSimpleMovementControlsErrorCode::ERROR_OK: + return "OK"; + case MccSimpleMovementControlsErrorCode::ERROR_HW_GETSTATE: + return "cannot get hardware state"; + case MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE: + return "cannot set hardware state"; + case MccSimpleMovementControlsErrorCode::ERROR_PCM_COMP: + return "PCM computation error"; + case MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP: + return "celestial coordinate transformation error"; + case MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY: + return "cannot get telemetry"; + case MccSimpleMovementControlsErrorCode::ERROR_DIST_TELEMETRY: + return "cannot get target-to-mount-position distance"; + case MccSimpleMovementControlsErrorCode::ERROR_PZONE_CONTAINER_COMP: + return "pzone container computation error"; + case MccSimpleMovementControlsErrorCode::ERROR_TARGET_IN_PZONE: + return "target is in prohibited zone"; + case MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE: + return "near prohibited zone"; + case MccSimpleMovementControlsErrorCode::ERROR_TIMEOUT: + return "a timeout occured while slewing"; + case MccSimpleMovementControlsErrorCode::ERROR_ALREADY_SLEW: + return "already slewing"; + case MccSimpleMovementControlsErrorCode::ERROR_ALREADY_STOPPED: + return "slewing is already stopped"; + case MccSimpleMovementControlsErrorCode::ERROR_STOPPED: + return "slewing was stopped"; + default: + return "UNKNOWN"; + } + } + + static const MccSimpleMovementControlsCategory& get() + { + static const MccSimpleMovementControlsCategory constInst; + return constInst; + } +}; + + +inline std::error_code make_error_code(MccSimpleMovementControlsErrorCode ec) +{ + return std::error_code(static_cast(ec), MccSimpleMovementControlsCategory::get()); +} + + + +struct MccSimpleMovementControlsParameters { + // ******* common for all modes ******* + + // mean celestial rate + static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second + + // timeout to telemetry updating + std::chrono::milliseconds telemetryTimeout{3000}; + + // minimal time to prohibited zone (at current speed in slewing mode). if it is lesser then exit with error + std::chrono::seconds minTimeToPZone{10}; + + // time interval to update prohibited zones related quantities (e.g. intersection points) + std::chrono::milliseconds updatingPZoneInterval{5000}; + + + // ******* slewing mode ******* + + bool slewAndStop{false}; // slew to target and stop mount + + // coordinates difference to stop slewing (in radians) + double slewToleranceRadius{5.0_arcsecs}; + + // telemetry request interval + std::chrono::milliseconds slewingTelemetryInterval{100}; + + // 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 slewRateX{0.0}; // maximal slewing rate (0 means move with maximal allowed rate????!!!!!) + double slewRateY{0.0}; // maximal slewing rate (0 means move with maximal allowed rate????!!!!!) + + std::chrono::milliseconds adjustCycleInterval{500}; // minimum time between two successive adjustments + + double adjustRateX{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) + double adjustRateY{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage) + + // braking acceleration after execution of mount stopping command (in rads/s^2) + // it must be given as non-negative value!!! + double brakingAccelX{0.0}; + double brakingAccelY{0.0}; + + // slewing trajectory file. if empty - just skip saving + std::string slewingPathFilename{}; + // ******* tracking mode ******* + + // telemetry request interval + std::chrono::milliseconds trackingTelemetryInterval{100}; + + double trackSpeedX{}; + double trackSpeedY{}; + std::chrono::milliseconds trackingCycleInterval{500}; // minimum time between two successive tracking corrections + bool dualAxisTracking{true}; // mount must be of an equatorial type: false means guiding along only HA-axis + + // time shift into future to compute target position in future (UT1-scale time duration) + std::chrono::milliseconds timeShiftToTargetPoint{10000}; + // maximal target-to-mount difference for tracking process (in arcsecs) + // it it is greater then the current mount coordinates are used as target one + double trackingMaxCoordDiff{20.0}; + + // tracking trajectory file. if empty - just skip saving + std::string trackingPathFilename{}; +}; + + + +class MccSimpleMovementControls +{ +protected: + // a simple class for movement path saving + class PathFile + { + public: + PathFile(const std::string& filename = "") : _filename(filename), _st() {} + + void setFilename(const std::string& filename) + { + _filename = filename; + } + + std::string getFilename() const + { + return _filename; + } + + ~PathFile() + { + save(); + } + + friend PathFile& operator<<(PathFile& pf, auto&& v) + { + pf._st << std::forward(v); + + return pf; + } + + bool save() + { + std::fstream fst; + + if (_filename.empty()) { + return false; + } + + if (_st.str().empty()) { // nothing to save + return true; + } + + fst.open(_filename); + + if (!fst.is_open()) { + return false; + } + + fst << _st.str(); + _st.str(""); + _filename.clear(); + + return true; + } + + private: + std::string _filename; + std::istringstream _st; + }; + + +public: + typedef MccError error_t; + + typedef MccSimpleMovementControlsParameters movement_params_t; + + template CallbackFuncT = decltype([](STATUS_T const&) {}), + mcc_logger_c LOGGER_T = MccNullLogger> + MccSimpleMovementControls( + HARDWARE_T* hardware, + TELEMETRY_T* telemtry, + PZONE_CONT_T* pzone_cont, + CallbackFuncT&& mode_switch_callback = [](STATUS_T const&) {}, + LOGGER_T& logger = details::NullLogger) + { + auto log_ptr = &logger; + + auto send_to_hardware = [hardware, log_ptr](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t { + if constexpr (std::derived_from>) { + log_ptr->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", hw_state.XY.x().degrees(), + hw_state.XY.y().degrees())); + } else { // user defined mcc_coord_pair_c + log_ptr->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", + MccAngle((double)hw_state.XY.x()).degrees(), + MccAngle((double)hw_state.XY.y()).degrees())); + } + + auto hw_err = hardware->hardwareSetState(hw_state); + if (hw_err) { + mcc_deduced_err(hw_err, MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE); + } + + log_ptr->logDebug(" the 'hardwareSetState' method performed successfully!"); + + + return MccSimpleMovementControlsErrorCode::ERROR_OK; + }; + + + auto check_pzones = [pzone_cont, log_ptr](typename TELEMETRY_T::telemetry_data_t const& tdata, + double min_time_to_pzone_in_secs, double braking_accelX, + double braking_accelY) { + bool in_zone; + std::vector in_zone_vec; + + // calculate the distances along the X and Y axes that the mount will travel at the current speed in a given + // time, taking into account the braking acceleration + double speedX = tdata.hwState.speedXY.x(); + double speedY = tdata.hwState.speedXY.y(); + + // time to stop mount with given current speed and constant braking acceleration + double tx_stop = std::abs(speedX) / braking_accelX; + double ty_stop = std::abs(speedY) / braking_accelY; + + double tx = min_time_to_pzone_in_secs; + double ty = min_time_to_pzone_in_secs; + + if (std::isfinite(tx_stop) && (min_time_to_pzone_in_secs > tx_stop)) { + tx = tx_stop; + } + if (std::isfinite(ty_stop) && (min_time_to_pzone_in_secs > ty_stop)) { + ty = ty_stop; + } + + // the distance: + // here, one must take into account the sign of the speed!!! + double dx = speedX * tx - std::copysign(braking_accelX, speedX) * tx * tx / 2.0; + double dy = speedY * ty - std::copysign(braking_accelY, speedY) * ty * ty / 2.0; + + log_ptr->logTrace( + std::format(" the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}", + min_time_to_pzone_in_secs, MccAngleFancyString(dx), MccAngleFancyString(dy))); + + + // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead + // and check them for getting into the prohibited zones + + std::conditional_t> + mount_pos; + + static_assert(!std::is_null_pointer_v, "UNKNOWn MOUNT TYPE"); + + mount_pos.setX(tdata.hwState.XY.x() + tdata.pcmCorrection.x + dx); + mount_pos.setY(tdata.hwState.XY.y() + tdata.pcmCorrection.y + dy); + mount_pos.setEpoch(tdata.hwState.XY.epoch()); + + log_ptr->logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s", MccAngleFancyString(speedX), + MccAngleFancyString(speedY))); + + auto pz_err = pzone_cont->inPZone(mount_pos, &in_zone, &in_zone_vec); + if (pz_err) { + return mcc_deduced_err(pz_err, MccSimpleMovementControlsErrorCode::ERROR_PZONE_CONTAINER_COMP); + } + + if (in_zone) { + size_t i = 0; + for (; i < in_zone_vec.size(); ++i) { + if (in_zone_vec[i]) { + break; + } + } + + auto names = pzone_cont->pzoneNames(); + auto it = names.begin(); + std::ranges::advance(it, i); + + log_ptr->logError( + "target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i, + *it); + + MccSkyHADEC_OBS hadec; + MccSkyRADEC_OBS radec; + MccSkyAZZD azzd; + MccSkyAZALT azalt; + MccAngle lst; + + auto ccte_err = tdata.mountPos.appSideralTime(&lst, true); + if (ccte_err) { + return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + } + + ccte_err = tdata.mountPos.toAtSameEpoch(radec, hadec, azzd, azalt); + if (ccte_err) { + return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + } + + log_ptr->logError(std::format(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", + MccAngle{radec.x()}.sexagesimal(true), MccAngle{radec.y()}.sexagesimal(), + MccAngle{hadec.x()}.sexagesimal(true), lst.sexagesimal(true))); + log_ptr->logError(std::format(" AZ, ZD, ALT: {}, {}, {}", MccAngle{azzd.x()}.sexagesimal(), + MccAngle{azzd.y()}.sexagesimal(), MccAngle{azalt.y()}.sexagesimal())); + + log_ptr->logError(std::format(" hardware X, Y: {}, {}", MccAngle{tdata.hwState.XY.x()}.sexagesimal(), + MccAngle{tdata.hwState.XY.y()}.sexagesimal())); + + return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE; + } + + + return MccSimpleMovementControlsErrorCode::ERROR_OK; + }; + + + auto log_pos = [log_ptr, this](typename TELEMETRY_T::telemetry_data_t const& tdata) { + double x_mnt, y_mnt, x_tag, y_tag; + if constexpr (mccIsEquatorialMount(HARDWARE_T::mountType)) { + MccSkyHADEC_OBS hadec; + auto ccte_err = tdata.targetPos.toAtSameEpoch(hadec); + if (ccte_err) { + return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + } + + x_tag = hadec.x(); + y_tag = hadec.y(); + + log_ptr->logTrace(std::format(" current target: HA = {}, DEC = {}", hadec.x().sexagesimal(true), + hadec.y().sexagesimal())); + + ccte_err = tdata.mountPos.toAtSameEpoch(hadec); + if (ccte_err) { + return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + } + + x_mnt = hadec.x(); + y_mnt = hadec.y(); + + + log_ptr->logTrace(std::format(" current mount: HA = {}, DEC = {}", hadec.x().sexagesimal(true), + hadec.y().sexagesimal())); + + + } else if constexpr (mccIsAltAzMount(HARDWARE_T::mountType)) { + MccSkyAZZD azzd; + + auto ccte_err = tdata.targetPos.toAtSameEpoch(azzd); + if (ccte_err) { + return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + } + + x_tag = azzd.x(); + y_tag = azzd.y(); + + log_ptr->logTrace( + std::format(" target: AZ = {}, ZD = {}", azzd.x().sexagesimal(), azzd.y().sexagesimal())); + + + ccte_err = tdata.mountPos.toAtSameEpoch(azzd); + if (ccte_err) { + return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + } + + x_mnt = azzd.x(); + y_mnt = azzd.y(); + + log_ptr->logTrace( + std::format(" mount: AZ = {}, ZD = {}", azzd.x().sexagesimal(), azzd.y().sexagesimal())); + } + + _pathFile << tdata.mountPos.epoch().UTC.time_since_epoch().count() << " " << x_tag << " " << y_tag << " " + << x_mnt << " " << y_mnt << " " << (x_tag - x_mnt) << " " << (y_tag - y_mnt) << " " + << (int)tdata.hwState.movementState << "\n"; + }; + + auto cb_sptr = + std::make_shared>(std::forward(mode_switch_callback)); + + + /* stop moving function */ + + _stopMovingFunc = [hardware, cb_sptr, this]() { + typename HARDWARE_T::hardware_state_t hw_state; + hw_state.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPING; + + *_stopMoving = true; + + *_lastError = send_to_hardware(hw_state); + + if (_lastError->load()) { + (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR); + } + }; + } + + + error_t slewToTarget(bool slew_and_stop = false) + { + _slewingFunc(slew_and_stop); + + return *_lastError; + } + + error_t trackTarget() + { + _trackingFunc(); + + return *_lastError; + } + + + error_t stopMount() + { + if (*_stopMoving) { + *_lastError = MccSimpleMovementControlsErrorCode::ERROR_ALREADY_STOPPED; + } else { + _stopMovingFunc(); + } + + return *_lastError; + } + + error_t setMovementParams(movement_params_t const& params) + { + std::lock_guard lock{*_currentParamsMutex}; + + _currentParams = params; + + return MccSimpleMovementControlsErrorCode::ERROR_OK; + } + + movement_params_t getMovementParams() const + { + std::lock_guard lock{*_currentParamsMutex}; + + return _currentParams; + } + + error_t mountMovementLastError() const + { + return _lastError->load(); + } + +protected: + std::function _slewingFunc{}; + std::function _trackingFunc{}; + std::function _stopMovingFunc{}; + + std::unique_ptr _stopMoving{new std::atomic_bool{false}}; + + std::unique_ptr _currentParamsMutex{new std::mutex{}}; + movement_params_t _currentParams{}; + + std::unique_ptr> _lastError{ + new std::atomic{MccSimpleMovementControlsErrorCode::ERROR_OK}}; + + PathFile _pathFile{}; +}; + +} // namespace mcc::impl diff --git a/include/mcc/mcc_pzone_container.h b/include/mcc/mcc_pzone_container.h index 3d65f12..de50079 100644 --- a/include/mcc/mcc_pzone_container.h +++ b/include/mcc/mcc_pzone_container.h @@ -144,6 +144,8 @@ public: return MccPZoneContainerErrorCode::ERROR_OK; }); + _names.push_back(std::format("{}", zone.pzoneName)); + return _inZoneFunc.size(); } @@ -152,9 +154,16 @@ public: _inZoneFunc.clear(); _timeToZoneFunc.clear(); _timeFromZoneFunc.clear(); + + _names.clear(); } + std::vector pzoneNames() const + { + return _names; + } + error_t inPZone(mcc_skypoint_c auto const& coords, bool* at_least_one, std::ranges::output_range auto* result) { auto err = forEach(_inZoneFunc, coords, result); @@ -188,9 +197,11 @@ public: protected: typedef std::chrono::nanoseconds duration_t; - std::vector> _inZoneFunc; - std::vector> _timeToZoneFunc; - std::vector> _timeFromZoneFunc; + std::vector> _inZoneFunc{}; + std::vector> _timeToZoneFunc{}; + std::vector> _timeFromZoneFunc{}; + + std::vector _names{}; error_t forEach(auto& func_cont, MccSkyPoint const& pt, auto* result) { @@ -205,7 +216,7 @@ protected: res_elem_t res_elem; - size_t res_sz = std::ranges::size(*result); + // size_t res_sz = std::ranges::size(*result); auto it = result->begin(); for (auto& func : func_cont) {