Compare commits

...

57 Commits

Author SHA1 Message Date
Timur A. Fatkhullin
9ad5bfb09b ... 2026-03-18 23:34:46 +03:00
40d36545f3 ... 2026-03-18 19:00:23 +03:00
3dbd68b21c ... 2026-03-17 17:18:11 +03:00
Timur A. Fatkhullin
617bcec1a1 MccDefaultPCM: reverse PCM is computed with using partial derivatives
(start developing)
2026-03-17 00:23:44 +03:00
Timur A. Fatkhullin
28352f6c64 ... 2026-03-12 23:50:47 +03:00
Timur A. Fatkhullin
0ee87ccbf9 ... 2026-03-12 22:20:56 +03:00
f2aad0807d fix addToPath 2026-03-12 10:44:34 +03:00
Timur A. Fatkhullin
de097d6db2 MccMovementPathFile: addToPath(mcc_telemetry_data_c auto const& tdata) add
mount current speed saving
2026-03-11 22:49:24 +03:00
be69df5068 fix trajectory file saving 2026-03-04 14:35:10 +03:00
5d498d47f8 ... 2026-03-02 19:11:41 +03:00
Timur A. Fatkhullin
5e9e8897f0 ... 2026-03-01 23:42:34 +03:00
Timur A. Fatkhullin
f72d0bc3f0 ... 2026-02-28 10:39:49 +03:00
d42fa37a04 rewrite MccGenericMovementControls class (single thread version) 2026-02-27 18:02:02 +03:00
ba03832ce6 add hardwareShutdown method requirement to mcc_hardware_c concept 2026-02-27 14:24:45 +03:00
3bd12bcf6d ... 2026-02-27 12:29:45 +03:00
Timur A. Fatkhullin
6199af6392 ... 2026-02-26 22:13:23 +03:00
4401616d44 ... 2026-02-26 18:28:22 +03:00
2f10fa2796 ... 2026-02-26 14:20:30 +03:00
8aa58b727b ... 2026-02-26 12:13:39 +03:00
d9cb52f755 ... 2026-02-25 18:08:21 +03:00
09a234ddc9 ... 2026-02-25 10:58:01 +03:00
76cebec136 ... 2026-02-24 17:43:38 +03:00
e0b3ef225d ... 2026-02-23 11:38:23 +03:00
32ed709222 ... 2026-02-20 17:39:25 +03:00
f8162779d6 ... 2026-02-20 14:44:14 +03:00
e6004a6c8a ... 2026-02-20 12:08:21 +03:00
099c6056d7 ... 2026-02-19 18:49:30 +03:00
18e6a99267 ... 2026-02-18 18:50:55 +03:00
889d4ff3b2 ... 2026-02-18 16:22:58 +03:00
53245002c5 ... 2026-02-18 14:14:32 +03:00
d11222a07f ... 2026-02-18 12:12:48 +03:00
0ffee8eceb ... 2026-02-18 11:04:35 +03:00
de98ead539 ... 2026-02-18 10:49:27 +03:00
Timur A. Fatkhullin
2f80bcc78b ... 2026-02-18 00:08:19 +03:00
Timur A. Fatkhullin
408d23c376 ... 2026-02-17 23:54:46 +03:00
Timur A. Fatkhullin
b98063b229 ... 2026-02-17 23:22:46 +03:00
Timur A. Fatkhullin
00c7641d50 ... 2026-02-17 22:35:56 +03:00
Timur A. Fatkhullin
af5d852db9 ... 2026-02-17 22:02:27 +03:00
1a8e1bbed0 ... 2026-02-17 18:27:57 +03:00
578339fd8b ... 2026-02-17 16:24:33 +03:00
775dedd835 ... 2026-02-17 15:59:14 +03:00
1841664b63 ... 2026-02-17 15:01:16 +03:00
5cfa67d6a1 ... 2026-02-17 15:00:45 +03:00
8dd5e4751c ... 2026-02-17 14:42:48 +03:00
b1c5b64879 ... 2026-02-17 14:41:42 +03:00
c5299d06bb ... 2026-02-17 14:23:10 +03:00
5202f8dc84 ... 2026-02-17 14:13:42 +03:00
ec7b7d8506 ... 2026-02-17 14:08:46 +03:00
cc57643315 ... 2026-02-17 14:02:02 +03:00
dce1c62419 ... 2026-02-17 13:56:53 +03:00
d1a31fc52c ... 2026-02-17 11:41:04 +03:00
7049317539 ... 2026-02-17 11:38:15 +03:00
Timur A. Fatkhullin
c174021c40 ... 2026-02-16 22:44:29 +03:00
Timur A. Fatkhullin
7b110fb7ff ... 2026-02-16 22:19:48 +03:00
Timur A. Fatkhullin
dff70225ef ... 2026-02-16 22:00:08 +03:00
Timur A. Fatkhullin
452bd1c3e4 ... 2026-02-16 21:48:22 +03:00
26011e7630 ... 2026-02-16 18:38:59 +03:00
20 changed files with 2629 additions and 345 deletions

View File

@@ -214,6 +214,8 @@ set(MCC_SRC
include/mcc/mcc_pzone_container.h
include/mcc/mcc_pcm.h
include/mcc/mcc_telemetry.h
include/mcc/mcc_movement_controls.h
include/mcc/mcc_generic_movecontrols.h
include/mcc/mcc_serialization_common.h
include/mcc/mcc_deserializer.h
include/mcc/mcc_serializer.h
@@ -225,7 +227,12 @@ if(USE_SPDLOG)
endif()
if(USE_ERFA)
list(APPEND MCC_SRC include/mcc/mcc_ccte_iers.h include/mcc/mcc_ccte_iers_default.h include/mcc/mcc_ccte_erfa.h)
list(
APPEND MCC_SRC
include/mcc/mcc_ccte_iers.h
include/mcc/mcc_ccte_iers_default.h
include/mcc/mcc_ccte_erfa.h
)
endif()
if(USE_ASIO)
@@ -239,14 +246,10 @@ endif()
if(USE_BSPLINE_PCM)
# fitpack by P. Dierckx
list(
APPEND MCC_SRC
include/mcc/mcc_bsplines.h
)
list(APPEND MCC_SRC include/mcc/mcc_bsplines.h)
add_subdirectory(fitpack)
endif()
add_library(${PROJECT_NAME} INTERFACE ${MCC_SRC})
target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_23)
target_include_directories(
@@ -263,6 +266,10 @@ endif()
if(USE_BSPLINE_PCM)
target_compile_definitions(${PROJECT_NAME} INTERFACE USE_BSPLINE_PCM)
target_link_libraries(${PROJECT_NAME} INTERFACE fitpack)
target_link_directories(
${PROJECT_NAME}
INTERFACE "$<BUILD_INTERFACE:${CMAKE_BINARY_DIR}/fitpack>"
)
endif()
if(USE_ASIO)
@@ -271,6 +278,11 @@ endif()
if(USE_SPDLOG)
target_link_libraries(${PROJECT_NAME} INTERFACE spdlog::spdlog_header_only)
# target_compile_definitions(${PROJECT_NAME} INTERFACE SPDLOG_USE_STD_FORMAT=1 SPDLOG_FMT_EXTERNAL=0)
target_compile_definitions(
${PROJECT_NAME}
INTERFACE SPDLOG_USE_STD_FORMAT=1
)
endif()
if(BUILD_TESTS)

View File

@@ -28,22 +28,26 @@ FortranCInterface_HEADER(
)
FortranCInterface_VERIFY(CXX)
# set(FITPACK_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR} PARENT_SCOPE)
# include_directories(${BSPLINES_INCLUDE_DIR})
# set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
# add_library(fitpack_project STATIC EXCLUDE_FROM_ALL ${src_files} mcc_bsplines.h)
add_library(fitpack_project STATIC EXCLUDE_FROM_ALL ${src_files})
# add_library(fitpack_project STATIC EXCLUDE_FROM_ALL ${src_files})
add_library(fitpack_project STATIC ${src_files})
# get_target_property(FP_LIBDIR fitpack_project LIBRARY_OUTPUT_DIRECTORY)
# message(STATUS "FP_LIBDIR: ${FP_LIBDIR}")
add_library(fitpack STATIC IMPORTED GLOBAL)
set_target_properties(
fitpack
PROPERTIES
IMPORTED_LOCATION ${CMAKE_BINARY_DIR}/fitpack/libfitpack_project.a
IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/libfitpack_project.a
# PROPERTIES IMPORTED_LOCATION ${FP_LIBDIR}/libfitpack_project.a
# IMPORTED_LOCATION ${CMAKE_BINARY_DIR}/fitpack/libfitpack_project.a
)
set_target_properties(
fitpack
PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES
"${CMAKE_CURRENT_SOURCE_DIR};${CMAKE_CURRENT_BINARY_DIR}"
PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${CMAKE_CURRENT_BINARY_DIR}"
# "${CMAKE_CURRENT_SOURCE_DIR};${CMAKE_CURRENT_BINARY_DIR}"
)
add_dependencies(fitpack fitpack_project)

View File

@@ -16,7 +16,6 @@
#include "mcc_traits.h"
#include "mcc_utils.h"
/* HELPERS TO REPRESENT ANGLE VALUE */
constexpr double operator""_rads(long double val) // angle in radians (no conversion)
@@ -434,25 +433,38 @@ static T1 operator/(const T1& v1, const T2& v2)
std::string MccAngleFancyString(std::convertible_to<MccAngle> auto const& ang,
std::format_string<double> val_fmt = "{}")
std::format_string<double> val_fmt = "{:.2f}")
{
using ang_t = std::decay_t<decltype(ang)>;
std::string s;
double abs_ang;
if constexpr (std::is_arithmetic_v<std::decay_t<decltype(ang)>>) {
if constexpr (std::is_arithmetic_v<ang_t>) {
abs_ang = std::abs(ang);
} else {
abs_ang = std::abs(MccAngle{ang});
}
if (abs_ang < 1.0_arcmins) {
std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcsecs());
if constexpr (std::derived_from<ang_t, MccAngle>) {
std::format_to(std::back_inserter(s), val_fmt, ang.arcsecs());
} else {
std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcsecs());
}
s += " arcsecs";
} else if (abs_ang < 1.0_degs) {
std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcmins());
if constexpr (std::derived_from<ang_t, MccAngle>) {
std::format_to(std::back_inserter(s), val_fmt, ang.arcmins());
} else {
std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcmins());
}
s += " arcmins";
} else {
std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.degrees());
if constexpr (std::derived_from<ang_t, MccAngle>) {
std::format_to(std::back_inserter(s), val_fmt, ang.degrees());
} else {
std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.degrees());
}
s += " degs";
}
@@ -649,7 +661,7 @@ static constexpr bool mcc_is_app_coordpair(MccCoordPairKind kind)
};
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_ICRS_STR = "RADEC-IRCS";
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_ICRS_STR = "RADEC-ICRS";
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_APP_STR = "RADEC-APP";
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_OBS_STR = "RADEC-OBS";
static constexpr std::string_view MCC_COORDPAIR_KIND_HADEC_APP_STR = "HADEC-APP";

View File

@@ -366,4 +366,111 @@ int fitpack_eval_spl2d(const TXT& tx,
return fitpack_eval_spl2d(tx, ty, coeffs, xv, yv, fv, kx, ky);
}
} // namespace mcc::fitpack
/* partial derivatives */
template <std::ranges::contiguous_range TXT,
std::ranges::contiguous_range TYT,
std::ranges::contiguous_range XT,
std::ranges::contiguous_range YT,
std::ranges::contiguous_range CoeffT,
std::ranges::contiguous_range PderT>
int fitpack_parder_spl2d(const TXT& tx,
const TYT& ty,
const CoeffT& coeffs,
const XT& x,
const YT& y,
PderT& pder,
int dx, // partial derivatives order along X
int dy, // partial derivatives order along Y
int kx = 3,
int ky = 3)
{
static_assert(std::same_as<std::ranges::range_value_t<TXT>, double> &&
std::same_as<std::ranges::range_value_t<TYT>, double> &&
std::same_as<std::ranges::range_value_t<XT>, double> &&
std::same_as<std::ranges::range_value_t<YT>, double> &&
std::same_as<std::ranges::range_value_t<CoeffT>, double> &&
std::same_as<std::ranges::range_value_t<PderT>, double>,
"Input ranges elements type must be double!");
if (kx < 0 || ky < 0 || dx < 0 || dy < 0) {
return 10;
}
int ntx = std::ranges::size(tx);
int nty = std::ranges::size(ty);
auto n_coeffs = (ntx - kx - 1) * (nty - ky - 1);
if (std::ranges::size(coeffs) < n_coeffs) {
return 10;
}
int mx = std::ranges::size(x), my = std::ranges::size(y);
int N = mx * my;
if (std::ranges::size(pder) < N) {
std::ranges::fill_n(std::back_inserter(pder), N - std::ranges::size(pder), 0.0);
}
// mx >=1, my >=1, 0 <= nux < kx, 0 <= nuy < ky, kwrk>=mx+my
// lwrk>=mx*(kx+1-nux)+my*(ky+1-nuy)+(nx-kx-1)*(ny-ky-1),
// compute sizes of working arrays according parder.f
int lwrk = mx * (kx + 1 - dx) + my * (ky + 1 - dy) + (ntx - kx - 1) * (nty - ky - 1);
std::vector<double> wrk(lwrk);
int kwrk = mx + my;
std::vector<int> iwrk(kwrk);
auto tx_ptr = const_cast<double*>(std::ranges::data(tx));
auto ty_ptr = const_cast<double*>(std::ranges::data(ty));
auto coeffs_ptr = const_cast<double*>(std::ranges::data(coeffs));
auto x_ptr = const_cast<double*>(std::ranges::data(x));
auto y_ptr = const_cast<double*>(std::ranges::data(y));
int ier = 0;
parder(tx_ptr, &ntx, ty_ptr, &nty, coeffs_ptr, &kx, &ky, &dx, &dy, x_ptr, &mx, y_ptr, &my, std::ranges::data(pder),
wrk.data(), &lwrk, iwrk.data(), &kwrk, &ier);
return ier;
}
// scalar version
template <std::ranges::contiguous_range TXT,
std::ranges::contiguous_range TYT,
typename XT,
typename YT,
std::ranges::contiguous_range CoeffT,
typename PderT>
int fitpack_parder_spl2d(const TXT& tx,
const TYT& ty,
const CoeffT& coeffs,
const XT& x,
const YT& y,
PderT& pder,
int dx, // partial derivatives order along X
int dy, // partial derivatives order along Y
int kx = 3,
int ky = 3)
{
static_assert(std::same_as<std::ranges::range_value_t<TXT>, double> &&
std::same_as<std::ranges::range_value_t<TYT>, double> &&
std::same_as<std::ranges::range_value_t<CoeffT>, double>,
"Input ranges elements type must be double!");
static_assert(
std::convertible_to<XT, double> && std::convertible_to<YT, double> && std::convertible_to<PderT, double>,
"XT, YT and FuncT types must be a scalar convertible to double!");
auto xv = std::vector<double>(1, x);
auto yv = std::vector<double>(1, y);
auto pv = std::vector<double>(1, pder);
return fitpack_parder_spl2d(tx, ty, coeffs, xv, yv, pv, dx, dy, kx, ky);
}
} // namespace mcc::bsplines

View File

@@ -499,15 +499,55 @@ struct mcc_skypoint_interface_t {
{
return std::forward<SelfT>(self).EO(eo);
}
// template <std::derived_from<mcc_skypoint_interface_t> SelfT>
// typename std::remove_cvref_t<SelfT>::dist_result_t distance(this SelfT&& self, auto const& sp)
// {
// return std::forward<SelfT>(self).distance(sp);
// }
// template <std::derived_from<mcc_skypoint_interface_t> SelfT>
// SelfT& operator+=(this SelfT& self, mcc_coord_pair_c auto const& dxy)
// {
// return self.operator+=(dxy);
// }
// template <std::derived_from<mcc_skypoint_interface_t> SelfT>
// SelfT& operator-=(this SelfT& self, mcc_coord_pair_c auto const& dxy)
// {
// return self.operator-=(dxy);
// }
};
template <typename T>
concept mcc_skypoint_c = std::derived_from<T, mcc_skypoint_interface_t> && requires(const T t_const) {
{ t_const.epoch() } -> mcc_coord_epoch_c;
concept mcc_skypoint_c =
std::derived_from<T, mcc_skypoint_interface_t> && requires(const T t_const, const T t_other_const, T t) {
{ t_const.epoch() } -> mcc_coord_epoch_c;
// currently stored coordinates pair
{ t_const.pairKind() } -> std::same_as<impl::MccCoordPairKind>;
};
// currently stored coordinates pair kind
{ t_const.pairKind() } -> std::same_as<impl::MccCoordPairKind>;
// currently stored co-longitude coordinate
{ t_const.co_lon() } -> std::convertible_to<double>;
// currently stored co-latitude coordinate
{ t_const.co_lat() } -> std::convertible_to<double>;
requires requires(typename T::dist_result_t res) {
requires mcc_angle_c<decltype(res.dist)>; // distance on sphere
requires mcc_angle_c<decltype(res.dx)>; // difference along co-longitude coordinate
requires mcc_angle_c<decltype(res.dy)>; // difference along co-latitude coordinate
requires mcc_angle_c<decltype(res.x2)>; // co-longitude coordinates of target sky point (in the same
// coordinate system as 'this')
requires mcc_angle_c<decltype(res.y2)>; // co-latitude coordinates of target sky point (in the same
// coordinate system as 'this')
};
// distance on sphere between two sky points
{ t_const.distance(std::declval<const T&>()) } -> std::same_as<typename T::dist_result_t>;
// { t_const - t_other_const } -> mcc_coord_pair_c;
// { t_const + t_other_const } -> mcc_coord_pair_c;
};
@@ -554,9 +594,11 @@ concept mcc_pcm_c = std::derived_from<T, mcc_pcm_interface_t<typename T::error_t
requires mcc_error_c<typename T::error_t>;
// the 'T' class must contain static constexpr member of 'MccMountType' type
requires std::same_as<decltype(T::mountType), const MccMountType>;
requires std::same_as<decltype(T::pcmMountType), const MccMountType>;
// requires std::same_as<decltype(T::mountType), const MccMountType>;
[]() {
[[maybe_unused]] static constexpr MccMountType val = T::mountType;
// [[maybe_unused]] static constexpr MccMountType val = T::mountType;
[[maybe_unused]] static constexpr MccMountType val = T::pcmMountType;
}(); // to ensure 'mountType' can be used in compile-time context
// static const variable with name of PCM
@@ -656,75 +698,20 @@ concept mcc_hardware_c = requires(T t) {
// static const variable with name of hardware
requires std::formattable<decltype(T::hardwareName), char> && std::is_const_v<decltype(T::hardwareName)>;
// a type that defines at least HW_MOVE_ERROR, HW_MOVE_STOPPING, 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 the stop and error states from
// hardware
//
// e.g. an implementations can be as follows:
// enum class hardware_movement_state_t: int {HW_MOVE_ERROR = -1, HW_MOVE_STOPPED = 0, HW_MOVE_STOPPING,
// HW_MOVE_SLEWING, HW_MOVE_ADJUSTING, HW_MOVE_TRACKING, HW_MOVE_GUIDING}
//
// struct hardware_movement_state_t {
// static constexpr uint16_t HW_MOVE_STOPPED = 0;
// static constexpr uint16_t HW_MOVE_SLEWING = 111;
// static constexpr uint16_t HW_MOVE_ADJUSTING = 222;
// static constexpr uint16_t HW_MOVE_TRACKING = 333;
// static constexpr uint16_t HW_MOVE_GUIDING = 444;
// static constexpr uint16_t HW_MOVE_ERROR = 555;
// static constexpr uint16_t HW_MOVE_STOPPING = 666;
// }
// the 'T' class must contain static constexpr member of 'MccMountType' type
requires std::same_as<decltype(T::hwMountType), const MccMountType>;
[]() {
[[maybe_unused]] static constexpr MccMountType val = T::hwMountType;
}(); // to ensure 'mountType' can be used in compile-time context
requires mcc_hardware_movement_state_c<typename T::hardware_movement_state_t>;
// requires requires(typename T::hardware_movement_state_t type) {
// []() {
// // mount axes were stopped
// static constexpr auto v0 = T::hardware_movement_state_t::HW_MOVE_STOPPED;
// // hardware was asked for slewing (move to given celestial point)
// static constexpr auto v1 = T::hardware_movement_state_t::HW_MOVE_SLEWING;
// // hardware was asked for adjusting after slewing
// // (adjusting actual mount position to align with target celestial point at the end of slewing process)
// static constexpr auto v2 = T::hardware_movement_state_t::HW_MOVE_ADJUSTING;
// // hardware was asked for tracking (track target celestial point)
// static constexpr auto v3 = T::hardware_movement_state_t::HW_MOVE_TRACKING;
// // hardware was asked for guiding
// // (small corrections to align actual mount position with target celestial point)
// static constexpr auto v4 = T::hardware_movement_state_t::HW_MOVE_GUIDING;
// // to detect possible hardware error
// static constexpr auto v5 = T::hardware_movement_state_t::HW_MOVE_ERROR;
// }();
// };
requires mcc_hardware_state_c<typename T::hardware_state_t> && requires(typename T::hardware_state_t state) {
requires std::same_as<decltype(state.movementState), typename T::hardware_movement_state_t>;
};
// requires requires(typename T::hardware_state_t state) {
// // encoder co-longitude and co-latiitude positions, as well as its measurement time point
// // the given constrains on coordinate pair kind can be used to deduce mount type
// requires mcc_coord_pair_c<decltype(state.XY)> &&
// ( // for equathorial mount:
// decltype(state.XY)::pairKind == impl::MccCoordPairKind::COORDS_KIND_HADEC_OBS ||
// // for alt-azimuthal mount:
// decltype(state.XY)::pairKind == impl::MccCoordPairKind::COORDS_KIND_AZALT ||
// decltype(state.XY)::pairKind == impl::MccCoordPairKind::COORDS_KIND_AZZD);
// // co-longitude and co-latiitude axis angular speeds, as well as its measurement/computation time point
// requires mcc_coord_pair_c<decltype(state.speedXY)> &&
// (decltype(state.XY)::pairKind == impl::MccCoordPairKind::COORDS_KIND_GENERIC ||
// decltype(state.XY)::pairKind == impl::MccCoordPairKind::COORDS_KIND_XY);
// requires std::same_as<typename T::hardware_movement_state_t, decltype(state.movementState)>;
// };
// set hardware state:
{ t.hardwareSetState(std::declval<typename T::hardware_state_t const&>()) } -> std::same_as<typename T::error_t>;
@@ -733,6 +720,8 @@ concept mcc_hardware_c = requires(T t) {
// { t.hardwareStop() } -> std::same_as<typename T::error_t>; // stop any moving
{ t.hardwareInit() } -> std::same_as<typename T::error_t>; // initialize hardware
{ t.hardwareShutdown() } -> std::same_as<typename T::error_t>; // shutdown hardware
};
@@ -743,6 +732,14 @@ concept mcc_telemetry_data_c = requires(T t) {
// target celestial point (position on sky where mount must be slewed)
requires mcc_skypoint_c<decltype(t.targetPos)>;
// computed target encoder coordinates
requires(mcc_coord_pair_c<decltype(t.targetXY)> &&
(decltype(t.targetXY)::pairKind == impl::MccCoordPairKind::COORDS_KIND_GENERIC ||
decltype(t.targetXY)::pairKind == impl::MccCoordPairKind::COORDS_KIND_XY));
// corrections to transform observed celestial coordinates of target to hardware encoder ones
requires mcc_pcm_result_c<decltype(t.pcmReverseCorrection)>;
// mount current celestial position
requires mcc_skypoint_c<decltype(t.mountPos)>;
@@ -796,30 +793,33 @@ struct mcc_telemetry_interface_t {
// set target position
template <std::derived_from<mcc_telemetry_interface_t> SelfT>
RetT setTarget(this SelfT&& self, mcc_skypoint_c auto const& pt)
RetT setPointingTarget(this SelfT&& self, mcc_skypoint_c auto const& pt)
{
return std::forward<SelfT>(self).setTarget(pt);
}
// get entered target position
template <std::derived_from<mcc_telemetry_interface_t> SelfT>
RetT getTarget(this SelfT&& self, mcc_skypoint_c auto* pt)
RetT getPointingTarget(this SelfT&& self, mcc_skypoint_c auto* pt)
{
return std::forward<SelfT>(self).getTarget(pt);
}
};
template <typename T>
concept mcc_telemetry_c = std::derived_from<T, mcc_telemetry_interface_t<typename T::error_t>> && requires(T t) {
// error type
requires mcc_error_c<typename T::error_t>;
concept mcc_telemetry_c =
std::derived_from<T, mcc_telemetry_interface_t<typename T::error_t>> && requires(T t, const T t_const) {
// error type
requires mcc_error_c<typename T::error_t>;
// telemetry data type definition
requires mcc_telemetry_data_c<typename T::telemetry_data_t>;
// telemetry data type definition
requires mcc_telemetry_data_c<typename T::telemetry_data_t>;
// get telemetry data
{ t.telemetryData(std::declval<typename T::telemetry_data_t*>()) } -> std::same_as<typename T::error_t>;
};
// get telemetry data
{ t.telemetryData(std::declval<typename T::telemetry_data_t*>()) } -> std::same_as<typename T::error_t>;
{ t_const.getPointingTarget() } -> mcc_skypoint_c;
};

View File

@@ -234,7 +234,7 @@ struct MccSkyRADEC_ICRS : MccCoordPair<MccAngleRA_ICRS, MccAngleDEC_ICRS> {
// ignore epoch setting (it is always J2000.0)
void setEpoch(mcc_coord_epoch_c auto const&)
{
static_assert(false, "CANNOT SET EPOCH FOR ICRS-KIND COORDINATE PAIR!!!");
// static_assert(false, "CANNOT SET EPOCH FOR ICRS-KIND COORDINATE PAIR!!!");
}
};
@@ -344,14 +344,24 @@ struct MccSkyAZALT : MccCoordPair<MccAngleAZ, MccAngleALT> {
}
};
using MccGenXY = MccCoordPair<MccAngleX, MccAngleY>;
using MccGeoLONLAT = MccCoordPair<MccAngleLON, MccAngleLAT>;
// utility type definition: deduce a coordinate pair kind according to mount type
// mcc_deduced_coord_pair_t = MccSkyHADEC_OBS for equathorial mounts,
// mcc_deduced_coord_pair_t = MccSkyAZZD for altazimuthal ones and
// mcc_deduced_coord_pair_t = std::nullptr_t otherwise
template <MccMountType MOUNT_TYPE>
using mcc_deduced_coord_pair_t =
std::conditional_t<mccIsEquatorialMount(MOUNT_TYPE),
MccSkyHADEC_OBS,
std::conditional_t<mccIsAltAzMount(MOUNT_TYPE), MccSkyAZZD, std::nullptr_t>>;
static MccSkyHADEC_APP hadec = MccGenXY{};
static MccSkyAZALT azalt{MccSkyAZZD{1.0, 1.1}};
// static MccSkyHADEC_APP hadec = MccGenXY{};
// static MccSkyAZALT azalt{MccSkyAZZD{1.0, 1.1}};
/* MCC-LIBRARY DEFAULT GENERIC SKY POINT CLASS IMPLEMENTATION */
@@ -370,6 +380,14 @@ public:
using error_t = typename CCTE_T::error_t;
struct dist_result_t {
MccAngle dist{};
MccAngle dx{};
MccAngle dy{};
MccAngle x2{};
MccAngle y2{};
};
MccGenericSkyPoint() {}
template <mcc_coord_pair_c PT>
@@ -423,6 +441,17 @@ public:
return _epoch;
}
double co_lon() const
{
return _x;
}
double co_lat() const
{
return _y;
}
template <mcc_coord_pair_c PT>
MccGenericSkyPoint& from(const PT& coord_pair)
{
@@ -516,6 +545,78 @@ public:
return error_t{};
}
// 'inner' transformation
error_t to(MccCoordPairKind pair_kind)
{
return to(pair_kind, _epoch);
}
error_t to(MccCoordPairKind pair_kind, mcc_coord_epoch_c auto const& epoch)
{
// do not use here "mcc_coord_epoch_c::operator==" to avoid
// unnecessary computations (astrometrical algorithms mainly use Julian date as input)
if (pair_kind == _pairKind && utils::isEqual(epoch.MJD(), _epoch.MJD())) {
_epoch = epoch;
return error_t{};
}
auto tr_func = [&, this]<mcc_coord_pair_c T>(T& cp) {
if constexpr (T::pairKind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
cp.setEpoch(epoch);
}
auto err = to(cp);
if (!err) {
from(cp);
}
return err;
};
switch (pair_kind) {
case MccCoordPairKind::COORDS_KIND_RADEC_ICRS: {
MccSkyRADEC_ICRS cp;
return tr_func(cp);
} break;
case MccCoordPairKind::COORDS_KIND_RADEC_OBS: {
MccSkyRADEC_OBS cp;
return tr_func(cp);
} break;
case MccCoordPairKind::COORDS_KIND_RADEC_APP: {
MccSkyRADEC_APP cp;
return tr_func(cp);
} break;
case MccCoordPairKind::COORDS_KIND_HADEC_OBS: {
MccSkyHADEC_OBS cp;
return tr_func(cp);
} break;
case MccCoordPairKind::COORDS_KIND_HADEC_APP: {
MccSkyHADEC_APP cp;
return tr_func(cp);
} break;
case MccCoordPairKind::COORDS_KIND_AZZD: {
MccSkyAZZD cp;
return tr_func(cp);
} break;
case MccCoordPairKind::COORDS_KIND_AZALT: {
MccSkyAZALT cp;
return tr_func(cp);
} break;
case MccCoordPairKind::COORDS_KIND_GENERIC:
case MccCoordPairKind::COORDS_KIND_UNKNOWN:
case MccCoordPairKind::COORDS_KIND_LONLAT:
case MccCoordPairKind::COORDS_KIND_XY: {
MccGenXY cp;
return tr_func(cp);
} break;
default:
return error_t{};
}
return error_t{};
}
error_t refractCorrection(mcc_angle_c auto* dZ) const
{
@@ -592,11 +693,34 @@ public:
return cctEngine.equationOrigins(_epoch, eo);
}
dist_result_t distance(MccGenericSkyPoint const& sp) const
// dist_result_t distance(mcc_skypoint_c auto const& sp)
{
double x, y;
if (_pairKind == sp.pairKind() && utils::isEqual(_epoch.MJD(), sp.epoch().MJD())) {
x = sp.co_lon();
y = sp.co_lat();
} else { // convert to the same coordinates kind
MccGenericSkyPoint p{sp};
p.to(_pairKind, _epoch);
x = p.co_lon();
y = p.co_lat();
}
auto d = utils::distanceOnSphere(_x, _y, x, y);
return {.dist = std::get<2>(d), .dx = std::get<0>(d), .dy = std::get<1>(d), .x2 = x, .y2 = y};
}
protected:
double _x{0.0}, _y{0.0};
MccCoordPairKind _pairKind{MccCoordPairKind::COORDS_KIND_RADEC_ICRS};
MccCelestialCoordEpoch _epoch{}; // J2000.0
template <mcc_skypoint_c T>
void fromOtherSkyPoint(T&& other)
{
@@ -730,6 +854,8 @@ protected:
double phi = cctEngine.getStateERFA().lat;
double ra_icrs, dec_icrs, ra, dec, ha, az, zd, alt, lst, eo;
static_assert(PT::pairKind != MccCoordPairKind::COORDS_KIND_LONLAT, "UNSUPPORTED SKY POINT TRANSFORMATION!");
// static_assert(PT::pairKind != MccCoordPairKind::COORDS_KIND_XY, "UNSUPPORTED SKY POINT TRANSFORMATION!");
static_assert(PT::pairKind != MccCoordPairKind::COORDS_KIND_GENERIC, "UNSUPPORTED SKY POINT TRANSFORMATION!");
static_assert(PT::pairKind != MccCoordPairKind::COORDS_KIND_UNKNOWN, "UNSUPPORTED SKY POINT TRANSFORMATION!");
@@ -765,7 +891,8 @@ protected:
// 1) convert stored coordinates to ICRS ones
// 2) convert from the computed ICRS coordinates to required ones
MccCoordPairKind pkind = _pairKind;
if (!utils::isEqual(_epoch.MJD(), cpair.MJD())) { // convert stored pair to ICRS one (ra_icrs, dec_icrs)
if (!utils::isEqual(_epoch.MJD(),
cpair.MJD())) { // convert stored pair to ICRS one (ra_icrs, dec_icrs)
if (_pairKind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
pkind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS;
@@ -934,7 +1061,8 @@ protected:
cpair.setY(dec);
} else {
obj->hadec2azalt(ha, dec, phi, az, alt);
if constexpr (mccIsObsCoordPairKind<PT::pairKind>) { // RADEC_OBS, HADEC_OBS, AZALT, AZZD
if constexpr (mccIsObsCoordPairKind<PT::pairKind>) { // RADEC_OBS, HADEC_OBS,
// AZALT, AZZD
// correct for refraction: alt += dz_refr
double dZ;
ccte_err = cctEngine.refractionInverseCorrection(MCC_HALF_PI - alt, &dZ);
@@ -987,7 +1115,8 @@ protected:
};
/* MCC-LIBRARY DEFAULT SKY POINT CLASS IMPLEMENTATION BASED ON THE ERFA LIBRARY */
/* MCC-LIBRARY DEFAULT SKY POINT CLASS IMPLEMENTATION BASED ON THE ERFA LIBRARY
*/
typedef MccGenericSkyPoint<mcc::ccte::erfa::MccCCTE_ERFA> MccSkyPoint;

View File

@@ -279,14 +279,18 @@ struct MccDeserializer<VT> : MccDeserializerBase {
{
auto pars = params;
// valid format: X<elem-delim>Y[<elem-delim>TIME-POINT<elem-delim>PAIR-KIND]
// X<elem-delim>Y (assumed RADEC_ICRS and J2000.0 epoch)
// valid format: X<elem-delim>Y[<elem-delim>TIME-POINT<elem-delim>PAIR-KIND]
// X<elem-delim>Y (assumed RADEC_ICRS and J2000.0 epoch)
// valid format: X<elem-delim>Y<elem-delim>PAIRKIND<elem-delim>EPOCH
// X<elem-delim>Y<elem-delim>PAIRKIND (assumed epoch is NOW, or J2000.0 if PAIRKIND == RADEC-ICRS)
// X<elem-delim>Y (assumed RADEC-ICRS and J2000.0 epoch)
bool empty;
auto elems = MccDeserializerBase::splitValueIntoElements(input, params, empty);
if (empty || (elems.size() < 2) || (elems.size() == 3)) {
if (empty || (elems.size() < 2)) {
return MccDeserializerErrorCode::ERROR_INVALID_SERIALIZED_VALUE;
}
@@ -298,20 +302,30 @@ struct MccDeserializer<VT> : MccDeserializerBase {
typename MccDeserializer<MccAngle>::error_t dsr_err;
if (elems.size() > 3) { // full format
if (elems.size() > 2) { // no epoch
// deserialize pair kind string
pair_kind = MccCoordStrToPairKind(elems[3]);
pair_kind = MccCoordStrToPairKind(elems[2]);
if (pair_kind == MccCoordPairKind::COORDS_KIND_UNKNOWN) {
return MccDeserializerErrorCode::ERROR_INVALID_SERIALIZED_VALUE;
}
// epoch
bool ok = epoch.fromCharRange(elems[2]);
if (!ok) {
return MccDeserializerErrorCode::ERROR_INVALID_SERIALIZED_VALUE;
if (pair_kind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) {
epoch = MccCelestialCoordEpoch::now();
}
}
if (elems.size() > 3) { // full format
// epoch
if (pair_kind != MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { // ignore epoch if PAIRKIND == RADEC-ICRS
bool ok = epoch.fromCharRange(elems[3]);
if (!ok) {
return MccDeserializerErrorCode::ERROR_INVALID_SERIALIZED_VALUE;
}
}
}
// deserialize X and Y
if (params.coordpair_format == MccSerializedCoordPairFormat::MCC_SERIALIZED_FORMAT_SXGM_HOURDEG) {
pars.angle_format = MccSerializedAngleFormat::MCC_SERIALIZED_FORMAT_SXGM_HOURS;
@@ -375,10 +389,9 @@ struct MccDeserializer<MccCoordPairKind> : MccDeserializerBase {
static constexpr std::string_view deserializerName{"MCC-COORDPAIR-DESERIALIZER"};
template <mcc_serialization_params_c ParamsT = mcc_serialization_params_t>
error_t operator ()(
traits::mcc_input_char_range auto const& input,
MccCoordPairKind& value,
ParamsT const& params = mcc_serialization_params_t{})
error_t operator()(traits::mcc_input_char_range auto const& input,
MccCoordPairKind& value,
ParamsT const& params = mcc_serialization_params_t{})
{
value = MccCoordStrToPairKind(input);
@@ -396,10 +409,9 @@ struct MccDeserializer<MccSerializedAngleFormatPrec> : MccDeserializerBase {
static constexpr std::string_view deserializerName{"MCC-ANGLE-FORMAT-PREC-DESERIALIZER"};
template <mcc_serialization_params_c ParamsT = mcc_serialization_params_t>
error_t operator ()(
traits::mcc_input_char_range auto const& input,
MccSerializedAngleFormatPrec& value,
ParamsT const& params = mcc_serialization_params_t{})
error_t operator()(traits::mcc_input_char_range auto const& input,
MccSerializedAngleFormatPrec& value,
ParamsT const& params = mcc_serialization_params_t{})
{
// valid format: hour_prec[<params.elem_delim>deg_prec<params.elem_delim>decimals]
@@ -433,10 +445,9 @@ struct MccDeserializer<MccSerializedCoordPairFormat> : MccDeserializerBase {
static constexpr std::string_view deserializerName{"MCC-COORDPAIR-FORMAT-DESERIALIZER"};
template <mcc_serialization_params_c ParamsT = mcc_serialization_params_t>
error_t operator ()(
traits::mcc_input_char_range auto const& input,
MccSerializedCoordPairFormat& value,
ParamsT const& params = mcc_serialization_params_t{})
error_t operator()(traits::mcc_input_char_range auto const& input,
MccSerializedCoordPairFormat& value,
ParamsT const& params = mcc_serialization_params_t{})
{
value = MccSerializedCoordPairFormatStrToValue(input);

View File

@@ -53,7 +53,7 @@ struct std::formatter<mcc::impl::MccError, char> {
// _currFmt.push_back(*(++it));
// }
if (it++ != ctx.end() || it++ != '}') {
if (it++ != ctx.end() || *(it++) != '}') {
_delim = *(++it);
}
case '}':

View File

@@ -14,6 +14,7 @@
#include <thread>
#include "mcc/mcc_spdlog.h"
#include "mcc_error.h"
namespace mcc::impl
@@ -33,8 +34,6 @@ enum class MccGenericMountErrorCode : int {
ERROR_TARGET_IN_ZONE
};
enum class MccGenericFsmMountErrorCode : int { ERROR_OK, ERROR_INVALID_OPERATION, ERROR_UNKNOWN_EVENT };
} // namespace mcc::impl
@@ -70,7 +69,7 @@ struct MccGenericMountCategory : public std::error_category {
case MccGenericMountErrorCode::ERROR_OK:
return "OK";
case MccGenericMountErrorCode::ERROR_HW_INIT:
return "an error occured while initializing mount";
return "an error occured while initializing mount hardware";
case MccGenericMountErrorCode::ERROR_HW_STOP:
return "an error occured while stopping mount";
case MccGenericMountErrorCode::ERROR_HW_GETSTATE:
@@ -109,16 +108,11 @@ inline std::error_code make_error_code(MccGenericMountErrorCode ec)
template <mcc_hardware_c HARDWARE_T,
mcc_telemetry_c TELEMETRY_T,
template <mcc_telemetry_c TELEMETRY_T,
mcc_pzone_container_c PZONE_CONT_T,
mcc_movement_controls_c MOVE_CNTRL_T,
mcc_logger_c LOGGER_T>
class MccGenericMount : protected HARDWARE_T,
public TELEMETRY_T,
public PZONE_CONT_T,
public MOVE_CNTRL_T,
public LOGGER_T
mcc_logger_c LOGGER_T = utils::MccSpdlogLogger>
class MccGenericMount : public TELEMETRY_T, public PZONE_CONT_T, public MOVE_CNTRL_T, public LOGGER_T
{
public:
using LOGGER_T::logDebug;
@@ -144,25 +138,43 @@ public:
MOUNT_STATUS_TRACKING
};
template <typename... HardwareCtorTs,
typename... TelemetryCtorTs,
template <typename... TelemetryCtorTs,
typename... PzoneContCtorTs,
typename... MoveCntrCtorTs,
typename... LoggerCtorTs>
MccGenericMount(std::tuple<HardwareCtorTs...> hw_ctor_args,
std::tuple<TelemetryCtorTs...> telemetry_ctor_args,
MccGenericMount(std::tuple<TelemetryCtorTs...> telemetry_ctor_args,
std::tuple<PzoneContCtorTs...> pzone_cont_ctor_ars,
std::tuple<MoveCntrCtorTs...> move_cntrl_ctor_ars,
std::tuple<LoggerCtorTs...> logger_ctor_args)
: HARDWARE_T(std::make_from_tuple<HARDWARE_T>(std::move(hw_ctor_args))),
TELEMETRY_T(std::make_from_tuple<TELEMETRY_T>(std::move(telemetry_ctor_args))),
: TELEMETRY_T(std::make_from_tuple<TELEMETRY_T>(std::move(telemetry_ctor_args))),
PZONE_CONT_T(std::make_from_tuple<PZONE_CONT_T>(std::move(pzone_cont_ctor_ars))),
MOVE_CNTRL_T(std::make_from_tuple<MOVE_CNTRL_T>(std::move(move_cntrl_ctor_ars))),
LOGGER_T(std::make_from_tuple<LOGGER_T>(std::move(logger_ctor_args)))
{
// logDebug(std::format("Create MccGenericMount class instance (thread: {})", std::this_thread::get_id()));
logDebug("Create MccGenericMount class instance (thread: {})", std::this_thread::get_id());
}
template <typename... TelemetryCtorTs,
typename... PzoneContCtorTs,
typename... MoveCntrCtorTs,
typename... LoggerCtorTs>
requires std::derived_from<LOGGER_T, utils::MccSpdlogLogger>
MccGenericMount(std::tuple<TelemetryCtorTs...> telemetry_ctor_args,
std::tuple<PzoneContCtorTs...> pzone_cont_ctor_ars,
std::tuple<MoveCntrCtorTs...> move_cntrl_ctor_ars,
LoggerCtorTs... logger_ctor_args)
: TELEMETRY_T(std::make_from_tuple<TELEMETRY_T>(std::move(telemetry_ctor_args))),
PZONE_CONT_T(std::make_from_tuple<PZONE_CONT_T>(pzone_cont_ctor_ars)),
MOVE_CNTRL_T(std::make_from_tuple<MOVE_CNTRL_T>(move_cntrl_ctor_ars)),
LOGGER_T(std::make_from_tuple<LOGGER_T>(logger_ctor_args))
mcc::utils::MccSpdlogLogger(logger_ctor_args...)
{
logDebug(std::format("Create MccGenericMount class instance (thread: {})", std::this_thread::get_id()));
// logDebug(std::format("Create MccGenericMount class instance (thread: {})", std::this_thread::get_id()));
logDebug("Create MccGenericMount class instance (thread: {})", std::this_thread::get_id());
}
/* movable-only class */
MccGenericMount(const MccGenericMount&) = delete;
MccGenericMount(MccGenericMount&&) = default;
@@ -171,34 +183,25 @@ public:
virtual ~MccGenericMount()
{
logDebug(std::format("Delete MccGenericMount class instance (thread: {})", std::this_thread::get_id()));
// auto err = MOVE_CNTRL_T::stopMount();
// if (err) {
// logError(formatError(err));
// }
auto err = MOVE_CNTRL_T::stopMount();
if (err) {
logError(formatError(err));
}
// WARNING: it is assumed here that mount stopping is performed in a derived class or, it is more logicaly,
// in MOVE_CNTRL_T-class
// logDebug(std::format("Delete MccGenericMount class instance (thread: {})", std::this_thread::get_id()));
logDebug("Delete MccGenericMount class instance (thread: {})", std::this_thread::get_id());
}
error_t initMount()
{
logInfo(std::format("Start MccGenericMount class initialization (thread: {}) ...", std::this_thread::get_id()));
logInfo("Start MccGenericMount class initialization (thread: {}) ...", std::this_thread::get_id());
*_lastMountError = MccGenericMountErrorCode::ERROR_OK;
*_mountStatus = mount_status_t::MOUNT_STATUS_IDLE;
*_mountStatus = mount_status_t::MOUNT_STATUS_INITIALIZATION;
auto hw_err = this->hardwareInit();
if (hw_err) {
*_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
*_lastMountError = mcc_deduce_err(hw_err, MccGenericMountErrorCode::ERROR_HW_INIT);
} else {
logInfo("Generic mount initialization was performed");
*_mountStatus = mount_status_t::IDLE;
}
return *_lastMountError;
return *_lastMountError = MccGenericMountErrorCode::ERROR_OK;
}
mount_status_t mountStatus() const
@@ -211,6 +214,45 @@ public:
return _lastMountError->load();
}
/* log-method wrappers for non-MccSpdlogger classes */
template <std::formattable<char>... ArgTs>
requires(!std::derived_from<LOGGER_T, utils::MccSpdlogLogger>)
void logInfo(std::format_string<ArgTs...> fmt, ArgTs&&... args)
{
LOGGER_T::logInfo(std::format(fmt, std::forward<ArgTs>(args)...));
}
template <std::formattable<char>... ArgTs>
requires(!std::derived_from<LOGGER_T, utils::MccSpdlogLogger>)
void logDebug(std::format_string<ArgTs...> fmt, ArgTs&&... args)
{
LOGGER_T::logDebug(std::format(fmt, std::forward<ArgTs>(args)...));
}
template <std::formattable<char>... ArgTs>
requires(!std::derived_from<LOGGER_T, utils::MccSpdlogLogger>)
void logError(std::format_string<ArgTs...> fmt, ArgTs&&... args)
{
LOGGER_T::logError(std::format(fmt, std::forward<ArgTs>(args)...));
}
template <std::formattable<char>... ArgTs>
requires(!std::derived_from<LOGGER_T, utils::MccSpdlogLogger>)
void logWarn(std::format_string<ArgTs...> fmt, ArgTs&&... args)
{
LOGGER_T::logWarn(std::format(fmt, std::forward<ArgTs>(args)...));
}
template <std::formattable<char>... ArgTs>
requires(!std::derived_from<LOGGER_T, utils::MccSpdlogLogger>)
void logTrace(std::format_string<ArgTs...> fmt, ArgTs&&... args)
{
LOGGER_T::logTrace(std::format(fmt, std::forward<ArgTs>(args)...));
}
protected:
std::unique_ptr<std::atomic<mount_status_t>> _mountStatus{
new std::atomic<mount_status_t>{mount_status_t::MOUNT_STATUS_UNINITIALIZED}};
@@ -219,7 +261,7 @@ protected:
std::string formatError(error_t const& err, std::string_view prefix = "") const
{
return std::format("{}{} (category: {}, code: {})", prefix, err.message(), err.value(), err.category().name());
return std::format("{}{} (category: {}, code: {})", prefix, err.message(), err.category().name(), err.value());
}
};

View File

@@ -0,0 +1,715 @@
#pragma once
/****************************************************************************************
* *
* MOUNT CONTROL COMPONENTS LIBRARY *
* *
* *
* GENERIC IMPLEMENTATION OF MOUNT MOVEMENT CONTROLS *
* *
****************************************************************************************/
#include <atomic>
#include <fstream>
#include <future>
#include <thread>
#include <type_traits>
#include <print>
#include "mcc_coordinate.h"
#include "mcc_error.h"
namespace mcc::impl
{
// mount movement-related generic errors
enum class MccGenericMovementControlsErrorCode : int {
ERROR_OK,
ERROR_IN_PZONE,
ERROR_NEAR_PZONE,
ERROR_SLEW_TIMEOUT,
ERROR_STOP_TIMEOUT,
ERROR_HARDWARE,
ERROR_TELEMETRY_TIMEOUT
};
} // namespace mcc::impl
namespace std
{
template <>
class is_error_code_enum<mcc::impl::MccGenericMovementControlsErrorCode> : public true_type
{
};
} // namespace std
namespace mcc::impl
{
// error category
struct MccGenericMovementControlsErrorCategory : std::error_category {
const char* name() const noexcept
{
return "MCC-GENERIC-MOVECONTRL";
}
std::string message(int ec) const
{
MccGenericMovementControlsErrorCode err = static_cast<MccGenericMovementControlsErrorCode>(ec);
switch (err) {
case MccGenericMovementControlsErrorCode::ERROR_OK:
return "OK";
case MccGenericMovementControlsErrorCode::ERROR_IN_PZONE:
return "target is in zone";
case MccGenericMovementControlsErrorCode::ERROR_NEAR_PZONE:
return "mount is near zone";
case MccGenericMovementControlsErrorCode::ERROR_SLEW_TIMEOUT:
return "a timeout occured while slewing";
case MccGenericMovementControlsErrorCode::ERROR_STOP_TIMEOUT:
return "a timeout occured while mount stopping";
case MccGenericMovementControlsErrorCode::ERROR_HARDWARE:
return "a hardware error occured";
case MccGenericMovementControlsErrorCode::ERROR_TELEMETRY_TIMEOUT:
return "telemetry data timeout";
default:
return "unknown";
}
}
static const MccGenericMovementControlsErrorCategory& get()
{
static const MccGenericMovementControlsErrorCategory constInst;
return constInst;
}
};
inline std::error_code make_error_code(MccGenericMovementControlsErrorCode ec)
{
return std::error_code(static_cast<int>(ec), MccGenericMovementControlsErrorCategory::get());
}
struct MccGenericMovementControlsParams {
// timeout to telemetry updating
std::chrono::milliseconds telemetryTimeout{3000};
// braking acceleration after execution of mount stopping command (in rads/s^2)
// it must be given as non-negative value!!!
double brakingAccelX{0.0};
double brakingAccelY{0.0};
// ******* slewing mode *******
// coordinates difference to stop slewing (in radians)
double slewToleranceRadius{5.0_arcsecs};
// slewing trajectory file. if empty - just skip saving
std::string slewingPathFilename{};
// ******* tracking mode *******
// maximal target-to-mount difference for tracking process (in arcsecs)
// it it is greater then the current mount coordinates are used as target one
double trackingMaxCoordDiff{20.0};
// tracking trajectory file. if empty - just skip saving
std::string trackingPathFilename{};
};
/* UTILITY CLASS TO HOLD AND SAVE MOUNT MOVING TRAJECTORY */
struct MccMovementPathFile {
static constexpr std::string_view commentSeq{"# "};
static constexpr std::string_view lineDelim{"\n"};
void setCommentSeq(traits::mcc_input_char_range auto const& s)
{
_commentSeq.clear();
std::ranges::copy(s, std::back_inserter(_commentSeq));
}
void setLineDelim(traits::mcc_input_char_range auto const& s)
{
_lineDelim.clear();
std::ranges::copy(s, std::back_inserter(_lineDelim));
}
// add comment string/strings
template <traits::mcc_input_char_range RT, traits::mcc_input_char_range... RTs>
void addComment(RT const& r, RTs const&... rs)
{
std::ranges::copy(_commentSeq, std::back_inserter(_buffer));
if constexpr (std::is_pointer_v<std::decay_t<RT>>) { // const char*, char*, char[]
std::ranges::copy(std::string_view{r}, std::back_inserter(_buffer));
} else {
std::ranges::copy(r, std::back_inserter(_buffer));
}
std::ranges::copy(lineDelim, std::back_inserter(_buffer));
if constexpr (sizeof...(RTs)) {
addComment(rs...);
}
}
// comment corresponded to addToPath(mcc_telemetry_data_c auto const& tdata)
void addDefaultComment()
{
addComment("Format (time is in milliseconds, coordinates are in degrees, speeds are in degrees/s):");
addComment(
" <UNIXTIME> <mount X> <mount Y> <target X> <target Y> <dX_{mount-target}> "
"<dY_{mount-target}> <mount-to-target-distance> <mount X-speed> <mount Y-speed> <moving state>");
}
// general purpose method
// template <std::formattable<char>... ArgTs>
// void addToPath(std::format_string<ArgTs...> fmt, ArgTs&&... args)
// {
// std::format_to(std::back_inserter(_buffer), fmt, std::forward<ArgTs>(args)...);
// std::ranges::copy(lineDelim, std::back_inserter(_buffer));
// }
// general purpose method
template <std::formattable<char>... ArgTs>
void addToPath(std::string_view fmt, ArgTs&&... args)
{
std::vformat_to(std::back_inserter(_buffer), fmt, std::make_format_args(args...));
std::ranges::copy(lineDelim, std::back_inserter(_buffer));
}
// default-implemented method
void addToPath(mcc_telemetry_data_c auto const& tdata)
{
// UNIX-time millisecs, mount X, mount Y, target X, target Y, dX(mount-target), dY(mount-target), dist, speedX,
// speedY, state
auto dist = tdata.mountPos.distance(tdata.targetPos);
using d_t = std::chrono::milliseconds;
auto tp = std::chrono::duration_cast<d_t>(tdata.mountPos.epoch().UTC().time_since_epoch());
const std::string_view d_fmt = "{:14.8f}";
const auto v = std::views::repeat(d_fmt, 9) | std::views::join_with(' ');
std::string fmt = "{} " + std::string(v.begin(), v.end()) + " {}";
int state = (int)tdata.hwState.movementState;
auto tp_val = tp.count();
double mnt_x = MccAngle(tdata.mountPos.co_lon()).degrees(), mnt_y = MccAngle(tdata.mountPos.co_lon()).degrees(),
tag_x = dist.x2.degrees(), tag_y = dist.y2.degrees(), dx = dist.dx.degrees(), dy = dist.dy.degrees(),
dd = dist.dist.degrees();
addToPath(std::string_view(fmt.begin(), fmt.end()), tp_val, mnt_x, mnt_y, tag_x, tag_y, dx, dy, dd,
tdata.hwState.speedXY.x().degrees(), tdata.hwState.speedXY.y().degrees(), state);
}
void clearPath()
{
_buffer.clear();
}
bool saveToFile(std::string const& filename, std::ios_base::openmode mode = std::ios::out | std::ios::trunc)
{
if (filename.empty()) {
return true;
}
std::ofstream fst(filename, mode);
if (fst.is_open()) {
fst << _buffer;
return true;
} else {
return false;
}
}
protected:
std::string _buffer{};
std::string _commentSeq{commentSeq};
std::string _lineDelim{lineDelim};
};
enum class MccGenericMovementControlsPolicy : int { POLICY_ASYNC, POLICY_BLOCKING };
template <std::movable PARAMS_T,
MccGenericMovementControlsPolicy EXEC_POLICY = MccGenericMovementControlsPolicy::POLICY_ASYNC>
class MccGenericMovementControls
{
public:
static constexpr MccGenericMovementControlsPolicy executePolicy = EXEC_POLICY;
static constexpr std::chrono::seconds defaultWaitTimeout{3};
typedef MccError error_t;
typedef PARAMS_T movement_params_t;
template <std::invocable<bool> SLEW_FUNC_T, std::invocable<> TRACK_FUNC_T, std::invocable<> STOP_FUNC_T>
MccGenericMovementControls(SLEW_FUNC_T&& slew_func, TRACK_FUNC_T&& track_func, STOP_FUNC_T&& stop_func)
: _slewFunc(std::forward<SLEW_FUNC_T>(slew_func)),
_trackFunc(std::forward<TRACK_FUNC_T>(track_func)),
_stopFunc(std::forward<STOP_FUNC_T>(stop_func))
{
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
// *_stopMovementRequest = false;
// *_fsmState = STATE_IDLE;
// // start thread of movements
// _fstFuture = std::async(
// [this](std::stop_token stoken) {
// auto do_state = _fsmState->load();
// while (!stoken.stop_requested()) {
// std::println("\n{:*^80}\n", " WAIT LOCK ");
// // wait here ...
// _wakeupRequest->wait(false, std::memory_order_relaxed);
// _wakeupRequest->clear();
// std::println("\n{:*^80}\n", " UNLOCKED ");
// // if (stoken.stop_requested()) {
// // break;
// // }
// do_state = _fsmState->load();
// if (do_state & STATE_STOP) {
// // if (_fsmState->load() & STATE_STOP) {
// *_stopMovementRequest = true;
// _stopFunc();
// }
// if (do_state & STATE_SLEW) {
// // if (_fsmState->load() & STATE_SLEW) {
// *_stopMovementRequest = false;
// _slewFunc(_slewAndStop->load());
// } else if (do_state & STATE_TRACK) {
// // } else if (_fsmState->load() & STATE_TRACK) {
// *_stopMovementRequest = false;
// _trackFunc();
// }
// }
// },
// _fstStopSource.get_token());
startAsyncMovementCycle();
}
}
MccGenericMovementControls(const MccGenericMovementControls&) = delete;
MccGenericMovementControls(MccGenericMovementControls&& other) = default;
MccGenericMovementControls& operator=(const MccGenericMovementControls&) = delete;
MccGenericMovementControls& operator=(MccGenericMovementControls&&) = default;
virtual ~MccGenericMovementControls()
{
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
// *_fsmState = STATE_IDLE;
// _fstStopSource.request_stop();
// _wakeupRequest->test_and_set();
// _wakeupRequest->notify_one();
// std::this_thread::sleep_for(std::chrono::milliseconds(200));
// if (_fstFuture.valid()) {
// auto status = _fstFuture.wait_for(_waitTimeout->load());
// }
stopAsyncMovementCycle();
}
}
template <typename SelfT>
void stopAsyncMovementCycle(this SelfT&& self)
requires(EXEC_POLICY == MccGenericMovementControlsPolicy::POLICY_ASYNC)
{
*self._fsmState = STATE_IDLE;
self._fstStopSource.request_stop();
self._wakeupRequest->test_and_set();
self._wakeupRequest->notify_all();
std::this_thread::sleep_for(std::chrono::milliseconds(200));
if (self._fstFuture.valid()) {
auto status = self._fstFuture.wait_for(self._waitTimeout->load());
}
}
template <typename SelfT>
void startAsyncMovementCycle(this SelfT& self)
requires(EXEC_POLICY == MccGenericMovementControlsPolicy::POLICY_ASYNC)
{
using self_t = std::decay_t<SelfT>;
if (self._fstFuture.valid()) {
self.stopAsyncMovementCycle();
}
*self._stopMovementRequest = false;
self._fstStopSource = std::stop_source{};
*self._fsmState = STATE_IDLE;
// start thread of movements
self._fstFuture = std::async(
std::launch::async,
[&self](std::stop_token stoken) mutable {
auto do_state = self._fsmState->load();
std::string log_str;
while (!stoken.stop_requested()) {
log_str = std::format("\n{:*^80}\n", " WAIT LOCK ");
if constexpr (mcc_generic_mount_c<self_t>) {
self.logTrace(log_str);
} else {
std::println("{}", log_str);
}
// wait here ...
self._wakeupRequest->wait(false, std::memory_order_relaxed);
self._wakeupRequest->clear();
log_str = std::format("\n{:*^80}\n", " UNLOCKED ");
if constexpr (mcc_generic_mount_c<self_t>) {
self.logTrace(log_str);
} else {
std::println("{}", log_str);
}
// if (stoken.stop_requested()) {
// break;
// }
do_state = self._fsmState->load();
if (do_state & STATE_STOP) {
// if (_fsmState->load() & STATE_STOP) {
*self._stopMovementRequest = true;
self._stopFunc();
}
if (do_state & STATE_SLEW) {
// if (_fsmState->load() & STATE_SLEW) {
*self._stopMovementRequest = false;
self._slewFunc(self._slewAndStop->load());
} else if (do_state & STATE_TRACK) {
// } else if (_fsmState->load() & STATE_TRACK) {
*self._stopMovementRequest = false;
self._trackFunc();
}
}
},
self._fstStopSource.get_token());
}
error_t slewToTarget(bool slew_and_stop)
{
// *_stopMovementRequest = false;
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
auto prev_state = _fsmState->load();
*_fsmState = STATE_SLEW;
if (prev_state & STATE_SLEW || prev_state & STATE_TRACK) {
*_fsmState |= STATE_STOP;
*_stopMovementRequest = true; // to exit from slewing or tracking
}
*_slewAndStop = slew_and_stop;
_wakeupRequest->test_and_set();
_wakeupRequest->notify_one();
return MccGenericMovementControlsErrorCode::ERROR_OK;
} else if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_BLOCKING) {
return _slewFunc(slew_and_stop);
} else {
static_assert(false, "UNKNOWN EXECUTION POLICY!");
}
}
error_t trackTarget()
{
// *_stopMovementRequest = false;
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
auto prev_state = _fsmState->load();
if (!(prev_state & STATE_TRACK)) {
*_fsmState = STATE_TRACK;
if (prev_state & STATE_SLEW) {
*_fsmState |= STATE_STOP;
*_stopMovementRequest = true; // to exit from slewing
}
_wakeupRequest->test_and_set();
_wakeupRequest->notify_one();
} // already tracking, just ignore
return MccGenericMovementControlsErrorCode::ERROR_OK;
} else if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_BLOCKING) {
return _trackFunc();
} else {
static_assert(false, "UNKNOWN EXECUTION POLICY!");
}
}
error_t stopMount()
{
// *_stopMovementRequest = true;
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
*_fsmState = STATE_STOP;
*_stopMovementRequest = true; // to exit from slewing or tracking
_wakeupRequest->test_and_set();
_wakeupRequest->notify_one();
return MccGenericMovementControlsErrorCode::ERROR_OK;
} else if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_BLOCKING) {
return _stopFunc();
} else {
static_assert(false, "UNKNOWN EXECUTION POLICY!");
}
}
error_t setMovementParams(movement_params_t const& pars)
{
std::lock_guard lock{*_currentMovementParamsMutex};
_currentMovementParams = pars;
return MccGenericMovementControlsErrorCode::ERROR_OK;
}
movement_params_t getMovementParams() const
{
std::lock_guard lock{*_currentMovementParamsMutex};
return _currentMovementParams;
}
protected:
std::unique_ptr<std::mutex> _currentMovementParamsMutex{new std::mutex{}};
PARAMS_T _currentMovementParams{};
std::unique_ptr<std::atomic_bool> _stopMovementRequest{new std::atomic_bool{false}};
std::function<error_t(bool)> _slewFunc{};
std::function<error_t()> _trackFunc{};
std::function<error_t()> _stopFunc{};
std::unique_ptr<std::atomic<std::chrono::nanoseconds>> _waitTimeout{
new std::atomic<std::chrono::nanoseconds>{defaultWaitTimeout}};
std::conditional_t<executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC,
std::future<void>,
std::nullptr_t>
_fstFuture{};
std::stop_source _fstStopSource{};
std::unique_ptr<std::atomic_flag> _wakeupRequest{new std::atomic_flag};
std::unique_ptr<std::atomic_bool> _slewAndStop{new std::atomic_bool{false}};
enum { STATE_IDLE = 0x00, STATE_SLEW = 0x01, STATE_TRACK = 0x02, STATE_STOP = 0x04 };
std::unique_ptr<std::atomic_int> _fsmState{new std::atomic_int};
// template <typename SelfT>
// void mainCycle(this SelfT&& self, std::stop_token stoken)
// {
// using self_t = std::decay_t<SelfT>;
// auto do_state = _fsmState->load();
// std::string log_str;
// while (!stoken.stop_requested()) {
// log_str = std::format("\n{:*^80}\n", " WAIT LOCK ");
// if constexpr (mcc_generic_mount_c<self_t>) {
// self.logTrace(log_str);
// } else {
// std::println(log_str);
// }
// // wait here ...
// _wakeupRequest->wait(false, std::memory_order_relaxed);
// _wakeupRequest->clear();
// log_str = std::format("\n{:*^80}\n", " UNLOCKED ");
// if constexpr (mcc_generic_mount_c<self_t>) {
// self.logTrace(log_str);
// } else {
// std::println(log_str);
// }
// // if (stoken.stop_requested()) {
// // break;
// // }
// do_state = _fsmState->load();
// if (do_state & STATE_STOP) {
// // if (_fsmState->load() & STATE_STOP) {
// *_stopMovementRequest = true;
// _stopFunc();
// }
// if (do_state & STATE_SLEW) {
// // if (_fsmState->load() & STATE_SLEW) {
// *_stopMovementRequest = false;
// _slewFunc(_slewAndStop->load());
// } else if (do_state & STATE_TRACK) {
// // } else if (_fsmState->load() & STATE_TRACK) {
// *_stopMovementRequest = false;
// _trackFunc();
// }
// }
// }
// utility methods
// the method calculates the change in coordinates of a point over a given time given the current speed and braking
// acceleration. a position after given 'time' interval is returned
auto coordsAfterTime(mcc_coord_pair_c auto const& cp,
mcc_coord_pair_c auto const& speedXY, // in radians per seconds
mcc_coord_pair_c auto const& braking_accelXY, // in radians per seconds in square
traits::mcc_time_duration_c auto const& time,
mcc_coord_pair_c auto* dxy = nullptr)
{
// time to stop mount with given current speed and constant braking acceleration
double tx_stop = std::abs(speedXY.x()) / braking_accelXY.x();
double ty_stop = std::abs(speedXY.y()) / braking_accelXY.y();
using secs_t = std::chrono::duration<double>; // seconds as double
double tx = std::chrono::duration_cast<secs_t>(time).count();
double ty = std::chrono::duration_cast<secs_t>(time).count();
if (std::isfinite(tx_stop) && (tx > tx_stop)) {
tx = tx_stop;
}
if (std::isfinite(ty_stop) && (ty > ty_stop)) {
ty = ty_stop;
}
// the distance:
// here, one must take into account the sign of the speed!!!
double dx = speedXY.x() * tx - std::copysign(braking_accelXY.x(), speedXY.x()) * tx * tx / 2.0;
double dy = speedXY.y() * ty - std::copysign(braking_accelXY.y(), speedXY.y()) * ty * ty / 2.0;
std::remove_cvref_t<decltype(cp)> cp_res{};
cp_res.setEpoch(cp.epoch() + time);
cp_res.setX((double)cp.x() + dx);
cp_res.setY((double)cp.y() + dy);
if (dxy) {
dxy->setX(dx);
dxy->setY(dy);
dxy->setEpoch(cp_res.epoch());
}
return cp_res;
}
auto coordsAfterTime(mcc_skypoint_c auto const& sp,
mcc_coord_pair_c auto const& speedXY, // in radians per seconds
mcc_coord_pair_c auto const& braking_accelXY, // in radians per seconds in square
traits::mcc_time_duration_c auto const& time,
mcc_coord_pair_c auto* dxy = nullptr)
{
auto run_func = [&, this](auto& cp) {
sp.toAtSameEpoch(cp);
auto new_cp = coordsAfterTime(cp, speedXY, braking_accelXY, time, dxy);
std::remove_cvref_t<decltype(sp)> sp_res{};
sp_res.from(cp);
return sp_res;
};
switch (sp.pairKind()) {
case MccCoordPairKind::COORDS_KIND_RADEC_ICRS: {
MccSkyRADEC_ICRS rd;
return run_func(rd);
};
case MccCoordPairKind::COORDS_KIND_RADEC_OBS: {
MccSkyRADEC_OBS rd;
return run_func(rd);
};
case MccCoordPairKind::COORDS_KIND_RADEC_APP: {
MccSkyRADEC_APP rd;
return run_func(rd);
};
case MccCoordPairKind::COORDS_KIND_HADEC_OBS: {
MccSkyHADEC_OBS hd;
return run_func(hd);
};
case MccCoordPairKind::COORDS_KIND_HADEC_APP: {
MccSkyHADEC_APP hd;
return run_func(hd);
};
case MccCoordPairKind::COORDS_KIND_AZZD: {
MccSkyAZZD azzd;
return run_func(azzd);
};
case MccCoordPairKind::COORDS_KIND_AZALT: {
MccSkyAZALT azalt;
return run_func(azalt);
};
case MccCoordPairKind::COORDS_KIND_GENERIC:
case MccCoordPairKind::COORDS_KIND_XY: {
MccGenXY xy;
return run_func(xy);
};
default:
return sp;
}
}
};
template <std::default_initializable PARAMS_T>
using MccGenericBlockingMovementControls =
MccGenericMovementControls<PARAMS_T, MccGenericMovementControlsPolicy::POLICY_BLOCKING>;
template <std::default_initializable PARAMS_T>
using MccGenericAsyncMovementControls =
MccGenericMovementControls<PARAMS_T, MccGenericMovementControlsPolicy::POLICY_ASYNC>;
static_assert(mcc_movement_controls_c<MccGenericAsyncMovementControls<MccGenericMovementControlsParams>>, "!!!");
} // namespace mcc::impl

View File

@@ -11,8 +11,25 @@
* *
****************************************************************************************/
/*
The user needs to accept some assumptions implemented in this class:
1) It is assumed that the implementation of the HARDWARE_T class must return movement state
as "HW_MOVE_STOPPED" to indicate that underlying hardware is at required position
2) In the case of "slew_and_stop == false" the slewing cycle waits for "HW_MOVE_STOPPED" to switch
to state "tracking", but the implementation of the HARDWARE_T class may return movement state
as "HW_MOVE_GUIDING" to indicate that underlying hardware is near required position, i.e., its
inner condition "hardware is at position" is true. The latter is usefull to avoid full hardware stop
and allows to switch to state "tracking" as soon as possible
*/
#include <atomic>
#include <fstream>
#include <thread>
#include "mcc/mcc_coordinate.h"
#include "mcc_concepts.h"
@@ -32,6 +49,7 @@ enum class MccSimpleMovementControlsErrorCode : int {
ERROR_OK,
ERROR_HW_GETSTATE,
ERROR_HW_SETSTATE,
ERROR_HW_ERROR,
ERROR_PCM_COMP,
ERROR_CCTE_COMP,
ERROR_GET_TELEMETRY,
@@ -83,6 +101,8 @@ struct MccSimpleMovementControlsCategory : public std::error_category {
return "cannot get hardware state";
case MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE:
return "cannot set hardware state";
case MccSimpleMovementControlsErrorCode::ERROR_HW_ERROR:
return "hardware error occured";
case MccSimpleMovementControlsErrorCode::ERROR_PCM_COMP:
return "PCM computation error";
case MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP:
@@ -203,6 +223,14 @@ protected:
public:
PathFile(const std::string& filename = "") : _filename(filename), _st() {}
PathFile(const PathFile&) = delete;
PathFile(PathFile&&) = default;
~PathFile()
{
save();
}
void setFilename(const std::string& filename)
{
_filename = filename;
@@ -213,11 +241,6 @@ protected:
return _filename;
}
~PathFile()
{
save();
}
friend PathFile& operator<<(PathFile& pf, auto&& v)
{
pf._st << std::forward<decltype(v)>(v);
@@ -237,7 +260,7 @@ protected:
return true;
}
fst.open(_filename);
fst.open(_filename, std::ios::app);
if (!fst.is_open()) {
return false;
@@ -252,7 +275,7 @@ protected:
private:
std::string _filename;
std::istringstream _st;
std::ostringstream _st;
};
@@ -262,28 +285,46 @@ public:
typedef MccSimpleMovementControlsParameters movement_params_t;
template <mcc_hardware_c HARDWARE_T,
mcc_telemetry_c TELEMETRY_T,
mcc_pzone_container_c PZONE_CONT_T,
mcc_mount_status_c STATUS_T,
std::invocable<STATUS_T const&> CallbackFuncT = decltype([](STATUS_T const&) {}),
mcc_logger_c LOGGER_T = MccNullLogger>
mcc_generic_mount_c MOUNT_T, // MOUNT_T is mcc_telemetry_c, mcc_pzone_container_c and mcc_logger_c
// (see mcc_concepts.h)
std::invocable<typename MOUNT_T::mount_status_t const&> CallbackFuncT =
decltype([](typename MOUNT_T::mount_status_t const&) {})>
MccSimpleMovementControls(
HARDWARE_T* hardware,
TELEMETRY_T* telemtry,
PZONE_CONT_T* pzone_cont,
CallbackFuncT&& mode_switch_callback = [](STATUS_T const&) {},
LOGGER_T& logger = details::NullLogger)
MOUNT_T* mount,
CallbackFuncT&& mode_switch_callback = [](typename MOUNT_T::mount_status_t const&) {})
: MccSimpleMovementControls(hardware, mount, mount, std::forward<CallbackFuncT>(mode_switch_callback), mount)
{
auto log_ptr = &logger;
}
template <mcc_hardware_c HARDWARE_T,
mcc_telemetry_c TELEMETRY_T,
mcc_pzone_container_c PZONE_CONT_T,
typename CallbackFuncT,
mcc_logger_c LOGGER_T = MccNullLogger>
MccSimpleMovementControls(HARDWARE_T* hardware,
TELEMETRY_T* telemetry,
PZONE_CONT_T* pzone_cont,
CallbackFuncT&& mode_switch_callback,
LOGGER_T* logger = &details::NullLogger)
{
static_assert(traits::mcc_func_traits<CallbackFuncT>::arity == 1 &&
mcc_mount_status_c<std::decay_t<traits::mcc_func_arg1_t<CallbackFuncT>>>,
"INVALID 'CallbackFuncT' CALLABLE SIGNATURE!");
using STATUS_T = std::decay_t<traits::mcc_func_arg1_t<CallbackFuncT>>;
auto send_to_hardware = [hardware, logger](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t {
auto tp =
std::chrono::duration_cast<std::chrono::milliseconds>(hw_state.XY.epoch().UTC().time_since_epoch());
auto send_to_hardware = [hardware, log_ptr](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t {
if constexpr (std::derived_from<decltype(hw_state.XY), MccCoordPair<MccAngleX, MccAngleY>>) {
log_ptr->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", hw_state.XY.x().degrees(),
hw_state.XY.y().degrees()));
logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})",
hw_state.XY.x().degrees(), hw_state.XY.y().degrees(), tp));
} else { // user defined mcc_coord_pair_c
log_ptr->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
MccAngle((double)hw_state.XY.x()).degrees(),
MccAngle((double)hw_state.XY.y()).degrees()));
logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})",
MccAngle((double)hw_state.XY.x()).degrees(),
MccAngle((double)hw_state.XY.y()).degrees(), tp));
}
auto hw_err = hardware->hardwareSetState(hw_state);
@@ -291,16 +332,16 @@ public:
mcc_deduced_err(hw_err, MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE);
}
log_ptr->logDebug(" the 'hardwareSetState' method performed successfully!");
logger->logDebug(" the 'hardwareSetState' method performed successfully!");
return MccSimpleMovementControlsErrorCode::ERROR_OK;
};
auto check_pzones = [pzone_cont, log_ptr](typename TELEMETRY_T::telemetry_data_t const& tdata,
double min_time_to_pzone_in_secs, double braking_accelX,
double braking_accelY) {
auto check_pzones = [pzone_cont, logger](typename TELEMETRY_T::telemetry_data_t const& tdata,
double min_time_to_pzone_in_secs, double braking_accelX,
double braking_accelY) -> error_t {
bool in_zone;
std::vector<bool> in_zone_vec;
@@ -328,7 +369,7 @@ public:
double dx = speedX * tx - std::copysign(braking_accelX, speedX) * tx * tx / 2.0;
double dy = speedY * ty - std::copysign(braking_accelY, speedY) * ty * ty / 2.0;
log_ptr->logTrace(
logger->logTrace(
std::format(" the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}",
min_time_to_pzone_in_secs, MccAngleFancyString(dx), MccAngleFancyString(dy)));
@@ -336,20 +377,20 @@ public:
// calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
// and check them for getting into the prohibited zones
std::conditional_t<mccIsEquatorialMount(HARDWARE_T::mountType), MccSkyHADEC_OBS,
std::conditional_t<mccIsAltAzMount(HARDWARE_T::mountType), MccSkyAZZD, std::nullptr_t>>
std::conditional_t<mccIsEquatorialMount(HARDWARE_T::hwMountType), MccSkyHADEC_OBS,
std::conditional_t<mccIsAltAzMount(HARDWARE_T::hwMountType), MccSkyAZZD, std::nullptr_t>>
mount_pos;
static_assert(!std::is_null_pointer_v<decltype(mount_pos)>, "UNKNOWn MOUNT TYPE");
mount_pos.setX(tdata.hwState.XY.x() + tdata.pcmCorrection.x + dx);
mount_pos.setY(tdata.hwState.XY.y() + tdata.pcmCorrection.y + dy);
mount_pos.setX((double)tdata.hwState.XY.x() + tdata.pcmCorrection.pcmX + dx);
mount_pos.setY((double)tdata.hwState.XY.y() + tdata.pcmCorrection.pcmY + dy);
mount_pos.setEpoch(tdata.hwState.XY.epoch());
log_ptr->logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s", MccAngleFancyString(speedX),
MccAngleFancyString(speedY)));
logger->logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s", MccAngleFancyString(speedX),
MccAngleFancyString(speedY)));
auto pz_err = pzone_cont->inPZone(mount_pos, &in_zone, &in_zone_vec);
auto pz_err = pzone_cont->inPZone(MccSkyPoint(mount_pos), &in_zone, &in_zone_vec);
if (pz_err) {
return mcc_deduced_err(pz_err, MccSimpleMovementControlsErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
@@ -366,7 +407,7 @@ public:
auto it = names.begin();
std::ranges::advance(it, i);
log_ptr->logError(
logger->logError(
"target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i,
*it);
@@ -386,14 +427,14 @@ public:
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
log_ptr->logError(std::format(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
MccAngle{radec.x()}.sexagesimal(true), MccAngle{radec.y()}.sexagesimal(),
MccAngle{hadec.x()}.sexagesimal(true), lst.sexagesimal(true)));
log_ptr->logError(std::format(" AZ, ZD, ALT: {}, {}, {}", MccAngle{azzd.x()}.sexagesimal(),
MccAngle{azzd.y()}.sexagesimal(), MccAngle{azalt.y()}.sexagesimal()));
logger->logError(std::format(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
MccAngle{radec.x()}.sexagesimal(true), MccAngle{radec.y()}.sexagesimal(),
MccAngle{hadec.x()}.sexagesimal(true), lst.sexagesimal(true)));
logger->logError(std::format(" AZ, ZD, ALT: {}, {}, {}", MccAngle{azzd.x()}.sexagesimal(),
MccAngle{azzd.y()}.sexagesimal(), MccAngle{azalt.y()}.sexagesimal()));
log_ptr->logError(std::format(" hardware X, Y: {}, {}", MccAngle{tdata.hwState.XY.x()}.sexagesimal(),
MccAngle{tdata.hwState.XY.y()}.sexagesimal()));
logger->logError(std::format(" hardware X, Y: {}, {}", MccAngle{tdata.hwState.XY.x()}.sexagesimal(),
MccAngle{tdata.hwState.XY.y()}.sexagesimal()));
return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE;
}
@@ -403,64 +444,65 @@ public:
};
auto log_pos = [log_ptr, this](typename TELEMETRY_T::telemetry_data_t const& tdata) {
auto log_pos = [logger, this](typename TELEMETRY_T::telemetry_data_t const& tdata,
std::tuple<double, double, double>& dist) -> error_t {
double x_mnt, y_mnt, x_tag, y_tag;
if constexpr (mccIsEquatorialMount(HARDWARE_T::mountType)) {
MccSkyHADEC_OBS hadec;
auto ccte_err = tdata.targetPos.toAtSameEpoch(hadec);
if (ccte_err) {
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
x_tag = hadec.x();
y_tag = hadec.y();
std::conditional_t<
mccIsEquatorialMount(HARDWARE_T::hwMountType), MccSkyHADEC_OBS,
std::conditional_t<mccIsAltAzMount(HARDWARE_T::hwMountType), MccSkyAZALT, std::nullptr_t>>
coord_pair;
log_ptr->logTrace(std::format(" current target: HA = {}, DEC = {}", hadec.x().sexagesimal(true),
hadec.y().sexagesimal()));
static_assert(!std::is_null_pointer_v<decltype(coord_pair)>, "UNKNOWN MOUNT TYPE!");
ccte_err = tdata.mountPos.toAtSameEpoch(hadec);
if (ccte_err) {
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
const std::string_view x_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "HA"
: mccIsAltAzMount(HARDWARE_T::hwMountType) ? "AZ"
: "GEN_X";
const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "DEC"
: mccIsAltAzMount(HARDWARE_T::hwMountType) ? "ALT"
: "GEN_Y";
;
x_mnt = hadec.x();
y_mnt = hadec.y();
log_ptr->logTrace(std::format(" current mount: HA = {}, DEC = {}", hadec.x().sexagesimal(true),
hadec.y().sexagesimal()));
} else if constexpr (mccIsAltAzMount(HARDWARE_T::mountType)) {
MccSkyAZZD azzd;
auto ccte_err = tdata.targetPos.toAtSameEpoch(azzd);
if (ccte_err) {
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
x_tag = azzd.x();
y_tag = azzd.y();
log_ptr->logTrace(
std::format(" target: AZ = {}, ZD = {}", azzd.x().sexagesimal(), azzd.y().sexagesimal()));
ccte_err = tdata.mountPos.toAtSameEpoch(azzd);
if (ccte_err) {
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
x_mnt = azzd.x();
y_mnt = azzd.y();
log_ptr->logTrace(
std::format(" mount: AZ = {}, ZD = {}", azzd.x().sexagesimal(), azzd.y().sexagesimal()));
auto ccte_err = tdata.targetPos.toAtSameEpoch(coord_pair);
if (ccte_err) {
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
_pathFile << tdata.mountPos.epoch().UTC.time_since_epoch().count() << " " << x_tag << " " << y_tag << " "
<< x_mnt << " " << y_mnt << " " << (x_tag - x_mnt) << " " << (y_tag - y_mnt) << " "
<< (int)tdata.hwState.movementState << "\n";
x_tag = coord_pair.x();
y_tag = coord_pair.y();
logger->logTrace(std::format(" current target: {} = {}, {} = {} (encoders: {} {})", x_str,
coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal(),
tdata.targetXY.x().sexagesimal(), tdata.targetXY.y().sexagesimal()));
ccte_err = tdata.mountPos.toAtSameEpoch(coord_pair);
if (ccte_err) {
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
logger->logTrace(std::format(" current mount: {} = {}, {} = {} (encoders: {} {})", x_str,
coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal(),
tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal()));
x_mnt = coord_pair.x();
y_mnt = coord_pair.y();
dist = utils::distanceOnSphere(x_tag, y_tag, x_mnt, y_mnt);
_pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << " "
<< x_mnt << " " << y_mnt << " " << std::get<0>(dist) << " " << std::get<1>(dist) << " "
<< std::get<2>(dist) << " " << (int)tdata.hwState.movementState << "\n";
// _pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << "
// "
// << x_mnt << " " << y_mnt << " " << (x_tag - x_mnt) << " " << (y_tag - y_mnt) << " "
// << (int)tdata.hwState.movementState << "\n";
logger->logTrace(std::format(" target-to-mount distance: {} (dx = {}, dy = {})",
MccAngleFancyString(std::get<2>(dist)), MccAngleFancyString(std::get<0>(dist)),
MccAngleFancyString(std::get<1>(dist))));
return MccSimpleMovementControlsErrorCode::ERROR_OK;
};
auto cb_sptr =
@@ -469,7 +511,7 @@ public:
/* stop moving function */
_stopMovingFunc = [hardware, cb_sptr, this]() {
_stopMovingFunc = [send_to_hardware, hardware, cb_sptr, this]() {
typename HARDWARE_T::hardware_state_t hw_state;
hw_state.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPING;
@@ -481,8 +523,475 @@ public:
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
}
};
/* slewing function */
_slewingFunc = [log_pos, send_to_hardware, check_pzones, cb_sptr, hardware, telemetry, logger,
this](bool slew_and_stop) {
double braking_accelX, braking_accelY;
double min_time_to_pzone_in_secs;
{
// std::lock_guard lock{*_currentParamsMutex};
if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
braking_accelX = std::numeric_limits<double>::min();
} else {
braking_accelX = std::abs(_currentParams.brakingAccelX);
}
if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
braking_accelY = std::numeric_limits<double>::min();
} else {
braking_accelY = std::abs(_currentParams.brakingAccelY);
}
min_time_to_pzone_in_secs =
std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
if (!_currentParams.slewingPathFilename.empty()) { // open slewing trajectory file
logger->logInfo(std::format("Set slewing path filename to {}", _currentParams.slewingPathFilename));
_pathFile.setFilename(_currentParams.slewingPathFilename);
} else {
logger->logWarn("Slewing path filename is empty! Do not save it!");
}
}
logger->logInfo(
std::format("Start slewing in mode '{}'", slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK"));
logger->logInfo(std::format(" slewing process timeout: {} secs", _currentParams.slewTimeout.count()));
if (!slew_and_stop) {
logger->logInfo(std::format(" slewing tolerance radius: {} arcsecs",
MccAngle{_currentParams.slewToleranceRadius}.arcsecs()));
}
logger->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
logger->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
logger->logInfo(std::format(" min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));
_pathFile << "# \n";
_pathFile << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n";
_pathFile << "# Config:\n";
_pathFile << "# slewing tolerance radius: " << MccAngle{_currentParams.slewToleranceRadius}.arcsecs()
<< " arcsecs\n";
_pathFile << "# slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n";
_pathFile << "# \n";
_pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n";
_pathFile << "# <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
"<dY_{target-mount}> <taget-mount-distance> <moving state>\n";
typename TELEMETRY_T::telemetry_data_t tdata;
typename HARDWARE_T::hardware_state_t hw_state;
auto t_err = telemetry->telemetryData(&tdata);
if (t_err) {
*_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
_pathFile.save();
return;
}
*_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
if (_lastError->load()) {
_pathFile.save();
return;
}
using ep_t = decltype(hw_state.XY.epoch());
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
hw_state.XY.setX(tdata.targetXY.x());
hw_state.XY.setY(tdata.targetXY.y());
hw_state.XY.setEpoch(ep_t::now());
// start slewing ...
*_lastError = send_to_hardware(hw_state);
if (_lastError->load()) {
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
_pathFile.save();
return;
}
(*cb_sptr)(STATUS_T::MOUNT_STATUS_SLEWING);
auto start_point = std::chrono::steady_clock::now();
auto last_hw_time = tdata.hwState.XY.epoch().UTC();
mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp;
std::tuple<double, double, double> dist;
*_stopMoving = false;
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_ADJUSTING;
while (!*_stopMoving) {
t_err = telemetry->telemetryData(&tdata);
if (t_err) {
*_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
return;
}
hw_state.XY.setX(tdata.targetXY.x());
hw_state.XY.setY(tdata.targetXY.y());
hw_state.XY.setEpoch(ep_t::now());
log_pos(tdata, dist);
// logger->logInfo("\tMNT.XY {} {}; MNT.HADEC {} {}", (double)tdata.hwState.XY.x(),
// (double)tdata.hwState.XY.y(), tdata.mountPos.co_lon(), tdata.mountPos.co_lat());
// logger->logInfo("\tTAG.XY {} {}; TAG.HADEC {} {}", (double)tdata.targetXY.x(),
// (double)tdata.targetXY.y(), tdata.targetPos.co_lon(), tdata.targetPos.co_lat());
if (*_stopMoving) {
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
break;
}
*_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
if (_lastError->load()) {
break;
}
{
std::lock_guard lock{*_currentParamsMutex};
if ((std::chrono::steady_clock::now() - start_point) > _currentParams.slewTimeout) {
logger->logError("slewing process timeout!");
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_TIMEOUT;
break;
}
if (std::get<2>(dist) <= _currentParams.slewToleranceRadius) {
logger->logInfo("target-to-mount distance is lesser than slew acceptable radius - exit!");
break;
}
// start adjusting (precise pointing) the slewing
if (std::get<2>(dist) <= _currentParams.adjustCoordDiff) {
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ADJUSTING);
*_lastError = send_to_hardware(hw_state);
if (_lastError->load()) {
break;
}
}
if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) {
// the mount stopped but still to far from target position!!!
if (std::get<2>(dist) > _currentParams.slewToleranceRadius) {
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ADJUSTING);
*_lastError = send_to_hardware(hw_state);
if (_lastError->load()) {
break;
}
} else {
break;
}
}
}
if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_ERROR) {
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_HW_ERROR;
break;
}
/*
if (slew_and_stop) { // just wait until the mount stops
if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) {
// if (std::get<2>(dist) > _currentParams.slewToleranceRadius) {
// // resend new position since target coordinates are changed in time
// hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
// hw_state.XY.setX(tdata.targetXY.x());
// hw_state.XY.setY(tdata.targetXY.y());
// hw_state.XY.setEpoch(ep_t::now());
// *_lastError = send_to_hardware(hw_state);
// if (_lastError->load()) {
// break;
// }
// } else {
logger->logInfo("mount movement state is STOPPED! Exit from slewing process!");
break;
// }
}
} else {
if (last_hw_time == tdata.hwState.XY.epoch().UTC()) {
logger->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
continue;
}
last_hw_time = tdata.hwState.XY.epoch().UTC();
// stop slewing and exit from the cycle?
if (std::get<2>(dist) <= _currentParams.slewToleranceRadius) {
if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED ||
tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_GUIDING) {
logger->logInfo("target-to-mount distance is lesser than slew acceptable radius - exit!");
break;
} else {
logger->logDebug(
"target-to-mount distance is lesser than slew acceptable radius but hardware is still "
"not ready!");
}
}
if (*_stopMoving) {
break;
}
// resend new position since target coordinates are changed in time
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
hw_state.XY.setX(tdata.targetXY.x());
hw_state.XY.setY(tdata.targetXY.y());
hw_state.XY.setEpoch(ep_t::now());
*_lastError = send_to_hardware(hw_state);
if (_lastError->load()) {
break;
}
}
*/
// sleep here
std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
}
if (_stopMoving->load()) { // external stop
logger->logWarn("Slewing was stopped!");
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
_pathFile.save();
return;
}
*_stopMoving = true;
logger->logInfo("Slewing finished");
auto err = _lastError->load();
logger->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message()));
_pathFile.save();
// get final position
if (!err) {
t_err = telemetry->telemetryData(&tdata);
if (t_err) {
*_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
return;
}
log_pos(tdata, dist);
if (!slew_and_stop) { // start tracking
_trackingFunc();
} else {
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
}
}
};
/* tracking function */
_trackingFunc = [log_pos, check_pzones, send_to_hardware, telemetry, cb_sptr, logger, this]() {
double braking_accelX, braking_accelY;
double min_time_to_pzone_in_secs;
logger->logInfo("Start tracking");
{
// std::lock_guard lock{*_currentParamsMutex};
if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
braking_accelX = std::numeric_limits<double>::min();
} else {
braking_accelX = std::abs(_currentParams.brakingAccelX);
}
if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
braking_accelY = std::numeric_limits<double>::min();
} else {
braking_accelY = std::abs(_currentParams.brakingAccelY);
}
min_time_to_pzone_in_secs =
std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
if (!_currentParams.trackingPathFilename.empty()) { // open slewing trajectory file
logger->logInfo(
std::format(" Set tracking filename to {}", _currentParams.trackingPathFilename));
_pathFile.setFilename(_currentParams.trackingPathFilename);
} else {
logger->logWarn(" Tracking path filename is empty! Do not save it!");
}
}
logger->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
logger->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
logger->logInfo(std::format(" min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));
logger->logInfo(
std::format(" max target-to-mount distance: {} arcseconds", _currentParams.trackingMaxCoordDiff));
_pathFile << "# \n";
_pathFile << "# Tracking trajectory, " << std::chrono::system_clock::now() << "\n";
_pathFile << "# \n";
_pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n";
_pathFile << "# <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
"<dY_{target-mount}> <taget-mount-distance> <moving state>\n";
typename TELEMETRY_T::telemetry_data_t tdata;
typename HARDWARE_T::hardware_state_t hw_state;
std::tuple<double, double, double> dist;
auto t_err = telemetry->telemetryData(&tdata);
if (t_err) {
*_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
_pathFile.save();
return;
}
log_pos(tdata, dist);
{
std::lock_guard lock{*_currentParamsMutex};
if (_currentParams.trackingMaxCoordDiff < std::get<2>(dist)) {
logger->logInfo(
std::format("The target-to-mount distance {} is greater than allowed one ({})! Track current "
"mount position!",
MccAngleFancyString(std::get<2>(dist)),
MccAngleFancyString(_currentParams.trackingMaxCoordDiff)));
telemetry->setPointingTarget(tdata.mountPos);
}
}
t_err = telemetry->telemetryData(&tdata);
if (t_err) {
*_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
_pathFile.save();
return;
}
*_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
if (_lastError->load()) {
_pathFile.save();
return;
}
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_TRACKING;
auto last_hw_time = tdata.hwState.XY.epoch().UTC();
mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp;
*_stopMoving = false;
while (!_stopMoving->load()) {
t_err = telemetry->telemetryData(&tdata);
if (t_err) {
*_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
return;
}
log_pos(tdata, dist);
if (*_stopMoving) {
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
break;
}
*_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
if (_lastError->load()) {
break;
}
if (last_hw_time == tdata.hwState.XY.epoch().UTC()) {
logger->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
continue;
}
last_hw_time = tdata.hwState.XY.epoch().UTC();
// resend new position since target coordinates are changed in time
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_TRACKING;
hw_state.XY.setX(tdata.targetXY.x());
hw_state.XY.setY(tdata.targetXY.y());
hw_state.XY.setEpoch(tdata.hwState.XY.epoch());
*_lastError = send_to_hardware(hw_state);
if (_lastError->load()) {
break;
}
if (*_stopMoving) {
break;
}
// sleep here
std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval);
}
if (_stopMoving->load()) { // external stop
logger->logWarn("Tracking was stopped!");
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
} else {
*_stopMoving = true;
logger->logInfo("Tracking finished");
auto err = _lastError->load();
logger->logInfo(
std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message()));
}
_pathFile.save();
};
}
virtual ~MccSimpleMovementControls() = default;
MccSimpleMovementControls(const MccSimpleMovementControls&) = delete;
MccSimpleMovementControls(MccSimpleMovementControls&&) = default;
error_t slewToTarget(bool slew_and_stop = false)
{

View File

@@ -38,6 +38,7 @@
#include "mcc_concepts.h"
#include "mcc_coordinate.h"
#include "mcc_netserver_endpoint.h"
#include "mcc_netserver_proto.h"
#include "mcc_traits.h"
@@ -90,6 +91,8 @@ struct MccGenericMountNetworkServerErrorCategory : std::error_category {
defaut:
return "unknown";
};
return "unknown";
}
static const MccGenericMountNetworkServerErrorCategory& get()
@@ -964,9 +967,6 @@ public:
return output_msg.template byteRepr<typename base_t::handle_message_func_result_t>();
};
// special functor (used in the destructor)
_stopMountFunc = [mount_ptr]() { mount_ptr->stopMount(); };
}
virtual ~MccGenericMountNetworkServer()
@@ -974,8 +974,6 @@ public:
std::stringstream st;
st << std::this_thread::get_id();
_stopMountFunc();
logInfo(std::format("Delete MccGenericMountNetworkServer class instance (thread ID = {})", st.str()));
}
@@ -984,8 +982,6 @@ protected:
MccSerializedAngleFormatPrec _coordPrec{2, 1, 7};
std::function<void()> _stopMountFunc{};
template <typename RESULT_MSG_T, typename INPUT_MSG_T, mcc_generic_mount_c MountT>
RESULT_MSG_T handleMessage(const INPUT_MSG_T& input_msg, MountT* mount_ptr)
requires(
@@ -1011,35 +1007,35 @@ protected:
} else if (input_msg.withKey(MCC_COMMPROTO_KEYWORD_INIT_STR)) {
m_err = mount_ptr->initMount();
if (m_err) {
err = mcc_deduce_error_code(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_INIT);
err = mcc_deduced_err(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_INIT);
} else {
output_msg.construct(MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_INIT_STR);
}
} else if (input_msg.withKey(MCC_COMMPROTO_KEYWORD_STOP_STR)) {
m_err = mount_ptr->stopMount();
if (m_err) {
err = mcc_deduce_error_code(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_STOP);
err = mcc_deduced_err(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_STOP);
} else {
output_msg.construct(MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_STOP_STR);
}
} else if (input_msg.withKey(MCC_COMMPROTO_KEYWORD_SLEW_STR)) {
m_err = mount_ptr->slewToTarget(false);
if (m_err) {
err = mcc_deduce_error_code(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_SLEW);
err = mcc_deduced_err(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_SLEW);
} else {
output_msg.construct(MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_SLEW_STR);
}
} else if (input_msg.withKey(MCC_COMMPROTO_KEYWORD_MOVE_STR)) {
m_err = mount_ptr->slewToTarget(true);
if (m_err) {
err = mcc_deduce_error_code(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_MOVE);
err = mcc_deduced_err(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_MOVE);
} else {
output_msg.construct(MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_MOVE_STR);
}
} else if (input_msg.withKey(MCC_COMMPROTO_KEYWORD_TRACK_STR)) {
m_err = mount_ptr->trackTarget();
if (m_err) {
err = mcc_deduce_error_code(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_TRACK);
err = mcc_deduced_err(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_TRACK);
} else {
output_msg.construct(MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_TRACK_STR);
}
@@ -1074,7 +1070,7 @@ protected:
impl::MccSkyPoint sp;
auto sz = input_msg.paramSize();
if (sz) { // set or get operation
if (sz) { // set or get operation
auto vp = input_msg.template paramValue<impl::MccCoordPairKind>(0); // is it get operation?
if (vp) { // coordinate pair kind is given, it is get operation
pair_kind = vp.value();
@@ -1088,7 +1084,7 @@ protected:
if (vc) { // set operation
auto m_err = mount_ptr->setPointingTarget(vc.value());
if (m_err) {
err = mcc_deduce_err(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_SET_TARGET);
err = mcc_deduced_err(m_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_SET_TARGET);
} else {
output_msg.construct(MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, input_msg.byteRepr());
}
@@ -1108,7 +1104,7 @@ protected:
impl::MccCoordPairKind pair_kind = impl::MccCoordPairKind::COORDS_KIND_UNKNOWN;
impl::MccSkyPoint sp;
if (input_msg.paramSize()) { // ccordinate pair kind is given
if (input_msg.paramSize()) { // cordinate pair kind is given
auto vp = input_msg.template paramValue<impl::MccCoordPairKind>(0);
if (vp) { // coordinate pair kind is given
pair_kind = vp.value();
@@ -1125,12 +1121,16 @@ protected:
}
}
} else if (input_msg.withKey(MCC_COMMPROTO_KEYWORD_ENTEREDTAG_STR)) {
output_msg.construct(MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_ENTEREDTAG_STR,
_coordFormat, _coordPrec, mount_ptr->getPointingTarget());
} else if (input_msg.withKey(MCC_COMMPROTO_KEYWORD_TELEMETRY_STR)) {
typename MountT::telemetry_data_t tdata;
auto t_err = mount_ptr->telemetryData(&tdata);
if (t_err) {
err = mcc_deduce_error_code(t_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_GET_TELEMETRY);
err = mcc_deduced_err(t_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_GET_TELEMETRY);
} else {
output_msg.construct(MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_TELEMETRY_STR,
_coordFormat, _coordPrec, tdata);
@@ -1182,10 +1182,10 @@ protected:
auto t_err = mount->telemetryData(&tdata);
if (t_err) {
return mcc_deduce_err(t_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_GET_TELEMETRY);
return mcc_deduced_err(t_err, MccGenericMountNetworkServerErrorCode::ERROR_MOUNT_GET_TELEMETRY);
}
auto get_coords = [&]<mcc_coord_pair_c T>(T& cp) {
auto get_coords = [&]<mcc_coord_pair_c T>(T& cp) -> std::error_code {
cp.setEpoch(tdata.mountPos.epoch());
if (target) {
@@ -1237,7 +1237,7 @@ protected:
case impl::MccCoordPairKind::COORDS_KIND_XY: // interpretated as encoder coordinates
case impl::MccCoordPairKind::COORDS_KIND_GENERIC: { // interpretated as encoder coordinates
if (target) {
// WARNING: STILL NOT IMPLEMENTED!!!
sp.from(tdata.targetXY);
} else {
sp.from(tdata.hwState.XY);
}

View File

@@ -146,6 +146,10 @@ static constexpr std::string_view MCC_COMMPROTO_KEYWORD_TARGET_STR = "TARGET";
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_MOUNT_STR = "MOUNT";
// get entered target coordinates:
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_ENTEREDTAG_STR = "ENTEREDTAG";
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_TELEMETRY_STR = "TELEMETRY";
// init mount
@@ -173,18 +177,18 @@ static constexpr std::string_view MCC_COMMPROTO_KEYWORD_TRACK_STR = "TRACK";
static constexpr std::string_view MCC_COMMPROTO_KEYWORD_STATUS_STR = "STATUS";
// valid keywords
static constexpr std::array MCC_COMMPROTO_VALID_KEYS = {
MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR, MCC_COMMPROTO_KEYWORD_COORDFMT_STR,
MCC_COMMPROTO_KEYWORD_COORDPREC_STR, MCC_COMMPROTO_KEYWORD_TARGET_STR, MCC_COMMPROTO_KEYWORD_MOUNT_STR,
MCC_COMMPROTO_KEYWORD_TELEMETRY_STR, MCC_COMMPROTO_KEYWORD_INIT_STR, MCC_COMMPROTO_KEYWORD_STOP_STR,
MCC_COMMPROTO_KEYWORD_SLEW_STR, MCC_COMMPROTO_KEYWORD_MOVE_STR, MCC_COMMPROTO_KEYWORD_TRACK_STR,
MCC_COMMPROTO_KEYWORD_STATUS_STR};
// static constexpr std::array MCC_COMMPROTO_VALID_KEYS = {
// MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR, MCC_COMMPROTO_KEYWORD_COORDFMT_STR,
// MCC_COMMPROTO_KEYWORD_COORDPREC_STR, MCC_COMMPROTO_KEYWORD_TARGET_STR, MCC_COMMPROTO_KEYWORD_MOUNT_STR,
// MCC_COMMPROTO_KEYWORD_ENTEREDTAG_STR, MCC_COMMPROTO_KEYWORD_TELEMETRY_STR, MCC_COMMPROTO_KEYWORD_INIT_STR,
// MCC_COMMPROTO_KEYWORD_STOP_STR, MCC_COMMPROTO_KEYWORD_SLEW_STR, MCC_COMMPROTO_KEYWORD_MOVE_STR,
// MCC_COMMPROTO_KEYWORD_TRACK_STR, MCC_COMMPROTO_KEYWORD_STATUS_STR};
// hashes of valid keywords
static constexpr std::array MCC_COMMPROTO_VALID_KEYS_HASH = []<size_t... Is>(std::index_sequence<Is...>) {
return std::array{mcc::utils::FNV1aHash(MCC_COMMPROTO_VALID_KEYS[Is])...};
}(std::make_index_sequence<MCC_COMMPROTO_VALID_KEYS.size()>());
// // hashes of valid keywords
// static constexpr std::array MCC_COMMPROTO_VALID_KEYS_HASH = []<size_t... Is>(std::index_sequence<Is...>) {
// return std::array{mcc::utils::FNV1aHash(MCC_COMMPROTO_VALID_KEYS[Is])...};
// }(std::make_index_sequence<MCC_COMMPROTO_VALID_KEYS.size()>());
@@ -213,10 +217,10 @@ struct MccNetMessageValidKeywords {
MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR,
MCC_COMMPROTO_KEYWORD_COORDFMT_STR, MCC_COMMPROTO_KEYWORD_COORDPREC_STR,
MCC_COMMPROTO_KEYWORD_TARGET_STR, MCC_COMMPROTO_KEYWORD_MOUNT_STR,
MCC_COMMPROTO_KEYWORD_TELEMETRY_STR, MCC_COMMPROTO_KEYWORD_INIT_STR,
MCC_COMMPROTO_KEYWORD_STOP_STR, MCC_COMMPROTO_KEYWORD_SLEW_STR,
MCC_COMMPROTO_KEYWORD_MOVE_STR, MCC_COMMPROTO_KEYWORD_TRACK_STR,
MCC_COMMPROTO_KEYWORD_STATUS_STR};
MCC_COMMPROTO_KEYWORD_ENTEREDTAG_STR, MCC_COMMPROTO_KEYWORD_TELEMETRY_STR,
MCC_COMMPROTO_KEYWORD_INIT_STR, MCC_COMMPROTO_KEYWORD_STOP_STR,
MCC_COMMPROTO_KEYWORD_SLEW_STR, MCC_COMMPROTO_KEYWORD_MOVE_STR,
MCC_COMMPROTO_KEYWORD_TRACK_STR, MCC_COMMPROTO_KEYWORD_STATUS_STR};
// hashes of valid keywords

View File

@@ -2,9 +2,15 @@
#pragma once
/* MOUNT CONTROL COMPONENTS LIBRARY */
/****************************************************************************************
* *
* MOUNT CONTROL COMPONENTS LIBRARY *
* *
* *
* A REFERENCE "POINTING-CORRECTION-MODEL" CLASS IMPLEMENTATION *
* *
****************************************************************************************/
/* A REFERENCE "POINTING-CORRECTION-MODEL" CLASS IMPLEMENTATION */
#include <mutex>
@@ -26,7 +32,8 @@ enum class MccDefaultPCMErrorCode : int {
ERROR_INVALID_INPUTS_BISPLEV,
#endif
ERROR_EXCEED_MAX_ITERS,
ERROR_NULLPTR
ERROR_NULLPTR,
ERROR_JACINV
};
/* error category definition */
@@ -37,7 +44,7 @@ struct MccDefaultPCMCategory : public std::error_category {
const char* name() const noexcept
{
return "ADC_GENERIC_DEVICE";
return "MCC-DEFAULT-PCM-ERROR-CATEGORY";
}
std::string message(int ec) const
@@ -57,6 +64,8 @@ struct MccDefaultPCMCategory : public std::error_category {
return "exceed maximum of iterations number";
case MccDefaultPCMErrorCode::ERROR_NULLPTR:
return "nullptr input argument";
case MccDefaultPCMErrorCode::ERROR_JACINV:
return "Jacobian is near singular";
default:
return "UNKNOWN";
}
@@ -112,7 +121,8 @@ template <MccMountType MOUNT_TYPE>
class MccDefaultPCM : public mcc_pcm_interface_t<std::error_code>
{
public:
static constexpr MccMountType mountType = MOUNT_TYPE;
// static constexpr MccMountType mountType = MOUNT_TYPE;
static constexpr MccMountType pcmMountType = MOUNT_TYPE;
#ifdef USE_BSPLINE_PCM
static constexpr std::string_view pcmName{"MCC-GEOMETRY-BSPLINES-PCM"};
@@ -156,6 +166,9 @@ public:
std::vector<coeff_t> coeffsX{};
std::vector<coeff_t> coeffsY{};
std::vector<coeff_t> inverseCoeffsX{};
std::vector<coeff_t> inverseCoeffsY{};
};
#endif
@@ -293,7 +306,7 @@ public:
// to be computed as observed celestial X and Y cordinate according to mount type (HA-DEC or AZ-ZD)
double x, y;
auto getXY = [&, this](auto& cp) {
auto getXY = [&, this](auto& cp) -> error_t {
auto err = obs_skycoord.toAtSameEpoch(cp);
if (err) {
return mcc_deduced_err(err, MccDefaultPCMErrorCode::ERROR_CCTE);
@@ -315,13 +328,65 @@ public:
std::lock_guard lock(*_pcmDataMutex);
ret = _compResult(x, y, res, true);
if (!ret) {
if (_pcmData.type != MccDefaultPCMType::PCM_TYPE_BSPLINE) { // compute using Newton-Raphson correction
struct {
double pcmX, pcmY;
} dfx, dfy, df; // partial derivatives
ret = _computeFuncDeriv(x, y, res, true, &dfx, &dfy);
if (!ret) {
return ret;
}
dfx.pcmX += 1.0; // a
dfx.pcmY += 1.0; // b
dfy.pcmX += 1.0; // c
dfy.pcmY += 1.0; // d
// Jacobian determinant
auto detJ = dfx.pcmX * dfy.pcmY - dfx.pcmY * dfy.pcmX;
if (utils::isEqual(detJ, 0.0)) {
return MccDefaultPCMErrorCode::ERROR_JACINV;
}
// | a b |
// if A = | c d |, then
//
// -1 | d -b |
// A = 1/detA * | -c a |
//
df.pcmX = dfy.pcmY * res->pcmX - dfx.pcmY * res->pcmY;
df.pcmY = -dfy.pcmX * res->pcmX + dfx.pcmX * res->pcmY;
res->pcmX -= df.pcmX;
res->pcmY -= df.pcmY;
if constexpr (mcc_coord_pair_c<T>) {
*hw_pt = MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY};
*hw_pt = MccCoordPair<typename T::x_t, typename T::y_t>{res->pcmX, res->pcmY, obs_skycoord.epoch()};
}
} else { // for B-splines the result is computed directly from inverse B-spline coefficients
ret = _computeFuncDeriv(x, y, res);
if (!ret) {
if constexpr (mcc_coord_pair_c<T>) {
*hw_pt = MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY,
obs_skycoord.epoch()};
}
}
}
// ret = _compResult(x, y, res, true);
// if (!ret) {
// if constexpr (mcc_coord_pair_c<T>) {
// *hw_pt =
// MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY,
// obs_skycoord.epoch()};
// }
// }
return ret;
}
@@ -382,6 +447,204 @@ private:
std::unique_ptr<std::mutex> _pcmDataMutex{new std::mutex};
// compute PCM function and its partial derivatives if asked
template <typename DXT = std::nullptr_t, typename DYT = std::nullptr_t>
error_t _computeFuncDeriv(double x,
double y,
mcc_pcm_result_c auto* res,
bool inverse = false,
DXT derivX = nullptr,
DYT derivY = nullptr)
requires((std::is_null_pointer_v<DXT> || mcc_pcm_result_c<std::remove_pointer_t<DXT>>) &&
(std::is_null_pointer_v<DYT> || mcc_pcm_result_c<std::remove_pointer_t<DYT>>))
{
if constexpr (std::is_null_pointer_v<DXT> || std::is_null_pointer_v<DYT>) {
if (_pcmData.type != MccDefaultPCMType::PCM_TYPE_BSPLINE && inverse) {
return MccDefaultPCMErrorCode::ERROR_NULLPTR;
}
}
pcm_geom_coeffs_t* geom_coeffs = &_pcmData.geomCoefficients;
#ifdef USE_BSPLINE_PCM
pcm_bspline_t* bspline = &_pcmData.bspline;
// pcm_bspline_t* inv_bspline = &_pcmData.inverseBspline;
#endif
#ifdef USE_BSPLINE_PCM
if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY ||
_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
#endif
const auto tanY = std::tan(y);
const auto sinX = std::sin(x);
const auto cosX = std::cos(x);
const auto cosY = std::cos(y);
const auto sinY = std::sin(y);
if (utils::isEqual(cosY, 0.0)) {
res->pcmX = _pcmData.geomCoefficients.zeroPointX;
if constexpr (!std::is_null_pointer_v<DXT>) {
derivX->pcmX = 0.0; // dpcmX/dX
derivX->pcmY = 0.0; // dpcmX/dY
}
} else {
res->pcmX = geom_coeffs->zeroPointX + geom_coeffs->collimationErr / cosY +
geom_coeffs->nonperpendErr * tanY - geom_coeffs->misalignErr1 * cosX * tanY +
geom_coeffs->misalignErr2 * sinX * tanY + geom_coeffs->tubeFlexure * _cosPhi * sinX / cosY -
geom_coeffs->DECaxisFlexure * (_cosPhi * cosX + _sinPhi * tanY);
if constexpr (!std::is_null_pointer_v<DXT>) {
auto cos2Y = cosY * cosY;
derivX->pcmX = (geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX) * tanY +
geom_coeffs->tubeFlexure * _cosPhi * cosX / cosY +
geom_coeffs->DECaxisFlexure * _cosPhi * sinX; // dpcmX/dX
derivX->pcmY =
(geom_coeffs->collimationErr * sinY + geom_coeffs->nonperpendErr -
geom_coeffs->misalignErr1 * cosX + geom_coeffs->misalignErr2 * sinX +
geom_coeffs->tubeFlexure * _cosPhi * sinX * sinY - geom_coeffs->DECaxisFlexure * _sinPhi) /
cos2Y; // dpcmX/dY
}
}
res->pcmY = geom_coeffs->zeroPointY + geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX +
geom_coeffs->tubeFlexure * (_cosPhi * cosX * sinY - _sinPhi * cosY);
if constexpr (!std::is_null_pointer_v<DYT>) {
derivY->pcmX = geom_coeffs->misalignErr1 * cosX - geom_coeffs->misalignErr2 * sinX -
geom_coeffs->tubeFlexure * _cosPhi * sinX * sinY; // dpcmY/dX
derivY->pcmY = geom_coeffs->tubeFlexure * (_cosPhi * cosX * cosY + _sinPhi * sinY); // dpcmY/dY
}
if constexpr (pcmMountType == MccMountType::FORK_TYPE) {
if (!utils::isEqual(cosX, 0.0)) {
res->pcmY += geom_coeffs->forkFlexure / cosX;
if constexpr (!std::is_null_pointer_v<DYT>) {
derivY->pcmY += geom_coeffs->forkFlexure * sinX / cosX / cosY; // dpcmY/dY
}
}
}
#ifdef USE_BSPLINE_PCM
}
#endif
#ifdef USE_BSPLINE_PCM
if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE ||
(_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE && !inverse)) {
double spl_valX, spl_valY;
int ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsX, x, y, spl_valX,
bspline->bsplDegreeX, bspline->bsplDegreeY);
if (ret) {
res->pcmX = std::numeric_limits<double>::quiet_NaN();
res->pcmY = std::numeric_limits<double>::quiet_NaN();
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
}
ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y, spl_valY,
bspline->bsplDegreeX, bspline->bsplDegreeY);
if (ret) {
res->pcmX = std::numeric_limits<double>::quiet_NaN();
res->pcmY = std::numeric_limits<double>::quiet_NaN();
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
}
res->pcmX += spl_valX;
res->pcmY += spl_valY;
}
// compute partial derivatives of the bivariate B-spline
if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE && inverse) {
double dspl_valX, dspl_valY;
int ret = 0;
if constexpr (!std::is_null_pointer_v<DXT>) {
ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsX, x, y,
dspl_valX, 1, 0, bspline->bsplDegreeX, bspline->bsplDegreeY);
if (ret) {
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
}
derivX->pcmX += dspl_valX; // dpcmX/dX
ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsX, x, y,
dspl_valX, 0, 1, bspline->bsplDegreeX, bspline->bsplDegreeY);
if (ret) {
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
}
derivX->pcmY += dspl_valX; // dpcmX/dY
}
if constexpr (!std::is_null_pointer_v<DYT>) {
ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y,
dspl_valY, 1, 0, bspline->bsplDegreeX, bspline->bsplDegreeY);
if (ret) {
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
}
derivY->pcmX += dspl_valY; // dpcmY/dX
ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y,
dspl_valY, 0, 1, bspline->bsplDegreeX, bspline->bsplDegreeY);
if (ret) {
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
}
derivY->pcmY += dspl_valY; // dpcmY/dY
}
}
// for inverse PCM the inverse spline coefficients are used (derivatives are not computed)!!!
if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE && inverse) {
double spl_valX, spl_valY;
int ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->inverseCoeffsX, x, y,
spl_valX, bspline->bsplDegreeX, bspline->bsplDegreeY);
if (ret) {
res->pcmX = std::numeric_limits<double>::quiet_NaN();
res->pcmY = std::numeric_limits<double>::quiet_NaN();
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
}
ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->inverseCoeffsY, x, y,
spl_valY, bspline->bsplDegreeX, bspline->bsplDegreeY);
if (ret) {
res->pcmX = std::numeric_limits<double>::quiet_NaN();
res->pcmY = std::numeric_limits<double>::quiet_NaN();
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
}
res->pcmX = spl_valX;
res->pcmY = spl_valY;
}
#endif
return MccDefaultPCMErrorCode::ERROR_OK;
}
error_t _compResult(double x, double y, mcc_pcm_result_c auto* res, bool inverse)
{
pcm_geom_coeffs_t* geom_coeffs;
@@ -394,11 +657,10 @@ private:
bspline = inverse ? &_pcmData.inverseBspline : &_pcmData.bspline;
#endif
if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY
#ifdef USE_BSPLINE_PCM
|| _pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE
if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY ||
_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
#endif
) {
const auto tanY = std::tan(y);
const auto sinX = std::sin(x);
const auto cosX = std::cos(x);
@@ -417,12 +679,15 @@ private:
res->pcmY = geom_coeffs->zeroPointY + geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX +
geom_coeffs->tubeFlexure * (_cosPhi * cosX * std::sin(y) - _sinPhi * cosY);
if constexpr (mountType == MccMountType::FORK_TYPE) {
// if constexpr (mountType == MccMountType::FORK_TYPE) {
if constexpr (pcmMountType == MccMountType::FORK_TYPE) {
if (!utils::isEqual(cosX, 0.0)) {
res->pcmY += geom_coeffs->forkFlexure / cosX;
}
}
#ifdef USE_BSPLINE_PCM
}
#endif
#ifdef USE_BSPLINE_PCM

View File

@@ -0,0 +1,373 @@
#pragma once
/****************************************************************************************
* *
* MOUNT CONTROL COMPONENTS LIBRARY *
* *
* *
* "POINTING-CORRECTION-MODEL" FITTER *
* *
****************************************************************************************/
#include <eigen3/Eigen/Dense>
#include "mcc_concepts.h"
#include "mcc_coordinate.h"
#include "mcc_pcm.h"
namespace mcc::impl
{
enum class MccDefaultPCMConstructorErrorCode : int {
ERROR_OK,
ERROR_NOT_ENOUGH_DATA,
#ifdef USE_BSPLINE_PCM
ERROR_INVALID_KNOTS_NUMBER,
ERROR_BSPLINE_FIT
#endif
};
// error category
struct MccDefaultPCMConstructorErrorCategory : std::error_category {
MccDefaultPCMConstructorErrorCategory() = default;
const char* name() const noexcept
{
return "MCC-DEFAULT-PCM-CONSTRUCTOR-ERROR-CATEGORY";
}
std::string message(int ec) const
{
MccDefaultPCMConstructorErrorCode err = static_cast<MccDefaultPCMConstructorErrorCode>(ec);
switch (err) {
case MccDefaultPCMConstructorErrorCode::ERROR_OK:
return "OK";
case MccDefaultPCMConstructorErrorCode::ERROR_NOT_ENOUGH_DATA:
return "not enough data point";
#ifdef USE_BSPLINE_PCM
case MccDefaultPCMConstructorErrorCode::ERROR_INVALID_KNOTS_NUMBER:
return "invalid number of B-spline knots";
case MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT:
return "B-spline fitting error";
#endif
default:
return "UNKNOWN";
}
}
static const MccDefaultPCMConstructorErrorCategory& get()
{
static const MccDefaultPCMConstructorErrorCategory constInst;
return constInst;
}
};
inline std::error_code make_error_code(MccDefaultPCMConstructorErrorCode ec)
{
return std::error_code(static_cast<int>(ec), MccDefaultPCMConstructorErrorCategory::get());
}
} // namespace mcc::impl
namespace std
{
template <>
class is_error_code_enum<mcc::impl::MccDefaultPCMConstructorErrorCode> : public true_type
{
};
} // namespace std
namespace mcc::impl
{
template <MccMountType MOUNT_TYPE>
class MccPCMConstructor
{
public:
using ref_coordpair_t = std::conditional_t<mcc_is_equatorial_mount<MOUNT_TYPE>,
MccSkyHADEC_OBS,
std::conditional_t<mcc_is_altaz_mount<MOUNT_TYPE>, MccSkyAZZD, void>>;
static_assert(!std::is_void_v<ref_coordpair_t>, "UNSUPPORTED MOUNT TYPE!");
struct table_elem_t {
double target_colon, target_colat;
double hw_colon, hw_colat;
double colon_res, colat_res; // target - hw
};
struct table_t {
std::vector<double> target_colon, target_colat;
std::vector<double> hw_colon, hw_colat;
std::vector<double> colon_res, colat_res; // target - hw
};
struct compute_result_t {
MccDefaultPCMType pcm_type;
MccError error{}; // final model computation error
std::vector<double> model_colon, model_colat; // fitted model values
std::vector<double> colon_res, colat_res; // target - model
#ifdef USE_BSPLINE_PCM
int bspline_fit_err{}; // bivariate B-spline fitting exit code (see FITPACK)
// quantities below are computed only fo pcm_type == MccDefaultPCMType::PCM_TYPE_BSPLINE
std::vector<double> inv_model_colon, inv_model_colat; // fitted inverse model values
std::vector<double> inv_colon_res, inv_colat_res; // encoder - model
#endif
};
MccError addPoint(mcc_skypoint_c auto target_coords,
mcc_coord_pair_c auto const& hw_counts,
mcc_coord_epoch_c auto const& ref_epoch)
requires(decltype(hw_counts)::pairKind == MccCoordPairKind::COORDS_KIND_XY)
{
auto err = target_coords.to(ref_coordpair_t::pairKind, ref_epoch);
if (err) {
return mcc_deduced_err(err, MccDefaultPCMErrorCode::ERROR_CCTE);
}
// _table.push_back({target_coords.co_lon(), target_coords.co_lat(), hw_counts.x(), hw_counts.y(),
// target_coords.co_lon() - hw_counts.x(), target_coords.co_lat() - hw_counts.y()});
_table.target_colon.emplace_back(target_coords.co_lon());
_table.target_colat.emplace_back(target_coords.co_lon());
_table.hw_colon.emplace_back(hw_counts.x());
_table.hw_colat.emplace_back(hw_counts.y());
_table.colon_res.emplace_back(target_coords.co_lon() - hw_counts.x());
_table.colat_res.emplace_back(target_coords.co_lat() - hw_counts.y());
return {};
}
MccError addPoint(mcc_skypoint_c auto target_coords, mcc_coord_pair_c auto const& hw_counts)
{
auto ref_epoch = hw_counts.epoch();
return addPoint(std::move(target_coords), hw_counts, ref_epoch);
}
size_t numberOfPoints() const
{
return _table.colon_res.size();
}
void deletePoints()
{
_table.target_colon.clear();
_table.target_colat.clear();
_table.hw_colon.clear();
_table.hw_colat.clear();
_table.colon_res.clear();
_table.colat_res.clear();
}
//
// for B-splines interior knots along axes must be given in 'pcm_data'
// NOTE: the size of the interior knots array must be at least 2 as
// it are interpretated as border knots and final full knots set is:
// knots = [input_knots[0], input_knots[0], input_knots[0], input_knots[0], input_knots[1], input_knots[2],
// ..., input_knots[N-1], input_knots[N-1], input_knots[N-1], input_knots[N-1]], where N = input_knots.size()
//
// WARNING: the input knots for inverse B-spline are ignored so the direct and inverse B-spline coefficients are
// calculated on the same mesh!
compute_result_t computeModel(MccDefaultPCM<MOUNT_TYPE>::pcm_data_t& pcm_data)
{
compute_result_t result{.pcm_type = pcm_data.type, .error = MccDefaultPCMConstructorErrorCode::ERROR_OK};
size_t min_data_size = 2; // 2 is for BSPLINE
if (pcm_data.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY
#ifdef USE_BSPLINE_PCM
|| pcm_data.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE
#endif
) {
if constexpr (MOUNT_TYPE == MccMountType::FORK_TYPE) {
min_data_size = 9;
} else {
min_data_size = 8;
}
if (_table.size() < min_data_size) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_NOT_ENOUGH_DATA;
return result;
}
// robust linear regression with Tukey's loss function
}
#ifdef USE_BSPLINE_PCM
if (pcm_data.type == MccDefaultPCMType::PCM_TYPE_BSPLINE ||
pcm_data.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
if (pcm_data.bspline.knotsX.size() < 2 || pcm_data.bspline.knotsY.size() < 2) {
return MccDefaultPCMConstructorErrorCode::ERROR_INVALID_KNOTS_NUMBER;
}
double resi2x, resi2y; // fitting residuals
std::vector<double> tx(pcm_data.bspline.knotsX.size() + 6), ty(pcm_data.bspline.knotsY.size() + 6);
size_t Ncoeffs = (tx.size() - 4) * (ty.size() - 4);
pcm_data.bspline.coeffsX.resize(Ncoeffs);
pcm_data.bspline.coeffsY.resize(Ncoeffs);
if (pcm_data.type == MccDefaultPCMType::PCM_TYPE_BSPLINE) {
if (_table.size() < min_data_size) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_NOT_ENOUGH_DATA;
return result;
}
// here both direct and inverse coefficients will be calculated
pcm_data.inverseBspline.coeffsX.resize(Ncoeffs);
pcm_data.inverseBspline.coeffsY.resize(Ncoeffs);
// direct (celestial = encoder + pcm)
result.bspline_fit_err = bsplines::fitpack_sphere_fit(
_table.hw_colat, _table.hw_colon, _table.colon_res, 1.0, pcm_data.bspline.knotsY,
pcm_data.bspline.knotsX, pcm_data.bspline.coeffsX, resi2x);
if (result.bspline_fit_err > 0) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
return result;
}
result.bspline_fit_err = bsplines::fitpack_sphere_fit(
_table.hw_colat, _table.hw_colon, _table.colat_res, 1.0, pcm_data.bspline.knotsY,
pcm_data.bspline.knotsX, pcm_data.bspline.coeffsY, resi2y);
if (result.bspline_fit_err > 0) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
return result;
}
// inverse (encoder = celestial + pcm)
std::vector<double> colon_res = _table.colon_res;
std::vector<double> colat_res = _table.colat_res;
for (size_t i = 0; i < colat_res.size(); ++i) {
colon_res[i] = -colon_res[i];
colat_res[i] = -colat_res[i];
}
result.bspline_fit_err = bsplines::fitpack_sphere_fit(
_table.target_colon, _table.target_colat, colon_res, 1.0, pcm_data.bspline.knotsY,
pcm_data.bspline.knotsX, pcm_data.bspline.inverseCoeffsX, resi2x);
if (result.bspline_fit_err > 0) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
return result;
}
result.bspline_fit_err = bsplines::fitpack_sphere_fit(
_table.target_colon, _table.target_colat, colat_res, 1.0, pcm_data.bspline.knotsY,
pcm_data.bspline.knotsX, pcm_data.bspline.inverseCoeffsY, resi2y);
if (result.bspline_fit_err > 0) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
return result;
}
} else { // geometry + B-spline
// the fitting for geometrical coefficients is already done above so
// one must fit residuals by bivariate B-splines
std::vector<double> xres(_table.size()), yres(_table.size());
// for (size_t i = 0; i < _table.size(); ++i) {
// xres = _table[i].target_colon;
// yres = _table[i].target_colat;
// }
for (size_t i = 0; i < _table.size(); ++i) {
xres = _table.target_colon[i];
yres = _table.target_colat[i];
}
}
}
}
#endif
protected:
// std::vector<table_elem_t> _table;
table_t _table;
compute_result_t bsplineFitting(MccDefaultPCM<MOUNT_TYPE>::pcm_data_t& pcm_data)
{
compute_result_t result{.pcm_type = pcm_data.type, .error = MccDefaultPCMConstructorErrorCode::ERROR_OK};
if (pcm_data.bspline.knotsX.size() < 2 || pcm_data.bspline.knotsY.size() < 2) {
return MccDefaultPCMConstructorErrorCode::ERROR_INVALID_KNOTS_NUMBER;
}
double resi2x, resi2y; // fitting residuals
std::vector<double> tx(pcm_data.bspline.knotsX.size() + 6), ty(pcm_data.bspline.knotsY.size() + 6);
size_t Ncoeffs = (tx.size() - 4) * (ty.size() - 4);
pcm_data.bspline.coeffsX.resize(Ncoeffs);
pcm_data.bspline.coeffsY.resize(Ncoeffs);
if (pcm_data.type == MccDefaultPCMType::PCM_TYPE_BSPLINE) {
// here both direct and inverse coefficients will be calculated
pcm_data.inverseBspline.coeffsX.resize(Ncoeffs);
pcm_data.inverseBspline.coeffsY.resize(Ncoeffs);
// direct (celestial = encoder + pcm)
result.bspline_fit_err = bsplines::fitpack_sphere_fit(_table.hw_colat, _table.hw_colon, _table.colon_res,
1.0, pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
pcm_data.bspline.coeffsX, resi2x);
if (result.bspline_fit_err > 0) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
return result;
}
result.bspline_fit_err = bsplines::fitpack_sphere_fit(_table.hw_colat, _table.hw_colon, _table.colat_res,
1.0, pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
pcm_data.bspline.coeffsY, resi2y);
if (result.bspline_fit_err > 0) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
return result;
}
// inverse (encoder = celestial + pcm)
std::vector<double> colon_res = _table.colon_res;
std::vector<double> colat_res = _table.colat_res;
for (size_t i = 0; i < colat_res.size(); ++i) {
colon_res[i] = -colon_res[i];
colat_res[i] = -colat_res[i];
}
result.bspline_fit_err = bsplines::fitpack_sphere_fit(_table.target_colon, _table.target_colat, colon_res,
1.0, pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
pcm_data.bspline.inverseCoeffsX, resi2x);
if (result.bspline_fit_err > 0) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
return result;
}
result.bspline_fit_err = bsplines::fitpack_sphere_fit(_table.target_colon, _table.target_colat, colat_res,
1.0, pcm_data.bspline.knotsY, pcm_data.bspline.knotsX,
pcm_data.bspline.inverseCoeffsY, resi2y);
if (result.bspline_fit_err > 0) {
result.error = MccDefaultPCMConstructorErrorCode::ERROR_BSPLINE_FIT;
return result;
}
}
}
};
} // namespace mcc::impl

View File

@@ -149,7 +149,7 @@ public:
return _inZoneFunc.size();
}
void clearZones()
void clearPZones()
{
_inZoneFunc.clear();
_timeToZoneFunc.clear();

View File

@@ -115,8 +115,10 @@ protected:
PAIRKIND == MccCoordPairKind::COORDS_KIND_XY ||
PAIRKIND == MccCoordPairKind::COORDS_KIND_GENERIC) { // azimuth is in degrees
pars.angle_format = MccSerializedAngleFormat::MCC_SERIALIZED_FORMAT_SXGM_DEGS;
pars.norm_sxgm = true;
} else { // RA or HA
pars.angle_format = MccSerializedAngleFormat::MCC_SERIALIZED_FORMAT_SXGM_HOURS;
pars.norm_sxgm = true;
}
} else {
// !!!!!!!!!!!!!!!!!!
@@ -348,10 +350,10 @@ struct MccSerializer<VT> : MccSerializerBase {
VT const& value,
ParamsT const& params = mcc_serialization_params_t{})
{
// format: X<elem-delim>Y<elem-delim>PAIRKIND<elem-delim>EPOCH
auto pars = params;
// pars.norm_sxgm = true;
// pars.coordpair_format = MccSerializedCoordPairFormat::MCC_SERIALIZED_FORMAT_SXGM_HOURDEG;
// X-coordinate
MccSerializerBase::angleFormatFromCoordPairType<VT::pairKind, MccSerializerBase::CO_LON>(pars);
@@ -363,7 +365,7 @@ struct MccSerializer<VT> : MccSerializerBase {
MccSerializerBase::addElemDelimiter(output, params);
pars.norm_sxgm = false; // do not normalize co-latitude angle
// pars.norm_sxgm = false; // do not normalize co-latitude angle
// Y-coordinate
MccSerializerBase::angleFormatFromCoordPairType<VT::pairKind, MccSerializerBase::CO_LAT>(pars);
@@ -375,6 +377,14 @@ struct MccSerializer<VT> : MccSerializerBase {
MccSerializerBase::addElemDelimiter(output, params);
// pair kind
auto pk_err = MccSerializer<std::string_view>{}(output, MccCoordPairKindToStr(VT::pairKind), params);
if (pk_err) {
return mcc_deduced_err(pk_err, MccSerializerErrorCode::ERROR_UNDERLYING_SERIALIZER);
}
MccSerializerBase::addElemDelimiter(output, params);
// epoch
auto ep = value.epoch();
auto ep_err = MccSerializer<decltype(ep)>{}(output, ep, params);
@@ -383,14 +393,6 @@ struct MccSerializer<VT> : MccSerializerBase {
}
MccSerializerBase::addElemDelimiter(output, params);
// pair kind
auto pk_err = MccSerializer<std::string_view>{}(output, MccCoordPairKindToStr(VT::pairKind), params);
if (pk_err) {
return mcc_deduced_err(pk_err, MccSerializerErrorCode::ERROR_UNDERLYING_SERIALIZER);
}
return MccSerializerErrorCode::ERROR_OK;
}
};
@@ -406,7 +408,7 @@ struct MccSerializer<VT> : MccSerializerBase {
ParamsT const& params = mcc_serialization_params_t{})
{
auto serialize_cpair = [&]<typename T>(T& cp) -> error_t {
auto ccte_err = value.to(cp);
auto ccte_err = value.toAtSameEpoch(cp);
if (ccte_err) {
return mcc_deduced_err(ccte_err, MccSerializerErrorCode::ERROR_COORD_TRANSFORM);
}
@@ -501,7 +503,7 @@ struct MccSerializer<VT> : MccSerializerBase {
// quantities in degree representation
MccSerializerBase::angleFormatFromCoordPairType<MccCoordPairKind::COORDS_KIND_RADEC_ICRS,
MccSerializerBase::CO_LON>(pars_d);
MccSerializerBase::CO_LAT>(pars_d);
MccSerializer<MccAngle> ang_sr;
@@ -518,7 +520,7 @@ struct MccSerializer<VT> : MccSerializerBase {
MccSerializerBase::addElemDelimiter(output, pars_h);
err = ang_sr(output, rd_obs.y(), pars_h);
err = ang_sr(output, rd_obs.y(), pars_d);
if (err) {
return mcc_deduced_err(err, MccSerializerErrorCode::ERROR_UNDERLYING_SERIALIZER);
}

View File

@@ -104,20 +104,26 @@ inline std::error_code make_error_code(MccTelemetryErrorCode ec)
template <mcc_hardware_c HARDWARE_T>
class MccTelemetry
class MccTelemetry : public mcc_telemetry_interface_t<MccError>
{
public:
typedef HARDWARE_T hardware_t;
typedef std::error_code error_t;
typedef MccError error_t;
struct telemetry_data_t {
MccSkyPoint targetPos{};
MccSkyPoint targetPos{}; // celestial coordinates
MccSkyPoint mountPos{};
MccGenXY targetXY{}; // encoder coordinates
typename HARDWARE_T::hardware_state_t hwState{};
struct {
double pcmX{}, pcmY{};
} pcmReverseCorrection{};
MccSkyPoint mountPos{}; // celestial coordinates
typename HARDWARE_T::hardware_state_t hwState{}; // here encoder coordinates
struct {
double pcmX{}, pcmY{};
@@ -210,8 +216,8 @@ public:
mcc_deduced_err(hw_err, MccTelemetryErrorCode::ERROR_HARDWARE_GETSTATE);
} else {
// compute PCM corrections and observed (corrected for PCM) mount coordinates
auto pcm_err = pcm_ptr->computePCM(_tdataPtr->hwState.XY, &_tdataPtr->pcmCorrection,
&_tdataPtr->mountPos);
auto pcm_err = pcm_ptr->computePCM(_tdataPtr->hwState.XY, &(_tdataPtr->pcmCorrection),
&(_tdataPtr->mountPos));
if (!pcm_err) {
// set target coordinates
@@ -229,14 +235,46 @@ public:
mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
_tdataPtr->pcmReverseCorrection.pcmX = -pcm_corr.pcmX;
_tdataPtr->pcmReverseCorrection.pcmY = -pcm_corr.pcmY;
} else {
_lastUpdateError =
mcc_deduced_err(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
}
} else { // observed, apparent or ICRS
_tdataPtr->targetPos = _enteredTargetPos;
}
} else { // observed, apparent or ICRS
// _tdataPtr->targetPos = _enteredTargetPos;
using pcm_t = std::remove_pointer_t<decltype(pcm_ptr)>;
std::conditional_t<mcc_is_equatorial_mount<pcm_t::pcmMountType>,
MccSkyHADEC_OBS,
std::conditional_t<mcc_is_altaz_mount<pcm_t::pcmMountType>,
MccSkyAZZD, std::nullptr_t>>
cp;
static_assert(!std::is_null_pointer_v<decltype(cp)>, "UNKNOW MOUNT TYPE!");
// calculate target celestial coordinates (of the same type as .mountPos) at the
// epoch of the current mount position
cp.setEpoch(_tdataPtr->mountPos.epoch());
auto ccte_err = _enteredTargetPos.to(cp);
if (ccte_err) {
_lastUpdateError =
mcc_deduced_err(ccte_err, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM);
} else {
_tdataPtr->targetPos.from(cp);
// calculate reverse PCM corrections for the current target position and
// its encoder XY
auto pcm_err = pcm_ptr->computeInversePCM(_tdataPtr->targetPos,
&_tdataPtr->pcmReverseCorrection,
&_tdataPtr->targetXY);
if (pcm_err) {
_lastUpdateError =
mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
}
}
} else {
_lastUpdateError = mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
@@ -278,22 +316,34 @@ public:
}
}
error_t setTarget(mcc_skypoint_c auto const& sp)
error_t setPointingTarget(mcc_skypoint_c auto const& sp)
{
std::lock_guard lock{*_updateMutex};
_enteredTargetPos = sp;
return MccTelemetryErrorCode::ERROR_OK;
}
error_t getTarget(mcc_skypoint_c auto* sp)
auto getPointingTarget() const
{
sp = _enteredTargetPos;
std::lock_guard lock{*_updateMutex};
return MccTelemetryErrorCode::ERROR_OK;
return _enteredTargetPos;
}
// error_t getPointingTarget(mcc_skypoint_c auto* sp)
// {
// if (sp) {
// *sp = _enteredTargetPos;
// }
// return MccTelemetryErrorCode::ERROR_OK;
// }
//
// blocks the current thread until telemetry data is received.
// the maximum blocking time is equal to the set timeout (see setTelemetryDataTimeout method)
@@ -321,12 +371,14 @@ public:
return _lastUpdateError;
}
//
// Set a timeout for the telemetry receiving process
//
void setTelemetryDataTimeout(traits::mcc_time_duration_c auto const& timeout)
template <traits::mcc_time_duration_c DT>
void setTelemetryDataTimeout(DT const& timeout)
{
if constexpr (std::floating_point<typename decltype(timeout)::rep>) {
if constexpr (std::floating_point<typename DT::rep>) {
if (utils::isEqual(timeout.count(), 0.0) || timeout.count() < 0.0) {
return;
}

View File

@@ -68,7 +68,7 @@ constexpr static std::string_view trimSpaces(const char* r, TrimType type = Trim
}
template <typename T, std::ranges::contiguous_range R>
std::optional<T> numFromStr(R&& r)
static std::optional<T> numFromStr(R&& r)
requires((std::integral<T> || std::floating_point<T>) &&
std::same_as<char, std::remove_cvref_t<std::ranges::range_value_t<R>>>)
{
@@ -340,7 +340,10 @@ static std::string AZZD_rad2sxg(double az, double zd, std::string_view delim = "
// "12:43:23.423, 102:43:12.124"
// " 12.3453467, 102:43:12.124 "
template <mcc::traits::mcc_input_char_range R>
std::pair<double, double> parseAnglePair(R&& str, bool hms1 = false, bool hms2 = false, std::string_view delim = ",")
static std::pair<double, double> parseAnglePair(R&& str,
bool hms1 = false,
bool hms2 = false,
std::string_view delim = ",")
{
std::pair<double, double> res{std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN()};
auto found1 = std::ranges::search(std::forward<R>(str), delim);
@@ -376,6 +379,40 @@ std::pair<double, double> parseAnglePair(R&& str, bool hms1 = false, bool hms2 =
}
// calculate distance between two point on sphere:
// the function returns a std::tuple with
// 0th element: difference along co-longitude axis
// 1st element: difference along co-latitude axis
// 2nd element: distance
static std::tuple<double, double, double> distanceOnSphere(double co_lon1,
double co_lat1,
double co_lon2,
double co_lat2)
{
std::tuple<double, double, double> res{};
std::get<0>(res) = co_lon1 - co_lon2;
std::get<1>(res) = co_lat1 - co_lat2;
double cosDco_lon = std::cos(std::abs(std::get<0>(res)));
double cos1 = std::cos(co_lat1);
double sin1 = std::sin(co_lat1);
double cos2 = std::cos(co_lat2);
double sin2 = std::sin(co_lat2);
double term1 = cos2 * std::sin(std::abs(std::get<0>(res)));
double term2 = cos1 * sin2 - sin1 * cos2 * cosDco_lon;
std::get<2>(res) = std::atan2(sqrt(term1 * term1 + term2 * term2), sin1 * sin2 + cos1 * cos2 * cosDco_lon);
// std::get<2>(res) = acos(sin1 * sin2 + cos1 * cos2 * cosDco_lon);
return res;
}
template <traits::mcc_input_char_range R>
static constexpr size_t FNV1aHash(const R& r)
{

View File

@@ -14,7 +14,11 @@ static std::uniform_real_distribution<double> y_distrib(1000, 7000);
static std::uniform_real_distribution<double> xs_distrib(100, 700);
static std::uniform_real_distribution<double> ys_distrib(100, 700);
static std::uniform_int_distribution<uint8_t> err_distrib(0, 255);
struct hw_t {
static constexpr mcc::MccMountType hwMountType{mcc::MccMountType::ALTAZ_TYPE};
static constexpr std::string_view hardwareName{"HW-TEST"};
typedef int error_t;
@@ -52,13 +56,19 @@ struct hw_t {
*state = hardware_state_t{.XY{x_distrib(gen), y_distrib(gen)}, .speedXY{xs_distrib(gen), ys_distrib(gen)}};
return 0;
// return 0;
return err_distrib(gen) < 5 ? 1 : 0;
}
error_t hardwareInit()
{
return 0;
}
error_t hardwareShutdown()
{
return 0;
}
};
template <>