758 lines
27 KiB
C++
758 lines
27 KiB
C++
#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(),
|
|
_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(*controls)>;
|
|
using hardware_t = std::remove_cvref_t<decltype(*controls)>;
|
|
|
|
_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);
|
|
}
|
|
}
|
|
|
|
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 = (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 = 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 = (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;
|
|
// }
|
|
|
|
// 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;
|
|
};
|
|
|
|
|
|
// arm internal update loop
|
|
_internalUpdatingStopSource = std::stop_source{};
|
|
|
|
_internalUpdatingLoopFuture = std::async(
|
|
std::launch::async,
|
|
[this](std::stop_token stoken) {
|
|
while (!(*_internalUpdatingLoopStop)) {
|
|
{
|
|
std::unique_lock ulock(*_internalUpdatingLoopMutex);
|
|
_internalUpdatingLoopCondVar->wait(ulock, [this]() -> bool { return *_dataUpdatingRequested; });
|
|
}
|
|
|
|
{
|
|
std::lock_guard lock_update(*_updateMutex);
|
|
|
|
*_isDataUpdated = false;
|
|
_lastUpdateError = _updateFunc(stoken);
|
|
}
|
|
|
|
*_isDataUpdated = true;
|
|
|
|
// unlock all waiting threads
|
|
_updateCondVar->notify_all();
|
|
}
|
|
},
|
|
_internalUpdatingStopSource.get_token());
|
|
}
|
|
|
|
|
|
MccTelemetry(MccTelemetry&&) = default;
|
|
MccTelemetry& operator=(MccTelemetry&&) = default;
|
|
|
|
MccTelemetry(const MccTelemetry&) = delete;
|
|
MccTelemetry& operator=(const MccTelemetry&) = delete;
|
|
|
|
|
|
virtual ~MccTelemetry()
|
|
{
|
|
*_internalUpdatingLoopStop = true;
|
|
|
|
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 lock(*_internalUpdatingLoopMutex);
|
|
*_dataUpdatingRequested = true;
|
|
}
|
|
std::unique_lock ulock(*_updateMutex);
|
|
|
|
_internalUpdatingLoopCondVar->notify_one();
|
|
*_dataUpdatingRequested = false;
|
|
|
|
bool ok = _updateCondVar->wait_for(ulock, timeout, [this]() -> bool { return *_isDataUpdated; });
|
|
if (!ok) {
|
|
_lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_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;
|
|
// // }
|
|
|
|
// *_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};
|
|
|
|
_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;
|
|
|
|
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
|