diff --git a/asibfm700/asibfm700_servocontroller.cpp b/asibfm700/asibfm700_servocontroller.cpp index c6dd94a..46edf90 100644 --- a/asibfm700/asibfm700_servocontroller.cpp +++ b/asibfm700/asibfm700_servocontroller.cpp @@ -108,6 +108,38 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(har { std::lock_guard lock{*_setStateMutex}; + if (state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) { // stop! + error_t err = static_cast(Mount.stop()); + if (err) { + return err; + } + + hardware_state_t hw_state; + + auto start_tp = std::chrono::steady_clock::now(); + + // poll hardware till stopped-state detected ... + while (true) { + err = hardwareGetState(&hw_state); + if (err) { + return err; + } + + if (hw_state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) { + break; + } + + if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) { + err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT; + break; + } + + std::this_thread::sleep_for(_hardwareConfig.pollingInterval); + } + + return err; + } + // static thread_local coordval_pair_t cvalpair{.X{0.0, 0.0}, .Y{0.0, 0.0}}; // static thread_local coordpair_t cpair{.X = 0.0, .Y = 0.0}; diff --git a/mcc/mcc_angle.h b/mcc/mcc_angle.h index 365a358..211d4b0 100644 --- a/mcc/mcc_angle.h +++ b/mcc/mcc_angle.h @@ -459,11 +459,31 @@ class MccAngleDEC_APP : public MccAngle using MccAngle::MccAngle; }; +class MccAngleRA_OBS : public MccAngle +{ + using MccAngle::MccAngle; +}; + +class MccAngleDEC_OBS : public MccAngle +{ + using MccAngle::MccAngle; +}; + class MccAngleHA : public MccAngle { using MccAngle::MccAngle; }; +class MccAngleHA_APP : public MccAngle +{ + using MccAngle::MccAngle; +}; + +class MccAngleHA_OBS : public MccAngle +{ + using MccAngle::MccAngle; +}; + class MccAngleAZ : public MccAngle { using MccAngle::MccAngle; @@ -516,7 +536,11 @@ enum class MccCoordKind : size_t { COORDS_KIND_DEC_ICRS = traits::mcc_type_hash, COORDS_KIND_RA_APP = traits::mcc_type_hash, COORDS_KIND_DEC_APP = traits::mcc_type_hash, + COORDS_KIND_RA_OBS = traits::mcc_type_hash, + COORDS_KIND_DEC_OBS = traits::mcc_type_hash, COORDS_KIND_HA = traits::mcc_type_hash, + COORDS_KIND_HA_APP = traits::mcc_type_hash, + COORDS_KIND_HA_OBS = traits::mcc_type_hash, COORDS_KIND_AZ = traits::mcc_type_hash, COORDS_KIND_ZD = traits::mcc_type_hash, COORDS_KIND_ALT = traits::mcc_type_hash, @@ -531,18 +555,23 @@ enum class MccCoordPairKind : size_t { COORDS_KIND_GENERIC = traits::mcc_type_pair_hash(), COORDS_KIND_RADEC_ICRS = traits::mcc_type_pair_hash(), COORDS_KIND_RADEC_APP = traits::mcc_type_pair_hash(), + COORDS_KIND_RADEC_OBS = traits::mcc_type_pair_hash(), COORDS_KIND_HADEC_APP = traits::mcc_type_pair_hash(), + // COORDS_KIND_HADEC_APP = traits::mcc_type_pair_hash(), + COORDS_KIND_HADEC_OBS = traits::mcc_type_pair_hash(), COORDS_KIND_AZZD = traits::mcc_type_pair_hash(), COORDS_KIND_AZALT = traits::mcc_type_pair_hash(), COORDS_KIND_XY = traits::mcc_type_pair_hash(), - COORDS_KIND_LATLON = traits::mcc_type_pair_hash(), + COORDS_KIND_LONLAT = traits::mcc_type_pair_hash(), COORDS_KIND_UNKNOWN = traits::mcc_type_pair_hash() }; static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_ICRS_STR = "RADEC-IRCS"; static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_APP_STR = "RADEC-APP"; +static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_OBS_STR = "RADEC-OBS"; static constexpr std::string_view MCC_COORDPAIR_KIND_HADEC_APP_STR = "HADEC-APP"; +static constexpr std::string_view MCC_COORDPAIR_KIND_HADEC_OBS_STR = "HADEC-OBS"; static constexpr std::string_view MCC_COORDPAIR_KIND_AZALT_STR = "AZALT"; static constexpr std::string_view MCC_COORDPAIR_KIND_AZZD_STR = "AZZD"; static constexpr std::string_view MCC_COORDPAIR_KIND_XY_STR = "XY"; @@ -554,11 +583,13 @@ template static constexpr std::string_view MccCoordPairKindStr = KIND == MccCoordPairKind::COORDS_KIND_RADEC_ICRS ? MCC_COORDPAIR_KIND_RADEC_ICRS_STR : KIND == MccCoordPairKind::COORDS_KIND_RADEC_APP ? MCC_COORDPAIR_KIND_RADEC_APP_STR + : KIND == MccCoordPairKind::COORDS_KIND_RADEC_OBS ? MCC_COORDPAIR_KIND_RADEC_OBS_STR : KIND == MccCoordPairKind::COORDS_KIND_HADEC_APP ? MCC_COORDPAIR_KIND_HADEC_APP_STR + : KIND == MccCoordPairKind::COORDS_KIND_HADEC_OBS ? MCC_COORDPAIR_KIND_HADEC_OBS_STR : KIND == MccCoordPairKind::COORDS_KIND_AZALT ? MCC_COORDPAIR_KIND_AZALT_STR : KIND == MccCoordPairKind::COORDS_KIND_AZZD ? MCC_COORDPAIR_KIND_AZZD_STR : KIND == MccCoordPairKind::COORDS_KIND_XY ? MCC_COORDPAIR_KIND_XY_STR - : KIND == MccCoordPairKind::COORDS_KIND_LATLON ? MCC_COORDPAIR_KIND_LATLON_STR + : KIND == MccCoordPairKind::COORDS_KIND_LONLAT ? MCC_COORDPAIR_KIND_LATLON_STR : KIND == MccCoordPairKind::COORDS_KIND_GENERIC ? MCC_COORDPAIR_KIND_GENERIC_STR : MCC_COORDPAIR_KIND_UNKNOWN_STR; @@ -567,11 +598,13 @@ static constexpr std::string_view MccCoordPairKindToStr(MccCoordPairKind KIND) { return KIND == MccCoordPairKind::COORDS_KIND_RADEC_ICRS ? MCC_COORDPAIR_KIND_RADEC_ICRS_STR : KIND == MccCoordPairKind::COORDS_KIND_RADEC_APP ? MCC_COORDPAIR_KIND_RADEC_APP_STR + : KIND == MccCoordPairKind::COORDS_KIND_RADEC_OBS ? MCC_COORDPAIR_KIND_RADEC_OBS_STR : KIND == MccCoordPairKind::COORDS_KIND_HADEC_APP ? MCC_COORDPAIR_KIND_HADEC_APP_STR + : KIND == MccCoordPairKind::COORDS_KIND_HADEC_OBS ? MCC_COORDPAIR_KIND_HADEC_OBS_STR : KIND == MccCoordPairKind::COORDS_KIND_AZALT ? MCC_COORDPAIR_KIND_AZALT_STR : KIND == MccCoordPairKind::COORDS_KIND_AZZD ? MCC_COORDPAIR_KIND_AZZD_STR : KIND == MccCoordPairKind::COORDS_KIND_XY ? MCC_COORDPAIR_KIND_XY_STR - : KIND == MccCoordPairKind::COORDS_KIND_LATLON ? MCC_COORDPAIR_KIND_LATLON_STR + : KIND == MccCoordPairKind::COORDS_KIND_LONLAT ? MCC_COORDPAIR_KIND_LATLON_STR : KIND == MccCoordPairKind::COORDS_KIND_GENERIC ? MCC_COORDPAIR_KIND_GENERIC_STR : MCC_COORDPAIR_KIND_UNKNOWN_STR; } @@ -588,11 +621,13 @@ static constexpr MccCoordPairKind MccCoordStrToPairKind(R&& spair) return hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_RADEC_ICRS_STR) ? MccCoordPairKind::COORDS_KIND_RADEC_ICRS : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_RADEC_APP_STR) ? MccCoordPairKind::COORDS_KIND_RADEC_APP + : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_RADEC_OBS_STR) ? MccCoordPairKind::COORDS_KIND_RADEC_OBS : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_HADEC_APP_STR) ? MccCoordPairKind::COORDS_KIND_HADEC_APP + : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_HADEC_OBS_STR) ? MccCoordPairKind::COORDS_KIND_HADEC_OBS : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_AZALT_STR) ? MccCoordPairKind::COORDS_KIND_AZALT : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_AZZD_STR) ? MccCoordPairKind::COORDS_KIND_AZZD : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_XY_STR) ? MccCoordPairKind::COORDS_KIND_XY - : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_LATLON_STR) ? MccCoordPairKind::COORDS_KIND_LATLON + : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_LATLON_STR) ? MccCoordPairKind::COORDS_KIND_LONLAT : hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_GENERIC_STR) ? MccCoordPairKind::COORDS_KIND_GENERIC : MccCoordPairKind::COORDS_KIND_UNKNOWN; } diff --git a/mcc/mcc_coord.h b/mcc/mcc_coord.h index d32f916..0cc94f3 100644 --- a/mcc/mcc_coord.h +++ b/mcc/mcc_coord.h @@ -14,7 +14,51 @@ public: typedef CO_LON_T x_t; typedef CO_LAT_T y_t; - MccCoordPair(CO_LON_T const& x, CO_LAT_T const& y) : _x(x), _y(y) {} + static constexpr MccCoordPairKind pairKind = + !(std::derived_from || + std::derived_from) // unknown type (possibly just double or float) + ? MccCoordPairKind::COORDS_KIND_GENERIC + : (std::same_as || std::same_as) // one of the types is MccAngle + ? MccCoordPairKind::COORDS_KIND_GENERIC + // ICRS RA and DEC + : (std::same_as && std::same_as) + ? MccCoordPairKind::COORDS_KIND_RADEC_ICRS + // apparent RA and DEC + : (std::same_as && std::same_as) + ? MccCoordPairKind::COORDS_KIND_RADEC_APP + // observed RA and DEC + : (std::same_as && std::same_as) + ? MccCoordPairKind::COORDS_KIND_RADEC_APP + // apparent HA and DEC + : (std::same_as && std::same_as) + ? MccCoordPairKind::COORDS_KIND_HADEC_APP + // observed HA and DEC + : (std::same_as && std::same_as) + ? MccCoordPairKind::COORDS_KIND_HADEC_APP + // apparent AZ and ZD + : (std::same_as && std::same_as) + ? MccCoordPairKind::COORDS_KIND_AZZD + // apparent AZ and ALT + : (std::same_as && std::same_as) + ? MccCoordPairKind::COORDS_KIND_AZZD + // general purpose X and Y + : (std::same_as && std::same_as) + ? MccCoordPairKind::COORDS_KIND_XY + // geographical longitude and latitude + : (std::same_as && std::same_as) + ? MccCoordPairKind::COORDS_KIND_LONLAT + : MccCoordPairKind::COORDS_KIND_UNKNOWN; + + template + MccCoordPair(CO_LON_T const& x, CO_LAT_T const& y, EpT const& epoch = EpT::now()) : _x(x), _y(y), _mjd(epoch.MJD) + { + } + + MccCoordPair(const MccCoordPair&) = default; + MccCoordPair(MccCoordPair&&) = default; + + MccCoordPair& operator=(const MccCoordPair&) = default; + MccCoordPair& operator=(MccCoordPair&&) = default; virtual ~MccCoordPair() = default; @@ -28,9 +72,16 @@ public: return _y; } - operator std::tuple() const + double MJD() const { - return {_x, _y}; + return _mjd; + } + + // for something like: + // auto [ra, dec, mjd] = coord_pair; + operator std::tuple() const + { + return {_x, _y, _mjd}; } void setX(const CO_LON_T& x) @@ -43,9 +94,16 @@ public: _y = y; } + void setMJD(double mjd) + { + _mjd = mjd; + } + protected: CO_LON_T _x; CO_LAT_T _y; + + double _mjd; }; @@ -58,7 +116,50 @@ concept mcc_coord_pair_c = requires { }; +/* predefined coordinate pairs */ + +template + requires(std::derived_from && std::derived_from) +class MccNamedCoordPair : public MccCoordPair +{ +public: + template + requires(std::is_arithmetic_v && std::is_arithmetic_v) + MccNamedCoordPair(CxT const& x, CyT const& y, EpT const& epoch = EpT::now()) + : MccCoordPair(CO_LON_T{x}, CO_LAT_T{y}, epoch) + { + } + + template + MccNamedCoordPair(MccAngle const& x, MccAngle const& y, EpT const& epoch = EpT::now()) + : MccCoordPair(CO_LON_T{(double)x}, CO_LAT_T{(double)y}, epoch) + { + } + + MccNamedCoordPair(const MccNamedCoordPair&) = default; + MccNamedCoordPair(MccNamedCoordPair&&) = default; + + MccNamedCoordPair& operator=(const MccNamedCoordPair&) = default; + MccNamedCoordPair& operator=(MccNamedCoordPair&&) = default; + + + virtual ~MccNamedCoordPair() = default; +}; + +using MccSkyRADEC_ICRS = MccNamedCoordPair; +using MccSkyRADEC_APP = MccNamedCoordPair; +using MccSkyRADEC_OBS = MccNamedCoordPair; +using MccSkyHADEC_APP = MccNamedCoordPair; +using MccSkyHADEC_OBS = MccNamedCoordPair; +using MccSkyAZZD = MccNamedCoordPair; +using MccSkyAZALT = MccNamedCoordPair; +using MccGenXY = MccNamedCoordPair; +using MccGeoLONLAT = MccNamedCoordPair; + + struct mcc_skycoord_interface_t { + virtual ~mcc_skycoord_interface_t() = default; + template SelfT, mcc_angle_c XT, mcc_angle_c YT> SelfT& from(this SelfT&& self, XT&& x, YT&& y) { @@ -81,7 +182,7 @@ struct mcc_skycoord_interface_t { template SelfT, mcc_coord_pair_c PT, mcc_coord_pair_c... PTs> SelfT& to(this SelfT&& self, PT& cpair, PTs&... cpairs) { - return std::forward(self).to(cpair, cpairs); + return std::forward(self).to(cpair, cpairs...); } template SelfT, mcc_coord_pair_c PT> @@ -103,4 +204,11 @@ concept mcc_skycoord_c = std::derived_from && requi { T::meteo(std::declval()) }; }; -} // end namespace mcc \ No newline at end of file + + +class MccSkyPoint : public mcc_skycoord_interface_t +{ +public: +}; + +} // end namespace mcc diff --git a/mcc/mcc_moving_controls.h b/mcc/mcc_moving_controls.h index 2184489..95b6d06 100644 --- a/mcc/mcc_moving_controls.h +++ b/mcc/mcc_moving_controls.h @@ -105,6 +105,62 @@ inline std::error_code make_error_code(MccSimpleMovingControlsErrorCode ec) class MccSimpleMovingControls { + static constexpr auto DEG90INRADS = std::numbers::pi / 2.0; + + 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; + } + + 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 std::error_code error_t; @@ -125,30 +181,10 @@ public: 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) { + auto send_to_hardware = [mount](typename MountT::hardware_state_t const& hw_state) { 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); @@ -160,19 +196,134 @@ public: }; + auto check_pzones = [mount, this](MccTelemetryData const& tdata, double min_time_to_pzone_in_secs, + double braking_accelX, double braking_accelY) { + bool in_zone; + std::vector in_zone_vec; + MccCelestialPoint cpt; + + auto distXY = mcc_compute_distance(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY); + + mount->logTrace( + std::format(" the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}", + min_time_to_pzone_in_secs, mcc::MccAngleFancyString(distXY.first), + mcc::MccAngleFancyString(distXY.second))); + + // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead + // and check them for getting into the prohibited zones + if constexpr (mccIsEquatorialMount(MountT::mountType)) { + cpt.X = tdata.HA + distXY.first; + cpt.Y = tdata.DEC_APP + distXY.second; + + if (cpt.Y > DEG90INRADS) { + cpt.Y = DEG90INRADS; + } + if (cpt.Y < -DEG90INRADS) { + cpt.Y = -DEG90INRADS; + } + + } else if constexpr (mccIsAltAzMount(MountT::mountType)) { + cpt.X = tdata.AZ + distXY.first; + cpt.Y = tdata.ZD + distXY.second; + if (cpt.Y < 0.0) { + cpt.Y = 0.0; + } + if (cpt.Y > std::numbers::pi) { + cpt.Y = std::numbers::pi; + } + } + mcc_tp2tp(tdata.time_point, cpt.time_point); + + mount->logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s", + mcc::MccAngleFancyString(tdata.speedX), + mcc::MccAngleFancyString(tdata.speedY))); + + in_zone_vec.clear(); + auto pz_err = mount->inPZone(cpt, &in_zone, &in_zone_vec); + if (pz_err) { + return mcc_deduce_error_code(pz_err, MccSimpleMovingControlsErrorCode::ERROR_PZONE_CONTAINER_COMP); + } + + if (in_zone) { + size_t i = 0; + for (; i < in_zone_vec.size(); ++i) { + if (in_zone_vec[i]) { + break; + } + } + + mount->logError("target point is near prohibited zone (zone index: {})! Entered target coordinates:", + i); + mount->logError(std::format( + " RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", mcc::MccAngle{tdata.RA_APP}.sexagesimal(true), + mcc::MccAngle{tdata.DEC_APP}.sexagesimal(), mcc::MccAngle{tdata.HA}.sexagesimal(true), + mcc::MccAngle{tdata.LST}.sexagesimal(true))); + mount->logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.AZ}.sexagesimal(), + mcc::MccAngle{tdata.ZD}.sexagesimal(), + mcc::MccAngle{tdata.ALT}.sexagesimal())); + + mount->logError(std::format(" hardware X, Y: {}, {}", mcc::MccAngle{tdata.X}.sexagesimal(), + mcc::MccAngle{tdata.Y}.sexagesimal())); + + return MccSimpleMovingControlsErrorCode::ERROR_NEAR_PZONE; + } + + return MccSimpleMovingControlsErrorCode::ERROR_OK; + }; + + auto log_pos = [mount, this](typename MountT::hardware_state_t const& hw_state, MccTelemetryData const& tdata) { + if constexpr (mccIsEquatorialMount(MountT::mountType)) { + mount->logTrace(std::format(" current target: HA = {}, DEC = {}", + mcc::MccAngle(tdata.target.HA).sexagesimal(true), + mcc::MccAngle(tdata.target.DEC_APP).sexagesimal())); + mount->logTrace(std::format(" current mount: HA = {}, DEC = {}", + mcc::MccAngle(tdata.HA).sexagesimal(true), + mcc::MccAngle(tdata.DEC_APP).sexagesimal())); + + _pathFile << tdata.time_point.time_since_epoch().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"; + + } else if constexpr (mccIsAltAzMount(MountT::mountType)) { + mount->logTrace(std::format(" target: AZ = {}, ZD = {}", + mcc::MccAngle(tdata.target.AZ).sexagesimal(), + mcc::MccAngle(tdata.target.ZD).sexagesimal())); + mount->logTrace(std::format(" mount: AZ = {}, ZD = {}", mcc::MccAngle(tdata.AZ).sexagesimal(), + mcc::MccAngle(tdata.ZD).sexagesimal())); + + _pathFile << tdata.time_point.time_since_epoch().count() << " " << tdata.target.AZ << " " + << tdata.target.ZD << " " << tdata.AZ << " " << tdata.ZD << " " + << (tdata.target.AZ - tdata.AZ) << " " << (tdata.target.ZD - tdata.ZD) << " " + << (int)hw_state.moving_state << "\n"; + } + }; + *_stopMoving = true; *_lastError = MccSimpleMovingControlsErrorCode::ERROR_OK; using cb_func_t = std::function; auto cb_sptr = std::shared_ptr(new cb_func_t(std::forward(mode_switch_calback))); - _slewingFunc = [mount, cb_sptr, this](bool slew_and_stop) { + + /* stop moving function */ + + _stopMovingFunc = [mount, this]() { + typename MountT::hardware_state_t hw_state; + hw_state.moving_state == MountT::hardware_moving_state_t::HW_MOVE_STOPPED; + + *_stopMoving = true; + + *_lastError = send_to_hardware(hw_state); + }; + + + /* slewing function */ + + _slewingFunc = [mount, cb_sptr, send_to_hardware, check_pzones, log_pos, 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}; @@ -192,13 +343,9 @@ public: 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)); - } + _pathFile.setFilename(_currentParams.slewingPathFilename); + } else { + mount->logError("Slewing path filename is empty! Do not save it!"); } } @@ -216,18 +363,16 @@ public: 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"; - } + _pathFile << "# \n"; + _pathFile << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n"; + _pathFile << "# Config:\n"; + _pathFile << "# slewing tolerance radius: " + << mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs() << " arcsecs\n"; + _pathFile << "# slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n"; + _pathFile << "# \n"; + _pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n"; + _pathFile << "# " + " \n"; typename MountT::error_t t_err; @@ -238,32 +383,33 @@ public: t_err = mount->telemetryData(&tdata); if (t_err) { - return *_lastError = - mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY); + *_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY); + return; } } auto last_hw_time = tdata.time_point; - bool in_zone; - std::vector in_zone_vec; + // bool in_zone; + // std::vector in_zone_vec; - MccCelestialPoint cpt; + // 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!"); - } + // 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); + *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE); + return; } @@ -278,14 +424,321 @@ public: } hw_state.moving_state = MountT::hardware_moving_state_t::HW_MOVE_SLEWING; - if (*_stopMoving) { - mount->logDebug("slewing was stopped!"); - return *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED; + // start slewing ... + error_t err = send_to_hardware(hw_state); + if (err) { + *_lastError = err; + mount->logError(std::format("start slewing: an error occured while sending hardware state: {} {} {}", + err.value(), err.category().name(), err.message())); + return; + } + + *cb_sptr(MountT::mount_status_t::SLEWING); // send the status to the mount + + + double dist; + + + std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp; + start_slewing_tp = std::chrono::steady_clock::now(); + last_adjust_tp = start_slewing_tp; + + std::pair distXY; + bool tag_var_coord = true; + + if (tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT || + tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) { + tag_var_coord = false; + } + + auto start_point = tdata.time_point; // needed for trajectory file + + // main loop (simply monitors the current position taking into account the prohibited zones, as well as the + // timeout of the entire process) + while (!*_stopMoving) { + // wait for updated telemetry data + { + std::lock_guard lock{*_currentParamsMutex}; + + t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); + + if (t_err) { + *_lastError = + mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY); + break; + } + + last_hw_time = tdata.time_point; + } + + hw_err = mount->hardwareGetState(&hw_state); + if (hw_err) { + *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE); + break; + } + + log_pos(hw_state, tdata); + + if (*_stopMoving) { + *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED; + break; + } + + err = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY); + if (err) { + *_lastError = err; + break; + } + + { + std::lock_guard lock{*_currentParamsMutex}; + + if ((std::chrono::steady_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) { + mount->logError("slewing process timeout!"); + *_lastError = MccSimpleMovingControlsErrorCode::ERROR_TIMEOUT; + break; + } + } + + if (slew_and_stop && !tag_var_coord) { // just wait for mount to be stopped + if (hw_state.moving_state == MountT::hardware_moving_state_t::HW_MOVE_STOPPED) { + mount->logInfo("mount moving state is STOPPED - exit!"); + break; + } + } else { + if (last_hw_time == tdata.time_point) { + mount->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n"); + continue; + } + + last_hw_time = tdata.time_point; + + t_err = mount->targetToMountDist(&dist); + if (t_err) { + *_lastError = + mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY); + break; + } + + mount->logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist))); + + if ((dist <= _currentParams.slewToleranceRadius) && + (hw_state.moving_state == + MountT::hardware_moving_state_t::HW_MOVE_GUIDING)) { // stop slewing and exit from + // cycle + mount->logInfo("target-to-mount distance is lesser than slew tolerance radius - exit!"); + + if (slew_and_stop) { + stopMount(); + } + + break; + } + + + if (*_stopMoving) { + *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED; + break; + // return MccSimpleMovingControlsErrorCode::ERROR_STOPPED; + } + + + // resend new position since target coordinates are changed in time + hw_state.X = (double)tdata.target.X; + hw_state.Y = (double)tdata.target.Y; + + err = send_to_hardware(hw_state); + if (err) { + *_lastError = err; + break; + } + } + + if (*_stopMoving) { + *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED; + break; + } + + // sleep here + std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval); + } + + *_stopMoving = true; + + mount->logInfo("Slewing finished"); + err = *_lastError; + mount->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message())); + + _pathFile.save(); + + // get final position + + if (!err) { + // wait for updated telemetry data + { + std::lock_guard lock{*_currentParamsMutex}; + + t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); + + if (t_err) { + *_lastError = + mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY); + return; + } + } + + t_err = mount->targetToMountDist(&dist); + if (t_err) { + *_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY); + return; + } + + log_pos(hw_state, tdata); + + mount->logDebug(std::format(" target-to-mount distance {}", mcc::MccAngleFancyString(dist))); + + if (!slew_and_stop) { // start tracking + _trackingFunc(); + } else { + *_lastError = MccSimpleMovingControlsErrorCode::ERROR_OK; + } } }; - _trackingFunc = [mount, cb_sptr, this]() { + /* tracking function */ + + _trackingFunc = [mount, cb_sptr, check_pzones, send_to_hardware, log_pos, this]() { + double braking_accelX, braking_accelY; + double min_time_to_pzone_in_secs; + + + { + // 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.trackingPathFilename.empty()) { // open slewing trajectory file + _pathFile.setFilename(_currentParams.trackingPathFilename); + } else { + mount->logError("Tracking path filename is empty! Do not save it!"); + } + } + + mount->logInfo("Start tracking"); + 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)); + + + _pathFile << "# \n"; + _pathFile << "# Tracking trajectory, " << std::chrono::system_clock::now() << "\n"; + _pathFile << "# \n"; + _pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n"; + _pathFile << "# " + " \n"; + + + typename MountT::hardware_state_t hw_state; + + error_t err; + MccTelemetryData tdata; + double dist; + + auto last_hw_time = tdata.time_point; + + *cb_sptr(MountT::mount_status_t::TRACKING); // send the status to the mount + + while (!*_stopMoving) { + // wait for updated telemetry data + { + std::lock_guard lock{*_currentParamsMutex}; + + auto t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); + + if (t_err) { + *_lastError = + mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY); + break; + } + + last_hw_time = tdata.time_point; + } + + auto hw_err = mount->hardwareGetState(&hw_state); + if (hw_err) { + *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE); + break; + } + + log_pos(hw_state, tdata); + + if (*_stopMoving) { + *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED; + break; + } + + err = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY); + if (err) { + *_lastError = err; + break; + } + + if (last_hw_time == tdata.time_point) { + mount->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n"); + continue; + } + + last_hw_time = tdata.time_point; + + auto t_err = mount->targetToMountDist(&dist); + if (t_err) { + *_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY); + break; + } + + mount->logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist))); + + // resend new position since target coordinates are changed in time + hw_state.X = (double)tdata.target.X; + hw_state.Y = (double)tdata.target.Y; + + err = send_to_hardware(hw_state); + if (err) { + *_lastError = err; + break; + } + + if (*_stopMoving) { + *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED; + break; + } + + // sleep here + std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval); + } + + *_stopMoving = true; + + mount->logInfo("Tracking finished"); + err = *_lastError; + mount->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message())); + + _pathFile.save(); }; } @@ -305,12 +758,12 @@ public: } - error_t stopMountMoving() + error_t stopMount() { if (*_stopMoving) { *_lastError = MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED; } else { - *_stopMoving = true; + _stopMovingFunc(); } return *_lastError; @@ -340,6 +793,7 @@ public: protected: std::function _slewingFunc{}; std::function _trackingFunc{}; + std::function _stopMovingFunc{}; std::unique_ptr _stopMoving; @@ -347,6 +801,8 @@ protected: MccSimpleMovingModelParams _currentParams{}; std::unique_ptr> _lastError; + + PathFile _pathFile{}; }; } // namespace mcc