eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

remove guiding model

Browse Source 
now it are only slewing and tracking states
This commit is contained in:
Timur A. Fatkhullin 2025-09-03 18:28:52 +03:00
parent 460fc360c6
commit 2478c1e8d2
14 changed files with 757 additions and 698 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -14,3 +14,4 @@ set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
add_subdirectory(cxx)
add_subdirectory(mcc)
add_subdirectory(asibfm700)

15
asibfm700/CMakeLists.txt Normal file
View File

@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.14)
# set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_STANDARD 23)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")
set(ASIBFM700_LIB_SRC asibfm700_common.h asibfm700_servocontroller.h asibfm700_servocontroller.cpp)
set(ASIBFM700_LIB asibfm700mount)
add_library(${ASIBFM700_LIB} STATIC ${ASIBFM700_LIB_SRC})
target_link_libraries(${ASIBFM700_LIB} PRIVATE mcc1)

11
asibfm700/asibfm700_common.h Normal file
View File

@ -0,0 +1,11 @@
#pragma once
/* AstroSib FORK MOUNT FM-700 CONTROL LIBRARY */
/* COMMON LIBRARY DEFINITIONS */
namespace asibfm700
{
} // namespace asibfm700

84
asibfm700/asibfm700_servocontroller.cpp Normal file
View File

@ -0,0 +1,84 @@
#include "asibfm700_servocontroller.h"
namespace asibfm700
{
AsibFM700ServoController::AsibFM700ServoController() : _hardwareConfig(), _setStateMutex(new std::mutex) {}
AsibFM700ServoController::AsibFM700ServoController(hardware_config_t config) : AsibFM700ServoController()
{
_hardwareConfig = std::move(config);
_hardwareConfig.devConfig.MountDevPath = const_cast<char*>(_hardwareConfig.MountDevPath.c_str());
_hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
_hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
_hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
}
constexpr std::string_view AsibFM700ServoController::hardwareName() const
{
return "Sidereal-ServoControllerII";
}
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareStop()
{
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.stop());
}
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareInit()
{
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.init(&_hardwareConfig.devConfig));
}
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(hardware_state_t state)
{
// time point from sidservo library is 'double' number represented UNIXTIME with
// microseconds/nanoseconds precision
double tp = std::chrono::duration<double>(state.time_point.time_since_epoch()).count();
std::lock_guard lock{*_setStateMutex};
// according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
coordval_pair_t cvalpair{.X{.val = state.Y, .t = tp}, .Y{.val = state.X, .t = tp}};
coordpair_t cpair{.X = state.Y, .Y = state.X};
// correctTo is asynchronous function!!!
//
// according to the Eddy's implementation of the LibSidServo library it is safe
// to pass the addresses of 'cvalpair' and 'cpair' automatic variables
auto err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.correctTo(&cvalpair, &cpair));
return err;
}
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(hardware_state_t* state)
{
using tp_t = decltype(hardware_state_t::time_point);
mountdata_t mdata;
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.getMountData(&mdata));
if (!err) {
// time point from sidservo library is 'double' number represented UNIXTIME with
// microseconds/nanoseconds precision (must be equal for encXposition and encYposition)
using secs_t = std::chrono::duration<double>;
secs_t secs = secs_t{mdata.encXposition.t};
state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
// according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis
state->X = mdata.encYposition.val;
state->Y = mdata.encXposition.val;
state->speedX = mdata.encYspeed.val;
state->speedY = mdata.encXspeed.val;
}
return err;
}
} // namespace asibfm700

124
asibfm700/asibfm700_servocontroller.h Normal file
View File

@ -0,0 +1,124 @@
#pragma once
#include <mcc_defaults.h>
#include <mcc_generics.h>
#include "../LibSidServo/sidservo.h"
namespace asibfm700
{
/* error codes enum definition */
enum class AsibFM700ServoControllerErrorCode : int {
// error codes from sidservo library
ERROR_OK = MCC_E_OK,
ERROR_FATAL = MCC_E_FATAL,
ERROR_BADFORMAT = MCC_E_BADFORMAT,
ERROR_ENCODERDEV = MCC_E_ENCODERDEV,
ERROR_MOUNTDEV = MCC_E_MOUNTDEV,
ERROR_FAILED = MCC_E_FAILED,
// my codes ...
};
// error category
struct AsibFM700HardwareErrorCategory : public std::error_category {
const char* name() const noexcept;
std::string message(int ec) const;
static const AsibFM700HardwareErrorCategory& get();
};
static inline std::error_code make_error_code(AsibFM700ServoControllerErrorCode ec)
{
return std::error_code(static_cast<int>(ec), AsibFM700HardwareErrorCategory::get());
}
} // namespace asibfm700
namespace std
{
template <>
class is_error_code_enum<asibfm700::AsibFM700ServoControllerErrorCode> : public true_type
{
};
} // namespace std
namespace asibfm700
{
class AsibFM700ServoController final
{
public:
typedef std::error_code error_t;
enum class hardware_moving_state_t : int {
HW_MOVE_STOPPED,
HW_MOVE_SLEWING,
HW_MOVE_ADJUSTING,
HW_MOVE_TRACKING,
HW_MOVE_GUIDING
};
struct hardware_state_t {
static constexpr mcc::MccCoordPairKind pair_kind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
mcc::MccTimePoint time_point;
double X, Y, speedX, speedY;
hardware_moving_state_t moving_state;
};
struct hardware_config_t {
// the 'char*' fields from conf_t:
// wrap it to std::string
std::string MountDevPath;
std::string EncoderDevPath;
std::string EncoderXDevPath;
std::string EncoderYDevPath;
conf_t devConfig;
hardware_configuration_t hwConfig;
};
/* constructors and destructor */
AsibFM700ServoController();
AsibFM700ServoController(hardware_config_t config);
AsibFM700ServoController(const AsibFM700ServoController&) = delete;
AsibFM700ServoController& operator=(const AsibFM700ServoController&) = delete;
AsibFM700ServoController(AsibFM700ServoController&&);
AsibFM700ServoController& operator=(AsibFM700ServoController&&);
~AsibFM700ServoController();
/* public methods */
constexpr std::string_view hardwareName() const;
error_t hardwareSetState(hardware_state_t state);
error_t hardwareGetState(hardware_state_t* state);
error_t hardwareStop();
error_t hardwareInit();
void hardwareUpdateConfig(conf_t cfg);
void hardwareUpdateConfig(hardware_configuration_t cfg);
private:
hardware_config_t _hardwareConfig;
std::unique_ptr<std::mutex> _setStateMutex;
};
} // namespace asibfm700

8
mcc/CMakeLists.txt
View File

@ -70,8 +70,7 @@ include_directories(${BSPLINES_INCLUDE_DIR})
set(MCC_LIBRARY_SRC1 mcc_generics.h mcc_defaults.h mcc_traits.h mcc_utils.h
mcc_ccte_iers.h mcc_ccte_iers_default.h mcc_ccte_erfa.h mcc_pcm.h mcc_telemetry.h
mcc_angle.h mcc_pzone.h mcc_pzone_container.h mcc_finite_state_machine.h
mcc_generic_mount.h mcc_tracking_model.h mcc_slewing_model.h mcc_guiding_model.h
mcc_moving_model_common.h)
mcc_generic_mount.h mcc_tracking_model.h mcc_slewing_model.h mcc_moving_model_common.h)
list(APPEND MCC_LIBRARY_SRC1 mcc_spdlog.h)
@ -79,6 +78,11 @@ set(MCC_LIBRARY1 mcc1)
add_library(${MCC_LIBRARY1} INTERFACE ${MCC_LIBRARY_SRC1})
target_compile_features(${MCC_LIBRARY1} INTERFACE cxx_std_23)
target_include_directories(${MCC_LIBRARY1} INTERFACE ${ERFA_INCLUDE_DIR} ${BSPLINES_INCLUDE_DIR})
target_include_directories(${MCC_LIBRARY1} INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}/mcc>
)
option(WITH_TESTS "Build tests" ON)

102
mcc/mcc_generic_mount.h
View File

@ -37,14 +37,14 @@ template <mcc_hardware_c HardwareT,
mcc_pzone_container_c PZoneContT,
mcc_slewing_model_c SlewModelT,
mcc_tracking_model_c TrackModelT,
mcc_guiding_model_c GuidingModelT,
// mcc_guiding_model_c GuidingModelT,
mcc_logger_c LoggerT = MccNullLogger>
class MccGenericMount : public HardwareT,
public TelemetryT,
public PZoneContT,
public SlewModelT,
public TrackModelT,
public GuidingModelT,
// public GuidingModelT,
public LoggerT
{
public:
@ -56,7 +56,7 @@ public:
using LoggerT::logWarn;
using typename GuidingModelT::guiding_params_t;
// using typename GuidingModelT::guiding_params_t;
using typename SlewModelT::slewing_params_t;
using typename TrackModelT::tracking_params_t;
@ -65,14 +65,14 @@ public:
PZoneContT pzone_cont,
SlewModelT slew_model,
TrackModelT track_model,
GuidingModelT guiding_model,
// GuidingModelT guiding_model,
LoggerT logger = MccNullLogger{})
: HardwareT(std::move(hardware)),
TelemetryT(std::move(telemetry)),
PZoneContT(std::move(pzone_cont)),
SlewModelT(std::move(slew_model)),
TrackModelT(std::move(track_model)),
GuidingModelT(std::move(guiding_model)),
// GuidingModelT(std::move(guiding_model)),
LoggerT(std::move(logger))
{
}
@ -83,7 +83,7 @@ public:
{
logInfo("stop any movements ...");
this->stopGuidingTarget();
// this->stopGuidingTarget();
this->stopTracking();
this->stopSlewing();
@ -119,15 +119,16 @@ template <mcc_hardware_c HardwareT,
mcc_pzone_container_c PZoneContT,
mcc_slewing_model_c SlewModelT,
mcc_tracking_model_c TrackModelT,
mcc_guiding_model_c GuidingModelT,
// mcc_guiding_model_c GuidingModelT,
mcc_logger_c LoggerT = MccNullLogger>
class MccGenericMountFSM
: public fsm::MccFiniteStateMachine,
public MccGenericMount<HardwareT, TelemetryT, PZoneContT, SlewModelT, TrackModelT, GuidingModelT, LoggerT>
// public MccGenericMount<HardwareT, TelemetryT, PZoneContT, SlewModelT, TrackModelT, GuidingModelT, LoggerT>
public MccGenericMount<HardwareT, TelemetryT, PZoneContT, SlewModelT, TrackModelT, LoggerT>
{
protected:
typedef MccGenericMount<HardwareT, TelemetryT, PZoneContT, SlewModelT, TrackModelT, GuidingModelT, LoggerT>
base_gmount_t;
// typedef MccGenericMount<HardwareT, TelemetryT, PZoneContT, SlewModelT, TrackModelT, GuidingModelT, LoggerT>
typedef MccGenericMount<HardwareT, TelemetryT, PZoneContT, SlewModelT, TrackModelT, LoggerT> base_gmount_t;
public:
typedef typename base_gmount_t::error_t error_t;
@ -206,11 +207,11 @@ public:
// to guiding state
struct MccGenericMountEventGuiding : MccGenericMountBaseEvent {
static constexpr std::string_view ID = "MCC-MOUNT-GUIDING-EVENT";
// struct MccGenericMountEventGuiding : MccGenericMountBaseEvent {
// static constexpr std::string_view ID = "MCC-MOUNT-GUIDING-EVENT";
MccGenericMountEventGuiding(MccGenericMountFSM& mount) : MccGenericMountBaseEvent(mount) {}
};
// MccGenericMountEventGuiding(MccGenericMountFSM& mount) : MccGenericMountBaseEvent(mount) {}
// };
// to stoping state
@ -252,7 +253,7 @@ public:
struct MccGenericMountStateError;
struct MccGenericMountStateSlew;
struct MccGenericMountStateTrack;
struct MccGenericMountStateGuiding;
// struct MccGenericMountStateGuiding;
struct MccGenericMountStateStopping;
struct MccGenericMountStateUninit : MccGenericMountBaseState {
@ -394,8 +395,7 @@ public:
struct MccGenericMountStateTrack : MccGenericMountBaseState {
using transition_t =
fsm::fsm_transition_table_t<std::pair<MccGenericMountEventStop, MccGenericMountStateStopping>,
std::pair<MccGenericMountEventError, MccGenericMountStateError>,
std::pair<MccGenericMountEventGuiding, MccGenericMountStateGuiding>>;
std::pair<MccGenericMountEventError, MccGenericMountStateError>>;
template <std::derived_from<MccGenericMountBaseEvent> EvT>
void exit(EvT& event)
@ -421,34 +421,34 @@ public:
};
struct MccGenericMountStateGuiding : MccGenericMountBaseState {
using transition_t =
fsm::fsm_transition_table_t<std::pair<MccGenericMountEventStop, MccGenericMountStateStopping>,
std::pair<MccGenericMountEventError, MccGenericMountStateError>,
std::pair<MccGenericMountEventTrack, MccGenericMountStateTrack>>;
// struct MccGenericMountStateGuiding : MccGenericMountBaseState {
// using transition_t =
// fsm::fsm_transition_table_t<std::pair<MccGenericMountEventStop, MccGenericMountStateStopping>,
// std::pair<MccGenericMountEventError, MccGenericMountStateError>,
// std::pair<MccGenericMountEventTrack, MccGenericMountStateTrack>>;
template <std::derived_from<MccGenericMountBaseEvent> EvT>
void exit(EvT& event)
{
error_t err = event.mount()._stopGuidingMount();
if (err) {
event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
}
// template <std::derived_from<MccGenericMountBaseEvent> EvT>
// void exit(EvT& event)
// {
// error_t err = event.mount()._stopGuidingMount();
// if (err) {
// event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
// }
this->exitLog(event);
}
// this->exitLog(event);
// }
template <std::derived_from<MccGenericMountBaseEvent> EvT>
void enter(EvT& event)
{
this->enterLog(event);
// template <std::derived_from<MccGenericMountBaseEvent> EvT>
// void enter(EvT& event)
// {
// this->enterLog(event);
error_t err = event.mount()._startGuidingMount();
if (err) {
event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
}
}
};
// error_t err = event.mount()._startGuidingMount();
// if (err) {
// event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
// }
// }
// };
/* CONSTRUCTORS AND DESTRUCTOR */
@ -458,7 +458,7 @@ public:
PZoneContT pzone_cont,
SlewModelT slew_model,
TrackModelT track_model,
GuidingModelT guiding_model,
// GuidingModelT guiding_model,
LoggerT logger = MccNullLogger{})
: fsm::MccFiniteStateMachine(MccGenericMountStateUninit{}),
base_gmount_t(std::move(hardware),
@ -466,7 +466,7 @@ public:
std::move(pzone_cont),
std::move(slew_model),
std::move(track_model),
std::move(guiding_model),
// std::move(guiding_model),
std::move(logger))
{
}
@ -506,15 +506,15 @@ public:
this->dispatchEvent(MccGenericMountEventIDLE{*this});
}
auto startGuidingTarget()
{
this->dispatchEvent(MccGenericMountEventGuiding{*this});
}
// auto startGuidingTarget()
// {
// this->dispatchEvent(MccGenericMountEventGuiding{*this});
// }
auto stopGuidingTarget()
{
this->dispatchEvent(MccGenericMountEventTrack{*this});
}
// auto stopGuidingTarget()
// {
// this->dispatchEvent(MccGenericMountEventTrack{*this});
// }
protected:
// wrappers

63
mcc/mcc_generics.h
View File

@ -64,18 +64,10 @@ static consteval bool mccIsAltAzMount(const MccMountType type)
};
// enum MccCoordPairKind : size_t {
// COORDS_KIND_GENERIC,
// COORDS_KIND_RADEC_ICRS,
// COORDS_KIND_RADEC_APP,
// COORDS_KIND_HADEC_APP,
// COORDS_KIND_AZZD,
// COORDS_KIND_AZALT,
// COORDS_KIND_XY,
// COORDS_KIND_LATLON
// };
enum class MccProhibitedZonePolicy : int {
PZ_POLICY_STOP, // stop mount near the zone
PZ_POLICY_FLIP // flip mount, e.g., near the meridian, near HA-axis encoder limit switch
};
/* GENERIC LOGGER CLASS CONCEPT */
@ -480,7 +472,7 @@ concept mcc_hardware_c = requires(T t, const T t_const) {
requires mcc_angle_c<decltype(state.speedX)>; // moving speed along co-longitude coordinate
requires mcc_angle_c<decltype(state.speedY)>; // moving speed along co-latitude coordinate
requires std::same_as<typename T::hardware_moving_state_t, decltype(state.moving_type)>;
requires std::same_as<typename T::hardware_moving_state_t, decltype(state.moving_state)>;
};
// set hardware state:
@ -745,12 +737,12 @@ concept mcc_prohibited_zone_c =
std::derived_from<T, mcc_pzone_interface_t<typename T::error_t>> && requires(const T t_const) {
{ t_const.name() } -> std::formattable<char>;
requires requires(typename T::pzone_policy_t pp) {
// the 'T' class must contain static constexpr member of 'MccMountType' type
requires std::same_as<decltype(T::pzPolicy), const MccProhibitedZonePolicy>;
[]() {
static constexpr auto v1 = T::pzone_policy_t::STOP_POLICY;
static constexpr auto v2 = T::pzone_policy_t::FLIP_POLICY;
}();
};
static constexpr MccProhibitedZonePolicy val = T::pzPolicy;
return val;
}(); // to ensure 'pzPolicy' can be used in compile-time context
};
@ -867,22 +859,22 @@ concept mcc_tracking_model_c = requires(T t) {
{ t.getTrackingParams() } -> std::same_as<typename T::tracking_params_t>;
};
template <typename T>
concept mcc_guiding_model_c = requires(T t) {
requires mcc_error_c<typename T::error_t>;
// template <typename T>
// concept mcc_guiding_model_c = requires(T t) {
// requires mcc_error_c<typename T::error_t>;
// a class of guiding process parameters
requires requires(typename T::guiding_params_t pars) {
// guide along both mount axis
requires std::convertible_to<decltype(pars.dualAxisGuiding), bool>;
};
// // a class of guiding process parameters
// requires requires(typename T::guiding_params_t pars) {
// // guide along both mount axis
// requires std::convertible_to<decltype(pars.dualAxisGuiding), bool>;
// };
{ t.startGuidingTarget() } -> std::same_as<typename T::error_t>;
{ t.stopGuidingTarget() } -> std::same_as<typename T::error_t>;
// { t.startGuidingTarget() } -> std::same_as<typename T::error_t>;
// { t.stopGuidingTarget() } -> std::same_as<typename T::error_t>;
{ t.setGuidingParams(std::declval<typename T::guiding_params_t>()) } -> std::same_as<typename T::error_t>;
{ t.getGuidingParams() } -> std::same_as<typename T::guiding_params_t>;
};
// { t.setGuidingParams(std::declval<typename T::guiding_params_t>()) } -> std::same_as<typename T::error_t>;
// { t.getGuidingParams() } -> std::same_as<typename T::guiding_params_t>;
// };
/* GENERIC MOUNT CLASS CONCEPT */
@ -897,7 +889,10 @@ template <typename T>
concept mcc_all_controls_c = mcc_position_controls_c<T> && mcc_telemetry_c<T> && mcc_pzone_container_c<T>;
// generic mount:
// 1) telemetry-related methods
// 2) prohibited zones related methods
// 3) slewing and tracking, stop and init mount methods
template <typename T>
concept mcc_generic_mount_c = mcc_telemetry_c<T> && mcc_pzone_container_c<T> && requires(T t) {
requires mcc_error_c<typename T::error_t>;
@ -909,8 +904,8 @@ concept mcc_generic_mount_c = mcc_telemetry_c<T> && mcc_pzone_container_c<T> &&
{ t.trackTarget() } -> std::same_as<typename T::error_t>;
{ t.startGuidingTarget() } -> std::same_as<typename T::error_t>;
{ t.stopGuidingTarget() } -> std::same_as<typename T::error_t>;
// { t.startGuidingTarget() } -> std::same_as<typename T::error_t>;
// { t.stopGuidingTarget() } -> std::same_as<typename T::error_t>;
// stop any movement
{ t.stopMount() } -> std::same_as<typename T::error_t>;

302
mcc/mcc_guiding_model.h
View File

@ -1,302 +0,0 @@
#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_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::MccSimpleGuidingModelErrorCode> : public true_type
{
};
} // namespace std
namespace mcc
{
class MccSimpleGuidingModel
{
public:
typedef std::error_code error_t;
typedef MccSimpleMovingModelParams guiding_params_t;
template <mcc_all_controls_c CONTROLS_T>
MccSimpleGuidingModel(CONTROLS_T* controls)
: _stopGuiding(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
{
_guidingFunc = [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, MccSimpleGuidingModelErrorCode::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,
MccSimpleGuidingModelErrorCode::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!");
}
// MccPCMResult pcm_inv_res;
// // endpoint of the mount moving
// auto pcm_err = controls->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
// if (pcm_err) {
// return mcc_deduce_error<error_t>(pcm_err, MccSimpleGuidingModelErrorCode::ERROR_PCM_COMP);
// }
// if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
// hw_state.speedX = _currentParams.trackSpeedX;
// hw_state.speedY = _currentParams.trackSpeedY;
// }
};
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, MccSimpleGuidingModelErrorCode::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, MccSimpleGuidingModelErrorCode::ERROR_PCM_COMP);
}
mcc_tp2tp(tdata.time_point, hw_state.time_point);
}
return MccSimpleGuidingModelErrorCode::ERROR_OK;
};
auto pz_err = update_pzones_ipoint();
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err, MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
hw_state.moving_type = CONTROLS_T::hardware_moving_state_t::HW_MOVE_GUIDING;
{
std::lock_guard lock{*_currentParamsMutex};
auto ccte_err = target_point(&target_in_future_pt);
if (ccte_err) {
return mcc_deduce_error(ccte_err, MccSimpleGuidingModelErrorCode::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, MccSimpleGuidingModelErrorCode::ERROR_HW_SETSTATE);
}
std::chrono::steady_clock::time_point last_corr_tp, last_ipzone_update_tp;
while (*_stopGuiding) {
// 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, MccSimpleGuidingModelErrorCode::ERROR_GET_TELEMETRY);
}
}
if (*_stopGuiding) {
break;
}
// control prohibited zones
if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) {
return MccSimpleGuidingModelErrorCode::ERROR_NEAR_PZONE;
}
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err,
MccSimpleGuidingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (*_stopGuiding) {
break;
}
{
std::lock_guard lock{*_currentParamsMutex};
auto now = std::chrono::steady_clock::now();
if ((now - last_corr_tp) < _currentParams.guidingMinInterval) {
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, MccSimpleGuidingModelErrorCode::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, MccSimpleGuidingModelErrorCode::ERROR_CCTE);
}
}
// send corrections
hw_state.moving_type = CONTROLS_T::hardware_moving_state_t::HW_MOVE_GUIDING;
hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleGuidingModelErrorCode::ERROR_HW_SETSTATE);
}
}
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

10
mcc/mcc_moving_model_common.h
View File

@ -43,19 +43,21 @@ struct MccSimpleMovingModelParams {
// slew process timeout
std::chrono::seconds slewTimeout{3600};
double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
double slewRateX{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
double slewRateY{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
std::chrono::milliseconds adjustCycleInterval{500}; // minimum time between two successive adjustments
double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
double adjustRateX{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
double adjustRateY{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
// ******* tracking mode *******
double trackSpeedX{};
double trackSpeedY{};
std::chrono::milliseconds trackingCycleInterval{500}; // minimum time between two successive tracking corrections
bool dualAxisTracking{true}; // mount must be of an equatorial type: false means guiding along only HA-axis
// time shift into future to compute target position in future (UT1-scale time duration)
std::chrono::duration<double> timeShiftToTargetPoint{10.0};

189
mcc/mcc_pzone.h
View File

@ -12,7 +12,7 @@ namespace mcc
{
enum MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM };
enum MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM, ERROR_PCM_COMP };
} // namespace mcc
@ -56,6 +56,8 @@ struct MccAltLimitPZCategory : public std::error_category {
return "input argument os nullptr";
case MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM:
return "coordinate transformation error";
case MccAltLimitPZErrorCode::ERROR_PCM_COMP:
return "PCM computation error";
default:
return "UNKNOWN";
}
@ -85,6 +87,8 @@ protected:
static constexpr auto pi2 = std::numbers::pi * 2.0;
public:
static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_STOP;
typedef std::error_code error_t;
MccAltLimitPZ(mcc_angle_c auto const& alt_limit, mcc_angle_c auto const& latitude, mcc_ccte_c auto* ccte_engine)
@ -551,4 +555,187 @@ protected:
}
};
/* co-longitude axis (HA or AZ) limit switch prohibited zone */
template <MccCoordKind AXIS_KIND>
class MccAxisLimitSwitchPZ : public mcc_pzone_interface_t<std::error_code>
{
public:
static_assert(AXIS_KIND == MccCoordKind::COORDS_KIND_AZ || AXIS_KIND == MccCoordKind::COORDS_KIND_HA,
"UNSUPPORTED AXIS TYPE!");
typedef std::error_code error_t;
static constexpr MccCoordKind axisKind = AXIS_KIND;
static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_FLIP;
//
// min_limit_val and max_limit_val are hardware encoder angles in radians!
//
MccAxisLimitSwitchPZ(mcc_angle_c auto const& min_limit_val,
mcc_angle_c auto const& max_limit_val,
mcc_position_controls_c auto* controls)
: _minLimit(min_limit_val), _maxLimit(max_limit_val)
{
_transformCoordinates = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t {
if (to_pt == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
auto err = controls->transformCoordinates(from_pt, to_pt);
if (!err) {
return MccAltLimitPZErrorCode::ERROR_OK;
}
if (std::same_as<decltype(err), error_t>) {
return err;
} else {
return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM;
}
};
_transformCoordinatesEqtHrzCoords = [controls](MccCelestialPoint from_pt, MccEqtHrzCoords* to_pt) -> error_t {
if (to_pt == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
auto err = controls->transformCoordinates(from_pt, to_pt);
if (!err) {
return MccAltLimitPZErrorCode::ERROR_OK;
}
if (std::same_as<decltype(err), error_t>) {
return err;
} else {
return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM;
}
};
_computePCM = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t {
MccPCMResult inv_res;
auto err = controls->computeInversePCM(std::move(from_pt), &inv_res, to_pt);
if (err) {
return mcc_deduce_error<error_t>(err, MccAltLimitPZErrorCode::ERROR_PCM_COMP);
}
return MccAltLimitPZErrorCode::ERROR_OK;
};
}
consteval std::string_view name()
{
return axisKind == MccCoordKind::COORDS_KIND_AZ ? "AZ_AXIS-LIMITSWITCH_ZONE"
: axisKind == MccCoordKind::COORDS_KIND_HA ? "HA_AXIS-LIMITSWITCH_ZONE"
: "UKNOWN";
}
template <typename InputT>
error_t inPZone(InputT coords, bool* result)
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
{
if (result == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
// assume here .X and are hardware encoder coordinate of corresponding axis
*result = (coords.X < _maxLimit) && (coords.X > _minLimit);
} else { // mcc_celestial_point_c
if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware
*result = (coords.X < _maxLimit) && (coords.X > _minLimit);
} else { // here one needs transform input coordinates to hardware encoder ones
MccCelestialPoint pt;
auto ret = getHWCoords(std::move(coords), &pt);
if (ret) {
return ret;
}
*result = (pt.X < _maxLimit) && (pt.X > _minLimit);
}
}
return MccAltLimitPZErrorCode::ERROR_OK;
}
template <typename InputT>
error_t timeToPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
{
using res_t = std::remove_cvref_t<decltype(*res_time)>;
using period_t = typename res_t::period;
double time_ang;
if (res_time == nullptr) {
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
}
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
// assume here .X and are hardware encoder coordinate of corresponding axis
if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) {
time_ang = (_maxLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
} else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) {
}
} else { // mcc_celestial_point_c
}
std::chrono::nanoseconds ns{
static_cast<std::chrono::nanoseconds::rep>(time_ang * 43200.0 / std::numbers::pi * 1.0E9)};
period_t rat;
*res_time = res_t{static_cast<typename res_t::rep>(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)};
}
template <typename InputT>
error_t timeFromPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
{
}
template <typename InputT, typename ResultT>
error_t intersectPZone(InputT coords, ResultT* point)
requires((mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>) &&
(mcc_eqt_hrz_coord_c<ResultT> || mcc_celestial_point_c<ResultT>))
{
}
protected:
double _minLimit, _maxLimit;
std::function<error_t(MccCelestialPoint, MccCelestialPoint*)> _transformCoordinates{};
std::function<error_t(MccCelestialPoint, MccEqtHrzCoords*)> _transformCoordinatesEqtHrzCoords{};
std::function<error_t(MccCelestialPoint, MccCelestialPoint*)> _computePCM{};
error_t getHWCoords(MccCelestialPoint from_pt, MccCelestialPoint* to_pt)
{
error_t ret = MccAltLimitPZErrorCode::ERROR_OK;
if (from_pt.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware
to_pt->X = from_pt.X;
to_pt->Y = from_pt.Y;
} else { // here one needs transform input coordinates to hardware encoder ones
if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) {
to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
} else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) {
to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
}
mcc_tp2tp(from_pt.time_point, to_pt->time_point);
ret = _transformCoordinates(std::move(from_pt), to_pt);
if (!ret) {
ret = _computePCM(*to_pt, to_pt);
}
}
return ret;
}
};
} // namespace mcc

145
mcc/mcc_slewing_model.h
View File

@ -22,6 +22,7 @@ enum class MccSimpleSlewingModelErrorCode : int {
ERROR_DIST_TELEMETRY,
ERROR_DIFF_TELEMETRY,
ERROR_PZONE_CONTAINER_COMP,
ERROR_TARGET_IN_PZONE,
ERROR_IN_PZONE,
ERROR_NEAR_PZONE,
ERROR_TIMEOUT,
@ -58,42 +59,18 @@ public:
typedef MccSimpleMovingModelParams slewing_params_t;
// struct slewing_params_t {
// bool slewAndStop{false}; // slew to target and stop mount
// std::chrono::seconds telemetryTimeout{3};
// // minimal time to prohibited zone at current speed. if it is lesser then exit with error
// std::chrono::seconds minTimeToPZone{10};
// // target-mount coordinate difference to start adjusting of slewing (in radians)
// double adjustCoordDiff{10.0_degs};
// // coordinates difference to stop slewing (in radians)
// double slewToleranceRadius{5.0_arcsecs};
// // slew process timeout
// std::chrono::seconds slewTimeout{3600};
// double slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
// double slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
// double adjustXRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
// double adjustYRate{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
// };
template <mcc_telemetry_data_c TelemetryT, mcc_hardware_c HardwareT, mcc_pzone_container_c PZoneContT>
MccSimpleSlewingModel(TelemetryT* telemetry, HardwareT* hardware, PZoneContT* pz_cont)
template <mcc_all_controls_c CONTROLS_T>
MccSimpleSlewingModel(CONTROLS_T* controls)
: _stopSlewing(new std::atomic_bool()), _currentParamsMutex(new std::mutex)
{
_slewingFunc = [telemetry, hardware, pz_cont, this]() -> error_t {
_slewingFunc = [controls, this]() -> error_t {
// first, check target coordinates
typename TelemetryT::error_t t_err;
typename CONTROLS_T::error_t t_err;
MccTelemetryData tdata;
{
std::lock_guard lock{*_currentParamsMutex};
t_err = telemetry->telemetryData(&tdata);
t_err = controls->telemetryData(&tdata);
if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
@ -101,13 +78,13 @@ public:
}
bool in_zone;
auto pz_err = pz_cont->inPZone(tdata.target, &in_zone);
auto pz_err = controls->inPZone(tdata.target, &in_zone);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (in_zone) {
return MccSimpleSlewingModelErrorCode::ERROR_IN_PZONE;
return MccSimpleSlewingModelErrorCode::ERROR_TARGET_IN_PZONE;
}
if (*_stopSlewing) {
@ -115,29 +92,23 @@ public:
}
MccCelestialPoint cpt;
mcc_tp2tp(tdata.time_point, cpt.time_point);
double min_time_to_pzone_in_secs;
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
std::vector<MccCelestialPoint> isct_pt(pz_cont->sizePZones, cpt);
pz_err = pz_cont->intersectPZone(tdata.target, &isct_pt);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err, MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
typename HardwareT::hardware_state_t hw_state;
typename CONTROLS_T::hardware_state_t hw_state;
auto hw_err = hardware->hardwareGetState(&hw_state);
auto hw_err = controls->hardwareGetState(&hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
}
@ -147,17 +118,20 @@ public:
{
std::lock_guard lock{*_currentParamsMutex};
hw_state.speedX = _currentParams.slewXRate;
hw_state.speedY = _currentParams.slewYRate;
hw_state.speedX = _currentParams.slewRateX;
hw_state.speedY = _currentParams.slewRateY;
min_time_to_pzone_in_secs =
std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
}
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_SLEWING;
hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_SLEWING;
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
// start slewing
hw_err = hardware->hardwareSetState(hw_state);
hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
@ -165,8 +139,10 @@ public:
std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
mcc_tp2tp(hw_state.time_point, start_slewing_tp);
double dist, dx, dy, sinY, rate2, xrate;
std::chrono::duration<double> dtx, dty; // seconds in double
// double dist, dx, dy, sinY, rate2, xrate;
// std::chrono::duration<double> dtx, dty; // seconds in double
double dist;
bool adjust_mode = false;
static constexpr auto sideral_rate2 = slewing_params_t::sideralRate * slewing_params_t::sideralRate;
@ -176,7 +152,7 @@ public:
{
std::lock_guard lock{*_currentParamsMutex};
t_err = telemetry->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_GET_TELEMETRY);
@ -187,42 +163,27 @@ public:
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
// compute time to prohibited zones at current speed
for (auto const& pt : isct_pt) {
if (std::isfinite(pt.X) && std::isfinite(pt.Y)) {
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
// sinY = sin(std::numbers::pi / 2.0 - tdata.DEC_APP);
dx = pt.X - tdata.HA;
dy = pt.Y - tdata.DEC_APP;
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
// sinY = sin(tdata.ZD);
dx = pt.X - tdata.AZ;
dy = pt.Y - tdata.ZD;
// calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
// and check them for getting into the prohibited zones
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
cpt.X = tdata.HA + tdata.speedX * min_time_to_pzone_in_secs;
cpt.X = tdata.DEC_APP + tdata.speedY * min_time_to_pzone_in_secs;
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
cpt.X = tdata.AZ + tdata.speedX * min_time_to_pzone_in_secs;
cpt.X = tdata.ZD + tdata.speedY * min_time_to_pzone_in_secs;
}
mcc_tp2tp(tdata.time_point, cpt.time_point);
pz_err = controls->inPZone(cpt, &in_zone);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err,
MccSimpleSlewingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
// if (utils::isEqual(sinY, 0.0)) {
// dtx = decltype(dtx){std::numeric_limits<double>::infinity()};
// rate2 = std::numeric_limits<double>::infinity();
// } else {
// xrate = tdata.speedX * sinY;
// dtx = decltype(dtx){std::abs(dx / xrate)};
// }
dtx = decltype(dtx){std::abs(dx / tdata.speedX)};
dty = decltype(dty){std::abs(dy / tdata.speedY)};
{
std::lock_guard lock{*_currentParamsMutex};
if (dtx < _currentParams.minTimeToPZone || dty < _currentParams.minTimeToPZone) {
if (in_zone) {
return MccSimpleSlewingModelErrorCode::ERROR_NEAR_PZONE;
}
}
}
if (*_stopSlewing) {
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
}
}
{
@ -233,12 +194,12 @@ public:
}
}
hw_err = hardware->hardwareGetState(&hw_state);
hw_err = controls->hardwareGetState(&hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
}
t_err = telemetry->targetToMountDist(&dist);
t_err = controls->targetToMountDist(&dist);
if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY);
}
@ -252,7 +213,7 @@ public:
if (adjust_mode && !_currentParams.slewAndStop) {
// do not allow mount speed fall below sideral
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
// turn on sideral rate only if the current position point catches up with the target
if ((tdata.target.HA - tdata.HA) <= 0.0 && tdata.speedX < slewing_params_t::sideralRate) {
hw_state.X = (double)tdata.target.X;
@ -260,23 +221,23 @@ public:
hw_state.speedX = slewing_params_t::sideralRate;
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_TRACKING;
hw_err = hardware->hardwareSetState(hw_state);
hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err,
MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}
}
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!!");
}
}
if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle
if (hw_state.moving_type ==
HardwareT::hardware_moving_state_t::HW_MOVE_STOPPED) { // mount was stopped
if (hw_state.moving_state ==
CONTROLS_T::hardware_moving_state_t::HW_MOVE_STOPPED) { // mount was stopped
break;
}
}
@ -290,12 +251,12 @@ public:
hw_state.X = (double)tdata.target.X;
hw_state.Y = (double)tdata.target.Y;
hw_state.speedX = _currentParams.adjustXRate;
hw_state.speedY = _currentParams.adjustYRate;
hw_state.speedX = _currentParams.adjustRateX;
hw_state.speedY = _currentParams.adjustRateY;
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_ADJUSTING;
hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_ADJUSTING;
hw_err = hardware->hardwareSetState(hw_state);
hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE);
}

54
mcc/mcc_telemetry.h
View File

@ -125,43 +125,6 @@ public:
using pcm_t = std::remove_cvref_t<decltype(*pcm)>;
using hardware_t = std::remove_cvref_t<decltype(*hardware)>;
_toHardwareFunc = [ccte, pcm](const MccCelestialPoint& from_pt, MccCelestialPoint* to_pt) -> error_t {
if (to_pt == nullptr) {
return MccTelemetryErrorCode::ERROR_NULLPTR;
}
if constexpr (mccIsEquatorialMount(pcm_t::mountType)) {
to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(pcm_t::mountType)) {
to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_AZALT;
} else {
static_assert(false, "UNKNOWN MOUNT TYPE!");
}
auto err = ccte->transformCoordinates(from_pt, to_pt);
if (err) {
return mcc_deduce_error(err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
}
// compute hardware coordinates
// WARNING: It is assumed here that PCM corrections have small (arcseconds-arcminutes) values
// since ususaly there is no reverse transformation for "hardware-to-apparent" relation!
struct {
double dx, dy;
} pcm_res;
auto pcm_err = pcm->compute(from_pt, &pcm_res);
if (pcm_err) {
return mcc_deduce_error<error_t>(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
to_pt->X -= pcm_res.dx;
to_pt->Y -= pcm_res.dy;
return MccTelemetryErrorCode::ERROR_OK;
};
_updateTargetFunc = [ccte, pcm, this](bool only_hw, std::stop_token stop_token) -> error_t {
if (!only_hw) {
//
@ -584,22 +547,6 @@ public:
}
error_t transformToHardwareCoords(mcc_celestial_point_c auto pt, mcc_celestial_point_c auto* res)
{
MccCelestialPoint cpt, rcpt;
mcc_copy_celestial_point(pt, &cpt);
rcpt.time_point = std::chrono::time_point_cast<decltype(rcpt.time_point)>(res->time_point);
auto err = _toHardwareFunc(cpt, &rcpt);
if (err) {
return err;
}
mcc_copy_celestial_point(rcpt, res);
return MccTelemetryErrorCode::ERROR_OK;
}
error_t targetToMountDiff(MccCoordPairKind pair_kind, mcc_angle_c auto* dx, mcc_angle_c auto* dy)
{
@ -658,7 +605,6 @@ protected:
std ::function<error_t(bool, std::stop_token)> _updateTargetFunc{};
std::function<error_t(std::stop_token)> _updateFunc{};
std::function<error_t()> _setTargetFunc{};
std::function<error_t(const MccCelestialPoint&, MccCelestialPoint*)> _toHardwareFunc{};
std::unique_ptr<std::mutex> _updateMutex;
std::unique_ptr<std::condition_variable> _updateCondVar;

281
mcc/mcc_tracking_model.h
View File

@ -3,11 +3,10 @@
/* MOUNT CONTROL COMPONENTS LIBRARY */
/* SIMPLE TRACKING MODEL IMPLEMENTATION */
/* SIMPLE Tracking MODEL IMPLEMENTATION */
#include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_moving_model_common.h"
namespace mcc
@ -15,14 +14,19 @@ 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_UNEXPECTED_AXIS_RATES
ERROR_TIMEOUT,
ERROR_UNEXPECTED_AXIS_RATES,
ERROR_STOPPED
};
} // namespace mcc
@ -50,70 +54,56 @@ public:
typedef MccSimpleMovingModelParams tracking_params_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};
// // minimal time to prohibited zone. if it is lesser then exit with error
// std::chrono::seconds minTimeToPZone{10};
// };
template <mcc_telemetry_data_c TelemetryT,
mcc_hardware_c HardwareT,
mcc_PCM_c PcmT,
mcc_pzone_container_c PZoneContT>
MccSimpleTrackingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont)
: _stopTracking(new std::atomic_bool()), _currentTrackParamsMutex(new std::mutex)
template <mcc_all_controls_c CONTROLS_T>
MccSimpleTrackingModel(CONTROLS_T* controls)
: _stopTracking(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
{
*_stopTracking = false;
// set default values
if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
_currentTrackParams.trackSpeedX = tracking_params_t::sideralRate; // move along HA-axis with sideral rate
_currentTrackParams.trackSpeedY = 0.0;
_currentTrackParams.telemetryTimeout = std::chrono::seconds(3);
_currentTrackParams.minTimeToPZone = std::chrono::seconds(10);
}
_trackingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t {
typename HardwareT::hardware_state_t hw_state;
MccEqtHrzCoords intsc_coords;
_trackingFunc = [controls, this]() -> error_t {
typename CONTROLS_T::hardware_state_t hw_state;
MccTelemetryData tdata;
auto t_err = telemetry->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout);
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);
}
mcc_tp2tp(tdata.time_point, intsc_coords.time_point);
std::vector<std::chrono::duration<double>> pz_timeto; // in seconds
std::chrono::duration<double> min_time{0.0};
// compute intersection points with the prohibited zones
auto pz_err = mcc_find_closest_pzone(pz_cont, tdata, &intsc_coords);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
bool no_intersects = false;
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
// 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;
// intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min
// intsc_coords.Y = tdata.DEC_APP;
}
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
} 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;
@ -123,154 +113,195 @@ public:
} else {
static_assert(false, "UNKNOW MOUNT TYPE!");
}
};
// compute position in future
auto hw_err = hardware->hardwareGetState(&hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
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 = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
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);
}
if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
// just set sideral rate once
mcc_tp2tp(tdata.time_point, hw_state.time_point);
{
std::lock_guard lock{*_currentTrackParamsMutex};
hw_state.speedX = _currentTrackParams.trackSpeedX;
hw_state.speedY = _currentTrackParams.trackSpeedY;
}
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
// start tracking
hw_err = hardware->hardwareSetState(std::move(hw_state));
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);
}
while (!*_stopTracking) {
// control prohibited zones
pz_err = pz_cont->timeToPZone(tdata, &pz_timeto);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err,
MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
std::chrono::steady_clock::time_point last_corr_tp, last_ipzone_update_tp;
min_time = std::chrono::duration<double>{0};
for (size_t i = 0; i < pz_cont->sizePZones(); ++i) {
if (pz_timeto[i] < _currentTrackParams.minTimeToPZone) {
return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
}
if (pz_timeto[i] < min_time) {
min_time = pz_timeto[i];
}
}
while (*_stopTracking) {
// wait for updated telemetry data
{
std::lock_guard lock{*_currentParamsMutex};
t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
t_err = telemetry->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout);
if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
}
}
if (*_stopTracking) {
break;
}
if (no_intersects) {
if ((intsc_coords.HA - tdata.HA) < 10.0_mins) { // recompute target point
intsc_coords.X += 11.0_hours;
hw_err = hardware->hardwareGetState(&hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err,
MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
// 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);
}
pcm_err = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
if (pcm_err) {
return mcc_deduce_error<error_t>(pcm_err,
MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
if (*_stopTracking) {
break;
}
// just set sideral rate once
mcc_tp2tp(tdata.time_point, hw_state.time_point);
{
std::lock_guard lock{*_currentTrackParamsMutex};
std::lock_guard lock{*_currentParamsMutex};
auto now = std::chrono::steady_clock::now();
hw_state.speedX = _currentTrackParams.trackSpeedX;
hw_state.speedY = _currentTrackParams.trackSpeedY;
if ((now - last_corr_tp) < _currentParams.trackingCycleInterval) {
continue;
}
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
// start tracking
hw_err = hardware->hardwareSetState(std::move(hw_state));
// 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 mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
}
}
return MccSimpleTrackingModelErrorCode::ERROR_OK;
} else if constexpr (mccIsAltAzMount(PcmT::mountType)) {
static_assert(false, "NOT IMPLEMENTED!");
} else {
static_assert(false, "UNKNOW MOUNT TYPE!");
}
};
}
MccSimpleTrackingModel(MccSimpleTrackingModel&&) = default;
MccSimpleTrackingModel& operator=(MccSimpleTrackingModel&&) = default;
MccSimpleTrackingModel(const MccSimpleTrackingModel&) = delete;
MccSimpleTrackingModel& operator=(const MccSimpleTrackingModel&) = delete;
error_t trackMount()
error_t trackTarget()
{
*_stopTracking = false;
return _trackingFunc();
}
error_t stopTracking()
error_t stoptTracking()
{
*_stopTracking = true;
return MccSimpleTrackingModelErrorCode::ERROR_OK;
}
error_t setTrackingParams(tracking_params_t params)
{
std::lock_guard lock{*_currentTrackParamsMutex};
std::lock_guard lock{*_currentParamsMutex};
_currentTrackParams = std::move(params);
_currentParams = std::move(params);
return MccSimpleTrackingModelErrorCode::ERROR_OK;
}
tracking_params_t getTrackingParams() const
{
std::lock_guard lock{*_currentTrackParamsMutex};
std::lock_guard lock{*_currentParamsMutex};
return _currentTrackParams;
return _currentParams;
}
protected:
std::function<error_t()> _trackingFunc{};
std::unique_ptr<std::atomic_bool> _stopTracking{};
std::unique_ptr<std::atomic_bool> _stopTracking;
tracking_params_t _currentTrackParams;
std::unique_ptr<std::mutex> _currentTrackParamsMutex;
tracking_params_t _currentParams{};
std::unique_ptr<std::mutex> _currentParamsMutex{};
};
} // namespace mcc