426 lines
15 KiB
C++
426 lines
15 KiB
C++
#pragma once
|
|
|
|
|
|
|
|
/****************************************************************************************
|
|
* *
|
|
* MOUNT CONTROL COMPONENTS LIBRARY *
|
|
* *
|
|
* *
|
|
* IMPLEMENTATION OF MOUNT TELEMETRY CLASS *
|
|
* *
|
|
****************************************************************************************/
|
|
|
|
|
|
#include <future>
|
|
#include "mcc_concepts.h"
|
|
#include "mcc_coordinate.h"
|
|
|
|
namespace mcc::impl
|
|
{
|
|
|
|
enum class MccTelemetryErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_NULLPTR,
|
|
ERROR_COORD_TRANSFORM,
|
|
ERROR_PCM_COMP,
|
|
ERROR_HARDWARE_GETSTATE,
|
|
ERROR_UPDATE_STOPPED,
|
|
ERROR_DATA_TIMEOUT,
|
|
ERROR_UNSUPPORTED_COORD_PAIR,
|
|
ERROR_UPDATE_LOOP_WAIT
|
|
};
|
|
|
|
|
|
|
|
} // namespace mcc::impl
|
|
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::impl::MccTelemetryErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
|
|
namespace mcc::impl
|
|
{
|
|
|
|
// 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_GETSTATE:
|
|
return "cannot get hardware state";
|
|
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";
|
|
case MccTelemetryErrorCode::ERROR_UPDATE_LOOP_WAIT:
|
|
return "a timeout occured while waiting to exit the update loop";
|
|
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());
|
|
}
|
|
|
|
|
|
|
|
template <mcc_hardware_c HARDWARE_T>
|
|
class MccTelemetry : public mcc_telemetry_interface_t<MccError>
|
|
{
|
|
public:
|
|
typedef HARDWARE_T hardware_t;
|
|
|
|
typedef MccError error_t;
|
|
|
|
|
|
struct telemetry_data_t {
|
|
MccSkyPoint targetPos{}; // celestial coordinates
|
|
|
|
MccGenXY targetXY{}; // encoder coordinates
|
|
|
|
struct {
|
|
double pcmX{}, pcmY{};
|
|
} pcmReverseCorrection{};
|
|
|
|
MccSkyPoint mountPos{}; // celestial coordinates
|
|
|
|
typename HARDWARE_T::hardware_state_t hwState{}; // here encoder coordinates
|
|
|
|
struct {
|
|
double pcmX{}, pcmY{};
|
|
} pcmCorrection{};
|
|
};
|
|
|
|
static constexpr std::chrono::milliseconds _defaultTelemetryDataTimeout{500};
|
|
|
|
/*
|
|
MccTelemetry(HARDWARE_T* hardware, mcc_pcm_c auto* pcm) : _hardware(hardware)
|
|
{
|
|
_updateFunc = [pcm, this](telemetry_data_t* tdata) {
|
|
if (tdata == nullptr) {
|
|
return MccTelemetryErrorCode::ERROR_NULLPTR;
|
|
}
|
|
|
|
// get hardware state (coordinates, speeds and so on)
|
|
auto hw_err = _hardware->hardwareGetState(&(tdata->hwState));
|
|
if (hw_err) {
|
|
return mcc_deduced_err(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETSTATE);
|
|
}
|
|
|
|
// compute PCM corrections and observed (corrected for PCM) mount coordinates
|
|
auto pcm_err = pcm->computePCM(tdata->hwState.XY, &(tdata->pcmCorrection), &(tdata->mountPos));
|
|
|
|
if (pcm_err) {
|
|
return mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
|
}
|
|
|
|
return MccTelemetryErrorCode::ERROR_OK;
|
|
};
|
|
|
|
|
|
_updatingStopSource = std::stop_source{};
|
|
|
|
_dataUpdatingRequested->clear();
|
|
_dataUpdatingStart->clear();
|
|
|
|
// start thread for data updating
|
|
_updatingFuture = std::async(
|
|
std::launch::async,
|
|
[this](std::stop_token stoken, telemetry_data_t* tdata) {
|
|
while (!stoken.stop_requested()) {
|
|
_dataUpdatingRequested->wait(false);
|
|
|
|
if (!stoken.stop_requested()) {
|
|
std::lock_guard lock{*_timeoutMutex};
|
|
|
|
_dataUpdatingStart->test_and_set();
|
|
_dataUpdatingStart->notify_all();
|
|
|
|
_lastUpdateError = _updateFunc(tdata);
|
|
|
|
_dataUpdatingStart->clear();
|
|
_dataUpdatingRequested->clear();
|
|
}
|
|
}
|
|
},
|
|
_updatingStopSource.get_token(), _tdataPtr);
|
|
}
|
|
*/
|
|
|
|
MccTelemetry(HARDWARE_T* hardware, mcc_pcm_c auto* pcm)
|
|
{
|
|
_updatingStopSource = std::stop_source{};
|
|
|
|
_dataUpdatingRequested->clear();
|
|
_dataUpdatingStart->clear();
|
|
|
|
// start thread for data updating
|
|
_updatingFuture = std::async(
|
|
std::launch::async,
|
|
[this](std::stop_token stoken, HARDWARE_T* hw, auto* pcm_ptr) {
|
|
while (!stoken.stop_requested()) {
|
|
_dataUpdatingRequested->wait(false);
|
|
|
|
if (!stoken.stop_requested()) {
|
|
std::lock_guard lock{*_timeoutMutex};
|
|
|
|
_dataUpdatingStart->test_and_set();
|
|
_dataUpdatingStart->notify_all();
|
|
|
|
if (_tdataPtr != nullptr && hw && pcm_ptr) {
|
|
_lastUpdateError = MccTelemetryErrorCode::ERROR_OK;
|
|
|
|
// get hardware state (coordinates, speeds and so on)
|
|
auto hw_err = hw->hardwareGetState(&_tdataPtr->hwState);
|
|
if (hw_err) {
|
|
_lastUpdateError =
|
|
mcc_deduced_err(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETSTATE);
|
|
} else {
|
|
// compute PCM corrections and observed (corrected for PCM) mount coordinates
|
|
auto pcm_err = pcm_ptr->computePCM(_tdataPtr->hwState.XY, &(_tdataPtr->pcmCorrection),
|
|
&(_tdataPtr->mountPos));
|
|
|
|
if (!pcm_err) {
|
|
// set target coordinates
|
|
if (_enteredTargetPos.pairKind() == MccCoordPairKind::COORDS_KIND_XY) {
|
|
// hardware coordinates
|
|
MccGenXY xy;
|
|
auto ccte_err = _enteredTargetPos.toAtSameEpoch(xy);
|
|
|
|
if (!ccte_err) {
|
|
decltype(_tdataPtr->pcmCorrection) pcm_corr;
|
|
|
|
pcm_err = pcm_ptr->computePCM(xy, &pcm_corr, &_tdataPtr->targetPos);
|
|
if (pcm_err) {
|
|
_lastUpdateError =
|
|
mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
|
}
|
|
|
|
_tdataPtr->pcmReverseCorrection.pcmX = -pcm_corr.pcmX;
|
|
_tdataPtr->pcmReverseCorrection.pcmY = -pcm_corr.pcmY;
|
|
} else {
|
|
_lastUpdateError =
|
|
mcc_deduced_err(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
|
}
|
|
|
|
} else { // observed, apparent or ICRS
|
|
// _tdataPtr->targetPos = _enteredTargetPos;
|
|
using pcm_t = std::remove_pointer_t<decltype(pcm_ptr)>;
|
|
|
|
std::conditional_t<mcc_is_equatorial_mount<pcm_t::pcmMountType>,
|
|
MccSkyHADEC_OBS,
|
|
std::conditional_t<mcc_is_altaz_mount<pcm_t::pcmMountType>,
|
|
MccSkyAZZD, std::nullptr_t>>
|
|
cp;
|
|
|
|
static_assert(!std::is_null_pointer_v<decltype(cp)>, "UNKNOW MOUNT TYPE!");
|
|
|
|
// calculate target celestial coordinates (of the same type as .mountPos) at the
|
|
// epoch of the current mount position
|
|
cp.setEpoch(_tdataPtr->mountPos.epoch());
|
|
auto ccte_err = _enteredTargetPos.to(cp);
|
|
if (ccte_err) {
|
|
_lastUpdateError =
|
|
mcc_deduced_err(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
|
} else {
|
|
_tdataPtr->targetPos.from(cp);
|
|
|
|
// calculate reverse PCM corrections for the current target position and
|
|
// its encoder XY
|
|
auto pcm_err = pcm_ptr->computeInversePCM(_tdataPtr->targetPos,
|
|
&_tdataPtr->pcmReverseCorrection,
|
|
&_tdataPtr->targetXY);
|
|
if (pcm_err) {
|
|
_lastUpdateError =
|
|
mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
_lastUpdateError = mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
|
}
|
|
}
|
|
|
|
} else {
|
|
_lastUpdateError = MccTelemetryErrorCode::ERROR_NULLPTR;
|
|
}
|
|
|
|
_dataUpdatingStart->clear();
|
|
_dataUpdatingRequested->clear();
|
|
}
|
|
}
|
|
},
|
|
_updatingStopSource.get_token(), hardware, pcm);
|
|
}
|
|
|
|
MccTelemetry(MccTelemetry&&) = default;
|
|
MccTelemetry& operator=(MccTelemetry&&) = default;
|
|
|
|
MccTelemetry(const MccTelemetry&) = delete;
|
|
MccTelemetry& operator=(const MccTelemetry&) = delete;
|
|
|
|
|
|
~MccTelemetry()
|
|
{
|
|
_tdataPtr = nullptr;
|
|
|
|
_updatingStopSource.request_stop();
|
|
|
|
// trigger updating and exit from the inner loop of the hardware polling thread (see constructor)
|
|
_dataUpdatingRequested->test_and_set();
|
|
// _dataUpdatingRequested->notify_one();
|
|
_dataUpdatingRequested->notify_all();
|
|
|
|
if (_updatingFuture.valid()) {
|
|
// try to exit correctly
|
|
_updatingFuture.wait_for(std::chrono::seconds(1));
|
|
}
|
|
}
|
|
|
|
error_t setPointingTarget(mcc_skypoint_c auto const& sp)
|
|
{
|
|
_enteredTargetPos = sp;
|
|
|
|
return MccTelemetryErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
auto getPointingTarget() const
|
|
{
|
|
return _enteredTargetPos;
|
|
}
|
|
|
|
|
|
// error_t getPointingTarget(mcc_skypoint_c auto* sp)
|
|
// {
|
|
// if (sp) {
|
|
// *sp = _enteredTargetPos;
|
|
// }
|
|
|
|
// return MccTelemetryErrorCode::ERROR_OK;
|
|
// }
|
|
|
|
|
|
//
|
|
// blocks the current thread until telemetry data is received.
|
|
// the maximum blocking time is equal to the set timeout (see setTelemetryDataTimeout method)
|
|
//
|
|
error_t telemetryData(telemetry_data_t* tdata)
|
|
{
|
|
std::lock_guard lock{*_updateMutex};
|
|
|
|
_lastUpdateError = MccTelemetryErrorCode::ERROR_OK;
|
|
|
|
_tdataPtr = tdata;
|
|
|
|
// trigger updating
|
|
_dataUpdatingRequested->test_and_set();
|
|
_dataUpdatingRequested->notify_one();
|
|
|
|
// wait for updating start
|
|
_dataUpdatingStart->wait(false);
|
|
if (_timeoutMutex->try_lock_for(_telemetryDataTimeout->load())) {
|
|
_timeoutMutex->unlock();
|
|
} else {
|
|
_lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
|
|
}
|
|
|
|
return _lastUpdateError;
|
|
}
|
|
|
|
|
|
//
|
|
// Set a timeout for the telemetry receiving process
|
|
//
|
|
template <traits::mcc_time_duration_c DT>
|
|
void setTelemetryDataTimeout(DT const& timeout)
|
|
{
|
|
if constexpr (std::floating_point<typename DT::rep>) {
|
|
if (utils::isEqual(timeout.count(), 0.0) || timeout.count() < 0.0) {
|
|
return;
|
|
}
|
|
} else {
|
|
if (timeout.count() <= 0.0) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
*_telemetryDataTimeout = std::chrono::duration_cast<std::chrono::nanoseconds>(timeout);
|
|
}
|
|
|
|
template <traits::mcc_time_duration_c DT>
|
|
DT getTelemetryDataTimeout() const
|
|
{
|
|
return std::chrono::duration_cast<DT>(_telemetryDataTimeout->load());
|
|
}
|
|
|
|
std::chrono::milliseconds getTelemetryDataTimeout() const
|
|
{
|
|
return getTelemetryDataTimeout<std::chrono::milliseconds>();
|
|
}
|
|
|
|
private:
|
|
// HARDWARE_T* _hardware{nullptr};
|
|
MccSkyPoint _enteredTargetPos{};
|
|
|
|
// std::function<error_t(telemetry_data_t*)> _updateFunc{};
|
|
telemetry_data_t* _tdataPtr;
|
|
|
|
std::unique_ptr<std::atomic_flag> _dataUpdatingRequested{new std::atomic_flag{}};
|
|
std::unique_ptr<std::atomic_flag> _dataUpdatingStart{new std::atomic_flag{}};
|
|
std::unique_ptr<std::timed_mutex> _timeoutMutex{new std::timed_mutex()};
|
|
|
|
std::stop_source _updatingStopSource{};
|
|
|
|
error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK};
|
|
|
|
std::unique_ptr<std::atomic<std::chrono::nanoseconds>> _telemetryDataTimeout{
|
|
new std::atomic<std::chrono::nanoseconds>{_defaultTelemetryDataTimeout}};
|
|
|
|
std::unique_ptr<std::mutex> _updateMutex{new std::mutex{}};
|
|
|
|
std::future<void> _updatingFuture{};
|
|
};
|
|
|
|
|
|
} // namespace mcc::impl
|