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-10 12:24:06 +03:00
parent 2478c1e8d2
commit 00354d9b41
4 changed files with 694 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
asibfm700/asibfm700_common.h
View File

@ -5,6 +5,8 @@
/* COMMON LIBRARY DEFINITIONS */
#include "asibfm700_servocontroller.h"
namespace asibfm700
{

54
mcc/mcc_pzone.h
View File

@ -662,6 +662,7 @@ public:
return MccAltLimitPZErrorCode::ERROR_OK;
}
// time to reach maximal limit
template <typename InputT>
error_t timeToPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
@ -681,6 +682,18 @@ public:
} else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) {
}
} else { // mcc_celestial_point_c
if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) {
time_ang = (_maxLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
} else {
MccCelestialPoint pt;
auto ret = getHWCoords(std::move(coords), &pt);
if (ret) {
return ret;
}
time_ang = (_maxLimit - pt.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
}
}
std::chrono::nanoseconds ns{
@ -688,19 +701,60 @@ public:
period_t rat;
*res_time = res_t{static_cast<typename res_t::rep>(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)};
return MccAltLimitPZErrorCode::ERROR_OK;
}
// time to reach minimal limit
template <typename InputT>
error_t timeFromPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
{
using res_t = std::remove_cvref_t<decltype(*res_time)>;
using period_t = typename res_t::period;
double time_ang;
if (res_time == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
// assume here .X and are hardware encoder coordinate of corresponding axis
if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) {
time_ang = (_minLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
} else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) {
}
} else { // mcc_celestial_point_c
if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) {
time_ang = (_minLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
} else {
MccCelestialPoint pt;
auto ret = getHWCoords(std::move(coords), &pt);
if (ret) {
return ret;
}
time_ang = (_minLimit - pt.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
}
}
std::chrono::nanoseconds ns{
static_cast<std::chrono::nanoseconds::rep>(time_ang * 43200.0 / std::numbers::pi * 1.0E9)};
period_t rat;
*res_time = res_t{static_cast<typename res_t::rep>(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)};
return MccAltLimitPZErrorCode::ERROR_OK;
}
template <typename InputT, typename ResultT>
error_t intersectPZone(InputT coords, ResultT* point)
requires((mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>) &&
(mcc_eqt_hrz_coord_c<ResultT> || mcc_celestial_point_c<ResultT>))
{
return MccAltLimitPZErrorCode::ERROR_OK;
}
protected:

39
mcc/mcc_telemetry.h
View File

@ -107,7 +107,8 @@ public:
typedef std::error_code error_t;
MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware)
template <mcc_position_controls_c CONTROLS_T>
MccTelemetry(CONTROLS_T* controls)
: _isDataUpdated(new std::atomic_bool()),
_data(),
_internalUpdating(new std::atomic_bool),
@ -122,10 +123,10 @@ public:
_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)>;
using pcm_t = std::remove_cvref_t<decltype(*controls)>;
using hardware_t = std::remove_cvref_t<decltype(*controls)>;
_updateTargetFunc = [ccte, pcm, this](bool only_hw, std::stop_token stop_token) -> error_t {
_updateTargetFunc = [controls, this](bool only_hw, std::stop_token stop_token) -> error_t {
if (!only_hw) {
//
// compute apparent coordinates
@ -140,7 +141,7 @@ public:
_data.target.Y = _data.target.DEC_ICRS;
// update apparent cordinates
auto ccte_err = ccte->transformCoordinates(_data.target, &_data.target);
auto ccte_err = controls->transformCoordinates(_data.target, &_data.target);
if (ccte_err) {
return mcc_deduce_error<error_t>(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
}
@ -159,7 +160,7 @@ public:
// 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);
pcm_err = controls->computeInversePCM(_data, &pcm_res, &_data);
// if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
// pcm_err = pcm->computeInversePCM(_data, &pcm_res, &_data);
@ -193,10 +194,10 @@ public:
return MccTelemetryErrorCode::ERROR_OK;
};
_updateFunc = [ccte, pcm, hardware, this](std::stop_token stop_token) {
_updateFunc = [controls, 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);
auto hw_err = controls->hardwareGetState(&hw_pos);
if (hw_err) {
return mcc_deduce_error(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS);
}
@ -210,19 +211,19 @@ public:
_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);
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 = ccte->juldayToAppSideral(_data.JD, &_data.LST, true);
ccte_err = controls->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);
ccte_err = controls->equationOrigins(_data.JD, &eo);
}
}
@ -240,7 +241,7 @@ public:
_data.speedY = (double)hw_pos.speedY;
// fill _data.pcmX, _data.pcmY and corresponded apparent coordinates
auto pcm_err = pcm->computePCM(_data, &_data, &_data);
auto pcm_err = controls->computePCM(_data, &_data, &_data);
if (pcm_err) {
return mcc_deduce_error(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
@ -259,7 +260,7 @@ public:
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
ccte_err = ccte->transformCoordinates(_data, &pt);
ccte_err = controls->transformCoordinates(_data, &pt);
if (!ccte_err) {
_data.AZ = pt.X;
_data.ALT = pt.Y;
@ -274,8 +275,8 @@ public:
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
ccte_err = ccte->transformCoordinates(_data, &pt);
if (!ccte) {
ccte_err = controls->transformCoordinates(_data, &pt);
if (!ccte_err) {
_data.HA = pt.X;
_data.DEC_APP = pt.Y;
_data.RA_APP = (double)_data.LST - (double)_data.HA + eo;
@ -294,7 +295,7 @@ public:
_data.X = _data.AZ;
_data.Y = _data.ZD;
ccte_err = ccte->refractionCorrection(_data, &_data.refCorr);
ccte_err = controls->refractionCorrection(_data, &_data.refCorr);
if (!ccte_err) {
// restore hardware encoders coordinates
_data.X = (double)hw_pos.X;
@ -326,7 +327,7 @@ public:
};
_setTargetFunc = [ccte, this](MccCelestialPoint const& pt) {
_setTargetFunc = [controls, this](MccCelestialPoint const& pt) {
// in the case of apparent input coordinates
// one must ensure the same time points
@ -334,7 +335,7 @@ public:
_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);
auto ret = controls->transformCoordinates(pt, &_data.target);
if (!ret) {
if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
@ -346,7 +347,7 @@ public:
} else { // apparent coordinates were computed above
// compute ICRS coordinates
MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
ret = ccte->transformCoordinates(pt, &cpt);
ret = controls->transformCoordinates(pt, &cpt);
_data.target.RA_ICRS = cpt.X;
_data.target.DEC_ICRS = cpt.Y;

618
mcc/mcc_telemetry.h.split Normal file
View File

@ -0,0 +1,618 @@
#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