...
This commit is contained in:
parent
a825a6935b
commit
cca58e8ba9
@ -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 <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();
|
||||
@ -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<error_t(bool)> _slewingFunc{};
|
||||
std::unique_ptr<std::atomic_bool> _stopSlewing;
|
||||
@ -626,6 +655,8 @@ protected:
|
||||
|
||||
slewing_params_t _currentParams{};
|
||||
std::unique_ptr<std::mutex> _currentParamsMutex{};
|
||||
|
||||
error_t _lastError;
|
||||
};
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user