eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of ssh://95.140.147.151:/home/git/mountcontrol

Browse Source
This commit is contained in:
Timur A. Fatkhullin 2025-12-01 10:07:45 +03:00
commit c514d4adcc
1 changed files with 63 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
mcc/mcc_slewing_model.h
View File

@ -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 <mcc_all_controls_c CONTROLS_T, mcc_logger_c LoggerT = MccNullLogger>
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<error_t(bool)> _slewingFunc{};
std::unique_ptr<std::atomic_bool> _stopSlewing;
@ -642,6 +671,8 @@ protected:
slewing_params_t _currentParams{};
std::unique_ptr<std::mutex> _currentParamsMutex{};
error_t _lastError;
};