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-18 16:22:58 +03:00
parent 53245002c5
commit 889d4ff3b2
7 changed files with 189 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

178
include/mcc/mcc_movement_controls.h
View File

@@ -29,6 +29,7 @@
#include <atomic>
#include <fstream>
#include <thread>
#include "mcc/mcc_coordinate.h"
#include "mcc_concepts.h"
@@ -370,8 +371,8 @@ 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");
@@ -440,18 +441,18 @@ public:
auto log_pos = [logger, this](typename TELEMETRY_T::telemetry_data_t const& tdata) -> error_t {
double x_mnt, y_mnt, x_tag, y_tag;
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>>
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";
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) ? "ZD"
: "GEN_Y";
;
auto ccte_err = tdata.targetPos.toAtSameEpoch(coord_pair);
@@ -529,7 +530,7 @@ public:
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->logWarn("Slewing path filename is empty! Do not save it!");
}
}
@@ -602,7 +603,7 @@ public:
auto start_point = std::chrono::steady_clock::now();
auto last_hw_time = tdata.hwState.XY.epoch().UTC();
mcc_deduced_coord_pair_t<HARDWARE_T::mountType> tag_cp, mnt_cp;
mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp;
while (!*_stopMoving) {
t_err = telemetry->telemetryData(&tdata);
@@ -695,6 +696,9 @@ public:
if (_lastError->load()) {
break;
}
// sleep here
std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
}
}
@@ -720,11 +724,15 @@ public:
auto ccte_err = tdata.targetPos.to(tag_cp);
if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
return;
}
ccte_err = tdata.mountPos.to(mnt_cp);
if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
return;
}
@@ -742,6 +750,152 @@ public:
}
}
};
_trackingFunc = [telemetry, cb_sptr, logger, this]() {
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.trackingPathFilename.empty()) { // open slewing trajectory file
_pathFile.setFilename(_currentParams.trackingPathFilename);
} else {
logger->logWarn("Tracking path filename is empty! Do not save it!");
}
}
logger->logInfo("Start tracking");
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 << "# 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}> <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_TRACKING;
auto last_hw_time = tdata.hwState.XY.epoch().UTC();
mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp;
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);
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();
auto ccte_err = tdata.targetPos.to(tag_cp);
if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
ccte_err = tdata.mountPos.to(mnt_cp);
if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
}
auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y());
logger->logTrace(std::format(" target-to-mount distance: {} (dx = {}, dy = {})",
MccAngleFancyString(std::get<2>(dist)), std::get<0>(dist),
std::get<1>(dist)));
// 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());
*_lastError = send_to_hardware(hw_state);
if (_lastError->load()) {
break;
}
if (*_stopMoving) {
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
break;
}
// sleep here
std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval);
}
*_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;