...
This commit is contained in:
parent
bc12777f18
commit
2c7d563994
@ -517,7 +517,7 @@ concept mcc_hardware_c = requires(T t, const T t_const) {
|
||||
|
||||
// a type that defines at least HW_MOVE_ERROR, HW_MOVE_STOPPED, HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING
|
||||
// and HW_MOVE_GUIDING compile-time constants. The main purpose of this type is a
|
||||
// possible tunning of hardware hardwareSetState-related commands and detect stop-state
|
||||
// possible tunning of hardware hardwareSetState-related commands and detect stop end error states from hardware
|
||||
//
|
||||
// e.g. an implementations can be as follows:
|
||||
// enum class hardware_moving_state_t: int {HW_MOVE_ERROR = -1, HW_MOVE_STOPPED = 0, HW_MOVE_SLEWING,
|
||||
@ -598,7 +598,7 @@ concept mcc_telemetry_data_c = mcc_eqt_hrz_coord_c<T> && std::default_initializa
|
||||
requires mcc_eqt_hrz_coord_c<decltype(t.target)>;
|
||||
|
||||
// t.X and t.Y (from mcc_celestial_point_c) are encoder coordinates
|
||||
// t.* from mcc_eqt_hrz_coord_c are apparent mount pointing coordinates
|
||||
// t.* from mcc_eqt_hrz_coord_c are current mount coordinates
|
||||
requires mcc_angle_c<decltype(t.speedX)>; // speed along X from hardware encoder
|
||||
requires mcc_angle_c<decltype(t.speedY)>; // speed along Y from hardware encoder
|
||||
|
||||
@ -613,6 +613,29 @@ concept mcc_telemetry_data_c = mcc_eqt_hrz_coord_c<T> && std::default_initializa
|
||||
requires mcc_angle_c<decltype(t.EO)>;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
concept mcc_tlm_data_c = mcc_eqt_hrz_coord_c<T> && std::default_initializable<T> && requires(T t) {
|
||||
// user entered target coordinates
|
||||
requires mcc_celestial_point_c<decltype(t.entered_target)>;
|
||||
|
||||
// target target coordinates
|
||||
requires mcc_eqt_hrz_coord_c<decltype(t.target)>;
|
||||
|
||||
// t.X and t.Y (from mcc_celestial_point_c) are encoder coordinates
|
||||
// t.* from mcc_eqt_hrz_coord_c are current mount coordinates
|
||||
|
||||
requires mcc_PCM_result_c<decltype(t.pcm)>; // PCM correction
|
||||
|
||||
// atmospheric refraction correction for current zenithal distance
|
||||
requires mcc_angle_c<decltype(t.refCorr)>; // for current .ZD
|
||||
|
||||
// equation of the origins (ERA-GST)
|
||||
requires mcc_angle_c<decltype(t.EO)>;
|
||||
|
||||
// local sideral time
|
||||
requires mcc_angle_c<decltype(t.LST)>;
|
||||
};
|
||||
|
||||
|
||||
static constexpr void mcc_copy_telemetry_data(mcc_telemetry_data_c auto const& from_pt,
|
||||
mcc_telemetry_data_c auto* to_pt)
|
||||
|
||||
@ -98,6 +98,293 @@ inline std::error_code make_error_code(MccTelemetryErrorCode ec)
|
||||
|
||||
|
||||
|
||||
template <mcc_hardware_c HARDWARE_T>
|
||||
class MccMountTelemetry : public mcc_telemetry_interface_t<std::error_code>
|
||||
{
|
||||
public:
|
||||
static constexpr std::chrono::milliseconds defaultDataUpdatingTimeout{100};
|
||||
|
||||
typedef std::error_code error_t;
|
||||
|
||||
struct telemetry_data_t : MccEqtHrzCoords {
|
||||
MccCelestialPoint entered_target{};
|
||||
MccEqtHrzCoords target;
|
||||
HARDWARE_T::hardware_state_t hardware;
|
||||
|
||||
double JD;
|
||||
double LST;
|
||||
double EO;
|
||||
double refCorr;
|
||||
};
|
||||
|
||||
|
||||
template <mcc_PCM_c PCM_T, mcc_ccte_c CCTE_T>
|
||||
MccMountTelemetry(HARDWARE_T* hardware, PCM_T* pcm, CCTE_T* ccte)
|
||||
{
|
||||
_data.entered_target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
|
||||
|
||||
if constexpr (mccIsEquatorialMount(PCM_T::mountType)) {
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
} else if constexpr (mccIsAltAzMount(PCM_T::mountType)) {
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
_data.target.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
} else {
|
||||
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
||||
}
|
||||
|
||||
// target coordinates updater
|
||||
_updateTargetFunc = [hardware, pcm, ccte, this](std::stop_token stop_token) -> error_t {
|
||||
MccPCMResult pcm_res;
|
||||
|
||||
mcc_tp2tp(_data.time_point, _data.target.time_point);
|
||||
mcc_tp2tp(_data.time_point, _data.entered_target.time_point);
|
||||
|
||||
if (_data.entered_target.pair_kind ==
|
||||
MccCoordPairKind::COORDS_KIND_XY) { // compute corresponded observed coordinates
|
||||
auto pcm_err = pcm->computePCM(_data.entered_target, &pcm_res, &_data.target);
|
||||
if (pcm_err) {
|
||||
return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
||||
}
|
||||
|
||||
_data.target.X = _data.entered_target.X;
|
||||
_data.target.Y = _data.entered_target.Y;
|
||||
|
||||
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!!!");
|
||||
}
|
||||
|
||||
auto ccte_err = ccte->transformCoordinates(_data.target, &_data.target);
|
||||
if (ccte_err) {
|
||||
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
} else {
|
||||
_data.target.pair_kind = _data.entered_target.pair_kind;
|
||||
|
||||
auto ccte_err = ccte->transformCoordinates(_data.entered_target, &_data.target);
|
||||
if (ccte_err) {
|
||||
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
|
||||
auto pcm_err = pcm->computeReversePCM(_data.target, &pcm_res, &_data.target);
|
||||
if (pcm_err) {
|
||||
return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
||||
}
|
||||
}
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
};
|
||||
|
||||
// mount current coordinates updater
|
||||
_updateFunc = [hardware, pcm, ccte, this](std::stop_token stop_token) -> std::error_code {
|
||||
// std::lock_guard lock{*_updateMutex};
|
||||
|
||||
// first, update mount quantities
|
||||
auto hw_err = hardware->hardwareGetState(&_data.hardware);
|
||||
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;
|
||||
|
||||
mcc_tp2tp(_data.hardware.time_point, _data.time_point);
|
||||
_data.X = _data.hardware.X;
|
||||
_data.Y = _data.hardware.Y;
|
||||
|
||||
auto ccte_err = _data->timepointToJulday(_data.time_point, &_data.JD);
|
||||
if (!ccte_err) {
|
||||
if (stop_token.stop_requested()) {
|
||||
return MccTelemetryErrorCode::ERROR_UPDATE_STOPPED;
|
||||
}
|
||||
|
||||
ccte_err = ccte_err->juldayToAppSideral(_data.JD, &_data.LST, true);
|
||||
if (!ccte_err) {
|
||||
ccte_err = ccte_err->equationOrigins(_data.JD, &eo);
|
||||
_data.EO = 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;
|
||||
// }
|
||||
|
||||
|
||||
MccCelestialPoint pt;
|
||||
mcc_tp2tp(_data.time_point, pt.time_point);
|
||||
|
||||
auto pcm_err = pcm->computePCM(_data, &_data, &pt);
|
||||
if (pcm_err) {
|
||||
return mcc_deduce_error_code(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
|
||||
}
|
||||
|
||||
if constexpr (mccIsEquatorialMount(PCM_T::mountType)) {
|
||||
pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
} else if constexpr (mccIsAltAzMount(PCM_T::mountType)) {
|
||||
pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
|
||||
_data.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
|
||||
} else {
|
||||
static_assert(false, "UNKNOWN MOUNT TYPE!");
|
||||
}
|
||||
|
||||
ccte_err = ccte->transformCoordinates(pt, &_data);
|
||||
if (!ccte_err) {
|
||||
ccte_err = ccte->refractionCorrection(_data, &_data.refCorr);
|
||||
if (!ccte_err) {
|
||||
return _updateTargetFunc(stop_token);
|
||||
} else {
|
||||
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
} else {
|
||||
return mcc_deduce_error_code(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
|
||||
}
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
};
|
||||
|
||||
|
||||
_internalUpdatingStopSource = std::stop_source{};
|
||||
|
||||
_dataUpdatingRequested->clear();
|
||||
_dataUpdatingStart->clear();
|
||||
|
||||
// start thread for data updating
|
||||
_updatingFuture = std::async(
|
||||
std::launch::async,
|
||||
[this](std::stop_token stoken) {
|
||||
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(stoken);
|
||||
|
||||
_dataUpdatingStart->clear();
|
||||
_dataUpdatingRequested->clear();
|
||||
}
|
||||
}
|
||||
},
|
||||
_internalUpdatingStopSource.get_token());
|
||||
}
|
||||
|
||||
|
||||
virtual ~MccMountTelemetry() = default;
|
||||
|
||||
|
||||
void setDataUpdatingTimeout(traits::mcc_time_duration_c auto const& timeout)
|
||||
{
|
||||
_dataUpdatingTimeout = std::chrono::duration_cast<decltype(_dataUpdatingTimeout)>(timeout);
|
||||
}
|
||||
|
||||
template <traits::mcc_time_duration_c DT>
|
||||
DT getDataUpdatingTimeout() const
|
||||
{
|
||||
return std::chrono::duration_cast<DT>(_dataUpdatingTimeout);
|
||||
}
|
||||
|
||||
std::chrono::milliseconds getDataUpdatingTimeout() const
|
||||
{
|
||||
return getDataUpdatingTimeout<std::chrono::milliseconds>();
|
||||
}
|
||||
|
||||
|
||||
error_t telemetryData(telemetry_data_t* tdata)
|
||||
{
|
||||
if (tdata == nullptr) {
|
||||
return MccTelemetryErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
// trigger updating
|
||||
_dataUpdatingRequested->test_and_set();
|
||||
_dataUpdatingRequested->notify_one();
|
||||
|
||||
// wait for updating start
|
||||
_dataUpdatingStart->wait(false);
|
||||
if (_timeoutMutex->try_lock_for(_dataUpdatingTimeout)) {
|
||||
_timeoutMutex->unlock();
|
||||
} else {
|
||||
_lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT;
|
||||
}
|
||||
|
||||
if (!_lastUpdateError) {
|
||||
std::lock_guard lock(*_updateMutex);
|
||||
*tdata = _data;
|
||||
}
|
||||
|
||||
return _lastUpdateError;
|
||||
}
|
||||
|
||||
error_t setPointingTarget(mcc_celestial_point_c auto const& cp)
|
||||
{
|
||||
std::lock_guard lock(*_updateMutex);
|
||||
|
||||
mcc_copy_celestial_point(cp, &_data.entered_target);
|
||||
|
||||
return MccTelemetryErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
protected:
|
||||
telemetry_data_t _data;
|
||||
|
||||
std ::function<error_t(std::stop_token)> _updateTargetFunc{};
|
||||
std::function<error_t(std::stop_token)> _updateFunc{};
|
||||
|
||||
std::unique_ptr<std::mutex> _updateMutex;
|
||||
std::unique_ptr<std::condition_variable> _updateCondVar;
|
||||
|
||||
std::stop_source _internalUpdatingStopSource{};
|
||||
|
||||
std::future<void> _updatingFuture{};
|
||||
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::chrono::nanoseconds _dataUpdatingTimeout{defaultDataUpdatingTimeout};
|
||||
|
||||
error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK};
|
||||
|
||||
void updateLoop(std::stop_token stoken)
|
||||
{
|
||||
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(stoken);
|
||||
|
||||
_dataUpdatingStart->clear();
|
||||
_dataUpdatingRequested->clear();
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class MccTelemetry : public mcc_telemetry_interface_t<std::error_code>
|
||||
{
|
||||
protected:
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user