164 lines
4.6 KiB
C++
164 lines
4.6 KiB
C++
#pragma once
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
|
|
/* SIMPLE TRACKING MODEL IMPLEMENTATION */
|
|
|
|
|
|
#include "mcc_defaults.h"
|
|
#include "mcc_generics.h"
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
enum class MccSimpleTrackingModelErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_HW_GETSTATE,
|
|
ERROR_HW_SETSTATE,
|
|
ERROR_TELEMETRY_TIMEOUT,
|
|
ERROR_PZONE_CONTAINER_COMP,
|
|
ERROR_IN_PZONE,
|
|
ERROR_UNEXPECTED_AXIS_RATES
|
|
};
|
|
|
|
} // namespace mcc
|
|
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::MccSimpleTrackingModelErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
template <MccMountType MOUNT_TYPE>
|
|
class MccSimpleTrackingModel
|
|
{
|
|
public:
|
|
static constexpr MccMountType mountType = MOUNT_TYPE;
|
|
|
|
typedef std::error_code error_t;
|
|
|
|
struct tracking_params_t {
|
|
static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second
|
|
|
|
double trackSpeedX{};
|
|
double trackSpeedY{};
|
|
|
|
std::chrono::seconds telemetryTimeout{3};
|
|
};
|
|
|
|
template <mcc_generic_mount_c MountT>
|
|
MccSimpleTrackingModel(MountT* mount) : _stopTracking(new std::atomic_bool())
|
|
{
|
|
*_stopTracking = false;
|
|
|
|
if constexpr (mccIsEquatorialMount(mountType)) {
|
|
_currentTrackParams.trackSpeedX = tracking_params_t::sideralRate; // move along HA-axis with sideral rate
|
|
_currentTrackParams.trackSpeedY = 0.0;
|
|
}
|
|
|
|
_trackingFunc = [mount, this]() -> error_t {
|
|
typename MountT::hardware_state_t hw_state;
|
|
|
|
if constexpr (mccIsEquatorialMount(mountType)) {
|
|
// just set sideral rate once
|
|
auto err = mount->hardwareGetState(&hw_state);
|
|
if (err) {
|
|
return mcc_deduce_error(err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
|
|
}
|
|
|
|
// hw_state.time_point;
|
|
hw_state.speedX = _currentTrackParams.trackSpeedX;
|
|
hw_state.speedY = _currentTrackParams.trackSpeedY;
|
|
hw_state.moving_type = MountT::hardware_moving_state_t::HW_MOVE_TRACKING;
|
|
|
|
err = mount->hardwareSetState(std::move(hw_state));
|
|
if (err) {
|
|
return mcc_deduce_error(err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
|
|
}
|
|
|
|
// control prohibited zones
|
|
MccTelemetryData tdata;
|
|
std::vector<bool> pz_flags;
|
|
bool inzone_flag;
|
|
|
|
while (!*_stopTracking) {
|
|
auto t_err = mount->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout);
|
|
if (t_err) {
|
|
return mcc_deduce_error(t_err, MccSimpleTrackingModelErrorCode::ERROR_TELEMETRY_TIMEOUT);
|
|
}
|
|
|
|
// check for current axis speed
|
|
if (utils::isEqual(tdata.speedX, 0.0) && utils::isEqual(tdata.speedY, 0.0)) {
|
|
// unhandled stop state?!!!
|
|
return MccSimpleTrackingModelErrorCode::ERROR_UNEXPECTED_AXIS_RATES;
|
|
}
|
|
|
|
auto pz_err = mount->inPZone(tdata, &inzone_flag, &pz_flags);
|
|
if (pz_err) {
|
|
return mcc_deduce_error(t_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
|
|
if (inzone_flag) {
|
|
// logging
|
|
return MccSimpleTrackingModelErrorCode::ERROR_IN_PZONE;
|
|
}
|
|
}
|
|
|
|
return MccSimpleTrackingModelErrorCode::ERROR_OK;
|
|
} else if constexpr (mccIsAltAzMount(mountType)) {
|
|
static_assert(false, "NOT IMPLEMENTED!");
|
|
} else {
|
|
static_assert(false, "UNKNOW MOUNT TYPE!");
|
|
}
|
|
};
|
|
}
|
|
|
|
MccSimpleTrackingModel(MccSimpleTrackingModel&&) = default;
|
|
MccSimpleTrackingModel& operator=(MccSimpleTrackingModel&&) = default;
|
|
|
|
|
|
error_t trackMount()
|
|
{
|
|
return _trackingFunc();
|
|
}
|
|
|
|
|
|
error_t stopTracking()
|
|
{
|
|
*_stopTracking = true;
|
|
|
|
return MccSimpleTrackingModelErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
error_t setTrackingParams(tracking_params_t params)
|
|
{
|
|
_currentTrackParams = std::move(params);
|
|
}
|
|
|
|
tracking_params_t getTrackingParams()
|
|
{
|
|
return _currentTrackParams;
|
|
}
|
|
|
|
protected:
|
|
std::function<error_t()> _trackingFunc{};
|
|
std::unique_ptr<std::atomic_bool> _stopTracking{};
|
|
|
|
tracking_params_t _currentTrackParams;
|
|
};
|
|
|
|
} // namespace mcc
|