From b14b20e0693877accacb8576fc811aa490c1534b Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Thu, 26 Feb 2026 12:13:14 +0300 Subject: [PATCH] ... --- asibfm700_common.h | 10 +- asibfm700_mount.cpp | 352 ++++++++++++++++++++++++++++++++++++++++++-- asibfm700_mount.h | 129 +--------------- 3 files changed, 350 insertions(+), 141 deletions(-) diff --git a/asibfm700_common.h b/asibfm700_common.h index adb2d5e..a96276f 100644 --- a/asibfm700_common.h +++ b/asibfm700_common.h @@ -33,6 +33,11 @@ struct movement_pars_t { // time interval to update prohibited zones related quantities (e.g. intersection points) std::chrono::milliseconds updatingPZoneInterval{5000}; + // 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 mode ******* @@ -58,11 +63,6 @@ struct movement_pars_t { 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{}; diff --git a/asibfm700_mount.cpp b/asibfm700_mount.cpp index 4f4cba0..af90516 100644 --- a/asibfm700_mount.cpp +++ b/asibfm700_mount.cpp @@ -396,21 +396,26 @@ void Asibfm700Mount::logMountPos(telemetry_t::telemetry_data_t const& tdata) } -Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemetry_data_t const& tdata) +Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemetry_data_t const& tdata, bool predicted) { - 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 (predicted) { + 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())); + + *_lastMountError = this->inPZone(new_pos, &in_zone, &in_zone_vec); + } else { + *_lastMountError = this->inPZone(tdata.mountPos, &in_zone, &in_zone_vec); + } + if (_lastMountError->load()) { return _lastMountError->load(); } @@ -453,11 +458,325 @@ Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemet logError(" hardware X, Y: {}, {}", tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal()); - return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE; + if (predicted) { + return *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_NEAR_PZONE; + } else { + return *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_IN_PZONE; + } } + + return *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK; } -Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) {} +Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) +{ + _pathFile.clearPath(); + + _enteredBackupCoordinates = getPointingTarget(); + + typename telemetry_t::telemetry_data_t tdata; + *_lastMountError = telemetryData(&tdata); + if (_lastMountError->load()) { + errorLogging("An error occured while telemetry requesting: ", _lastMountError->load()); + return _lastMountError->load(); + } + + *_lastMountError = checkPZone(tdata, false); + if (_lastMountError->load()) { + return _lastMountError->load(); + } + + logInfo("Start slewing in mode '{}'", slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK"); + logInfo(" slewing process timeout: {} secs", _currentMovementParams.slewTimeout.count()); + logInfo(" slewing tolerance radius: {} arcsecs", + mcc::impl::MccAngle{_currentMovementParams.slewToleranceRadius}.arcsecs()); + + logInfo(" braking acceleration X: {} degs/s^2", + mcc::impl::MccAngle(_currentMovementParams.brakingAccelX).degrees()); + logInfo(" braking acceleration Y: {} degs/s^2", + mcc::impl::MccAngle(_currentMovementParams.brakingAccelY).degrees()); + logInfo(" min time to prohibited zone: {} seconds", _currentMovementParams.minTimeToPZone.count()); + + if (!_currentMovementParams.slewingPathFilename.empty()) { // slewing trajectory file + logInfo("Set slewing path filename to {}", _currentMovementParams.slewingPathFilename); + } else { + logWarn("Slewing path filename is empty! Do not save it!"); + } + + + _pathFile.addComment(""); + _pathFile.addComment(std::format("Slewing trajectory, {}", std::chrono::system_clock::now())); + _pathFile.addComment("Config:"); + _pathFile.addComment(std::format(" slewing tolerance radius: {} arcsecs", + mcc::impl::MccAngle{_currentMovementParams.slewToleranceRadius}.arcsecs())); + _pathFile.addComment( + std::format(" slewing process timeout: {} secs", _currentMovementParams.slewTimeout.count())); + _pathFile.addComment(""); + _pathFile.addDefaultComment(); + + typename hardware_t::hardware_state_t hw_state; + using ep_t = decltype(hw_state.XY.epoch()); + + hw_state.movementState = hardware_t::hardware_movement_state_t::HW_MOVE_SLEWING; + hw_state.XY.setX(tdata.targetXY.x()); + hw_state.XY.setY(tdata.targetXY.y()); + hw_state.XY.setEpoch(ep_t::now()); + + + // start slewing (LibSidServo.moveTo) ... + + *_lastMountError = sendToHardware(hw_state); + + if (_lastMountError->load()) { + *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR; + _pathFile.clearPath(); + + return _lastMountError->load(); + } + + *_mountStatus = mount_status_t::MOUNT_STATUS_SLEWING; + + auto start_point = std::chrono::steady_clock::now(); + auto last_hw_time = tdata.hwState.XY.epoch().UTC(); + + hw_state.movementState = hardware_t::hardware_movement_state_t::HW_MOVE_ADJUSTING; + + mcc::impl::MccSkyPoint::dist_result_t dist; + + while (!_stopMovementRequest->load()) { + *_lastMountError = telemetryData(&tdata); + + if (_lastMountError->load()) { + errorLogging("An error occured while telemetry requesting: ", _lastMountError->load()); + // return _lastMountError->load(); + break; + } + + hw_state.XY.setX(tdata.targetXY.x()); + hw_state.XY.setY(tdata.targetXY.y()); + hw_state.XY.setEpoch(ep_t::now()); + + logMountPos(tdata); + + if (_stopMovementRequest->load()) { // external stop + break; + } + + *_lastMountError = checkPZone(tdata, true); + if (_lastMountError->load()) { + break; + // return _lastMountError->load(); + } + + if ((std::chrono::steady_clock::now() - start_point) > getMovementParams().slewTimeout) { + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_SLEW_TIMEOUT; + break; + } + + dist = tdata.mountPos.distance(tdata.targetPos); + + if (dist.dist <= getMovementParams().adjustCoordDiff) { // start adjusting (precise pointing) + *_mountStatus = mount_status_t::MOUNT_STATUS_ADJUSTING; + + *_lastMountError = sendToHardware(hw_state); + + if (_lastMountError->load()) { + break; + } + } + + if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_STOPPED && + !_stopMovementRequest->load()) { + // the mount stopped but still to far from target position!!! + if (dist.dist > getMovementParams().slewToleranceRadius) { + *_mountStatus = mount_status_t::MOUNT_STATUS_ADJUSTING; + + *_lastMountError = sendToHardware(hw_state); + + if (_lastMountError->load()) { + break; + } + } else { // just exit + break; + } + } + + if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_ERROR) { + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_HARDWARE; + break; + } + + // sleep here + std::this_thread::sleep_for(getMovementParams().slewingTelemetryInterval); + } + + if (_stopMovementRequest->load()) { // external stop + logWarn("Slewing was stopped!"); + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK; + *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE; + } else { + if (_lastMountError->load()) { + *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR; + auto err = _lastMountError->load(); + logError("Slewing finished with error: {} ({}::{})", err.message(), err.category().name(), err.value()); + } else { + logInfo("Slewing finished without errors!"); + } + } + + if (!_lastMountError->load()) { // get final position + *_lastMountError = telemetryData(&tdata); + + if (_lastMountError->load()) { + *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR; + errorLogging("An error occured while telemetry requesting: ", _lastMountError->load()); + } else { + logInfo("Final mount slewing position:"); + logMountPos(tdata); + } + } + + bool ok = _pathFile.saveToFile(_currentMovementParams.slewingPathFilename); + if (!ok && !_currentMovementParams.slewingPathFilename.empty()) { + logError("Cannot save trajectory file!"); + } + _pathFile.clearPath(); + + if (!_lastMountError->load()) { // do not start tracking if an error occured + if (!slew_and_stop) { + return trackTarget(); + } + } else { + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK; + } + + return _lastMountError->load(); +} + +Asibfm700Mount::error_t Asibfm700Mount::trackingImpl() +{ + *_mountStatus = mount_status_t::MOUNT_STATUS_TRACKING; + _pathFile.clearPath(); + + logInfo("Start tracking"); + logInfo(" braking acceleration X: {} degs/s^2", + mcc::impl::MccAngle(_currentMovementParams.brakingAccelX).degrees()); + logInfo(" braking acceleration Y: {} degs/s^2", + mcc::impl::MccAngle(_currentMovementParams.brakingAccelY).degrees()); + logInfo(" min time to prohibited zone: {} seconds", _currentMovementParams.minTimeToPZone.count()); + logInfo(" max target-to-mount distance: {} arcseconds", _currentMovementParams.trackingMaxCoordDiff); + + if (!_currentMovementParams.trackingPathFilename.empty()) { // tracking trajectory file + logInfo("Set tracking path filename to {}", _currentMovementParams.trackingPathFilename); + } else { + logWarn("Tracking path filename is empty! Do not save it!"); + } + + telemetry_data_t tdata; + + *_lastMountError = telemetryData(&tdata); + + if (_lastMountError->load()) { + errorLogging("An error occured while telemetry requesting: ", _lastMountError->load()); + + *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR; + + return _lastMountError->load(); + } + + + mcc::impl::MccSkyPoint::dist_result_t dist; + dist = tdata.mountPos.distance(tdata.targetPos); + + if (dist.dist > _currentMovementParams.trackingMaxCoordDiff) { + logInfo( + "The target-to-mount distance {} is greater than allowed one ({})! Track current " + "mount position!", + mcc::impl::MccAngleFancyString(dist.dist), + mcc::impl::MccAngleFancyString(_currentMovementParams.trackingMaxCoordDiff)); + + _enteredBackupCoordinates = getPointingTarget(); + setPointingTarget(tdata.mountPos); + + *_lastMountError = telemetryData(&tdata); + + if (_lastMountError->load()) { + errorLogging("An error occured while telemetry requesting: ", _lastMountError->load()); + + *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR; + + return _lastMountError->load(); + } + } + + *_lastMountError = checkPZone(tdata, true); + if (_lastMountError->load()) { + return _lastMountError->load(); + } + + _pathFile.addComment(""); + _pathFile.addComment(std::format("Tracking trajectory, {}", std::chrono::system_clock::now())); + _pathFile.addDefaultComment(); + + typename hardware_t::hardware_state_t hw_state; + using ep_t = decltype(hw_state.XY.epoch()); + + hw_state.movementState = hardware_t::hardware_movement_state_t::HW_MOVE_TRACKING; + + while (!_stopMovementRequest->load()) { + *_lastMountError = telemetryData(&tdata); + + if (_lastMountError->load()) { + errorLogging("An error occured while telemetry requesting: ", _lastMountError->load()); + break; + } + + logMountPos(tdata); + + *_lastMountError = checkPZone(tdata, true); + if (_lastMountError->load()) { + break; + } + + hw_state.XY.setX(tdata.targetXY.x()); + hw_state.XY.setY(tdata.targetXY.y()); + hw_state.XY.setEpoch(ep_t::now()); + + *_lastMountError = sendToHardware(hw_state); + if (_lastMountError->load()) { + break; + } + + if (_stopMovementRequest->load()) { + break; + } + + // sleep here + std::this_thread::sleep_for(_currentMovementParams.trackingTelemetryInterval); + } + + if (_stopMovementRequest->load()) { // external stop + logWarn("Tracking was stopped!"); + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK; + *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE; + } else { + if (_lastMountError->load()) { + *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR; + auto err = _lastMountError->load(); + logError("Tracking finished with error: {} ({}::{})", err.message(), err.category().name(), err.value()); + } else { // ???!!!!! + logInfo("Tracking finished without errors!"); + } + } + + bool ok = _pathFile.saveToFile(_currentMovementParams.trackingPathFilename, std::ios_base::app); + if (!ok && !_currentMovementParams.trackingPathFilename.empty()) { + logError("Cannot save trajectory file!"); + } + _pathFile.clearPath(); + + return *_lastMountError; +} Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl() { @@ -478,8 +797,7 @@ Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl() !_lastMountError->load()) { if ((std::chrono::steady_clock::now() - start_tp) > _mountConfig.getValue("stopTimeout").value()) { - // set timeout error! - // *_lastMountError = + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_STOP_TIMEOUT; break; } @@ -496,6 +814,10 @@ Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl() errorLogging("An error occured while stoppping mount: ", _lastMountError->load()); } + if (!_lastMountError->load()) { + *_lastMountError = mcc::impl::MccGenericMountErrorCode::ERROR_OK; + } + return _lastMountError->load(); } diff --git a/asibfm700_mount.h b/asibfm700_mount.h index d2989d3..cec66f5 100644 --- a/asibfm700_mount.h +++ b/asibfm700_mount.h @@ -2,7 +2,7 @@ #include #include -#include +// #include #include #include #include @@ -39,21 +39,11 @@ public: using typename movement_controls_t::movement_params_t; - // using Asibfm700CCTE::setStateERFA; - // using Asibfm700CCTE::updateBulletinA; - // using Asibfm700CCTE::updateLeapSeconds; - // using Asibfm700CCTE::updateMeteoERFA; - using gm_class_t::logCritical; using gm_class_t::logDebug; using gm_class_t::logError; using gm_class_t::logInfo; using gm_class_t::logWarn; - // using Asibfm700Logger::logCritical; - // using Asibfm700Logger::logDebug; - // using Asibfm700Logger::logError; - // using Asibfm700Logger::logInfo; - // using Asibfm700Logger::logWarn; // using Asibfm700PZoneContainer::addPZone; @@ -84,126 +74,23 @@ protected: // movement methods mcc::impl::MccMovementPathFile _pathFile{}; + decltype(telemetry_data_t::targetPos) _enteredBackupCoordinates; 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); + + // if predicted == true then the method checks the coordinates calculated for a point in time in the future, i.e. + // the predicted position if the movement is immediately stopped. + // if predicted == false then the method checks the current mount coordinates + error_t checkPZone(typename telemetry_t::telemetry_data_t const& tdata, bool predicted = true); + void logMountPos(telemetry_t::telemetry_data_t const& tdata); - - std::unique_ptr> _lastMountError{ - new std::atomic{mcc::impl::MccGenericMountErrorCode::ERROR_OK}}; }; -/* -class Asibfm700Mount : public Asibfm700CCTE, - public Asibfm700PCM, - public mcc::MccGenericFsmMount> -{ - typedef mcc::MccGenericMount - gm_class_t; - - typedef mcc::MccGenericFsmMount> - base_gm_class_t; - -protected: - struct Asibfm700ErrorState : base_gm_class_t::MccGenericFsmMountBaseState { - static constexpr std::string_view ID{"ASIBFM700-MOUNT-ERROR-STATE"}; - - // void exit(MccGenericFsmMountErrorEvent& event) - // { - // event.mount()->logWarn("The mount already in error state!"); - // } - - void enter(MccGenericFsmMountErrorEvent& event) - { - enterLog(event); - - // event.mount()->logWarn("The mount already in error state!"); - auto err = event.eventData(); - event.mount()->logError("An error occured: {} [{} {}]", err.message(), err.value(), err.category().name()); - } - - void exit(mcc::fsm::traits::fsm_event_c auto& event) - { - exitLog(event); - } - - void enter(mcc::fsm::traits::fsm_event_c auto& event) - { - enterLog(event); - - // ... - } - - using transition_t = mcc::fsm::fsm_transition_table_t< - std::pair, - std::pair>, - std::pair>>; - }; - - - typedef base_gm_class_t::MccGenericFsmMountStartState Asibfm700StartState; - -public: - using base_gm_class_t::error_t; - - using Asibfm700CCTE::setStateERFA; - using Asibfm700CCTE::updateBulletinA; - using Asibfm700CCTE::updateLeapSeconds; - using Asibfm700CCTE::updateMeteoERFA; - - using Asibfm700Logger::logCritical; - using Asibfm700Logger::logDebug; - using Asibfm700Logger::logError; - using Asibfm700Logger::logInfo; - using Asibfm700Logger::logWarn; - - // using Asibfm700PZoneContainer::addPZone; - - Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr logger); - - ~Asibfm700Mount(); - - Asibfm700Mount(Asibfm700Mount&&) = default; - Asibfm700Mount& operator=(Asibfm700Mount&&) = default; - - Asibfm700Mount(const Asibfm700Mount&) = delete; - Asibfm700Mount& operator=(const Asibfm700Mount&) = delete; - - error_t initMount(); - - error_t updateMountConfig(Asibfm700MountConfig const&); - Asibfm700MountConfig currentMountConfig(); - -protected: - Asibfm700MountConfig _mountConfig; - std::unique_ptr _mountConfigMutex; - - void errorLogging(const std::string&, const std::error_code&); -}; -*/ - -// static_assert(mcc::mcc_position_controls_c, ""); -// static_assert(mcc::mcc_all_controls_c, ""); static_assert(mcc::mcc_generic_mount_c, "");