From 69e49283dd62c530df2988fa020108d667d3e215 Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Wed, 25 Feb 2026 18:09:46 +0300 Subject: [PATCH] ... --- .vscode/c_cpp_properties.json | 16 ++++ asibfm700_common.h | 75 ++++++++++++++++ asibfm700_configfile.h | 7 +- asibfm700_mount.cpp | 159 +++++++++++++++++++++++++++++++++- asibfm700_mount.h | 37 ++++++-- 5 files changed, 283 insertions(+), 11 deletions(-) create mode 100644 .vscode/c_cpp_properties.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..a08a262 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,16 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "intelliSenseMode": "linux-gcc-x64", + "cppStandard": "c++26", + "cStandard": "gnu99" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/asibfm700_common.h b/asibfm700_common.h index 821531b..adb2d5e 100644 --- a/asibfm700_common.h +++ b/asibfm700_common.h @@ -15,6 +15,81 @@ namespace asibfm700 { +namespace details +{ + +struct movement_pars_t { + // ******* 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{}; +}; + +} // namespace details + + static constexpr mcc::MccMountType asibfm700MountType = mcc::MccMountType::FORK_TYPE; diff --git a/asibfm700_configfile.h b/asibfm700_configfile.h index d70775c..8eb6b0b 100644 --- a/asibfm700_configfile.h +++ b/asibfm700_configfile.h @@ -156,6 +156,9 @@ static auto Asibfm700MountConfigDefaults = std::make_tuple( // slew process timeout in seconds simple_config_record_t{"slewTimeout", std::chrono::seconds(3600), {"slew process timeout in seconds"}}, + // mount stopping process timeout in seconds + simple_config_record_t{"stopTimeout", std::chrono::seconds(30), {"mount stopping process timeout in seconds"}}, + // a time shift into future to compute target position in future (UT1-scale time duration, millisecs) simple_config_record_t{ "timeShiftToTargetPoint", @@ -581,11 +584,11 @@ public: } - mcc::impl::MccSimpleMovementControlsParameters movingModelParams() const + details::movement_pars_t movingModelParams() const { static constexpr double arcsecs2rad = std::numbers::pi / 180.0 / 3600.0; // arcseconds to radians - mcc::impl::MccSimpleMovementControlsParameters pars; + details::movement_pars_t pars; auto get_value = [this](std::string_view name, VT& val) { val = getValue(name).value_or(val); diff --git a/asibfm700_mount.cpp b/asibfm700_mount.cpp index 8f79353..4f4cba0 100644 --- a/asibfm700_mount.cpp +++ b/asibfm700_mount.cpp @@ -14,9 +14,12 @@ Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_p _pcm(config.pcmData()), gm_class_t(std::make_tuple(&_servolController, &_pcm), std::make_tuple(), - std::make_tuple(&_servolController, - this, - [this](Asibfm700Mount::mount_status_t const& status) { *_mountStatus = status; }), + // std::make_tuple(&_servolController, + // this, + // [this](Asibfm700Mount::mount_status_t const& status) { *_mountStatus = status; }), + std::make_tuple([this](bool sl) { return slewingImpl(sl); }, + [this]() { return trackingImpl(); }, + [this]() { return stoppingImpl(); }), std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)), _mountConfig(config), _mountConfigMutex(new std::mutex) @@ -346,4 +349,154 @@ void Asibfm700Mount::errorLogging(const std::string& msg, const std::error_code& } } + +/* MOVEMENT METHODS */ + +Asibfm700Mount::error_t Asibfm700Mount::sendToHardware(AsibFM700ServoController::hardware_state_t const& hw_state) +{ + auto tp = std::chrono::duration_cast(hw_state.XY.epoch().UTC().time_since_epoch()); + + logDebug("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})", hw_state.XY.x().degrees(), + hw_state.XY.y().degrees(), tp); + + *_lastMountError = _servolController.hardwareSetState(hw_state); + + if (_lastMountError->load()) { + errorLogging("An error occured while send command to servo controller: ", _lastMountError->load()); + } else { + logDebug(" the 'hardwareSetState' method performed successfully!"); + } + + return _lastMountError->load(); +} + + +void Asibfm700Mount::logMountPos(telemetry_t::telemetry_data_t const& tdata) +{ + // NOTE: the implementation of MccTelemetry class guarantees that + // tdata.mountPos and tdata.targetPos are coordinates in the HA-DEC + // equathorial system + + logTrace(" current target: HA = {}, DEC = {} (encoders: {} {})", + mcc::impl::MccAngle(tdata.targetPos.co_lon()).sexagesimal(true), + mcc::impl::MccAngle(tdata.targetPos.co_lat()).sexagesimal(), tdata.targetXY.x().sexagesimal(), + tdata.targetXY.y().sexagesimal()); + + logTrace(" current mount: HA = {}, DEC = {} (encoders: {} {})", + mcc::impl::MccAngle(tdata.mountPos.co_lon()).sexagesimal(true), + mcc::impl::MccAngle(tdata.mountPos.co_lat()).sexagesimal(), tdata.hwState.XY.x().sexagesimal(), + tdata.hwState.XY.y().sexagesimal()); + + _pathFile.addToPath(tdata); + + auto dist = tdata.mountPos.distance(tdata.targetPos); + + logTrace(" target-to-mount distance: {} (dx = {}, dy = {})", mcc::impl::MccAngleFancyString(dist.dist), + mcc::impl::MccAngleFancyString(dist.dx), mcc::impl::MccAngleFancyString(dist.dy)); +} + + +Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemetry_data_t const& tdata) +{ + mcc::impl::MccGenXY braking_accel{getMovementParams().brakingAccelX, getMovementParams().brakingAccelY}, dxy{}; + + auto new_pos = this->coordsAfterTime(tdata.mountPos, tdata.hwState.speedXY, braking_accel, + getMovementParams().minTimeToPZone, &dxy); + + logTrace(" the distance that will be covered in the next {} seconds: HA-axis: {}, DEC-axis: {}", + getMovementParams().minTimeToPZone.count(), mcc::impl::MccAngleFancyString(dxy.x()), + mcc::impl::MccAngleFancyString(dxy.y())); + + bool in_zone; + std::vector in_zone_vec; + + *_lastMountError = this->inPZone(new_pos, &in_zone, &in_zone_vec); + if (_lastMountError->load()) { + return _lastMountError->load(); + } + + if (in_zone) { + size_t i = 0; + for (; i < in_zone_vec.size(); ++i) { + if (in_zone_vec[i]) { + break; + } + } + + auto names = this->pzoneNames(); + auto it = names.begin(); + std::ranges::advance(it, i); + + logError("target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i, + *it); + + mcc::impl::MccSkyHADEC_OBS hadec; + mcc::impl::MccSkyRADEC_OBS radec; + mcc::impl::MccSkyAZZD azzd; + mcc::impl::MccSkyAZALT azalt; + mcc::impl::MccAngle lst; + + *_lastMountError = tdata.mountPos.appSideralTime(&lst, true); + if (_lastMountError->load()) { + return _lastMountError->load(); + } + + *_lastMountError = tdata.mountPos.toAtSameEpoch(radec, hadec, azzd, azalt); + if (_lastMountError->load()) { + return _lastMountError->load(); + } + + logError(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", radec.x().sexagesimal(true), radec.y().sexagesimal(), + hadec.x().sexagesimal(true), lst.sexagesimal(true)); + logError(" AZ, ZD, ALT: {}, {}, {}", azzd.x().sexagesimal(), azzd.y().sexagesimal(), + azalt.y().sexagesimal()); + + logError(" hardware X, Y: {}, {}", tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal()); + + return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE; + } +} + +Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) {} + +Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl() +{ + typename AsibFM700ServoController::hardware_state_t hw_state; + + hw_state.movementState == AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPING; + + *_lastMountError = sendToHardware(hw_state); + + if (!_lastMountError->load()) { + *_mountStatus = mount_status_t::MOUNT_STATUS_STOPPING; + + auto start_tp = std::chrono::steady_clock::now(); + + *_lastMountError = _servolController.hardwareGetState(&hw_state); + + while (hw_state.movementState != AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPED && + !_lastMountError->load()) { + if ((std::chrono::steady_clock::now() - start_tp) > + _mountConfig.getValue("stopTimeout").value()) { + // set timeout error! + // *_lastMountError = + break; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + *_lastMountError = _servolController.hardwareGetState(&hw_state); + } + } + + if (!_lastMountError->load()) { + *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE; + } else { + *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR; + errorLogging("An error occured while stoppping mount: ", _lastMountError->load()); + } + + return _lastMountError->load(); +} + } // namespace asibfm700 diff --git a/asibfm700_mount.h b/asibfm700_mount.h index d8b93ba..d2989d3 100644 --- a/asibfm700_mount.h +++ b/asibfm700_mount.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -14,19 +15,29 @@ namespace asibfm700 -class Asibfm700Mount : public mcc::impl::MccGenericMount, - Asibfm700PZoneContainer, - mcc::impl::MccSimpleMovementControls, - Asibfm700Logger> +class Asibfm700Mount + : public mcc::impl::MccGenericMount, + Asibfm700PZoneContainer, + // mcc::impl::MccSimpleMovementControls, + mcc::impl::MccGenericAsyncMovementControls, + Asibfm700Logger> { typedef mcc::impl::MccGenericMount, Asibfm700PZoneContainer, - mcc::impl::MccSimpleMovementControls, + // mcc::impl::MccSimpleMovementControls, + mcc::impl::MccGenericAsyncMovementControls, Asibfm700Logger> gm_class_t; + public: - using gm_class_t::error_t; + typedef mcc::impl::MccTelemetry telemetry_t; + + typedef mcc::impl::MccGenericAsyncMovementControls movement_controls_t; + + using typename gm_class_t::error_t; + using typename movement_controls_t::movement_params_t; + // using Asibfm700CCTE::setStateERFA; // using Asibfm700CCTE::updateBulletinA; @@ -70,6 +81,20 @@ protected: Asibfm700PCM _pcm; void errorLogging(const std::string&, const std::error_code&); + + // movement methods + mcc::impl::MccMovementPathFile _pathFile{}; + + error_t slewingImpl(bool); + error_t trackingImpl(); + error_t stoppingImpl(); + + error_t sendToHardware(AsibFM700ServoController::hardware_state_t const& hw_state); + error_t checkPZone(typename telemetry_t::telemetry_data_t const& tdata); + void logMountPos(telemetry_t::telemetry_data_t const& tdata); + + std::unique_ptr> _lastMountError{ + new std::atomic{mcc::impl::MccGenericMountErrorCode::ERROR_OK}}; };