308 lines
10 KiB
C++
308 lines
10 KiB
C++
#pragma once
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
|
|
/* SIMPLE Tracking MODEL IMPLEMENTATION */
|
|
|
|
|
|
#include "mcc_defaults.h"
|
|
#include "mcc_moving_model_common.h"
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
enum class MccSimpleTrackingModelErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_CCTE,
|
|
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::MccSimpleTrackingModelErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
class MccSimpleTrackingModel
|
|
{
|
|
public:
|
|
typedef std::error_code error_t;
|
|
|
|
typedef MccSimpleMovingModelParams tracking_params_t;
|
|
|
|
|
|
template <mcc_all_controls_c CONTROLS_T>
|
|
MccSimpleTrackingModel(CONTROLS_T* controls)
|
|
: _stopTracking(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
|
|
{
|
|
_trackingFunc = [controls, this]() -> error_t {
|
|
typename CONTROLS_T::hardware_state_t hw_state;
|
|
|
|
MccTelemetryData tdata;
|
|
MccEqtHrzCoords intsc_coords;
|
|
MccCelestialPoint target_in_future_pt;
|
|
|
|
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
|
|
target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
|
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
|
|
target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
|
} else {
|
|
static_assert(false, "UNKNOW MOUNT TYPE!");
|
|
}
|
|
|
|
// double dist, dx, dy;
|
|
|
|
auto t_err = controls->telemetryData(&tdata);
|
|
if (t_err) {
|
|
return mcc_deduce_error<error_t>(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
|
|
}
|
|
|
|
|
|
bool no_intersects = false;
|
|
|
|
// function to update the closest prohibited zone intersect point
|
|
auto update_pzones_ipoint = [controls, &tdata, &intsc_coords, &no_intersects, &hw_state,
|
|
this]() -> error_t {
|
|
// compute intersection points with the prohibited zones
|
|
auto pz_err = mcc_find_closest_pzone(controls, tdata, &intsc_coords);
|
|
if (pz_err) {
|
|
return mcc_deduce_error<error_t>(pz_err,
|
|
MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
|
|
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
|
|
if (std::isfinite(intsc_coords.HA)) {
|
|
intsc_coords.X = intsc_coords.HA;
|
|
intsc_coords.Y = intsc_coords.DEC_APP;
|
|
} else {
|
|
no_intersects = true;
|
|
// intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min
|
|
// intsc_coords.Y = tdata.DEC_APP;
|
|
}
|
|
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
|
|
if (std::isfinite(intsc_coords.AZ)) {
|
|
intsc_coords.X = intsc_coords.AZ;
|
|
intsc_coords.Y = intsc_coords.ZD;
|
|
} else {
|
|
no_intersects = true;
|
|
}
|
|
} else {
|
|
static_assert(false, "UNKNOW MOUNT TYPE!");
|
|
}
|
|
};
|
|
|
|
|
|
auto target_point = [&, this](MccCelestialPoint* point) {
|
|
auto dt = std::chrono::duration<double>{tdata.HA} +
|
|
_currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds
|
|
|
|
auto tp_dt = std::chrono::duration_cast<typename decltype(tdata.time_point)::duration>(
|
|
_currentParams.timeShiftToTargetPoint);
|
|
|
|
// point in +time_dist future
|
|
MccCelestialPoint pt{
|
|
.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP,
|
|
.X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0).normalize<MccAngle::NORM_KIND_0_360>(),
|
|
.Y = tdata.DEC_APP};
|
|
mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point);
|
|
|
|
point->time_point = pt.time_point;
|
|
|
|
// check for prohibited zone
|
|
if (std::isfinite(intsc_coords.HA)) {
|
|
bool through_pzone =
|
|
(intsc_coords.HA - pt.X) <= 0; // must be <= 0 if point in future will be in the zone
|
|
through_pzone &=
|
|
(intsc_coords.HA - tdata.HA) > 0; // must be > 0 if point in future was out of the zone
|
|
|
|
if (through_pzone) {
|
|
pt.X = intsc_coords.HA;
|
|
}
|
|
}
|
|
|
|
auto ret = controls->transformCoordinates(std::move(pt), point);
|
|
if (ret) {
|
|
return mcc_deduce_error<error_t>(ret, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
|
|
} else {
|
|
MccPCMResult pcm_inv_res;
|
|
|
|
// endpoint of the mount moving
|
|
auto pcm_err = controls->computeInversePCM(target_in_future_pt, &pcm_inv_res, &hw_state);
|
|
if (pcm_err) {
|
|
return mcc_deduce_error<error_t>(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
|
|
}
|
|
|
|
mcc_tp2tp(tdata.time_point, hw_state.time_point);
|
|
}
|
|
|
|
return MccSimpleTrackingModelErrorCode::ERROR_OK;
|
|
};
|
|
|
|
|
|
auto pz_err = update_pzones_ipoint();
|
|
if (pz_err) {
|
|
return mcc_deduce_error<error_t>(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
|
|
hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_Tracking;
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
auto ccte_err = target_point(&target_in_future_pt);
|
|
if (ccte_err) {
|
|
return mcc_deduce_error(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
|
|
}
|
|
|
|
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
|
|
hw_state.speedX = _currentParams.trackSpeedX;
|
|
hw_state.speedY = _currentParams.trackSpeedY;
|
|
}
|
|
}
|
|
|
|
// move mount
|
|
auto hw_err = controls->hardwareSetState(hw_state);
|
|
if (hw_err) {
|
|
return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
|
|
}
|
|
|
|
|
|
std::chrono::steady_clock::time_point last_corr_tp, last_ipzone_update_tp;
|
|
|
|
while (*_stopTracking) {
|
|
// wait for updated telemetry data
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
|
|
|
|
if (t_err) {
|
|
return mcc_deduce_error<error_t>(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
|
|
}
|
|
}
|
|
|
|
if (*_stopTracking) {
|
|
break;
|
|
}
|
|
|
|
// control prohibited zones
|
|
if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) {
|
|
return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
|
|
}
|
|
if (pz_err) {
|
|
return mcc_deduce_error<error_t>(pz_err,
|
|
MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
|
|
if (*_stopTracking) {
|
|
break;
|
|
}
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
auto now = std::chrono::steady_clock::now();
|
|
|
|
if ((now - last_corr_tp) < _currentParams.trackingCycleInterval) {
|
|
continue;
|
|
}
|
|
|
|
// update prohibited zones intersection point
|
|
if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) {
|
|
pz_err = update_pzones_ipoint();
|
|
if (pz_err) {
|
|
return mcc_deduce_error<error_t>(
|
|
pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
}
|
|
|
|
// compute new target-in-future point
|
|
auto ccte_err = target_point(&target_in_future_pt);
|
|
if (ccte_err) {
|
|
return mcc_deduce_error(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
|
|
}
|
|
}
|
|
|
|
// send corrections
|
|
hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_Tracking;
|
|
hw_err = controls->hardwareSetState(hw_state);
|
|
if (hw_err) {
|
|
return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
|
|
}
|
|
}
|
|
|
|
return MccSimpleTrackingModelErrorCode::ERROR_OK;
|
|
};
|
|
}
|
|
|
|
MccSimpleTrackingModel(MccSimpleTrackingModel&&) = default;
|
|
MccSimpleTrackingModel& operator=(MccSimpleTrackingModel&&) = default;
|
|
|
|
MccSimpleTrackingModel(const MccSimpleTrackingModel&) = delete;
|
|
MccSimpleTrackingModel& operator=(const MccSimpleTrackingModel&) = delete;
|
|
|
|
error_t trackTarget()
|
|
{
|
|
*_stopTracking = false;
|
|
|
|
return _trackingFunc();
|
|
}
|
|
|
|
error_t stoptTracking()
|
|
{
|
|
*_stopTracking = true;
|
|
|
|
return MccSimpleTrackingModelErrorCode::ERROR_OK;
|
|
}
|
|
|
|
error_t setTrackingParams(tracking_params_t params)
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
_currentParams = std::move(params);
|
|
|
|
return MccSimpleTrackingModelErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
tracking_params_t getTrackingParams() const
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
return _currentParams;
|
|
}
|
|
|
|
|
|
protected:
|
|
std::function<error_t()> _trackingFunc{};
|
|
std::unique_ptr<std::atomic_bool> _stopTracking;
|
|
|
|
tracking_params_t _currentParams{};
|
|
std::unique_ptr<std::mutex> _currentParamsMutex{};
|
|
};
|
|
|
|
} // namespace mcc
|