153 lines
4.4 KiB
C++
153 lines
4.4 KiB
C++
#pragma once
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
|
|
/* SIMPLE GUIDING MODEL IMPLEMENTATION */
|
|
|
|
|
|
#include "mcc_defaults.h"
|
|
#include "mcc_moving_model_common.h"
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
enum class MccSimpleGuidingModelErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_HW_GETSTATE,
|
|
ERROR_HW_SETSTATE,
|
|
ERROR_PCM_COMP,
|
|
ERROR_GET_TELEMETRY,
|
|
ERROR_DIST_TELEMETRY,
|
|
ERROR_DIFF_TELEMETRY,
|
|
ERROR_PZONE_CONTAINER_COMP,
|
|
ERROR_IN_PZONE,
|
|
ERROR_NEAR_PZONE,
|
|
ERROR_TIMEOUT,
|
|
ERROR_UNEXPECTED_AXIS_RATES,
|
|
ERROR_STOPPED
|
|
};
|
|
|
|
} // namespace mcc
|
|
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::MccSimpleGuidingModelErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
class MccSimpleGuidingModel
|
|
{
|
|
public:
|
|
typedef std::error_code error_t;
|
|
|
|
typedef MccSimpleMovingModelParams guiding_params_t;
|
|
|
|
|
|
template <mcc_telemetry_data_c TelemetryT, mcc_hardware_c HardwareT, mcc_pzone_container_c PZoneContT>
|
|
MccSimpleGuidingModel(TelemetryT* telemetry, HardwareT* hardware, PZoneContT* pz_cont)
|
|
: _stopGuiding(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
|
|
{
|
|
_guidingFunc = [telemetry, hardware, pz_cont, this]() -> error_t {
|
|
typename TelemetryT::error_t t_err;
|
|
|
|
MccCelestialPoint cpt;
|
|
typename HardwareT::hardware_state_t hw_state;
|
|
|
|
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
|
|
cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
|
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
|
|
cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
|
|
static_assert(false, "NOT IMPLEMENTED!");
|
|
} else {
|
|
static_assert(false, "UNKNOW MOUNT TYPE!");
|
|
}
|
|
|
|
|
|
|
|
MccTelemetryData tdata;
|
|
|
|
std::vector<std::chrono::duration<double>> pz_timeto; // in seconds
|
|
std::chrono::duration<double> min_time{0.0};
|
|
std::vector<MccCelestialPoint> intsc_pt(pz_cont->sizePZones(), cpt);
|
|
|
|
// compute intersection points with the prohibited zones
|
|
auto pz_err = pz_cont->intersectPZone(tdata, &intsc_pt);
|
|
if (pz_err) {
|
|
return mcc_deduce_error<error_t>(pz_err, MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
|
|
while (*_stopGuiding) {
|
|
// wait for updated telemetry data
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
|
|
|
|
if (t_err) {
|
|
return mcc_deduce_error<error_t>(t_err, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY);
|
|
}
|
|
}
|
|
|
|
// control prohibited zones
|
|
pz_err = pz_cont->timeToPZone(tdata, &pz_timeto);
|
|
if (pz_err) {
|
|
return mcc_deduce_error<error_t>(pz_err,
|
|
MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
|
|
min_time = std::chrono::duration<double>{0};
|
|
for (size_t i = 0; i < pz_cont->sizePZones(); ++i) {
|
|
if (pz_timeto[i] < _currentParams.minTimeToPZone) {
|
|
return MccSimpleGuidingModelErrorCode::ERROR_NEAR_PZONE;
|
|
}
|
|
if (pz_timeto[i] < min_time) {
|
|
min_time = pz_timeto[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
|
};
|
|
}
|
|
|
|
MccSimpleGuidingModel(MccSimpleGuidingModel&&) = default;
|
|
MccSimpleGuidingModel& operator=(MccSimpleGuidingModel&&) = default;
|
|
|
|
MccSimpleGuidingModel(const MccSimpleGuidingModel&) = delete;
|
|
MccSimpleGuidingModel& operator=(const MccSimpleGuidingModel&) = delete;
|
|
|
|
error_t startGuidingTarget()
|
|
{
|
|
*_stopGuiding = false;
|
|
|
|
return _guidingFunc();
|
|
}
|
|
|
|
error_t stoptGuidingTarget()
|
|
{
|
|
*_stopGuiding = true;
|
|
|
|
return MccSimpleGuidingModelErrorCode::ERROR_OK;
|
|
}
|
|
|
|
protected:
|
|
std::function<error_t()> _guidingFunc{};
|
|
std::unique_ptr<std::atomic_bool> _stopGuiding;
|
|
|
|
guiding_params_t _currentParams{};
|
|
std::unique_ptr<std::mutex> _currentParamsMutex{};
|
|
};
|
|
|
|
} // namespace mcc
|