374 lines
13 KiB
C++
374 lines
13 KiB
C++
#pragma once
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
|
|
/* SIMPLE SLEWING MODEL IMPLEMENTATION */
|
|
|
|
|
|
#include "mcc_defaults.h"
|
|
#include "mcc_generics.h"
|
|
#include "mcc_moving_model_common.h"
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
enum class MccSimpleSlewingModelErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_HW_GETSTATE,
|
|
ERROR_HW_SETSTATE,
|
|
ERROR_PCM_COMP,
|
|
ERROR_GET_TELEMETRY,
|
|
ERROR_DIST_TELEMETRY,
|
|
ERROR_DIFF_TELEMETRY,
|
|
ERROR_PZONE_CONTAINER_COMP,
|
|
ERROR_IN_PZONE,
|
|
ERROR_NEAR_PZONE,
|
|
ERROR_TIMEOUT,
|
|
ERROR_UNEXPECTED_AXIS_RATES,
|
|
ERROR_STOPPED
|
|
};
|
|
|
|
} // namespace mcc
|
|
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::MccSimpleSlewingModelErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
/*
|
|
The target celestial point must be set in telemetry->target
|
|
*/
|
|
|
|
class MccSimpleSlewingModel
|
|
{
|
|
public:
|
|
typedef std::error_code error_t;
|
|
|
|
typedef MccSimpleMovingModelParams slewing_params_t;
|
|
|
|
// struct slewing_params_t {
|
|
// bool slewAndStop{false}; // slew to target and stop mount
|
|
|
|
// std::chrono::seconds telemetryTimeout{3};
|
|
|
|
// // minimal time to prohibited zone at current speed. if it is lesser then exit with error
|
|
// std::chrono::seconds minTimeToPZone{10};
|
|
|
|
// // target-mount coordinate difference to start adjusting of slewing (in radians)
|
|
// double adjustCoordDiff{10.0_degs};
|
|
|
|
// // coordinates difference to stop slewing (in radians)
|
|
// double slewToleranceRadius{5.0_arcsecs};
|
|
|
|
// // slew process timeout
|
|
// std::chrono::seconds slewTimeout{3600};
|
|
|
|
// 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)
|
|
|
|
// 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, pz_cont, this]() -> error_t {
|
|
// first, check target coordinates
|
|
typename TelemetryT::error_t t_err;
|
|
MccTelemetryData tdata;
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
t_err = telemetry->telemetryData(&tdata);
|
|
|
|
if (t_err) {
|
|
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
|
|
}
|
|
}
|
|
|
|
bool in_zone;
|
|
auto pz_err = pz_cont->inPZone(tdata.target, &in_zone);
|
|
if (pz_err) {
|
|
return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
|
|
if (in_zone) {
|
|
return MccSimpleSlewingModelErrorCode::ERROR_IN_PZONE;
|
|
}
|
|
|
|
if (*_stopSlewing) {
|
|
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
|
|
}
|
|
|
|
MccCelestialPoint cpt;
|
|
mcc_tp2tp(tdata.time_point, cpt.time_point);
|
|
|
|
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
|
|
cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
|
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
|
|
cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
|
} else {
|
|
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
|
}
|
|
|
|
std::vector<MccCelestialPoint> isct_pt(pz_cont->sizePZones, cpt);
|
|
pz_err = pz_cont->intersectPZone(tdata.target, &isct_pt);
|
|
if (pz_err) {
|
|
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);
|
|
if (hw_err) {
|
|
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
|
|
}
|
|
|
|
hw_state.X = (double)tdata.target.X;
|
|
hw_state.Y = (double)tdata.target.Y;
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
hw_state.speedX = _currentParams.slewXRate;
|
|
hw_state.speedY = _currentParams.slewYRate;
|
|
}
|
|
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);
|
|
}
|
|
|
|
std::chrono::steady_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
|
|
|
|
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) {
|
|
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
|
|
}
|
|
}
|
|
|
|
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);
|
|
dx = pt.X - tdata.HA;
|
|
dy = pt.Y - tdata.DEC_APP;
|
|
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
|
|
// 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()};
|
|
// 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)};
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) {
|
|
return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (*_stopSlewing) {
|
|
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
|
|
}
|
|
}
|
|
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
if ((std::chrono::steady_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);
|
|
}
|
|
|
|
t_err = telemetry->targetToMountDist(&dist);
|
|
if (t_err) {
|
|
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
|
|
}
|
|
|
|
if (*_stopSlewing) {
|
|
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
|
|
}
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
if (adjust_mode && !_currentParams.slewAndStop) {
|
|
// do not allow mount speed fall below sideral
|
|
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
|
|
// turn on sideral rate only if the current position point catches up with the target
|
|
if ((tdata.target.HA - tdata.HA) <= 0.0 && 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
|
|
auto now = std::chrono::steady_clock::now();
|
|
if ((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 = now;
|
|
|
|
adjust_mode = true;
|
|
|
|
} else {
|
|
adjust_mode = false;
|
|
}
|
|
}
|
|
|
|
if (*_stopSlewing) {
|
|
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
|
|
}
|
|
}
|
|
|
|
return MccSimpleSlewingModelErrorCode::ERROR_OK;
|
|
};
|
|
}
|
|
|
|
|
|
MccSimpleSlewingModel(MccSimpleSlewingModel&&) = default;
|
|
MccSimpleSlewingModel& operator=(MccSimpleSlewingModel&&) = default;
|
|
|
|
MccSimpleSlewingModel(const MccSimpleSlewingModel&) = delete;
|
|
MccSimpleSlewingModel& operator=(const MccSimpleSlewingModel&) = delete;
|
|
|
|
|
|
virtual ~MccSimpleSlewingModel() = default;
|
|
|
|
error_t slewToTarget()
|
|
{
|
|
*_stopSlewing = false;
|
|
|
|
return _slewingFunc();
|
|
}
|
|
|
|
|
|
error_t stopSlewing()
|
|
{
|
|
*_stopSlewing = true;
|
|
|
|
return MccSimpleSlewingModelErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
error_t setSlewingParams(slewing_params_t pars)
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
_currentParams = std::move(pars);
|
|
|
|
return MccSimpleSlewingModelErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
slewing_params_t getSlewingParams() const
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
return _currentParams;
|
|
}
|
|
|
|
protected:
|
|
std::function<error_t()> _slewingFunc{};
|
|
std::unique_ptr<std::atomic_bool> _stopSlewing;
|
|
|
|
slewing_params_t _currentParams{};
|
|
std::unique_ptr<std::mutex> _currentParamsMutex{};
|
|
};
|
|
|
|
|
|
} // namespace mcc
|