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

remove guiding model

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

153
mcc/mcc_slewing_model.h
View File

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