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-08-20 18:05:47 +03:00
parent c6a1caea08
commit 99a28d87ec
1 changed files with 206 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

274
mcc/mcc_telemetry.h
View File

@ -6,6 +6,10 @@
/* IMPLEMENTATION OF TELEMETRY CLASS */
#include <condition_variable>
#include <mutex>
#include <thread>
#include "mcc_defaults.h"
namespace mcc
@ -19,7 +23,8 @@ enum MccTelemetryErrorCode : int {
ERROR_NULLPTR,
ERROR_COORD_TRANSFORM,
ERROR_PCM_COMP,
ERROR_HARDWARE_GETPOS
ERROR_HARDWARE_GETPOS,
ERROR_DATA_TIMEOUT
};
} // namespace mcc
@ -64,6 +69,8 @@ struct MccTelemetryCategory : public std::error_category {
return "PCM computation error";
case MccTelemetryErrorCode::ERROR_HARDWARE_GETPOS:
return "cannot get hardware position";
case MccTelemetryErrorCode::ERROR_DATA_TIMEOUT:
return "a timeout occured while waiting for new data";
default:
return "UNKNOWN";
}
@ -97,7 +104,8 @@ public:
typedef std::error_code error_t;
MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware) : _data()
MccTelemetry(mcc_ccte_c auto* ccte, mcc_PCM_c auto* pcm, mcc_hardware_c auto* hardware)
: _updated(false), _data(), _updateMutex(new std::mutex), _updateCondVar(new std::condition_variable)
{
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
@ -106,21 +114,72 @@ public:
using hardware_t = std::remove_cvref_t<decltype(*hardware)>;
_updateTargetFunc = [ccte, this]() {
//
// ICRS coordinates of the taget must be already set
//
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
_data.target.X = _data.target.RA_ICRS;
_data.target.Y = _data.target.DEC_ICRS;
_updateTargetFunc = [ccte, pcm, this](bool only_hw) -> 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);
// update apparent cordinates
auto ret = ccte->transformCoordinates(_data.target, &_data.target);
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
_data.target.X = _data.target.RA_ICRS;
_data.target.Y = _data.target.DEC_ICRS;
return mcc_deduce_error<error_t>(ret, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
// 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);
}
}
// compute hardware coordinates
// WARNING: It is assumed here that PCM corrections have small (arcseconds-arcminutes) values
// since ususaly there is no reverse transformation for "hardware-to-apparent" relation!
typename pcm_t::error_t pcm_err;
struct {
double dx, dy;
} pcm_res;
MccCelestialPoint pt;
pt.time_point =
std::chrono::time_point_cast<typename decltype(pt.time_point)::duration>(_data.target.time_point);
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
pt.X = _data.target.HA;
pt.Y = _data.target.DEC_APP;
pcm_err = pcm->compute(std::move(pt), &pcm_res);
if (!pcm_err) {
_data.target.X = _data.target.HA - pcm_res.dx;
_data.target.Y = _data.target.DEC_APP - pcm_res.dy;
}
} 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->compute(std::move(pt), &pcm_res);
if (!pcm_err) {
_data.target.X = _data.target.AZ - pcm_res.dx;
_data.target.Y = _data.target.ALT - pcm_res.dy;
}
} 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](MccTelemetryData* data) {
_updateFunc = [ccte, pcm, hardware, this]() {
// first, update mount quantities
typename hardware_t::axes_pos_t hw_pos;
auto hw_err = hardware->getPos(&hw_pos);
@ -130,14 +189,14 @@ public:
double eo;
data->time_point =
std::chrono::time_point_cast<typename decltype(data->time_point)::duration>(hw_pos.time_point);
_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 = ccte->timepointToJulday(_data.time_point, &_data.JD);
if (!ccte_err) {
ccte_err = ccte->juldayToAppSideral(data->JD, &data->LST, true);
ccte_err = ccte->juldayToAppSideral(_data.JD, &_data.LST, true);
if (!ccte_err) {
ccte_err = ccte->equationOrigins(data->JD, &eo);
ccte_err = ccte->equationOrigins(_data.JD, &eo);
}
}
@ -145,55 +204,55 @@ public:
return mcc_deduce_error(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
}
data->speedX = (double)hw_pos.speedX;
data->speedY = (double)hw_pos.speedY;
_data.speedX = (double)hw_pos.speedX;
_data.speedY = (double)hw_pos.speedY;
struct {
double dx, dy;
} pcm_res;
auto pcm_err = pcm->computePCM(data, &pcm_res);
auto pcm_err = pcm->computePCM(_data, &pcm_res);
if (pcm_err) {
return mcc_deduce_error(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
data->pcmX = pcm_res.dx;
data->pcmY = pcm_res.dy;
_data.pcmX = pcm_res.dx;
_data.pcmY = pcm_res.dy;
MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = data->time_point};
MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT, .time_point = _data.time_point};
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
data->HA = (double)hw_pos.X + pcm_res.dx;
data->DEC_APP = (double)hw_pos.Y + pcm_res.dy;
data->RA_APP = (double)data->LST - (double)data->HA + eo;
_data.HA = (double)hw_pos.X + pcm_res.dx;
_data.DEC_APP = (double)hw_pos.Y + pcm_res.dy;
_data.RA_APP = (double)_data.LST - (double)_data.HA + eo;
data->X = data->HA;
data->Y = data->DEC_APP;
_data.X = _data.HA;
_data.Y = _data.DEC_APP;
data->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
ccte_err = ccte->transformCoordinates(*data, &pt);
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;
_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->AZ = (double)hw_pos.X + pcm_res.dx;
data->ALT = (double)hw_pos.Y + pcm_res.dy;
data->ZD = std::numbers::pi / 2.0 - data->ALT;
_data.AZ = (double)hw_pos.X + pcm_res.dx;
_data.ALT = (double)hw_pos.Y + pcm_res.dy;
_data.ZD = std::numbers::pi / 2.0 - _data.ALT;
data->X = data->AZ;
data->Y = data->ALT;
_data.X = _data.AZ;
_data.Y = _data.ALT;
data->pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
ccte_err = ccte->transformCoordinates(*data, &pt);
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;
_data.HA = pt.X;
_data.DEC_APP = pt.Y;
_data.RA_APP = (double)_data.LST - (double)_data.HA + eo;
}
} else {
@ -201,27 +260,22 @@ public:
}
if (!ccte_err) {
data->pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
data->X = data->AZ;
data->Y = data->ZD;
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
_data.X = _data.AZ;
_data.Y = _data.ZD;
ccte_err = ccte->refractionCorrection(data, &data->refCorr);
ccte_err = ccte->refractionCorrection(_data, &_data.refCorr);
if (!ccte_err) {
// hardware encoders coordinates
data->X = (double)hw_pos.X;
data->Y = (double)hw_pos.Y;
// 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)
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;
ccte_err = ccte->transformCoordinates(data->target, &data->target);
auto ret = _updateTargetFunc(false);
if (ret) {
return ret;
}
}
}
@ -230,14 +284,16 @@ public:
}
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
data->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
data->pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
_updated = true;
return MccTelemetryErrorCode::ERROR_OK;
};
@ -258,7 +314,7 @@ public:
_data.target.DEC_ICRS = _data.target.Y;
// update apparent coordinates
ret = _updateTargetFunc();
ret = _updateTargetFunc(false);
} else { // apparent coordinates were computed above
// compute ICRS coordinates
MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
@ -266,6 +322,9 @@ public:
_data.target.RA_ICRS = cpt.X;
_data.target.DEC_ICRS = cpt.Y;
// compute only hardware coordinates
ret = _updateTargetFunc(true);
}
}
@ -276,13 +335,82 @@ public:
virtual ~MccTelemetry() = default;
error_t updateTelemetryData(traits::mcc_time_duration_c auto const& period)
{
using d_t = typename std::remove_cvref_t<decltype(period)>::rep;
bool is_zero;
if (std::floating_point<d_t>) {
is_zero = utils::isEqual(period.count(), d_t::zero());
} else {
is_zero = period.count() == d_t::zero();
}
if (is_zero) { // just update once
return _updateFunc();
} else {
// try to update once
auto ret = _updateFunc();
if (ret) {
return ret;
}
_lastUpdateError = MccTelemetryErrorCode::ERROR_OK;
_timerThread = [period, this](std::stop_token st) {
while (!st.stop_requested()) {
{
std::lock_guard thread_lock{*_updateMutex};
_lastUpdateError = _updateFunc();
_updateCondVar->notify_all();
if (_lastUpdateError) {
return;
}
}
std::this_thread::sleep_for(period);
}
};
_timerThread.detach();
return MccTelemetryErrorCode::ERROR_OK;
}
}
// block the thread and wait for data to be ready (external 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 thread_lock(*_updateMutex);
auto res = _updateCondVar->wait_for(thread_lock, timeout, [this]() { return _updated; });
if (res == std::cv_status::timeout) {
return MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
}
if (!_lastUpdateError) {
mcc_copy_telemetry_data(_data, tdata);
}
return _lastUpdateError;
}
// update and get data as soon as possible
error_t telemetryData(mcc_telemetry_data_c auto* tdata)
{
if (tdata == nullptr) {
return MccTelemetryErrorCode::ERROR_NULLPTR;
}
error_t ret = _updateFunc(&_data);
std::lock_guard thread_lock{*_updateMutex};
error_t ret = _updateFunc();
if (!ret) {
mcc_copy_telemetry_data(_data, tdata);
@ -294,15 +422,25 @@ public:
error_t setPointingTarget(mcc_celestial_point_c auto pt)
{
// return
std::lock_guard lock{*_updateMutex};
return _setTargetFunc();
}
protected:
std::atomic_bool _updated;
MccTelemetryData _data;
std ::function<error_t()> _updateTargetFunc{};
std::function<error_t(MccTelemetryData*)> _updateFunc{};
std::function<error_t(MccCelestialPoint const&)> _setTargetFunc{};
std ::function<error_t(bool)> _updateTargetFunc{};
std::function<error_t()> _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};
std::jthread _timerThread;
};
} // namespace mcc