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

378
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,21 +298,19 @@ 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(),
hw_state.XY.y().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",
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",
MccAngle((double)hw_state.XY.x()).degrees(),
MccAngle((double)hw_state.XY.y()).degrees()));
}
auto hw_err = hardware->hardwareSetState(hw_state);
@@ -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,
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 +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,8 +373,8 @@ 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),
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);
if (pz_err) {
@@ -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,14 +413,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,61 +430,43 @@ 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);
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::mountType), MccSkyHADEC_OBS,
std::conditional_t<mccIsAltAzMount(HARDWARE_T::mountType), MccSkyAZZD, 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::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";
;
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);
}
x_tag = coord_pair.x();
y_tag = coord_pair.y();
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);
}
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) << " "
<< (int)tdata.hwState.movementState << "\n";
@@ -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;
}
}
};
}