...
This commit is contained in:
Timur A. Fatkhullin
369
mcc_telemetry.h
Normal file
369
mcc_telemetry.h
Normal file
@@ -0,0 +1,369 @@
|
||||
#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:
|
||||
typedef HARDWARE_T hardware_t;
|
||||
|
||||
typedef std::error_code error_t;
|
||||
|
||||
|
||||
struct telemetry_data_t {
|
||||
MccSkyPoint targetPos{};
|
||||
|
||||
MccSkyPoint mountPos{};
|
||||
|
||||
typename HARDWARE_T::hardware_state_t hwState{};
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
} else {
|
||||
_lastUpdateError =
|
||||
mcc_deduced_err(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
} else { // observed, apparent or ICRS
|
||||
_tdataPtr->targetPos = _enteredTargetPos;
|
||||
}
|
||||
|
||||
} 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 setTarget(mcc_skypoint_c auto const& sp)
|
||||
{
|
||||
_enteredTargetPos = sp;
|
||||
|
||||
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
|
||||
//
|
||||
void setTelemetryDataTimeout(traits::mcc_time_duration_c auto const& timeout)
|
||||
{
|
||||
if constexpr (std::floating_point<typename decltype(timeout)::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
|
||||
Reference in New Issue
Block a user