From 28ecf307a8289dbccc7c5033cfde3dfac16e055b Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Tue, 9 Dec 2025 16:55:46 +0300 Subject: [PATCH] add saving slewing trajectory in a file --- asibfm700/asibfm700_configfile.h | 22 +++++++++ asibfm700/asibfm700_servocontroller.cpp | 14 +++++- mcc/mcc_moving_model_common.h | 7 +++ mcc/mcc_slewing_model.h | 65 +++++++++++++++++++++++-- 4 files changed, 103 insertions(+), 5 deletions(-) diff --git a/asibfm700/asibfm700_configfile.h b/asibfm700/asibfm700_configfile.h index f0995aa..5fe6561 100644 --- a/asibfm700/asibfm700_configfile.h +++ b/asibfm700/asibfm700_configfile.h @@ -139,6 +139,10 @@ static auto Asibfm700MountConfigDefaults = std::make_tuple( std::chrono::milliseconds(100), {"telemetry request interval (in millisecs) in slewing mode"}}, + simple_config_record_t{"slewingPathFilename", + std::string(), + {"slewing trajectory filename", "if it is an empty - just skip saving"}}, + // target-mount coordinate difference in arcsecs to start adjusting of slewing simple_config_record_t{"adjustCoordDiff", 50.0, @@ -176,6 +180,10 @@ static auto Asibfm700MountConfigDefaults = std::make_tuple( {"maximal valid target-to-mount distance for tracking process (arcsecs)", "if current distance is greater than assume current mount coordinate as target point"}}, + simple_config_record_t{"trackingPathFilename", + std::string(), + {"tracking trajectory filename", "if it is an empty - just skip saving"}}, + /* prohibited zones */ @@ -587,6 +595,9 @@ public: pars.slewTimeout = getValue("slewTimeout").value_or(pars.slewTimeout); + pars.slewingPathFilename = + getValue("slewingPathFilename").value_or(std::string()); + get_value("trackingTelemetryInterval", pars.trackingTelemetryInterval); pars.timeShiftToTargetPoint = getValue("timeShiftToTargetPoint") @@ -599,6 +610,9 @@ public: getValue("trackingMaxCoordDiff").value_or(pars.trackingMaxCoordDiff) * arcsecs2rad; + pars.trackingPathFilename = + getValue("trackingPathFilename").value_or(std::string()); + return pars; } @@ -740,6 +754,14 @@ public: val = getValue("EncoderYDevPath").value_or(std::string{}); fname = mcc::utils::trimSpaces(val); setValue("EncoderYDevPath", fname); + + val = getValue("slewingPathFilename").value_or(std::string{}); + fname = mcc::utils::trimSpaces(val); + setValue("slewingPathFilename", fname); + + val = getValue("trackingPathFilename").value_or(std::string{}); + fname = mcc::utils::trimSpaces(val); + setValue("trackingPathFilename", fname); } } catch (std::ios_base::failure const& ex) { ec = ex.code(); diff --git a/asibfm700/asibfm700_servocontroller.cpp b/asibfm700/asibfm700_servocontroller.cpp index d72c67f..c6dd94a 100644 --- a/asibfm700/asibfm700_servocontroller.cpp +++ b/asibfm700/asibfm700_servocontroller.cpp @@ -205,9 +205,19 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(har } else if (mdata.Xstate == AXIS_SLEWING) { state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING; } else if (mdata.Xstate == AXIS_POINTING) { - state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING; + if (mdata.Ystate == AXIS_SLEWING) { + state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING; + } else { + state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING; + } } else if (mdata.Xstate == AXIS_GUIDING) { - state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING; + if (mdata.Ystate == AXIS_SLEWING) { + state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING; + } else if (mdata.Ystate == AXIS_POINTING) { + state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING; + } else { + state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING; + } } else { state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN; } diff --git a/mcc/mcc_moving_model_common.h b/mcc/mcc_moving_model_common.h index 501d279..3c71fb6 100644 --- a/mcc/mcc_moving_model_common.h +++ b/mcc/mcc_moving_model_common.h @@ -59,6 +59,9 @@ struct MccSimpleMovingModelParams { double brakingAccelX{0.0}; double brakingAccelY{0.0}; + // slewing trajectory file. if empty - just skip saving + std::string slewingPathFilename{}; + // ******* tracking mode ******* @@ -76,6 +79,10 @@ struct MccSimpleMovingModelParams { // 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{}; + + // ******* guiding mode ******* double guidingCorrectionRange[2]{0.3_arcsecs, 3.0_arcsecs}; diff --git a/mcc/mcc_slewing_model.h b/mcc/mcc_slewing_model.h index d70e451..531aea1 100644 --- a/mcc/mcc_slewing_model.h +++ b/mcc/mcc_slewing_model.h @@ -6,6 +6,8 @@ /* SIMPLE SLEWING MODEL IMPLEMENTATION */ +#include + #include "mcc_defaults.h" #include "mcc_generics.h" #include "mcc_moving_model_common.h" @@ -196,6 +198,10 @@ public: 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}; if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) { @@ -212,6 +218,16 @@ public: min_time_to_pzone_in_secs = 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 { + logger.logError(std::format("Cannot open slewing path file: {}! Do not save it!", + _currentParams.slewingPathFilename)); + } + } } logger.logInfo( @@ -228,6 +244,20 @@ public: logger.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"; + } + + typename CONTROLS_T::error_t t_err; MccTelemetryData tdata; @@ -286,6 +316,9 @@ public: // start slewing logger.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(CONTROLS_T::mountType)) { logger.logDebug(std::format(" entered target: HA = {}, DEC = {}", mcc::MccAngle{tdata.target.HA}.sexagesimal(true), @@ -293,6 +326,13 @@ public: logger.logDebug(std::format(" current mount: HA = {}, DEC = {}", mcc::MccAngle{tdata.HA}.sexagesimal(true), mcc::MccAngle{tdata.DEC_APP}.sexagesimal())); + + if (store_path) { + fst << std::chrono::duration_cast(tdata.time_point - start_point).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(CONTROLS_T::mountType)) { logger.logDebug(std::format(" entered target: AZ = {}, ZD = {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(), @@ -462,8 +502,17 @@ public: // return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); } - logger.logTrace(std::format("hw state was updated ({}, {})", MccAngle(hw_state.X).sexagesimal(true), - MccAngle(hw_state.Y).sexagesimal())); + logger.logTrace(std::format("hw state was updated ({}, {} state = <{}>)", + MccAngle(hw_state.X).sexagesimal(true), MccAngle(hw_state.Y).sexagesimal(), + (int)hw_state.moving_state)); + + if (store_path) { + fst << std::chrono::duration_cast(tdata.time_point - start_point).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"; + } + if (slew_and_stop && !tag_var_coord) { // just wait for mount to be stopped if (hw_state.moving_state == CONTROLS_T::hardware_moving_state_t::HW_MOVE_STOPPED) { @@ -489,8 +538,12 @@ public: logger.logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist))); - if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle + if ((dist <= _currentParams.slewToleranceRadius) && + (hw_state.moving_state == + CONTROLS_T::hardware_moving_state_t::HW_MOVE_GUIDING)) { // stop slewing and exit from + // cycle logger.logInfo("target-to-mount distance is lesser than slew tolerance radius - exit!"); + if (slew_and_stop) { controls->hardwareStop(); } @@ -527,6 +580,8 @@ public: MccAngle(hw_state.Y).sexagesimal(), mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees())); + // hw_state.time_point += std::chrono::milliseconds(50); + hw_err = controls->hardwareSetState(hw_state); if (hw_err) { *_lastError = MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE; @@ -597,6 +652,10 @@ public: mcc::MccAngle{tdata.ZD}.sexagesimal())); } + if (store_path) { + fst.close(); + } + return *_lastError = MccSimpleSlewingModelErrorCode::ERROR_OK; }; }