From cca58e8ba9bd050c3a57415bf901b3f478fc57e5 Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Mon, 1 Dec 2025 02:49:01 +0300 Subject: [PATCH] ... --- mcc/mcc_slewing_model.h | 95 +++++++++++++++++++++++++++-------------- 1 file changed, 63 insertions(+), 32 deletions(-) diff --git a/mcc/mcc_slewing_model.h b/mcc/mcc_slewing_model.h index f8ad5e6..bb8f89a 100644 --- a/mcc/mcc_slewing_model.h +++ b/mcc/mcc_slewing_model.h @@ -51,10 +51,7 @@ namespace mcc struct MccSimpleSlewingModelCategory : public std::error_category { MccSimpleSlewingModelCategory() : std::error_category() {} - const char* name() const noexcept - { - return "SIMPLE-SLEWING-MODEL"; - } + const char* name() const noexcept { return "SIMPLE-SLEWING-MODEL"; } std::string message(int ec) const { @@ -120,7 +117,9 @@ public: template MccSimpleSlewingModel(CONTROLS_T* controls, LoggerT logger) - : _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex) + : _stopSlewing(new std::atomic_bool()), + _currentParamsMutex(new std::mutex), + _lastError(MccSimpleSlewingModelErrorCode::ERROR_OK) { std::ostringstream os; os << std::this_thread::get_id(); @@ -163,6 +162,9 @@ public: }; _slewingFunc = [controls, logger = std::move(logger), this](bool slew_and_stop) mutable -> error_t { + // reset error + _lastError = MccSimpleSlewingModelErrorCode::ERROR_OK; + // first, check target coordinates typename CONTROLS_T::error_t t_err; MccTelemetryData tdata; @@ -172,7 +174,8 @@ public: t_err = controls->telemetryData(&tdata); if (t_err) { - return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); + return _lastError = + mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); } } @@ -181,7 +184,8 @@ public: auto pz_err = controls->inPZone(tdata.target, &in_zone, &in_zone_vec); if (pz_err) { *_stopSlewing = true; - return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + return _lastError = + mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); } if (in_zone) { @@ -211,7 +215,7 @@ public: } if (*_stopSlewing) { - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + return _lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } double braking_accelX, braking_accelY; @@ -259,7 +263,7 @@ public: auto hw_err = controls->hardwareGetState(&hw_state); if (hw_err) { *_stopSlewing = true; - return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); + return _lastError = mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); } hw_state.X = (double)tdata.target.X; @@ -277,7 +281,7 @@ public: if (*_stopSlewing) { logger.logDebug("slewing was stopped!"); - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + return _lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } // start slewing @@ -302,7 +306,7 @@ public: hw_err = controls->hardwareSetState(hw_state); if (hw_err) { *_stopSlewing = true; - return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); + return _lastError = mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); } logger.logDebug(" the 'hardwareSetState' method performed successfully!"); @@ -348,13 +352,17 @@ public: t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); if (t_err) { - *_stopSlewing = true; - return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); + _lastError = mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); + break; + // *_stopSlewing = true; + // return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); } } if (*_stopSlewing) { - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + _lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + break; + // return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } distXY = mcc_compute_distance(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY); @@ -412,8 +420,11 @@ public: in_zone_vec.clear(); pz_err = controls->inPZone(cpt, &in_zone, &in_zone_vec); if (pz_err) { - *_stopSlewing = true; - return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + _lastError = + mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); + break; + // *_stopSlewing = true; + // return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP); } if (in_zone) { @@ -437,8 +448,10 @@ public: logger.logError(std::format(" hardware X, Y: {}, {}", mcc::MccAngle{tdata.X}.sexagesimal(), mcc::MccAngle{tdata.Y}.sexagesimal())); - *_stopSlewing = true; - return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE; + _lastError = MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE; + break; + // *_stopSlewing = true; + // return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE; } @@ -448,7 +461,9 @@ public: if ((std::chrono::steady_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) { logger.logError("slewing process timeout!"); - return MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT; + _lastError = MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT; + break; + // return MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT; } } @@ -456,8 +471,10 @@ public: hw_err = controls->hardwareGetState(&hw_state); if (hw_err) { - *_stopSlewing = true; - return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); + _lastError = mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); + break; + // *_stopSlewing = true; + // 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), @@ -471,8 +488,10 @@ public: } else { t_err = controls->targetToMountDist(&dist); if (t_err) { - *_stopSlewing = true; - return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY); + _lastError = mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY); + break; + // *_stopSlewing = true; + // return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY); } logger.logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist))); @@ -486,7 +505,9 @@ public: if (*_stopSlewing) { - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + _lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + break; + // return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } @@ -501,8 +522,10 @@ public: hw_err = controls->hardwareSetState(hw_state); if (hw_err) { - *_stopSlewing = true; - return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); + _lastError = MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE; + break; + // *_stopSlewing = true; + // return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); } @@ -516,8 +539,10 @@ public: hw_err = controls->hardwareGetState(&hw_state); if (hw_err) { - *_stopSlewing = true; - return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE); + _lastError = MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE; + break; + // *_stopSlewing = true; + // 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), @@ -525,7 +550,9 @@ public: } if (*_stopSlewing) { - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + _lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + break; + // return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } // sleep here @@ -543,8 +570,8 @@ public: t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout); if (t_err) { - *_stopSlewing = true; - return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); + return _lastError = + mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY); } } @@ -564,7 +591,7 @@ public: mcc::MccAngle{tdata.ZD}.sexagesimal())); } - return MccSimpleSlewingModelErrorCode::ERROR_OK; + return _lastError = MccSimpleSlewingModelErrorCode::ERROR_OK; }; } @@ -619,6 +646,8 @@ public: return _currentParams; } + error_t slewingLastError() const { return _lastError; } + protected: std::function _slewingFunc{}; std::unique_ptr _stopSlewing; @@ -626,6 +655,8 @@ protected: slewing_params_t _currentParams{}; std::unique_ptr _currentParamsMutex{}; + + error_t _lastError; };