diff --git a/mcc/mcc_slewing_model.h b/mcc/mcc_slewing_model.h index dceb5d9..6fa0fb5 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 { @@ -122,7 +119,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(); @@ -165,6 +164,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; @@ -174,7 +176,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); } } @@ -185,7 +188,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) { @@ -215,7 +219,7 @@ public: } if (*_stopSlewing) { - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + return _lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } double braking_accelX, braking_accelY; @@ -263,7 +267,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); } @@ -285,7 +289,7 @@ public: if (*_stopSlewing) { logger.logDebug("slewing was stopped!"); - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + return _lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } // start slewing @@ -310,7 +314,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!"); @@ -349,13 +353,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); @@ -413,8 +421,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) { @@ -438,8 +449,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; } @@ -449,7 +462,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; } } @@ -457,8 +472,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), @@ -479,8 +496,10 @@ public: 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))); @@ -495,7 +514,9 @@ public: if (*_stopSlewing) { - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + _lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + break; + // return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } @@ -518,8 +539,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); } @@ -532,8 +555,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), @@ -541,7 +566,9 @@ public: } if (*_stopSlewing) { - return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + _lastError = MccSimpleSlewingModelErrorCode::ERROR_STOPPED; + break; + // return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; } // sleep here @@ -559,8 +586,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); } } @@ -580,7 +607,7 @@ public: mcc::MccAngle{tdata.ZD}.sexagesimal())); } - return MccSimpleSlewingModelErrorCode::ERROR_OK; + return _lastError = MccSimpleSlewingModelErrorCode::ERROR_OK; }; } @@ -635,6 +662,8 @@ public: return _currentParams; } + error_t slewingLastError() const { return _lastError; } + protected: std::function _slewingFunc{}; std::unique_ptr _stopSlewing; @@ -642,6 +671,8 @@ protected: slewing_params_t _currentParams{}; std::unique_ptr _currentParamsMutex{}; + + error_t _lastError; };