timur/mcc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

...

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2026-02-16 18:38:59 +03:00
parent 82f1477edd
commit 26011e7630
7 changed files with 394 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
CMakeLists.txt
View File

@@ -214,6 +214,7 @@ 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_serialization_common.h
include/mcc/mcc_deserializer.h
include/mcc/mcc_serializer.h
@@ -225,7 +226,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 +245,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(

1
include/mcc/mcc_angle.h
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)

8
include/mcc/mcc_concepts.h
View File

@@ -743,6 +743,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)>;

350
include/mcc/mcc_movement_controls.h
View File

@@ -11,6 +11,22 @@
* *
****************************************************************************************/
/*
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>
@@ -261,6 +277,19 @@ public:
typedef MccSimpleMovementControlsParameters movement_params_t;
template <mcc_hardware_c HARDWARE_T,
mcc_generic_mount_c MOUNT_T, // MOUNT_T is mcc_telemetry_c, mcc_pzone_container_c and mcc_logger_c
// (see mcc_concepts.h)
mcc_mount_status_c STATUS_T,
std::invocable<STATUS_T const&> CallbackFuncT = decltype([](STATUS_T const&) {})>
MccSimpleMovementControls(
HARDWARE_T* hardware,
MOUNT_T* mount,
CallbackFuncT&& mode_switch_callback = [](STATUS_T const&) {})
: MccSimpleMovementControls(hardware, mount, mount, mode_switch_callback, *mount)
{
}
template <mcc_hardware_c HARDWARE_T,
mcc_telemetry_c TELEMETRY_T,
mcc_pzone_container_c PZONE_CONT_T,
@@ -269,19 +298,17 @@ public:
mcc_logger_c LOGGER_T = MccNullLogger>
MccSimpleMovementControls(
HARDWARE_T* hardware,
TELEMETRY_T* telemtry,
TELEMETRY_T* telemetry,
PZONE_CONT_T* pzone_cont,
CallbackFuncT&& mode_switch_callback = [](STATUS_T const&) {},
LOGGER_T& logger = details::NullLogger)
LOGGER_T* logger = &details::NullLogger)
{
auto log_ptr = &logger;
auto send_to_hardware = [hardware, log_ptr](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t {
auto send_to_hardware = [hardware, logger](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(),
logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", hw_state.XY.x().degrees(),
hw_state.XY.y().degrees()));
} else { // user defined mcc_coord_pair_c
log_ptr->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
MccAngle((double)hw_state.XY.x()).degrees(),
MccAngle((double)hw_state.XY.y()).degrees()));
}
@@ -291,16 +318,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,
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) {
double braking_accelY) -> error_t {
bool in_zone;
std::vector<bool> in_zone_vec;
@@ -328,7 +355,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)));
@@ -346,7 +373,7 @@ public:
mount_pos.setY(tdata.hwState.XY.y() + tdata.pcmCorrection.y + dy);
mount_pos.setEpoch(tdata.hwState.XY.epoch());
log_ptr->logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s", MccAngleFancyString(speedX),
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);
@@ -366,7 +393,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,13 +413,13 @@ public:
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
log_ptr->logError(std::format(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
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)));
log_ptr->logError(std::format(" AZ, ZD, ALT: {}, {}, {}", MccAngle{azzd.x()}.sexagesimal(),
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(),
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,60 +430,42 @@ 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) {
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);
std::conditional_t<mccIsEquatorialMount(HARDWARE_T::mountType), MccSkyHADEC_OBS,
std::conditional_t<mccIsAltAzMount(HARDWARE_T::mountType), MccSkyAZZD, std::nullptr_t>>
coord_pair;
static_assert(!std::is_null_pointer_v<decltype(coord_pair)>, "UNKNOWN MOUNT TYPE!");
const std::string_view x_str = mccIsEquatorialMount(HARDWARE_T::mountType) ? "HA"
: mccIsAltAzMount(HARDWARE_T::mountType) ? "AZ"
: "GEN_X";
const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::mountType) ? "DEC"
: mccIsAltAzMount(HARDWARE_T::mountType) ? "ZD"
: "GEN_Y";
;
auto ccte_err = tdata.targetPos.toAtSameEpoch(coord_pair);
if (ccte_err) {
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
x_tag = hadec.x();
y_tag = hadec.y();
log_ptr->logTrace(std::format(" current target: HA = {}, DEC = {}", hadec.x().sexagesimal(true),
hadec.y().sexagesimal()));
x_tag = coord_pair.x();
y_tag = coord_pair.y();
ccte_err = tdata.mountPos.toAtSameEpoch(hadec);
logger->logTrace(std::format(" current target: {} = {}, {} = {}", x_str,
coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal()));
ccte_err = tdata.mountPos.toAtSameEpoch(coord_pair);
if (ccte_err) {
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
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()));
}
logger->logTrace(std::format(" current mount: {} = {}, {} = {}", x_str, coord_pair.x().sexagesimal(true),
y_str, coord_pair.y().sexagesimal()));
_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) << " "
@@ -481,6 +490,229 @@ 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
_pathFile.setFilename(_currentParams.slewingPathFilename);
} else {
logger->logError("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}> <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;
}
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());
// 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 = tdata.hwState.XY.epoch().UTC(); // needed for trajectory file
auto last_hw_time = start_point;
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;
}
log_pos(tdata);
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 (slew_and_stop) { // just wait until the mount stops
if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) {
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();
auto dist = utils::distanceOnSphere(tdata.targetPos.x(), tdata.targetPos.y(), tdata.mountPos.x(),
tdata.mountPos.y());
logger->logTrace(std::format(" target-to-mount distance: {} (dx = {}, dy = {})",
MccAngleFancyString(std::get<2>(dist)), std::get<0>(dist),
std::get<1>(dist)));
// stop slewing and exit from the cycle?
if (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) {
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
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());
*_lastError = send_to_hardware(hw_state);
if (_lastError->load()) {
break;
}
}
}
*_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;
}
auto dist = utils::distanceOnSphere(tdata.targetPos.x(), tdata.targetPos.y(), tdata.mountPos.x(),
tdata.mountPos.y());
log_pos(tdata);
logger->logDebug(
std::format(" target-to-mount distance {}", MccAngleFancyString(std::get<2>(dist))));
if (!slew_and_stop) { // start tracking
_trackingFunc();
} else {
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
}
}
};
}

10
include/mcc/mcc_pcm.h
View File

@@ -318,7 +318,8 @@ public:
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};
*hw_pt =
MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY, obs_skycoord.epoch()};
}
}
@@ -394,11 +395,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);
@@ -422,7 +422,9 @@ private:
res->pcmY += geom_coeffs->forkFlexure / cosX;
}
}
#ifdef USE_BSPLINE_PCM
}
#endif
#ifdef USE_BSPLINE_PCM

43
include/mcc/mcc_telemetry.h
View File

@@ -115,6 +115,12 @@ public:
struct telemetry_data_t {
MccSkyPoint targetPos{};
MccGenXY targetXY{};
struct {
double pcmX{}, pcmY{};
} pcmReverseCorrection{};
MccSkyPoint mountPos{};
typename HARDWARE_T::hardware_state_t hwState{};
@@ -229,14 +235,44 @@ 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::mountType>, MccSkyHADEC_OBS,
std::conditional_t<mcc_is_altaz_mount<pcm_t::mountType>,
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->pcmReverseCorrection(
_tdataPtr->targetPos, &_tdataPtr->pcmReverseCorrection,
&_tdataPtr->targetXY);
if (pcm_err) {
return mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
}
}
} else {
_lastUpdateError = mcc_deduced_err(pcm_err, MccTelemetryErrorCode::ERROR_PCM_COMP);
}
@@ -321,6 +357,7 @@ public:
return _lastUpdateError;
}
//
// Set a timeout for the telemetry receiving process
//

27
include/mcc/mcc_utils.h
View File

@@ -376,6 +376,33 @@ 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
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 = cos(std::get<0>(res));
double cos1 = cos(co_lat1);
double sin1 = sin(co_lat1);
double cos2 = cos(co_lat2);
double sin2 = sin(co_lat2);
double term1 = cos1 * sin(std::get<0>(res));
double term2 = cos2 * sin1 - sin2 * cos1 * cosDco_lon;
std::get<2>(res) = atan2(sqrt(term1 * term1 + term2 * term2), sin2 * sin1 + cos2 * cos1 * cosDco_lon);
return res;
}
template <traits::mcc_input_char_range R>
static constexpr size_t FNV1aHash(const R& r)
{