...
This commit is contained in:
parent
c6b47d8ad6
commit
6214b82a6c
@ -135,6 +135,10 @@ static auto Asibfm700MountConfigDefaults = std::make_tuple(
|
||||
// coordinates difference in arcsecs to stop slewing
|
||||
simple_config_record_t{"slewToleranceRadius", 5.0, {"coordinates difference in arcsecs to stop slewing"}},
|
||||
|
||||
simple_config_record_t{"slewingTelemetryInterval",
|
||||
std::chrono::milliseconds(100),
|
||||
{"telemetry request interval (in millisecs) in slewing mode"}},
|
||||
|
||||
// target-mount coordinate difference in arcsecs to start adjusting of slewing
|
||||
simple_config_record_t{"adjustCoordDiff",
|
||||
50.0,
|
||||
@ -154,6 +158,12 @@ static auto Asibfm700MountConfigDefaults = std::make_tuple(
|
||||
std::chrono::milliseconds(10000),
|
||||
{"a time shift into future to compute target position in future (UT1-scale time duration, millisecs)"}},
|
||||
|
||||
|
||||
simple_config_record_t{"trackingTelemetryInterval",
|
||||
std::chrono::milliseconds(100),
|
||||
{"telemetry request interval (in millisecs) in tracking mode"}},
|
||||
|
||||
|
||||
// minimum time in millisecs between two successive tracking corrections
|
||||
simple_config_record_t{"trackingCycleInterval",
|
||||
std::chrono::milliseconds(300),
|
||||
@ -501,6 +511,10 @@ public:
|
||||
|
||||
mcc::MccSimpleMovingModelParams pars;
|
||||
|
||||
auto get_value = [&pars, this]<typename VT>(std::string_view name, VT& val) {
|
||||
val = getValue<VT>(name).value_or(val);
|
||||
};
|
||||
|
||||
pars.telemetryTimeout =
|
||||
getValue<decltype(pars.telemetryTimeout)>("telemetryTimeout").value_or(pars.telemetryTimeout);
|
||||
|
||||
@ -513,6 +527,8 @@ public:
|
||||
getValue<decltype(pars.slewToleranceRadius)>("slewToleranceRadius").value_or(pars.slewToleranceRadius) *
|
||||
arcsecs2rad;
|
||||
|
||||
get_value("slewingTelemetryInterval", pars.slewingTelemetryInterval);
|
||||
|
||||
pars.slewRateX = getValue<decltype(pars.slewRateX)>("hwMaxRateHA").value_or(pars.slewRateX);
|
||||
pars.slewRateY = getValue<decltype(pars.slewRateY)>("hwMaxRateDEC").value_or(pars.slewRateY);
|
||||
|
||||
@ -524,6 +540,8 @@ public:
|
||||
|
||||
pars.slewTimeout = getValue<decltype(pars.slewTimeout)>("slewTimeout").value_or(pars.slewTimeout);
|
||||
|
||||
get_value("trackingTelemetryInterval", pars.trackingTelemetryInterval);
|
||||
|
||||
pars.timeShiftToTargetPoint = getValue<decltype(pars.timeShiftToTargetPoint)>("timeShiftToTargetPoint")
|
||||
.value_or(pars.timeShiftToTargetPoint);
|
||||
|
||||
|
||||
@ -1124,7 +1124,7 @@ public:
|
||||
|
||||
// output format: <mount data>, speedX, speedY, pcmX, pcmY, refCorr (in arcsecs), <target data>
|
||||
// RA-APP_mnt, DEC-APP_mnt, HA_mnt, AZ_mnt, ZD_mnt, ALT_mnt, X_mnt, Y_mnt, COO-PAIR_mnt, TIME-POINT_mnt,
|
||||
// LST, SPEED_X_mnt, SPEED_Y_mnt, PCM_X, PCM_Y, REFCORR,
|
||||
// LST, EO, SPEED_X_mnt, SPEED_Y_mnt, PCM_X, PCM_Y, REFCORR,
|
||||
// RA-ICRS_tag, DEC-ICRS_tag, RA-APP_tag, DEC-APP_tag, HA_tag, AZ_tag, ZD_tag, ALT_tag, X_tag, Y_tag,
|
||||
// COO-PAIR_tag, TIME-POINT_tag
|
||||
|
||||
@ -1134,10 +1134,15 @@ public:
|
||||
|
||||
std::format_to(std::back_inserter(bytes), "{}", _delimiter);
|
||||
toSexagesimalHour(bytes, value.LST);
|
||||
|
||||
std::format_to(std::back_inserter(bytes), "{}", _delimiter);
|
||||
|
||||
toSexagesimalHour(bytes, value.EO);
|
||||
std::format_to(std::back_inserter(bytes), "{}", _delimiter);
|
||||
|
||||
// '*3600.0' to express refraction correction in arcseconds!
|
||||
toDegrees(bytes, value.speedX, value.speedY, value.pcmX, value.pcmY, value.refCorr * 3600.0);
|
||||
// toDegrees(bytes, value.speedX, value.speedY, value.pcmX, value.pcmY, value.refCorr * 3600.0);
|
||||
toSexagesimalDeg(bytes, value.speedX, value.speedY, value.pcmX, value.pcmY, value.refCorr);
|
||||
std::format_to(std::back_inserter(bytes), "{}", _delimiter);
|
||||
|
||||
pt_ser.setFormat(_currentFormat);
|
||||
@ -1158,7 +1163,7 @@ public:
|
||||
// valid format: <mount data>, speedX, speedY, pcmX, pcmY, refCorr, <target data>
|
||||
|
||||
auto els = splitToElements(std::forward<IR>(bytes));
|
||||
if (els.size() < 28) {
|
||||
if (els.size() < 29) {
|
||||
// return std::make_error_code(std::errc::invalid_argument);
|
||||
return MccCoordinateConvErrorCode::ERROR_ARG_LEN;
|
||||
}
|
||||
@ -1176,6 +1181,11 @@ public:
|
||||
return err;
|
||||
}
|
||||
|
||||
err = parseHourRepr(els[idx++], tdata.EO);
|
||||
if (err) {
|
||||
return err;
|
||||
}
|
||||
|
||||
err = parseDegreeRepr(els[idx++], tdata.speedX);
|
||||
if (err) {
|
||||
return err;
|
||||
|
||||
@ -37,6 +37,9 @@ struct MccSimpleMovingModelParams {
|
||||
// coordinates difference to stop slewing (in radians)
|
||||
double slewToleranceRadius{5.0_arcsecs};
|
||||
|
||||
// telemetry request interval
|
||||
std::chrono::milliseconds slewingTelemetryInterval{100};
|
||||
|
||||
// target-mount coordinate difference to start adjusting of slewing (in radians)
|
||||
double adjustCoordDiff{slewToleranceRadius * 10.0};
|
||||
|
||||
@ -59,6 +62,9 @@ struct MccSimpleMovingModelParams {
|
||||
|
||||
// ******* tracking mode *******
|
||||
|
||||
// telemetry request interval
|
||||
std::chrono::milliseconds trackingTelemetryInterval{100};
|
||||
|
||||
double trackSpeedX{};
|
||||
double trackSpeedY{};
|
||||
std::chrono::milliseconds trackingCycleInterval{500}; // minimum time between two successive tracking corrections
|
||||
|
||||
@ -391,6 +391,29 @@ public:
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
};
|
||||
|
||||
|
||||
// update thread
|
||||
// _updatingFuture = std::async(
|
||||
// std::launch::async,
|
||||
// [this](std::stop_token stoken) {
|
||||
// while (!stoken.stop_requested()) {
|
||||
// {
|
||||
// std::unique_lock ulock{*_updateMutex};
|
||||
|
||||
// bool ok = _updateCondVar->wait(ulock, [&stoken, this]() -> bool {
|
||||
// return _dataUpdatingRequested || stoken.stop_requested();
|
||||
// });
|
||||
// }
|
||||
|
||||
// if (!stoken.stop_requested()) {
|
||||
// std::lock_guard lock{*_timeoutMutex};
|
||||
|
||||
// _lastUpdateError = _updateFunc(stoken);
|
||||
// }
|
||||
// }
|
||||
// },
|
||||
// _internalUpdatingStopSource.get_token());
|
||||
}
|
||||
|
||||
|
||||
@ -712,18 +735,15 @@ protected:
|
||||
std::chrono::nanoseconds _currentUpdateTimeout{defaultInternalUpdateTimeout};
|
||||
|
||||
std ::function<error_t(std::stop_token)> _updateTargetFunc{};
|
||||
// std ::function<error_t(bool, std::stop_token)> _updateTargetFunc{};
|
||||
std::function<error_t(std::stop_token)> _updateFunc{};
|
||||
std::function<error_t()> _setTargetFunc{};
|
||||
|
||||
std::unique_ptr<std::mutex> _updateMutex;
|
||||
std::unique_ptr<std::condition_variable> _updateCondVar;
|
||||
|
||||
// std::future<void> _internalUpdatingLoopFuture{};
|
||||
// std::unique_ptr<std::mutex> _internalUpdatingLoopMutex{new std::mutex()};
|
||||
// std::unique_ptr<std::condition_variable> _internalUpdatingLoopCondVar{new std::condition_variable()};
|
||||
// std::unique_ptr<std::atomic_bool> _internalUpdatingLoopStop{new std::atomic_bool{false}};
|
||||
// std::unique_ptr<std::atomic_bool> _dataUpdatingRequested{new std::atomic_bool{false}};
|
||||
std::future<void> _updatingFuture{};
|
||||
std::unique_ptr<std::atomic_bool> _dataUpdatingRequested{new std::atomic_bool{false}};
|
||||
std::unique_ptr<std::timed_mutex> _timeoutMutex{new std::timed_mutex()};
|
||||
|
||||
error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK};
|
||||
};
|
||||
|
||||
734
mcc/mcc_telemetry.h.bad
Normal file
734
mcc/mcc_telemetry.h.bad
Normal file
@ -0,0 +1,734 @@
|
||||
#pragma once
|
||||
|
||||
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
||||
|
||||
|
||||
/* IMPLEMENTATION OF TELEMETRY CLASS */
|
||||
|
||||
|
||||
#include <condition_variable>
|
||||
#include <future>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
#include "mcc_defaults.h"
|
||||
|
||||
namespace mcc
|
||||
{
|
||||
|
||||
enum class MccTelemetryErrorCode : int {
|
||||
ERROR_OK,
|
||||
ERROR_NULLPTR,
|
||||
ERROR_COORD_TRANSFORM,
|
||||
ERROR_PCM_COMP,
|
||||
ERROR_HARDWARE_GETPOS,
|
||||
ERROR_UPDATE_STOPPED,
|
||||
ERROR_DATA_TIMEOUT,
|
||||
ERROR_UNSUPPORTED_COORD_PAIR
|
||||
};
|
||||
|
||||
} // namespace mcc
|
||||
|
||||
namespace std
|
||||
{
|
||||
|
||||
template <>
|
||||
class is_error_code_enum<mcc::MccTelemetryErrorCode> : public true_type
|
||||
{
|
||||
};
|
||||
|
||||
} // namespace std
|
||||
|
||||
|
||||
namespace mcc
|
||||
{
|
||||
|
||||
/* error category definition */
|
||||
|
||||
// error category
|
||||
struct MccTelemetryCategory : public std::error_category {
|
||||
MccTelemetryCategory() : std::error_category() {}
|
||||
|
||||
const char* name() const noexcept
|
||||
{
|
||||
return "MCC-TELEMETRY";
|
||||
}
|
||||
|
||||
std::string message(int ec) const
|
||||
{
|
||||
MccTelemetryErrorCode err = static_cast<MccTelemetryErrorCode>(ec);
|
||||
|
||||
switch (err) {
|
||||
case MccTelemetryErrorCode::ERROR_OK:
|
||||
return "OK";
|
||||
case MccTelemetryErrorCode::ERROR_NULLPTR:
|
||||
return "nullptr input argument";
|
||||
case MccTelemetryErrorCode::ERROR_COORD_TRANSFORM:
|
||||
return "coordinate transformation error";
|
||||
case MccTelemetryErrorCode::ERROR_PCM_COMP:
|
||||
return "PCM computation error";
|
||||
case MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS:
|
||||
return "cannot get hardware position";
|
||||
case MccTelemetryErrorCode::ERROR_UPDATE_STOPPED:
|
||||
return "telemetry update was stopped";
|
||||
case MccTelemetryErrorCode::ERROR_DATA_TIMEOUT:
|
||||
return "a timeout occured while waiting for new data";
|
||||
case MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR:
|
||||
return "unsupported coordinate pair";
|
||||
default:
|
||||
return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
static const MccTelemetryCategory& get()
|
||||
{
|
||||
static const MccTelemetryCategory constInst;
|
||||
return constInst;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
inline std::error_code make_error_code(MccTelemetryErrorCode ec)
|
||||
{
|
||||
return std::error_code(static_cast<int>(ec), MccTelemetryCategory::get());
|
||||
}
|
||||
|
||||
|
||||
|
||||
class MccTelemetry : public mcc_telemetry_interface_t<std::error_code>
|
||||
{
|
||||
protected:
|
||||
static constexpr uint16_t internalUpdatingIntervalDiv = 5;
|
||||
|
||||
public:
|
||||
static constexpr auto defaultUpdateInterval = std::chrono::milliseconds(100);
|
||||
static constexpr auto defaultInternalUpdateTimeout = defaultUpdateInterval * 5;
|
||||
|
||||
typedef std::error_code error_t;
|
||||
|
||||
|
||||
template <mcc_position_controls_c CONTROLS_T>
|
||||
MccTelemetry(CONTROLS_T* controls)
|
||||
: _isDataUpdated(new std::atomic_bool()),
|
||||
_data(),
|
||||
_userTarget(),
|
||||
_internalUpdating(new std::atomic_bool),
|
||||
_currentUpdateInterval(defaultUpdateInterval),
|
||||
_currentUpdateIntervalMutex(new std::mutex),
|
||||
_updateMutex(new std::mutex),
|
||||
_updateCondVar(new std::condition_variable)
|
||||
{
|
||||
*_isDataUpdated = false;
|
||||
*_internalUpdating = false;
|
||||
|
||||
|
||||
// using ccte_t = std::remove_cvref_t<decltype(*ccte)>;
|
||||
using pcm_t = std::remove_cvref_t<decltype(*controls)>;
|
||||
using hardware_t = std::remove_cvref_t<decltype(*controls)>;
|
||||
|
||||
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
} else {
|
||||
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
||||
}
|
||||
|
||||
_updateTargetFunc = [controls, this](std::stop_token stop_token) -> error_t {
|
||||
MccPCMResult pcm_res;
|
||||
|
||||
mcc_tp2tp(_data.time_point, _data.target.time_point);
|
||||
|
||||
bool hw_coords = _data.target.pair_kind == MccCoordPairKind::COORDS_KIND_XY;
|
||||
MccCelestialPoint hw_cp{.pair_kind = MccCoordPairKind::COORDS_KIND_XY};
|
||||
mcc_tp2tp(_data.time_point, hw_cp.time_point);
|
||||
|
||||
if (hw_coords) { // compute corresponded apparent coordinates
|
||||
hw_cp.X = _data.target.X;
|
||||
hw_cp.Y = _data.target.Y;
|
||||
|
||||
auto pcm_err = controls->computePCM(_data.target, &pcm_res, &_data.target);
|
||||
if (pcm_err) {
|
||||
return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
||||
}
|
||||
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
} else {
|
||||
static_assert(false, "UNKNOWN MOUNT TYPE!!!");
|
||||
}
|
||||
}
|
||||
|
||||
if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
||||
_data.target.X = _data.target.RA_ICRS;
|
||||
_data.target.Y = _data.target.DEC_ICRS;
|
||||
} else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||
_data.target.X = _data.target.AZ;
|
||||
_data.target.Y = _data.target.ZD;
|
||||
} else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
|
||||
_data.target.X = _data.target.AZ;
|
||||
_data.target.Y = _data.target.ALT;
|
||||
} else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
||||
_data.target.X = _data.target.HA;
|
||||
_data.target.Y = _data.target.DEC_APP;
|
||||
} else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
||||
_data.target.X = _data.target.RA_APP;
|
||||
_data.target.Y = _data.target.DEC_APP;
|
||||
} else {
|
||||
return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||
}
|
||||
|
||||
auto ccte_err = controls->transformCoordinates(_data.target, &_data.target);
|
||||
if (ccte_err) {
|
||||
if (hw_coords) { // restore coordinates pair kind
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY;
|
||||
}
|
||||
|
||||
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
if (_data.target.pair_kind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
||||
// fixed apparent coordinates (AZZD or HADEC)
|
||||
// needs to compute ICRS
|
||||
// (.X and .Y are already assigned above!)
|
||||
|
||||
|
||||
// if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||
// _data.target.X = _data.target.AZ;
|
||||
// _data.target.Y = _data.target.ZD;
|
||||
// } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
|
||||
// _data.target.X = _data.target.AZ;
|
||||
// _data.target.Y = _data.target.ALT;
|
||||
// } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
||||
// _data.target.X = _data.target.HA;
|
||||
// _data.target.Y = _data.target.DEC_APP;
|
||||
// } else if (_data.target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
||||
// _data.target.X = _data.target.RA_APP;
|
||||
// _data.target.Y = _data.target.DEC_APP;
|
||||
// } else {
|
||||
// return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||
// }
|
||||
|
||||
MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
|
||||
ccte_err = controls->transformCoordinates(_data.target, &pt);
|
||||
if (ccte_err) {
|
||||
if (hw_coords) { // restore coordinates pair kind
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY;
|
||||
_data.target.X = hw_cp.X;
|
||||
_data.target.Y = hw_cp.Y;
|
||||
}
|
||||
|
||||
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
_data.target.RA_ICRS = pt.X;
|
||||
_data.target.DEC_ICRS = pt.Y;
|
||||
} // from ICRS to apparent calculation is already performed above
|
||||
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
// hardware coordinates
|
||||
if (!hw_coords) {
|
||||
auto pcm_err = controls->computeInversePCM(_data.target, &pcm_res, &_data.target);
|
||||
if (pcm_err) {
|
||||
return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
||||
}
|
||||
} else { // restore coordinates pair kind
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_XY;
|
||||
_data.target.X = hw_cp.X;
|
||||
_data.target.Y = hw_cp.Y;
|
||||
}
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
};
|
||||
|
||||
_updateFunc = [controls, this](std::stop_token stop_token) -> std::error_code {
|
||||
// first, update mount quantities
|
||||
typename hardware_t::hardware_state_t hw_pos;
|
||||
auto hw_err = controls->hardwareGetState(&hw_pos);
|
||||
if (hw_err) {
|
||||
return mcc_deduce_error_code(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS);
|
||||
}
|
||||
|
||||
// if (stop_token.stop_requested()) {
|
||||
// return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
// }
|
||||
|
||||
double eo;
|
||||
|
||||
_data.time_point =
|
||||
std::chrono::time_point_cast<typename decltype(_data.time_point)::duration>(hw_pos.time_point);
|
||||
|
||||
auto ccte_err = controls->timepointToJulday(_data.time_point, &_data.JD);
|
||||
if (!ccte_err) {
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
ccte_err = controls->juldayToAppSideral(_data.JD, &_data.LST, true);
|
||||
if (!ccte_err) {
|
||||
ccte_err = controls->equationOrigins(_data.JD, &eo);
|
||||
_data.EO = eo;
|
||||
}
|
||||
}
|
||||
|
||||
if (ccte_err) {
|
||||
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
|
||||
// if (stop_token.stop_requested()) {
|
||||
// return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
// }
|
||||
|
||||
_data.X = (double)hw_pos.X;
|
||||
_data.Y = (double)hw_pos.Y;
|
||||
_data.speedX = (double)hw_pos.speedX;
|
||||
_data.speedY = (double)hw_pos.speedY;
|
||||
|
||||
// fill _data.pcmX, _data.pcmY and corresponded apparent coordinates
|
||||
auto pcm_err = controls->computePCM(_data, &_data, &_data);
|
||||
if (pcm_err) {
|
||||
return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
||||
}
|
||||
|
||||
// if (stop_token.stop_requested()) {
|
||||
// return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
// }
|
||||
|
||||
MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = _data.time_point};
|
||||
|
||||
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
|
||||
_data.RA_APP =
|
||||
MccAngle((double)_data.LST - (double)_data.HA - eo).normalize<MccAngle::NORM_KIND_0_360>();
|
||||
// MccAngle((double)_data.LST - (double)_data.HA + eo).normalize<MccAngle::NORM_KIND_0_360>();
|
||||
|
||||
_data.X = _data.HA;
|
||||
_data.Y = _data.DEC_APP;
|
||||
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
|
||||
ccte_err = controls->transformCoordinates(_data, &pt);
|
||||
if (!ccte_err) {
|
||||
_data.AZ = pt.X;
|
||||
_data.ALT = pt.Y;
|
||||
_data.ZD = std::numbers::pi / 2.0 - _data.ALT;
|
||||
}
|
||||
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
|
||||
_data.ALT = std::numbers::pi / 2.0 - _data.ZD;
|
||||
|
||||
_data.X = _data.AZ;
|
||||
_data.Y = _data.ZD;
|
||||
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
|
||||
pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
ccte_err = controls->transformCoordinates(_data, &pt);
|
||||
if (!ccte_err) {
|
||||
_data.HA = pt.X;
|
||||
_data.DEC_APP = pt.Y;
|
||||
_data.RA_APP =
|
||||
MccAngle((double)_data.LST - (double)_data.HA - eo).normalize<MccAngle::NORM_KIND_0_360>();
|
||||
// MccAngle((double)_data.LST - (double)_data.HA + eo).normalize<MccAngle::NORM_KIND_0_360>();
|
||||
}
|
||||
|
||||
} else {
|
||||
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
||||
}
|
||||
|
||||
if (!ccte_err) {
|
||||
// if (stop_token.stop_requested()) {
|
||||
// return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
// }
|
||||
|
||||
// to compute refraction coefficients
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
_data.X = _data.AZ;
|
||||
_data.Y = _data.ZD;
|
||||
|
||||
ccte_err = controls->refractionCorrection(_data, &_data.refCorr);
|
||||
if (!ccte_err) {
|
||||
// restore hardware encoders coordinates
|
||||
_data.X = (double)hw_pos.X;
|
||||
_data.Y = (double)hw_pos.Y;
|
||||
|
||||
// update target (assuming target ICRS coordinates are already set)
|
||||
|
||||
// auto ret = _updateTargetFunc(false, stop_token);
|
||||
// update target according to its .pair_kind!
|
||||
auto ret = _updateTargetFunc(stop_token);
|
||||
if (ret) {
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// restore according to the mount type
|
||||
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
|
||||
} else {
|
||||
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
||||
}
|
||||
|
||||
|
||||
if (ccte_err) {
|
||||
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
MccTelemetry(MccTelemetry&&) = default;
|
||||
MccTelemetry& operator=(MccTelemetry&&) = default;
|
||||
|
||||
MccTelemetry(const MccTelemetry&) = delete;
|
||||
MccTelemetry& operator=(const MccTelemetry&) = delete;
|
||||
|
||||
|
||||
virtual ~MccTelemetry()
|
||||
{
|
||||
stopInternalTelemetryDataUpdating();
|
||||
|
||||
if (_internalUpdatingFuture.valid()) {
|
||||
// try to exit correctly
|
||||
// auto status = _internalUpdatingFuture.wait_for(std::chrono::seconds(1));
|
||||
_internalUpdatingFuture.wait_for(std::chrono::seconds(1));
|
||||
// _internalUpdatingFuture.get();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
template <traits::mcc_time_duration_c DT>
|
||||
DT telemetryDataUpdateInterval() const
|
||||
{
|
||||
std::lock_guard lock{*_currentUpdateIntervalMutex};
|
||||
|
||||
return std::chrono::duration_cast<DT>(_currentUpdateInterval);
|
||||
}
|
||||
|
||||
std::chrono::milliseconds telemetryDataUpdateInterval() const
|
||||
{
|
||||
return telemetryDataUpdateInterval<std::chrono::milliseconds>();
|
||||
}
|
||||
|
||||
void setTelemetryDataUpdateInterval(traits::mcc_time_duration_c auto const& interval)
|
||||
{
|
||||
using d_t = std::remove_cvref_t<decltype(interval)>;
|
||||
|
||||
std::lock_guard lock{*_currentUpdateIntervalMutex};
|
||||
|
||||
if constexpr (std::floating_point<typename d_t::rep>) {
|
||||
_currentUpdateInterval = utils::isEqual(interval.count(), 0.0) ? defaultUpdateInterval : interval;
|
||||
} else {
|
||||
_currentUpdateInterval = interval.count() == 0 ? defaultUpdateInterval : interval;
|
||||
}
|
||||
}
|
||||
|
||||
void setTelemetryUpdateTimeout(traits::mcc_time_duration_c auto const& timeout)
|
||||
{
|
||||
if (timeout.count() > 0) {
|
||||
_currentUpdateTimeout = std::chrono::duration_cast<decltype(_currentUpdateTimeout)>(timeout);
|
||||
}
|
||||
}
|
||||
|
||||
auto getTelemetryUpdateTimeout() const
|
||||
{
|
||||
return _currentUpdateTimeout;
|
||||
}
|
||||
|
||||
// asynchronuosly periodicaly update telemetry data (internal synchronization)
|
||||
void startInternalTelemetryDataUpdating()
|
||||
{
|
||||
using intv_t = std::remove_cvref_t<decltype(_currentUpdateInterval)>;
|
||||
|
||||
_internalUpdatingStopSource = std::stop_source{}; // reset state
|
||||
|
||||
*_internalUpdating = true;
|
||||
|
||||
_internalUpdatingFuture = std::async(
|
||||
std::launch::async,
|
||||
[this](std::stop_token stop_token) -> error_t {
|
||||
while (!stop_token.stop_requested()) {
|
||||
// while (true) {
|
||||
_lastUpdateError = updateTelemetryData(_currentUpdateTimeout);
|
||||
if (_lastUpdateError) {
|
||||
*_internalUpdating = false;
|
||||
return _lastUpdateError;
|
||||
}
|
||||
|
||||
// auto nn = std::this_thread::get_id();
|
||||
|
||||
std::this_thread::sleep_for(_currentUpdateInterval);
|
||||
|
||||
// {
|
||||
// std::lock_guard lock{*_currentUpdateIntervalMutex};
|
||||
|
||||
// // compute it here because of possible changing _currentUpdateInterval
|
||||
// auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv;
|
||||
|
||||
// for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) {
|
||||
// if (stop_token.stop_requested()) {
|
||||
// break;
|
||||
// }
|
||||
|
||||
// std::this_thread::sleep_for(sleep_td);
|
||||
// }
|
||||
|
||||
// if (stop_token.stop_requested()) {
|
||||
// break;
|
||||
// }
|
||||
|
||||
// if constexpr (std::floating_point<intv_t>) {
|
||||
// std::this_thread::sleep_for(sleep_td);
|
||||
// } else {
|
||||
// auto rem = _currentUpdateInterval % internalUpdatingIntervalDiv;
|
||||
|
||||
// if (rem.count()) {
|
||||
// std::this_thread::sleep_for(rem);
|
||||
// } else {
|
||||
// std::this_thread::sleep_for(sleep_td);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
}
|
||||
|
||||
*_internalUpdating = false;
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
},
|
||||
_internalUpdatingStopSource.get_token());
|
||||
}
|
||||
|
||||
|
||||
void stopInternalTelemetryDataUpdating()
|
||||
{
|
||||
_internalUpdatingStopSource.request_stop();
|
||||
*_internalUpdating = false;
|
||||
}
|
||||
|
||||
|
||||
bool isInternalTelemetryDataUpdating() const
|
||||
{
|
||||
return *_internalUpdating;
|
||||
}
|
||||
|
||||
|
||||
error_t updateTelemetryData(traits::mcc_time_duration_c auto const& timeout)
|
||||
{
|
||||
{
|
||||
std::lock_guard thread_lock{*_updateMutex};
|
||||
|
||||
std::stop_source stop_source;
|
||||
|
||||
*_isDataUpdated = false;
|
||||
|
||||
// std::future<error_t> update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token());
|
||||
// // std::future<error_t> update_ft =
|
||||
// // std::async(std::launch::async, _updateFunc, _internalUpdatingStopSource.get_token());
|
||||
// auto status = update_ft.wait_for(timeout);
|
||||
|
||||
// if (status == std::future_status::ready) {
|
||||
// *_isDataUpdated = true;
|
||||
// _lastUpdateError = update_ft.get();
|
||||
// } else if (status == std::future_status::deferred) { // std::async was invoked in this thread, get
|
||||
// result
|
||||
// _lastUpdateError = update_ft.get();
|
||||
// if (!_lastUpdateError) {
|
||||
// *_isDataUpdated = true;
|
||||
// }
|
||||
// } else { // timeout
|
||||
// stop_source.request_stop();
|
||||
// _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
|
||||
// }
|
||||
|
||||
_lastUpdateError = _updateFunc(_internalUpdatingStopSource.get_token());
|
||||
*_isDataUpdated = true;
|
||||
}
|
||||
|
||||
// unblock waiting threads even in the case of timeout!
|
||||
_updateCondVar->notify_all();
|
||||
|
||||
// *_isDataUpdated = false;
|
||||
|
||||
return _lastUpdateError;
|
||||
}
|
||||
|
||||
// block the thread and wait for data to be ready (internal synchronization)
|
||||
error_t waitForTelemetryData(mcc_telemetry_data_c auto* tdata, traits::mcc_time_duration_c auto const& timeout)
|
||||
{
|
||||
if (tdata == nullptr) {
|
||||
return MccTelemetryErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
std::unique_lock ulock(*_updateMutex);
|
||||
|
||||
auto res = _updateCondVar->wait_for(ulock, timeout, [this]() -> bool { return *_isDataUpdated; });
|
||||
if (!res) {
|
||||
return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
|
||||
}
|
||||
|
||||
// std::lock_guard thread_lock{*_updateMutex};
|
||||
|
||||
if (!_lastUpdateError) {
|
||||
mcc_copy_telemetry_data(_data, tdata);
|
||||
}
|
||||
|
||||
return _lastUpdateError;
|
||||
}
|
||||
|
||||
// just get current data
|
||||
error_t telemetryData(mcc_telemetry_data_c auto* tdata)
|
||||
{
|
||||
if (tdata == nullptr) {
|
||||
return MccTelemetryErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
std::lock_guard thread_lock{*_updateMutex};
|
||||
|
||||
mcc_copy_telemetry_data(_data, tdata);
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
error_t lastUpdateError() const
|
||||
{
|
||||
return _lastUpdateError;
|
||||
}
|
||||
|
||||
error_t setPointingTarget(mcc_celestial_point_c auto pt)
|
||||
{
|
||||
/*
|
||||
* If apparent coordinates are specified (e.g. AZZD),
|
||||
* they are assumed to be fixed in time (i.e. pt.time_point will be ignored),
|
||||
* and other coordinates will be calculated from them
|
||||
*/
|
||||
|
||||
std::lock_guard lock{*_updateMutex};
|
||||
|
||||
mcc_copy_celestial_point(pt, &_userTarget);
|
||||
|
||||
|
||||
_data.target.pair_kind = pt.pair_kind;
|
||||
if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) {
|
||||
_data.target.AZ = pt.X;
|
||||
_data.target.ALT = pt.Y;
|
||||
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||
_data.target.AZ = pt.X;
|
||||
_data.target.ZD = pt.Y;
|
||||
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) {
|
||||
_data.target.HA = pt.X;
|
||||
_data.target.DEC_APP = pt.Y;
|
||||
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) {
|
||||
_data.target.RA_APP = pt.X;
|
||||
_data.target.DEC_APP = pt.Y;
|
||||
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
||||
_data.target.RA_ICRS = pt.X;
|
||||
_data.target.DEC_ICRS = pt.Y;
|
||||
} else if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_XY) {
|
||||
_data.target.X = pt.X;
|
||||
_data.target.Y = pt.Y;
|
||||
} else {
|
||||
return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||
}
|
||||
|
||||
return _updateTargetFunc({});
|
||||
|
||||
// return _setTargetFunc(pt);
|
||||
}
|
||||
|
||||
|
||||
|
||||
error_t targetToMountDiff(MccCoordPairKind pair_kind, mcc_angle_c auto* dx, mcc_angle_c auto* dy)
|
||||
{
|
||||
std::lock_guard lock{*_updateMutex};
|
||||
|
||||
if (pair_kind == MccCoordPairKind::COORDS_KIND_AZALT || pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||
*dx = (double)_data.target.AZ - (double)_data.AZ;
|
||||
*dy = (double)_data.target.ALT - (double)_data.ALT;
|
||||
} else if (pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP ||
|
||||
pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP ||
|
||||
pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
||||
*dx = (double)_data.target.HA - (double)_data.HA;
|
||||
*dy = (double)_data.target.DEC_APP - (double)_data.DEC_APP;
|
||||
} else {
|
||||
return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||
}
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
|
||||
error_t targetToMountDist(mcc_angle_c auto* dist)
|
||||
{
|
||||
if (dist == nullptr) {
|
||||
return MccTelemetryErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
std::lock_guard lock{*_updateMutex};
|
||||
|
||||
double dHA = _data.HA - _data.target.HA;
|
||||
double cosDHA = cos(dHA);
|
||||
|
||||
double cosT = cos(_data.target.DEC_APP);
|
||||
double sinT = sin(_data.target.DEC_APP);
|
||||
double cosM = cos(_data.DEC_APP);
|
||||
double sinM = sin(_data.DEC_APP);
|
||||
|
||||
double term1 = cosT * sin(dHA);
|
||||
double term2 = cosM * sinT - sinM * cosT * cosDHA;
|
||||
|
||||
*dist = atan2(sqrt(term1 * term1 + term2 * term2), (sinM * sinT + cosM * cosT * cos(dHA)));
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
protected:
|
||||
std::unique_ptr<std::atomic_bool> _isDataUpdated;
|
||||
MccTelemetryData _data;
|
||||
|
||||
MccCelestialPoint _userTarget{};
|
||||
|
||||
std::unique_ptr<std::atomic_bool> _internalUpdating;
|
||||
std::chrono::nanoseconds _currentUpdateInterval{std::chrono::milliseconds(100)};
|
||||
std::unique_ptr<std::mutex> _currentUpdateIntervalMutex;
|
||||
std::future<error_t> _internalUpdatingFuture{};
|
||||
std::stop_source _internalUpdatingStopSource{};
|
||||
|
||||
std::chrono::nanoseconds _currentUpdateTimeout{defaultInternalUpdateTimeout};
|
||||
|
||||
std ::function<error_t(std::stop_token)> _updateTargetFunc{};
|
||||
// std ::function<error_t(bool, std::stop_token)> _updateTargetFunc{};
|
||||
std::function<error_t(std::stop_token)> _updateFunc{};
|
||||
std::function<error_t()> _setTargetFunc{};
|
||||
|
||||
std::unique_ptr<std::mutex> _updateMutex;
|
||||
std::unique_ptr<std::condition_variable> _updateCondVar;
|
||||
|
||||
// std::future<void> _internalUpdatingLoopFuture{};
|
||||
// std::unique_ptr<std::mutex> _internalUpdatingLoopMutex{new std::mutex()};
|
||||
// std::unique_ptr<std::condition_variable> _internalUpdatingLoopCondVar{new std::condition_variable()};
|
||||
// std::unique_ptr<std::atomic_bool> _internalUpdatingLoopStop{new std::atomic_bool{false}};
|
||||
// std::unique_ptr<std::atomic_bool> _dataUpdatingRequested{new std::atomic_bool{false}};
|
||||
|
||||
error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK};
|
||||
};
|
||||
|
||||
|
||||
static_assert(mcc_telemetry_c<MccTelemetry>, "");
|
||||
|
||||
} // namespace mcc
|
||||
@ -1,618 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
||||
|
||||
|
||||
/* IMPLEMENTATION OF TELEMETRY CLASS */
|
||||
|
||||
|
||||
#include <condition_variable>
|
||||
#include <future>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
#include "mcc_defaults.h"
|
||||
|
||||
namespace mcc
|
||||
{
|
||||
|
||||
enum MccTelemetryErrorCode : int {
|
||||
ERROR_OK,
|
||||
ERROR_NULLPTR,
|
||||
ERROR_COORD_TRANSFORM,
|
||||
ERROR_PCM_COMP,
|
||||
ERROR_HARDWARE_GETPOS,
|
||||
ERROR_UPDATE_STOPPED,
|
||||
ERROR_DATA_TIMEOUT,
|
||||
ERROR_UNSUPPORTED_COORD_PAIR
|
||||
};
|
||||
|
||||
} // namespace mcc
|
||||
|
||||
namespace std
|
||||
{
|
||||
|
||||
template <>
|
||||
class is_error_code_enum<mcc::MccTelemetryErrorCode> : public true_type
|
||||
{
|
||||
};
|
||||
|
||||
} // namespace std
|
||||
|
||||
|
||||
namespace mcc
|
||||
{
|
||||
|
||||
/* error category definition */
|
||||
|
||||
// error category
|
||||
struct MccTelemetryCategory : public std::error_category {
|
||||
MccTelemetryCategory() : std::error_category() {}
|
||||
|
||||
const char* name() const noexcept
|
||||
{
|
||||
return "ALTITUDE-LIMIT-PZ";
|
||||
}
|
||||
|
||||
std::string message(int ec) const
|
||||
{
|
||||
MccTelemetryErrorCode err = static_cast<MccTelemetryErrorCode>(ec);
|
||||
|
||||
switch (err) {
|
||||
case MccTelemetryErrorCode::ERROR_OK:
|
||||
return "OK";
|
||||
case MccTelemetryErrorCode::ERROR_NULLPTR:
|
||||
return "nullptr input argument";
|
||||
case MccTelemetryErrorCode::ERROR_COORD_TRANSFORM:
|
||||
return "coordinate transformation error";
|
||||
case MccTelemetryErrorCode::ERROR_PCM_COMP:
|
||||
return "PCM computation error";
|
||||
case MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS:
|
||||
return "cannot get hardware position";
|
||||
case MccTelemetryErrorCode::ERROR_UPDATE_STOPPED:
|
||||
return "telemetry update was stopped";
|
||||
case MccTelemetryErrorCode::ERROR_DATA_TIMEOUT:
|
||||
return "a timeout occured while waiting for new data";
|
||||
case MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR:
|
||||
return "unsupported coordinate pair";
|
||||
default:
|
||||
return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
static const MccTelemetryCategory& get()
|
||||
{
|
||||
static const MccTelemetryCategory constInst;
|
||||
return constInst;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
inline std::error_code make_error_code(MccTelemetryErrorCode ec)
|
||||
{
|
||||
return std::error_code(static_cast<int>(ec), MccTelemetryCategory::get());
|
||||
}
|
||||
|
||||
|
||||
|
||||
class MccTelemetry : public mcc_telemetry_interface_t<std::error_code>
|
||||
{
|
||||
protected:
|
||||
static constexpr uint16_t internalUpdatingIntervalDiv = 5;
|
||||
|
||||
public:
|
||||
static constexpr auto defaultUpdateInterval = std::chrono::milliseconds(100);
|
||||
static constexpr auto defaultInternalUpdateTimeout = defaultUpdateInterval * 5;
|
||||
|
||||
typedef std::error_code error_t;
|
||||
|
||||
|
||||
MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware)
|
||||
: _isDataUpdated(new std::atomic_bool()),
|
||||
_data(),
|
||||
_internalUpdating(new std::atomic_bool),
|
||||
_currentUpdateInterval(defaultUpdateInterval),
|
||||
_currentUpdateIntervalMutex(new std::mutex),
|
||||
_updateMutex(new std::mutex),
|
||||
_updateCondVar(new std::condition_variable)
|
||||
{
|
||||
*_isDataUpdated = false;
|
||||
*_internalUpdating = false;
|
||||
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
|
||||
|
||||
// using ccte_t = std::remove_cvref_t<decltype(*ccte)>;
|
||||
using pcm_t = std::remove_cvref_t<decltype(*pcm)>;
|
||||
using hardware_t = std::remove_cvref_t<decltype(*hardware)>;
|
||||
|
||||
_updateTargetFunc = [ccte, pcm, this](bool only_hw, std::stop_token stop_token) -> error_t {
|
||||
if (!only_hw) {
|
||||
//
|
||||
// compute apparent coordinates
|
||||
// ICRS coordinates of the taget must be already set
|
||||
//
|
||||
_data.target.time_point =
|
||||
std::chrono::time_point_cast<typename decltype(_data.target.time_point)::duration>(
|
||||
_data.time_point);
|
||||
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
|
||||
_data.target.X = _data.target.RA_ICRS;
|
||||
_data.target.Y = _data.target.DEC_ICRS;
|
||||
|
||||
// update apparent cordinates
|
||||
auto ccte_err = ccte->transformCoordinates(_data.target, &_data.target);
|
||||
if (ccte_err) {
|
||||
return mcc_deduce_error<error_t>(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
}
|
||||
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
|
||||
typename pcm_t::error_t pcm_err;
|
||||
|
||||
MccPCMResult pcm_res;
|
||||
|
||||
// MccCelestialPoint pt;
|
||||
// pt.time_point =
|
||||
// std::chrono::time_point_cast<typename decltype(pt.time_point)::duration>(_data.target.time_point);
|
||||
|
||||
pcm_err = pcm->computeInversePCM(_data, &pcm_res, &_data);
|
||||
|
||||
// if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
|
||||
// pcm_err = pcm->computeInversePCM(_data, &pcm_res, &_data);
|
||||
|
||||
// pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
// pt.X = _data.target.HA;
|
||||
// pt.Y = _data.target.DEC_APP;
|
||||
// pcm_err = pcm->computeInversePCM(std::move(pt), &pcm_res);
|
||||
// if (!pcm_err) {
|
||||
// _data.target.X = _data.target.HA - pcm_res.pcmX;
|
||||
// _data.target.Y = _data.target.DEC_APP - pcm_res.pcmY;
|
||||
// }
|
||||
// } else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
|
||||
// pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
|
||||
// pt.X = _data.target.AZ;
|
||||
// pt.Y = _data.target.ALT;
|
||||
// pcm_err = pcm->computeInversePCM(std::move(pt), &pcm_res);
|
||||
// if (!pcm_err) {
|
||||
// _data.target.X = _data.target.AZ - pcm_res.pcmX;
|
||||
// _data.target.Y = _data.target.ALT - pcm_res.pcmY;
|
||||
// }
|
||||
// } else {
|
||||
// static_assert(false, "UNKNOWN MOUNT TYPE!");
|
||||
// }
|
||||
|
||||
if (pcm_err) {
|
||||
return mcc_deduce_error<error_t>(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
||||
}
|
||||
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
};
|
||||
|
||||
_updateFunc = [ccte, pcm, hardware, this](std::stop_token stop_token) {
|
||||
// first, update mount quantities
|
||||
typename hardware_t::hardware_state_t hw_pos;
|
||||
auto hw_err = hardware->hardwareGetState(&hw_pos);
|
||||
if (hw_err) {
|
||||
return mcc_deduce_error(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS);
|
||||
}
|
||||
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
double eo;
|
||||
|
||||
_data.time_point =
|
||||
std::chrono::time_point_cast<typename decltype(_data.time_point)::duration>(hw_pos.time_point);
|
||||
|
||||
auto ccte_err = ccte->timepointToJulday(_data.time_point, &_data.JD);
|
||||
if (!ccte_err) {
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
ccte_err = ccte->juldayToAppSideral(_data.JD, &_data.LST, true);
|
||||
if (!ccte_err) {
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
ccte_err = ccte->equationOrigins(_data.JD, &eo);
|
||||
}
|
||||
}
|
||||
|
||||
if (ccte_err) {
|
||||
return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
_data.X = (double)hw_pos.X;
|
||||
_data.Y = (double)hw_pos.Y;
|
||||
_data.speedX = (double)hw_pos.speedX;
|
||||
_data.speedY = (double)hw_pos.speedY;
|
||||
|
||||
// fill _data.pcmX, _data.pcmY and corresponded apparent coordinates
|
||||
auto pcm_err = pcm->computePCM(_data, &_data, &_data);
|
||||
if (pcm_err) {
|
||||
return mcc_deduce_error(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
||||
}
|
||||
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = _data.time_point};
|
||||
|
||||
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
|
||||
_data.RA_APP = (double)_data.LST - (double)_data.HA + eo;
|
||||
|
||||
_data.X = _data.HA;
|
||||
_data.Y = _data.DEC_APP;
|
||||
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
|
||||
ccte_err = ccte->transformCoordinates(_data, &pt);
|
||||
if (!ccte_err) {
|
||||
_data.AZ = pt.X;
|
||||
_data.ALT = pt.Y;
|
||||
_data.ZD = std::numbers::pi / 2.0 - _data.ALT;
|
||||
}
|
||||
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
|
||||
_data.ALT = std::numbers::pi / 2.0 - _data.ZD;
|
||||
|
||||
_data.X = _data.AZ;
|
||||
_data.Y = _data.ZD;
|
||||
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
|
||||
pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
ccte_err = ccte->transformCoordinates(_data, &pt);
|
||||
if (!ccte) {
|
||||
_data.HA = pt.X;
|
||||
_data.DEC_APP = pt.Y;
|
||||
_data.RA_APP = (double)_data.LST - (double)_data.HA + eo;
|
||||
}
|
||||
|
||||
} else {
|
||||
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
||||
}
|
||||
|
||||
if (!ccte_err) {
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
_data.X = _data.AZ;
|
||||
_data.Y = _data.ZD;
|
||||
|
||||
ccte_err = ccte->refractionCorrection(_data, &_data.refCorr);
|
||||
if (!ccte_err) {
|
||||
// restore hardware encoders coordinates
|
||||
_data.X = (double)hw_pos.X;
|
||||
_data.Y = (double)hw_pos.Y;
|
||||
|
||||
// update target (assuming target ICRS coordinates are already set)
|
||||
|
||||
auto ret = _updateTargetFunc(false, stop_token);
|
||||
if (ret) {
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (ccte_err) {
|
||||
return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
|
||||
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
|
||||
} else {
|
||||
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
||||
}
|
||||
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
};
|
||||
|
||||
|
||||
_setTargetFunc = [ccte, this](MccCelestialPoint const& pt) {
|
||||
// in the case of apparent input coordinates
|
||||
// one must ensure the same time points
|
||||
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
|
||||
_data.target.time_point =
|
||||
std::chrono::time_point_cast<typename decltype(_data.target.time_point)::duration>(pt.time_point);
|
||||
|
||||
auto ret = ccte->transformCoordinates(pt, &_data.target);
|
||||
|
||||
if (!ret) {
|
||||
if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
||||
_data.target.RA_ICRS = _data.target.X;
|
||||
_data.target.DEC_ICRS = _data.target.Y;
|
||||
|
||||
// update apparent coordinates
|
||||
ret = _updateTargetFunc(false, {});
|
||||
} else { // apparent coordinates were computed above
|
||||
// compute ICRS coordinates
|
||||
MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
|
||||
ret = ccte->transformCoordinates(pt, &cpt);
|
||||
|
||||
_data.target.RA_ICRS = cpt.X;
|
||||
_data.target.DEC_ICRS = cpt.Y;
|
||||
|
||||
// compute only hardware coordinates
|
||||
ret = _updateTargetFunc(true, {});
|
||||
}
|
||||
}
|
||||
|
||||
return mcc_deduce_error<error_t>(ret, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
MccTelemetry(MccTelemetry&&) = default;
|
||||
MccTelemetry& operator=(MccTelemetry&&) = default;
|
||||
|
||||
MccTelemetry(const MccTelemetry&) = delete;
|
||||
MccTelemetry& operator=(const MccTelemetry&) = delete;
|
||||
|
||||
|
||||
virtual ~MccTelemetry()
|
||||
{
|
||||
stopInternalTelemetryDataUpdating();
|
||||
|
||||
if (_internalUpdatingFuture.valid()) {
|
||||
// try to exit correctly
|
||||
// auto status = _internalUpdatingFuture.wait_for(std::chrono::seconds(1));
|
||||
_internalUpdatingFuture.wait_for(std::chrono::seconds(1));
|
||||
// _internalUpdatingFuture.get();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
template <traits::mcc_time_duration_c DT>
|
||||
DT telemetryDataUpdateInterval() const
|
||||
{
|
||||
std::lock_guard lock{_currentUpdateIntervalMutex};
|
||||
|
||||
return std::chrono::duration_cast<DT>(_currentUpdateInterval);
|
||||
}
|
||||
|
||||
std::chrono::milliseconds telemetryDataUpdateInterval() const
|
||||
{
|
||||
return telemetryDataUpdateInterval<std::chrono::milliseconds>();
|
||||
}
|
||||
|
||||
void setTelemetryDataUpdateInterval(traits::mcc_time_duration_c auto const& interval)
|
||||
{
|
||||
using d_t = std::remove_cvref_t<decltype(interval)>;
|
||||
|
||||
std::lock_guard lock{_currentUpdateIntervalMutex};
|
||||
|
||||
if constexpr (std::floating_point<typename d_t::rep>) {
|
||||
_currentUpdateInterval = utils::isEqual(interval.count(), 0.0) ? defaultUpdateInterval : interval;
|
||||
} else {
|
||||
_currentUpdateInterval = interval.count() == 0 ? defaultUpdateInterval : interval;
|
||||
}
|
||||
}
|
||||
|
||||
// asynchronuosly periodicaly update telemetry data (internal synchronization)
|
||||
void startInternalTelemetryDataUpdating()
|
||||
{
|
||||
using intv_t = std::remove_cvref_t<decltype(_currentUpdateInterval)>;
|
||||
|
||||
*_internalUpdating = true;
|
||||
|
||||
_internalUpdatingFuture = std::async(
|
||||
std::launch::async,
|
||||
[this](std::stop_token stop_token) -> error_t {
|
||||
while (!stop_token.stop_requested()) {
|
||||
_lastUpdateError = updateTelemetryData(defaultInternalUpdateTimeout);
|
||||
if (_lastUpdateError) {
|
||||
*_internalUpdating = false;
|
||||
return _lastUpdateError;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard lock{_currentUpdateIntervalMutex};
|
||||
|
||||
// compute it here because of possible changing _currentUpdateInterval
|
||||
auto sleep_td = _currentUpdateInterval / internalUpdatingIntervalDiv;
|
||||
|
||||
for (uint16_t i = 0; i < internalUpdatingIntervalDiv - 1; ++i) {
|
||||
if (stop_token.stop_requested()) {
|
||||
break;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(sleep_td);
|
||||
}
|
||||
|
||||
if (stop_token.stop_requested()) {
|
||||
break;
|
||||
}
|
||||
|
||||
if constexpr (std::floating_point<intv_t>) {
|
||||
std::this_thread::sleep_for(sleep_td);
|
||||
} else {
|
||||
auto rem = _currentUpdateInterval % internalUpdatingIntervalDiv;
|
||||
|
||||
if (rem.count()) {
|
||||
std::this_thread::sleep_for(rem);
|
||||
} else {
|
||||
std::this_thread::sleep_for(sleep_td);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
*_internalUpdating = false;
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
},
|
||||
_internalUpdatingStopSource.get_token());
|
||||
}
|
||||
|
||||
|
||||
void stopInternalTelemetryDataUpdating()
|
||||
{
|
||||
_internalUpdatingStopSource.request_stop();
|
||||
*_internalUpdating = false;
|
||||
}
|
||||
|
||||
|
||||
bool isInternalTelemetryDataUpdating() const
|
||||
{
|
||||
return *_internalUpdating;
|
||||
}
|
||||
|
||||
|
||||
error_t updateTelemetryData(traits::mcc_time_duration_c auto const& timeout)
|
||||
{
|
||||
std::lock_guard thread_lock{*_updateMutex};
|
||||
|
||||
std::stop_source stop_source;
|
||||
|
||||
*_isDataUpdated = false;
|
||||
|
||||
std::future<error_t> update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token());
|
||||
auto status = update_ft.wait_for(timeout);
|
||||
|
||||
if (status == std::future_status::ready) {
|
||||
*_isDataUpdated = true;
|
||||
_lastUpdateError = update_ft.get();
|
||||
} else {
|
||||
stop_source.request_stop();
|
||||
_lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
|
||||
}
|
||||
|
||||
// unblock waiting threads even in the case of timeout!
|
||||
_updateCondVar->notify_all();
|
||||
|
||||
return _lastUpdateError;
|
||||
}
|
||||
|
||||
// block the thread and wait for data to be ready (internal synchronization)
|
||||
error_t waitForTelemetryData(mcc_telemetry_data_c auto* tdata, traits::mcc_time_duration_c auto const& timeout)
|
||||
{
|
||||
if (tdata == nullptr) {
|
||||
return MccTelemetryErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
std::unique_lock ulock(*_updateMutex);
|
||||
|
||||
auto res = _updateCondVar->wait_for(ulock, timeout, [this]() { return *_isDataUpdated; });
|
||||
if (res == std::cv_status::timeout) {
|
||||
return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
|
||||
}
|
||||
|
||||
std::lock_guard thread_lock{*_updateMutex};
|
||||
|
||||
if (!_lastUpdateError) {
|
||||
mcc_copy_telemetry_data(_data, tdata);
|
||||
}
|
||||
|
||||
return _lastUpdateError;
|
||||
}
|
||||
|
||||
// just get current data
|
||||
error_t telemetryData(mcc_telemetry_data_c auto* tdata)
|
||||
{
|
||||
if (tdata == nullptr) {
|
||||
return MccTelemetryErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
std::lock_guard thread_lock{*_updateMutex};
|
||||
|
||||
mcc_copy_telemetry_data(_data, tdata);
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
|
||||
error_t setPointingTarget(mcc_celestial_point_c auto pt)
|
||||
{
|
||||
std::lock_guard lock{*_updateMutex};
|
||||
|
||||
return _setTargetFunc(pt);
|
||||
}
|
||||
|
||||
|
||||
|
||||
error_t targetToMountDiff(MccCoordPairKind pair_kind, mcc_angle_c auto* dx, mcc_angle_c auto* dy)
|
||||
{
|
||||
std::lock_guard lock{*_updateMutex};
|
||||
|
||||
if (pair_kind == MccCoordPairKind::COORDS_KIND_AZALT || pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
||||
*dx = (double)_data.target.AZ - (double)_data.AZ;
|
||||
*dy = (double)_data.target.ALT - (double)_data.ALT;
|
||||
} else if (pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP ||
|
||||
pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP ||
|
||||
pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
|
||||
*dx = (double)_data.target.HA - (double)_data.HA;
|
||||
*dy = (double)_data.target.DEC_APP - (double)_data.DEC_APP;
|
||||
} else {
|
||||
return MccTelemetryErrorCode::ERROR_UNSUPPORTED_COORD_PAIR;
|
||||
}
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
|
||||
error_t targetToMountDist(mcc_angle_c auto* dist)
|
||||
{
|
||||
if (dist == nullptr) {
|
||||
return MccTelemetryErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
std::lock_guard lock{*_updateMutex};
|
||||
|
||||
double dHA = _data.HA - _data.target.HA;
|
||||
double cosDHA = cos(dHA);
|
||||
|
||||
double cosT = cos(_data.target.DEC_APP);
|
||||
double sinT = sin(_data.target.DEC_APP);
|
||||
double cosM = cos(_data.DEC_APP);
|
||||
double sinM = sin(_data.DEC_APP);
|
||||
|
||||
double term1 = cosT * sin(dHA);
|
||||
double term2 = cosM * sinT - sinM * cosT * cosDHA;
|
||||
|
||||
*dist = atan2(sqrt(term1 * term1 + term2 * term2), (sinM * sinT + cosM * cosT * cos(dHA)));
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
protected:
|
||||
std::unique_ptr<std::atomic_bool> _isDataUpdated;
|
||||
MccTelemetryData _data;
|
||||
|
||||
std::unique_ptr<std::atomic_bool> _internalUpdating;
|
||||
std::chrono::nanoseconds _currentUpdateInterval{std::chrono::milliseconds(100)};
|
||||
std::unique_ptr<std::mutex> _currentUpdateIntervalMutex;
|
||||
std::future<error_t> _internalUpdatingFuture{};
|
||||
std::stop_source _internalUpdatingStopSource{};
|
||||
|
||||
std ::function<error_t(bool, std::stop_token)> _updateTargetFunc{};
|
||||
std::function<error_t(std::stop_token)> _updateFunc{};
|
||||
std::function<error_t()> _setTargetFunc{};
|
||||
|
||||
std::unique_ptr<std::mutex> _updateMutex;
|
||||
std::unique_ptr<std::condition_variable> _updateCondVar;
|
||||
|
||||
error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK};
|
||||
};
|
||||
|
||||
|
||||
static_assert(mcc_telemetry_c<MccTelemetry>, "");
|
||||
|
||||
} // namespace mcc
|
||||
Loading…
x
Reference in New Issue
Block a user