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

...

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2025-11-17 18:04:40 +03:00
parent e0c8d8f39b
commit 771619b832
8 changed files with 222 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
mcc/mcc_slewing_model.h
View File

@@ -9,7 +9,6 @@
#include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_moving_model_common.h"
#include "mcc_spdlog.h"
namespace mcc
{
@@ -127,11 +126,80 @@ public:
*_stopSlewing = true;
_checkTargetFunc = [controls, logger, this]() mutable -> error_t {
typename CONTROLS_T::error_t t_err;
MccTelemetryData tdata;
bool in_zone;
t_err = controls->telemetryData(&tdata);
if (t_err) {
return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
}
auto pz_err = controls->inPZone(tdata.target, &in_zone);
if (pz_err) {
return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (in_zone) {
logger.logError("target point is in prohibited zone! Entered target coordinates:");
logger.logError(std::format(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true),
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(),
mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
mcc::MccAngle{tdata.LST}.sexagesimal(true)));
logger.logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.target.ZD}.sexagesimal(),
mcc::MccAngle{tdata.target.ALT}.sexagesimal()));
return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE;
}
return MccSimpleSlewingModelErrorCode::ERROR_OK;
};
_slewingFunc = [controls, logger = std::move(logger), this](bool slew_and_stop) mutable -> error_t {
// first, check target coordinates
typename CONTROLS_T::error_t t_err;
MccTelemetryData tdata;
{
std::lock_guard lock{*_currentParamsMutex};
t_err = controls->telemetryData(&tdata);
if (t_err) {
return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
}
}
bool in_zone;
auto pz_err = controls->inPZone(tdata.target, &in_zone);
if (pz_err) {
*_stopSlewing = true;
return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (in_zone) {
*_stopSlewing = true;
logger.logError("target point is in prohibited zone! Entered target coordinates:");
logger.logError(std::format(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true),
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(),
mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
mcc::MccAngle{tdata.LST}.sexagesimal(true)));
logger.logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.target.ZD}.sexagesimal(),
mcc::MccAngle{tdata.target.ALT}.sexagesimal()));
return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE;
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
double braking_accelX, braking_accelY;
{
@@ -161,41 +229,6 @@ public:
logger.logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
{
std::lock_guard lock{*_currentParamsMutex};
t_err = controls->telemetryData(&tdata);
if (t_err) {
return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
}
}
bool in_zone;
auto pz_err = controls->inPZone(tdata.target, &in_zone);
if (pz_err) {
*_stopSlewing = true;
return mcc_deduce_error_code(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (in_zone) {
*_stopSlewing = true;
logger.logError("target point is in prohibited zone!");
logger.logError(std::format(
"RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true),
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(), mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
mcc::MccAngle{tdata.LST}.sexagesimal(true)));
logger.logError(std::format("AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.target.ZD}.sexagesimal(),
mcc::MccAngle{tdata.target.ALT}.sexagesimal()));
return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE;
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
MccCelestialPoint cpt;
double min_time_to_pzone_in_secs;
@@ -207,11 +240,6 @@ public:
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
if (*_stopSlewing) {
logger.logDebug("slewing was stopped!");
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
typename CONTROLS_T::hardware_state_t hw_state;
auto hw_err = controls->hardwareGetState(&hw_state);
@@ -242,16 +270,18 @@ public:
logger.logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
logger.logDebug(std::format(" target: HA = {}, DEC = {}",
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(),
mcc::MccAngle{tdata.target.HA}.sexagesimal(true)));
logger.logDebug(std::format(" mount: HA = {}, DEC = {}", mcc::MccAngle{tdata.DEC_APP}.sexagesimal(),
mcc::MccAngle{tdata.HA}.sexagesimal(true)));
logger.logDebug(std::format(" entered target: HA = {}, DEC = {}",
mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal()));
logger.logDebug(std::format(" current mount: HA = {}, DEC = {}",
mcc::MccAngle{tdata.HA}.sexagesimal(true),
mcc::MccAngle{tdata.DEC_APP}.sexagesimal()));
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
logger.logDebug(std::format(" target: AZ = {}, ZD = {}",
logger.logDebug(std::format(" entered target: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.target.ZD}.sexagesimal()));
logger.logDebug(std::format(" mount: AZ = {}, ZD = {}", mcc::MccAngle{tdata.AZ}.sexagesimal(),
logger.logDebug(std::format(" current mount: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.AZ}.sexagesimal(),
mcc::MccAngle{tdata.ZD}.sexagesimal()));
}
@@ -261,6 +291,8 @@ public:
return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
logger.logDebug(" the 'hardwareSetState' method performed successfully!");
// std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
// mcc_tp2tp(hw_state.time_point, start_slewing_tp); // not compiled!!
@@ -319,10 +351,10 @@ public:
cpt.Y = -DEG90INRADS;
}
logger.logTrace(std::format(" target: HA = {}, DEC = {}",
logger.logTrace(std::format(" current target: HA = {}, DEC = {}",
mcc::MccAngle(tdata.target.HA).sexagesimal(true),
mcc::MccAngle(tdata.target.DEC_APP).sexagesimal()));
logger.logTrace(std::format(" mount: HA = {}, DEC = {}",
logger.logTrace(std::format(" current mount: HA = {}, DEC = {}",
mcc::MccAngle(tdata.HA).sexagesimal(true),
mcc::MccAngle(tdata.DEC_APP).sexagesimal()));
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
@@ -409,6 +441,8 @@ public:
*_stopSlewing = true;
return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
logger.logDebug(" the 'hardwareSetState' method performed successfully!");
}
}
@@ -416,6 +450,34 @@ public:
logger.logInfo("Slewing finished");
// wait for updated telemetry data
{
std::lock_guard lock{*_currentParamsMutex};
t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) {
*_stopSlewing = true;
return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
}
}
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
logger.logDebug(std::format(" entered target: HA = {}, DEC = {}",
mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal()));
logger.logDebug(std::format(" current mount: HA = {}, DEC = {}",
mcc::MccAngle{tdata.HA}.sexagesimal(true),
mcc::MccAngle{tdata.DEC_APP}.sexagesimal()));
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
logger.logDebug(std::format(" entered target: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.target.ZD}.sexagesimal()));
logger.logDebug(std::format(" current mount: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.AZ}.sexagesimal(),
mcc::MccAngle{tdata.ZD}.sexagesimal()));
}
return MccSimpleSlewingModelErrorCode::ERROR_OK;
};
}
@@ -474,6 +536,7 @@ public:
protected:
std::function<error_t(bool)> _slewingFunc{};
std::unique_ptr<std::atomic_bool> _stopSlewing;
std::function<error_t()> _checkTargetFunc{};
slewing_params_t _currentParams{};
std::unique_ptr<std::mutex> _currentParamsMutex{};