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-09-01 18:25:47 +03:00
parent 227f501d6f
commit 3d3b57a311
4 changed files with 411 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

185
mcc/mcc_slewing_model.h
View File

@@ -8,7 +8,7 @@
#include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_moving_model_common.h"
namespace mcc
{
@@ -24,7 +24,9 @@ enum class MccSimpleSlewingModelErrorCode : int {
ERROR_PZONE_CONTAINER_COMP,
ERROR_IN_PZONE,
ERROR_NEAR_PZONE,
ERROR_UNEXPECTED_AXIS_RATES
ERROR_TIMEOUT,
ERROR_UNEXPECTED_AXIS_RATES,
ERROR_STOPPED
};
} // namespace mcc
@@ -54,38 +56,37 @@ class MccSimpleSlewingModel
public:
typedef std::error_code error_t;
struct slewing_params_t {
bool slewAndStop{false}; // slew to target and stop mount
typedef MccSimpleMovingModelParams slewing_params_t;
std::chrono::seconds telemetryTimeout{3};
// struct slewing_params_t {
// bool slewAndStop{false}; // slew to target and stop mount
// minimal time to prohibited zone at current speed. if it is lesser then exit with error
std::chrono::seconds minTimeToPZone{10};
// std::chrono::seconds telemetryTimeout{3};
// target-mount coordinate difference to start adjusting of slewing (in radians)
double adjustCoordDiff{10.0_degs};
// // minimal time to prohibited zone at current speed. if it is lesser then exit with error
// std::chrono::seconds minTimeToPZone{10};
// coordinates difference to stop slewing (in radians)
double slewToleranceRadius{5.0_arcsecs};
// // target-mount coordinate difference to start adjusting of slewing (in radians)
// double adjustCoordDiff{10.0_degs};
// slew process timeout
std::chrono::seconds slewTimeout{3600};
// // coordinates difference to stop slewing (in radians)
// double slewToleranceRadius{5.0_arcsecs};
double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
// // slew process timeout
// std::chrono::seconds slewTimeout{3600};
double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
};
// double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
// double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
template <mcc_telemetry_data_c TelemetryT,
mcc_hardware_c HardwareT,
mcc_PCM_c PcmT,
mcc_pzone_container_c PZoneContT>
MccSimpleSlewingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont)
// double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
// double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
// };
template <mcc_telemetry_data_c TelemetryT, mcc_hardware_c HardwareT, mcc_pzone_container_c PZoneContT>
MccSimpleSlewingModel(TelemetryT* telemetry, HardwareT* hardware, PZoneContT* pz_cont)
: _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex)
{
_slewingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t {
_slewingFunc = [telemetry, hardware, pz_cont, this]() -> error_t {
*_stopSlewing = false;
// first, check target coordinates
@@ -111,6 +112,10 @@ public:
return MccSimpleSlewingModelErrorCode::ERROR_IN_PZONE;
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
MccCelestialPoint cpt;
mcc_tp2tp(tdata.time_point, cpt.time_point);
@@ -128,6 +133,10 @@ public:
return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
typename HardwareT::hardware_state_t hw_state;
auto hw_err = hardware->hardwareGetState(&hw_state);
@@ -145,18 +154,30 @@ public:
}
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_SLEWING;
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
// start slewing
hw_err = hardware->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
double dist, dx, dy, sinY;
std::chrono::system_clock::time_point start_slewing_tp, last_adjust_tp;
mcc_tp2tp(hw_state.time_point, start_slewing_tp);
double dist, dx, dy, sinY, rate2, xrate;
std::chrono::duration<double> dtx, dty; // seconds in double
while (!*_stopSlewing) {
bool adjust_mode = false;
static constexpr auto sideral_rate2 = slewing_params_t::sideralRate * slewing_params_t::sideralRate;
while (true) {
// wait for updated telemetry data
{
std::lock_guard lock{*_currentParamsMutex};
t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) {
@@ -164,34 +185,57 @@ public:
}
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
// compute time to prohibited zones at current speed
for (auto const& pt : isct_pt) {
if (std::isfinite(pt.X) && std::isfinite(pt.Y)) {
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
sinY = sin(std::numbers::pi / 2.0 - tdata.DEC_APP);
// sinY = sin(std::numbers::pi / 2.0 - tdata.DEC_APP);
dx = pt.X - tdata.HA;
dy = pt.Y - tdata.DEC_APP;
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
sinY = sin(tdata.ZD);
// sinY = sin(tdata.ZD);
dx = pt.X - tdata.AZ;
dy = pt.Y - tdata.ZD;
}
if (utils::isEqual(sinY, 0.0)) {
dtx = decltype(dtx){std::numeric_limits<double>::infinity()};
} else {
dtx = decltype(dtx){std::abs(dx / tdata.speedX / sinY)};
}
// if (utils::isEqual(sinY, 0.0)) {
// dtx = decltype(dtx){std::numeric_limits<double>::infinity()};
// rate2 = std::numeric_limits<double>::infinity();
// } else {
// xrate = tdata.speedX * sinY;
// dtx = decltype(dtx){std::abs(dx / xrate)};
// }
dtx = decltype(dtx){std::abs(dx / tdata.speedX)};
dty = decltype(dty){std::abs(dy / tdata.speedY)};
if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) {
return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
{
std::lock_guard lock{*_currentParamsMutex};
if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) {
return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
}
}
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
}
auto hw_err = hardware->hardwareGetState(&hw_state);
{
std::lock_guard lock{*_currentParamsMutex};
if ((std::chrono::system_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) {
return MccSimpleSlewingModelErrorCode::ERROR_TIMEOUT;
}
}
hw_err = hardware->hardwareGetState(&hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
}
@@ -201,17 +245,72 @@ public:
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
}
if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle
break;
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
if (dist <= _currentParams.adjustCoordDiff) {
{
std::lock_guard lock{*_currentParamsMutex};
if (adjust_mode && !_currentParams.slewAndStop) {
// do not allow mount speed fall below sideral
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
if (tdata.speedX < slewing_params_t::sideralRate) {
hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y;
hw_state.speedX = slewing_params_t::sideralRate;
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
hw_err = hardware->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err,
MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
}
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!!");
}
}
if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle
if (hw_state.moving_type ==
HardwareT::hardware_moving_state_t::HW_MOVE_STOPPED) { // mount was stopped
break;
}
}
if (dist <= _currentParams.adjustCoordDiff) { // adjust mount pointing
if ((std::chrono::system_clock::now() - last_adjust_tp) < _currentParams.adjustCycleInterval) {
continue;
}
hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y;
hw_state.speedX = _currentParams.adjustXRate;
hw_state.speedY = _currentParams.adjustYRate;
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_ADJUSTING;
hw_err = hardware->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
last_adjust_tp = std::chrono::system_clock::now();
adjust_mode = true;
} else {
adjust_mode = false;
}
}
// check for current axis speed
if (utils::isEqual(tdata.speedX, 0.0) && utils::isEqual(tdata.speedY, 0.0)) {
// unhandled stop state?!!!
return MccSimpleSlewingModelErrorCode::ERROR_UNEXPECTED_AXIS_RATES;
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
}