diff --git a/asibfm700/asibfm700_mount.cpp b/asibfm700/asibfm700_mount.cpp index a5d928a..4f6a221 100644 --- a/asibfm700/asibfm700_mount.cpp +++ b/asibfm700/asibfm700_mount.cpp @@ -255,6 +255,7 @@ Asibfm700Mount::error_t Asibfm700Mount::initMount() setStateERFA(std::move(ccte_state)); + setTelemetryDataUpdateInterval(_mountConfig.hardwarePollingPeriod()); setTelemetryUpdateTimeout(_mountConfig.movingModelParams().telemetryTimeout); startInternalTelemetryDataUpdating(); diff --git a/asibfm700/asibfm700_netserver_main.cpp b/asibfm700/asibfm700_netserver_main.cpp index 9bacde4..c5eb040 100644 --- a/asibfm700/asibfm700_netserver_main.cpp +++ b/asibfm700/asibfm700_netserver_main.cpp @@ -44,8 +44,8 @@ int main(int argc, char* argv[]) options.positional_help("[endpoint0] [enpoint1] ... [endpointN]"); options.parse_positional({"endpoints"}); - // asio::io_context ctx(2); - asio::io_context ctx; + asio::io_context ctx(8); + // asio::io_context ctx; try { @@ -154,12 +154,12 @@ int main(int argc, char* argv[]) } - asio::thread_pool pool(5); + // asio::thread_pool pool(5); - asio::post(pool, [&ctx]() { ctx.run(); }); + // asio::post(pool, [&ctx]() { ctx.run(); }); - pool.join(); - // ctx.run(); + // pool.join(); + ctx.run(); } catch (const std::system_error& ex) { std::cerr << "An error occured: " << ex.code().message() << "\n"; diff --git a/mcc/mcc_angle.h b/mcc/mcc_angle.h index dd10373..365a358 100644 --- a/mcc/mcc_angle.h +++ b/mcc/mcc_angle.h @@ -603,10 +603,17 @@ std::string MccAngleFancyString(std::convertible_to auto const& ang, { std::string s; - if (ang < 1.0_arcmins) { + double abs_ang; + if constexpr (std::is_arithmetic_v>) { + abs_ang = std::abs(ang); + } else { + abs_ang = std::abs(MccAngle{ang}); + } + + if (abs_ang < 1.0_arcmins) { std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcsecs()); s += " arcsecs"; - } else if (ang < 1.0_degs) { + } else if (abs_ang < 1.0_degs) { std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcmins()); s += " arcmins"; } else { diff --git a/mcc/mcc_ccte_erfa.h b/mcc/mcc_ccte_erfa.h index 7c9d99c..873e800 100644 --- a/mcc/mcc_ccte_erfa.h +++ b/mcc/mcc_ccte_erfa.h @@ -394,8 +394,20 @@ public: // coordinates transformations + template + error_t transformCoordinates(mcc_celestial_point_c auto from_pt, ResT* to_pt) + requires(mcc_eqt_hrz_coord_c || mcc_celestial_point_c) + { + if constexpr (mcc_eqt_hrz_coord_c) { + return transformCoordinatesEQHR(std::move(from_pt), to_pt); + } else if constexpr (mcc_celestial_point_c) { + return transformCoordinatesCP(std::move(from_pt), to_pt); + } else { + static_assert(false, "UNSUPPORTED TYPE!"); + } + } - error_t transformCoordinates(mcc_celestial_point_c auto from_pt, mcc_celestial_point_c auto* to_pt) + error_t transformCoordinatesCP(mcc_celestial_point_c auto from_pt, mcc_celestial_point_c auto* to_pt) { error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK; @@ -557,7 +569,7 @@ public: } - error_t transformCoordinates(mcc_celestial_point_c auto from_pt, mcc_eqt_hrz_coord_c auto* to_pt) + error_t transformCoordinatesEQHR(mcc_celestial_point_c auto from_pt, mcc_eqt_hrz_coord_c auto* to_pt) { error_t ret = MccCCTE_ERFAErrorCode::ERROR_OK; diff --git a/mcc/mcc_defaults.h b/mcc/mcc_defaults.h index 95e208f..f555955 100644 --- a/mcc/mcc_defaults.h +++ b/mcc/mcc_defaults.h @@ -1059,7 +1059,7 @@ public: toSexagesimalHour(bytes, value.RA_ICRS); std::format_to(std::back_inserter(bytes), "{}", _delimiter); - toSexagesimalDeg(bytes, value.DEC_APP); + toSexagesimalDeg(bytes, value.DEC_ICRS); } std::format_to(std::back_inserter(bytes), "{}", _delimiter); @@ -1126,8 +1126,9 @@ public: eqhrz_ser.setPrecision(_currentPrec); eqhrz_ser(value, bytes); - // MccEqtHrzCoordsSerializer{}(value, bytes); std::format_to(std::back_inserter(bytes), "{}", _delimiter); + // toSexagesimalHour(bytes, value.LST); + // std::format_to(std::back_inserter(bytes), "{}", _delimiter); // '*3600.0' to express refraction correction in arcseconds! toDegrees(bytes, value.speedX, value.speedY, value.pcmX, value.pcmY, value.refCorr * 3600.0); @@ -1136,8 +1137,6 @@ public: pt_ser.setFormat(_currentFormat); pt_ser.setPrecision(_currentPrec); pt_ser(value.target, bytes); - - // MccPointingTargetSerializer{}(value.target, bytes); } }; diff --git a/mcc/mcc_generic_mount.h b/mcc/mcc_generic_mount.h index 76185e8..aab9ae4 100644 --- a/mcc/mcc_generic_mount.h +++ b/mcc/mcc_generic_mount.h @@ -268,6 +268,7 @@ public: // re-implements SlewModelT::slewToTarget to fetch input target coordinates from intermediate buffer error_t slewToTarget(bool slew_and_stop = false) { + _enteredTargetCoordiniates.time_point = std::chrono::system_clock::now(); auto err = TelemetryT::setPointingTarget(_enteredTargetCoordiniates); if (err) { *_mountStatus = mount_status_t::ERROR; @@ -287,6 +288,8 @@ public: if (slew_and_stop) { *_mountStatus = mount_status_t::IDLE; + } else { + s_err = trackTarget(); } return s_err; diff --git a/mcc/mcc_slewing_model.h b/mcc/mcc_slewing_model.h index 64a4e59..63c3691 100644 --- a/mcc/mcc_slewing_model.h +++ b/mcc/mcc_slewing_model.h @@ -9,7 +9,6 @@ #include "mcc_defaults.h" #include "mcc_generics.h" #include "mcc_moving_model_common.h" -#include "mcc_spdlog.h" namespace mcc { @@ -127,11 +126,80 @@ public: *_stopSlewing = true; + _checkTargetFunc = [controls, logger, this]() mutable -> error_t { + typename CONTROLS_T::error_t t_err; + + MccTelemetryData tdata; + bool in_zone; + + t_err = controls->telemetryData(&tdata); + + if (t_err) { + return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); + } + auto pz_err = controls->inPZone(tdata.target, &in_zone); + if (pz_err) { + return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + } + + if (in_zone) { + logger.logError("target point is in prohibited zone! Entered target coordinates:"); + logger.logError(std::format(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", + mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true), + mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(), + mcc::MccAngle{tdata.target.HA}.sexagesimal(true), + mcc::MccAngle{tdata.LST}.sexagesimal(true))); + logger.logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(), + mcc::MccAngle{tdata.target.ZD}.sexagesimal(), + mcc::MccAngle{tdata.target.ALT}.sexagesimal())); + + return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE; + } + + return MccSimpleSlewingModelErrorCode::ERROR_OK; + }; + _slewingFunc = [controls, logger = std::move(logger), this](bool slew_and_stop) mutable -> error_t { // first, check target coordinates typename CONTROLS_T::error_t t_err; MccTelemetryData tdata; + { + std::lock_guard lock{*_currentParamsMutex}; + t_err = controls->telemetryData(&tdata); + + if (t_err) { + return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); + } + } + + bool in_zone; + auto pz_err = controls->inPZone(tdata.target, &in_zone); + if (pz_err) { + *_stopSlewing = true; + return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + } + + if (in_zone) { + *_stopSlewing = true; + + logger.logError("target point is in prohibited zone! Entered target coordinates:"); + logger.logError(std::format(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", + mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true), + mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(), + mcc::MccAngle{tdata.target.HA}.sexagesimal(true), + mcc::MccAngle{tdata.LST}.sexagesimal(true))); + logger.logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(), + mcc::MccAngle{tdata.target.ZD}.sexagesimal(), + mcc::MccAngle{tdata.target.ALT}.sexagesimal())); + + return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE; + } + + if (*_stopSlewing) { + return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + } + double braking_accelX, braking_accelY; { @@ -161,41 +229,6 @@ public: logger.logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)", mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY)); - { - std::lock_guard lock{*_currentParamsMutex}; - t_err = controls->telemetryData(&tdata); - - if (t_err) { - return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); - } - } - - bool in_zone; - auto pz_err = controls->inPZone(tdata.target, &in_zone); - if (pz_err) { - *_stopSlewing = true; - return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); - } - - if (in_zone) { - *_stopSlewing = true; - - logger.logError("target point is in prohibited zone!"); - logger.logError(std::format( - "RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true), - mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(), mcc::MccAngle{tdata.target.HA}.sexagesimal(true), - mcc::MccAngle{tdata.LST}.sexagesimal(true))); - logger.logError(std::format("AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(), - mcc::MccAngle{tdata.target.ZD}.sexagesimal(), - mcc::MccAngle{tdata.target.ALT}.sexagesimal())); - - return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE; - } - - if (*_stopSlewing) { - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; - } - MccCelestialPoint cpt; double min_time_to_pzone_in_secs; @@ -207,11 +240,6 @@ public: static_assert(false, "UNKNOWN MOUNT TYPE!"); } - if (*_stopSlewing) { - logger.logDebug("slewing was stopped!"); - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; - } - typename CONTROLS_T::hardware_state_t hw_state; auto hw_err = controls->hardwareGetState(&hw_state); @@ -242,16 +270,18 @@ public: logger.logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees())); if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { - logger.logDebug(std::format(" target: HA = {}, DEC = {}", - mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(), - mcc::MccAngle{tdata.target.HA}.sexagesimal(true))); - logger.logDebug(std::format(" mount: HA = {}, DEC = {}", mcc::MccAngle{tdata.DEC_APP}.sexagesimal(), - mcc::MccAngle{tdata.HA}.sexagesimal(true))); + logger.logDebug(std::format(" entered target: HA = {}, DEC = {}", + mcc::MccAngle{tdata.target.HA}.sexagesimal(true), + mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal())); + logger.logDebug(std::format(" current mount: HA = {}, DEC = {}", + mcc::MccAngle{tdata.HA}.sexagesimal(true), + mcc::MccAngle{tdata.DEC_APP}.sexagesimal())); } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { - logger.logDebug(std::format(" target: AZ = {}, ZD = {}", + logger.logDebug(std::format(" entered target: AZ = {}, ZD = {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(), mcc::MccAngle{tdata.target.ZD}.sexagesimal())); - logger.logDebug(std::format(" mount: AZ = {}, ZD = {}", mcc::MccAngle{tdata.AZ}.sexagesimal(), + logger.logDebug(std::format(" current mount: AZ = {}, ZD = {}", + mcc::MccAngle{tdata.AZ}.sexagesimal(), mcc::MccAngle{tdata.ZD}.sexagesimal())); } @@ -261,6 +291,8 @@ public: return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); } + logger.logDebug(" the 'hardwareSetState' method performed successfully!"); + // std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp; // mcc_tp2tp(hw_state.time_point, start_slewing_tp); // not compiled!! @@ -319,10 +351,10 @@ public: cpt.Y = -DEG90INRADS; } - logger.logTrace(std::format(" target: HA = {}, DEC = {}", + logger.logTrace(std::format(" current target: HA = {}, DEC = {}", mcc::MccAngle(tdata.target.HA).sexagesimal(true), mcc::MccAngle(tdata.target.DEC_APP).sexagesimal())); - logger.logTrace(std::format(" mount: HA = {}, DEC = {}", + logger.logTrace(std::format(" current mount: HA = {}, DEC = {}", mcc::MccAngle(tdata.HA).sexagesimal(true), mcc::MccAngle(tdata.DEC_APP).sexagesimal())); } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { @@ -409,6 +441,8 @@ public: *_stopSlewing = true; return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); } + + logger.logDebug(" the 'hardwareSetState' method performed successfully!"); } } @@ -416,6 +450,34 @@ public: logger.logInfo("Slewing finished"); + // wait for updated telemetry data + { + std::lock_guard lock{*_currentParamsMutex}; + + t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); + + if (t_err) { + *_stopSlewing = true; + return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); + } + } + + if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { + logger.logDebug(std::format(" entered target: HA = {}, DEC = {}", + mcc::MccAngle{tdata.target.HA}.sexagesimal(true), + mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal())); + logger.logDebug(std::format(" current mount: HA = {}, DEC = {}", + mcc::MccAngle{tdata.HA}.sexagesimal(true), + mcc::MccAngle{tdata.DEC_APP}.sexagesimal())); + } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { + logger.logDebug(std::format(" entered target: AZ = {}, ZD = {}", + mcc::MccAngle{tdata.target.AZ}.sexagesimal(), + mcc::MccAngle{tdata.target.ZD}.sexagesimal())); + logger.logDebug(std::format(" current mount: AZ = {}, ZD = {}", + mcc::MccAngle{tdata.AZ}.sexagesimal(), + mcc::MccAngle{tdata.ZD}.sexagesimal())); + } + return MccSimpleSlewingModelErrorCode::ERROR_OK; }; } @@ -474,6 +536,7 @@ public: protected: std::function _slewingFunc{}; std::unique_ptr _stopSlewing; + std::function _checkTargetFunc{}; slewing_params_t _currentParams{}; std::unique_ptr _currentParamsMutex{}; diff --git a/mcc/mcc_telemetry.h b/mcc/mcc_telemetry.h index 16627d5..8fb5c6c 100644 --- a/mcc/mcc_telemetry.h +++ b/mcc/mcc_telemetry.h @@ -379,31 +379,32 @@ public: // arm internal update loop - _internalUpdatingStopSource = std::stop_source{}; + // _internalUpdatingStopSource = std::stop_source{}; - _internalUpdatingLoopFuture = std::async( - std::launch::async, - [this](std::stop_token stoken) { - while (!(*_internalUpdatingLoopStop)) { - { - std::unique_lock ulock(*_internalUpdatingLoopMutex); - _internalUpdatingLoopCondVar->wait(ulock, [this]() -> bool { return *_dataUpdatingRequested; }); - } + // _internalUpdatingLoopFuture = std::async( + // std::launch::async, + // [this](std::stop_token stoken) { + // while (!(*_internalUpdatingLoopStop)) { + // { + // std::unique_lock ulock(*_internalUpdatingLoopMutex); + // _internalUpdatingLoopCondVar->wait(ulock, [this]() -> bool { return *_dataUpdatingRequested; + // }); + // } - { - std::lock_guard lock_update(*_updateMutex); + // { + // std::lock_guard lock_update(*_updateMutex); - *_isDataUpdated = false; - _lastUpdateError = _updateFunc(stoken); - } + // *_isDataUpdated = false; + // _lastUpdateError = _updateFunc(stoken); + // } - *_isDataUpdated = true; + // *_isDataUpdated = true; - // unlock all waiting threads - _updateCondVar->notify_all(); - } - }, - _internalUpdatingStopSource.get_token()); + // // unlock all waiting threads + // _updateCondVar->notify_all(); + // } + // }, + // _internalUpdatingStopSource.get_token()); } @@ -489,38 +490,38 @@ public: // auto nn = std::this_thread::get_id(); - // std::this_thread::sleep_for(_currentUpdateInterval); + std::this_thread::sleep_for(_currentUpdateInterval); - { - std::lock_guard lock{*_currentUpdateIntervalMutex}; + // { + // std::lock_guard lock{*_currentUpdateIntervalMutex}; - // compute it here because of possible changing _currentUpdateInterval - auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv; + // // compute it here because of possible changing _currentUpdateInterval + // auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv; - for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) { - if (stop_token.stop_requested()) { - break; - } + // for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) { + // if (stop_token.stop_requested()) { + // break; + // } - std::this_thread::sleep_for(sleep_td); - } + // std::this_thread::sleep_for(sleep_td); + // } - if (stop_token.stop_requested()) { - break; - } + // if (stop_token.stop_requested()) { + // break; + // } - if constexpr (std::floating_point) { - std::this_thread::sleep_for(sleep_td); - } else { - auto rem = _currentUpdateInterval % internalUpdatingIntervalDiv; + // if constexpr (std::floating_point) { + // std::this_thread::sleep_for(sleep_td); + // } else { + // auto rem = _currentUpdateInterval % internalUpdatingIntervalDiv; - if (rem.count()) { - std::this_thread::sleep_for(rem); - } else { - std::this_thread::sleep_for(sleep_td); - } - } - } + // if (rem.count()) { + // std::this_thread::sleep_for(rem); + // } else { + // std::this_thread::sleep_for(sleep_td); + // } + // } + // } } *_internalUpdating = false; @@ -546,50 +547,37 @@ public: error_t updateTelemetryData(traits::mcc_time_duration_c auto const& timeout) { { - std::lock_guard lock(*_internalUpdatingLoopMutex); - *_dataUpdatingRequested = true; - } - std::unique_lock ulock(*_updateMutex); + std::lock_guard thread_lock{*_updateMutex}; - _internalUpdatingLoopCondVar->notify_one(); - *_dataUpdatingRequested = false; + std::stop_source stop_source; - bool ok = _updateCondVar->wait_for(ulock, timeout, [this]() -> bool { return *_isDataUpdated; }); - if (!ok) { - _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; + *_isDataUpdated = false; + + // std::future update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token()); + // // std::future update_ft = + // // std::async(std::launch::async, _updateFunc, _internalUpdatingStopSource.get_token()); + // auto status = update_ft.wait_for(timeout); + + // if (status == std::future_status::ready) { + // *_isDataUpdated = true; + // _lastUpdateError = update_ft.get(); + // } else if (status == std::future_status::deferred) { // std::async was invoked in this thread, get + // result + // _lastUpdateError = update_ft.get(); + // if (!_lastUpdateError) { + // *_isDataUpdated = true; + // } + // } else { // timeout + // stop_source.request_stop(); + // _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; + // } + + _lastUpdateError = _updateFunc(_internalUpdatingStopSource.get_token()); + *_isDataUpdated = true; } - // { - // std::lock_guard thread_lock{*_updateMutex}; - - // std::stop_source stop_source; - - // *_isDataUpdated = false; - - // // std::future update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token()); - // // // std::future update_ft = - // // // std::async(std::launch::async, _updateFunc, _internalUpdatingStopSource.get_token()); - // // auto status = update_ft.wait_for(timeout); - - // // if (status == std::future_status::ready) { - // // *_isDataUpdated = true; - // // _lastUpdateError = update_ft.get(); - // // } else if (status == std::future_status::deferred) { // std::async was invoked in this thread, get - // // result - // // _lastUpdateError = update_ft.get(); - // // if (!_lastUpdateError) { - // // *_isDataUpdated = true; - // // } - // // } else { // timeout - // // stop_source.request_stop(); - // // _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; - // // } - - // *_isDataUpdated = true; - // } - - // // unblock waiting threads even in the case of timeout! - // _updateCondVar->notify_all(); + // unblock waiting threads even in the case of timeout! + _updateCondVar->notify_all(); // *_isDataUpdated = false;