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

337
mcc/mcc_tracking_model.h
View File

@@ -3,11 +3,10 @@
/* MOUNT CONTROL COMPONENTS LIBRARY */
/* SIMPLE TRACKING MODEL IMPLEMENTATION */
/* SIMPLE Tracking MODEL IMPLEMENTATION */
#include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_moving_model_common.h"
namespace mcc
@@ -15,14 +14,19 @@ namespace mcc
enum class MccSimpleTrackingModelErrorCode : int {
ERROR_OK,
ERROR_CCTE,
ERROR_HW_GETSTATE,
ERROR_HW_SETSTATE,
ERROR_PCM_COMP,
ERROR_GET_TELEMETRY,
ERROR_DIST_TELEMETRY,
ERROR_DIFF_TELEMETRY,
ERROR_PZONE_CONTAINER_COMP,
ERROR_IN_PZONE,
ERROR_NEAR_PZONE,
ERROR_UNEXPECTED_AXIS_RATES
ERROR_TIMEOUT,
ERROR_UNEXPECTED_AXIS_RATES,
ERROR_STOPPED
};
} // namespace mcc
@@ -50,227 +54,254 @@ public:
typedef MccSimpleMovingModelParams tracking_params_t;
// struct tracking_params_t {
// static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second
// double trackSpeedX{};
// double trackSpeedY{};
// std::chrono::seconds telemetryTimeout{3};
// // minimal time to prohibited zone. if it is lesser then exit with error
// std::chrono::seconds minTimeToPZone{10};
// };
template <mcc_telemetry_data_c TelemetryT,
mcc_hardware_c HardwareT,
mcc_PCM_c PcmT,
mcc_pzone_container_c PZoneContT>
MccSimpleTrackingModel(TelemetryT* telemetry, HardwareT* hardware, PcmT* pcm, PZoneContT* pz_cont)
: _stopTracking(new std::atomic_bool()), _currentTrackParamsMutex(new std::mutex)
template <mcc_all_controls_c CONTROLS_T>
MccSimpleTrackingModel(CONTROLS_T* controls)
: _stopTracking(new std::atomic_bool()), _currentParamsMutex(new std::mutex())
{
*_stopTracking = false;
// set default values
if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
_currentTrackParams.trackSpeedX = tracking_params_t::sideralRate; // move along HA-axis with sideral rate
_currentTrackParams.trackSpeedY = 0.0;
_currentTrackParams.telemetryTimeout = std::chrono::seconds(3);
_currentTrackParams.minTimeToPZone = std::chrono::seconds(10);
}
_trackingFunc = [telemetry, hardware, pcm, pz_cont, this]() -> error_t {
typename HardwareT::hardware_state_t hw_state;
MccEqtHrzCoords intsc_coords;
_trackingFunc = [controls, this]() -> error_t {
typename CONTROLS_T::hardware_state_t hw_state;
MccTelemetryData tdata;
auto t_err = telemetry->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout);
MccEqtHrzCoords intsc_coords;
MccCelestialPoint target_in_future_pt;
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
target_in_future_pt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
} else {
static_assert(false, "UNKNOW MOUNT TYPE!");
}
// double dist, dx, dy;
auto t_err = controls->telemetryData(&tdata);
if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
}
mcc_tp2tp(tdata.time_point, intsc_coords.time_point);
bool no_intersects = false;
// function to update the closest prohibited zone intersect point
auto update_pzones_ipoint = [controls, &tdata, &intsc_coords, &no_intersects, &hw_state,
this]() -> error_t {
// compute intersection points with the prohibited zones
auto pz_err = mcc_find_closest_pzone(controls, tdata, &intsc_coords);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err,
MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
if (std::isfinite(intsc_coords.HA)) {
intsc_coords.X = intsc_coords.HA;
intsc_coords.Y = intsc_coords.DEC_APP;
} else {
no_intersects = true;
// intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min
// intsc_coords.Y = tdata.DEC_APP;
}
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
if (std::isfinite(intsc_coords.AZ)) {
intsc_coords.X = intsc_coords.AZ;
intsc_coords.Y = intsc_coords.ZD;
} else {
no_intersects = true;
}
} else {
static_assert(false, "UNKNOW MOUNT TYPE!");
}
};
std::vector<std::chrono::duration<double>> pz_timeto; // in seconds
std::chrono::duration<double> min_time{0.0};
auto target_point = [&, this](MccCelestialPoint* point) {
auto dt = std::chrono::duration<double>{tdata.HA} +
_currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds
// compute intersection points with the prohibited zones
auto pz_err = mcc_find_closest_pzone(pz_cont, tdata, &intsc_coords);
auto tp_dt = std::chrono::duration_cast<typename decltype(tdata.time_point)::duration>(
_currentParams.timeShiftToTargetPoint);
// point in +time_dist future
MccCelestialPoint pt{
.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP,
.X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0).normalize<MccAngle::NORM_KIND_0_360>(),
.Y = tdata.DEC_APP};
mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point);
point->time_point = pt.time_point;
// check for prohibited zone
if (std::isfinite(intsc_coords.HA)) {
bool through_pzone =
(intsc_coords.HA - pt.X) <= 0; // must be <= 0 if point in future will be in the zone
through_pzone &=
(intsc_coords.HA - tdata.HA) > 0; // must be > 0 if point in future was out of the zone
if (through_pzone) {
pt.X = intsc_coords.HA;
}
}
auto ret = controls->transformCoordinates(std::move(pt), point);
if (ret) {
return mcc_deduce_error<error_t>(ret, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
} else {
MccPCMResult pcm_inv_res;
// endpoint of the mount moving
auto pcm_err = controls->computeInversePCM(target_in_future_pt, &pcm_inv_res, &hw_state);
if (pcm_err) {
return mcc_deduce_error<error_t>(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
}
mcc_tp2tp(tdata.time_point, hw_state.time_point);
}
return MccSimpleTrackingModelErrorCode::ERROR_OK;
};
auto pz_err = update_pzones_ipoint();
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
bool no_intersects = false;
hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_Tracking;
if constexpr (mccIsEquatorialMount(HardwareT::mountType)) {
if (std::isfinite(intsc_coords.HA)) {
intsc_coords.X = intsc_coords.HA;
intsc_coords.Y = intsc_coords.DEC_APP;
} else {
no_intersects = true;
intsc_coords.X = tdata.HA + 710.0_mins; // 12h - 10min
intsc_coords.Y = tdata.DEC_APP;
{
std::lock_guard lock{*_currentParamsMutex};
auto ccte_err = target_point(&target_in_future_pt);
if (ccte_err) {
return mcc_deduce_error(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
}
} else if constexpr (mccIsAltAzMount(HardwareT::mountType)) {
if (std::isfinite(intsc_coords.AZ)) {
intsc_coords.X = intsc_coords.AZ;
intsc_coords.Y = intsc_coords.ZD;
} else {
no_intersects = true;
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
hw_state.speedX = _currentParams.trackSpeedX;
hw_state.speedY = _currentParams.trackSpeedY;
}
} else {
static_assert(false, "UNKNOW MOUNT TYPE!");
}
// compute position in future
auto hw_err = hardware->hardwareGetState(&hw_state);
// move mount
auto hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
}
MccPCMResult pcm_inv_res;
// endpoint of the mount moving
auto pcm_err = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
if (pcm_err) {
return mcc_deduce_error<error_t>(pcm_err, MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
}
std::chrono::steady_clock::time_point last_corr_tp, last_ipzone_update_tp;
if constexpr (mccIsEquatorialMount(PcmT::mountType)) {
// just set sideral rate once
mcc_tp2tp(tdata.time_point, hw_state.time_point);
while (*_stopTracking) {
// wait for updated telemetry data
{
std::lock_guard lock{*_currentTrackParamsMutex};
std::lock_guard lock{*_currentParamsMutex};
hw_state.speedX = _currentTrackParams.trackSpeedX;
hw_state.speedY = _currentTrackParams.trackSpeedY;
}
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
t_err = controls->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
// start tracking
hw_err = hardware->hardwareSetState(std::move(hw_state));
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
}
while (!*_stopTracking) {
// control prohibited zones
pz_err = pz_cont->timeToPZone(tdata, &pz_timeto);
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err,
MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
min_time = std::chrono::duration<double>{0};
for (size_t i = 0; i < pz_cont->sizePZones(); ++i) {
if (pz_timeto[i] < _currentTrackParams.minTimeToPZone) {
return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
}
if (pz_timeto[i] < min_time) {
min_time = pz_timeto[i];
}
}
t_err = telemetry->waitForTelemetryData(&tdata, _currentTrackParams.telemetryTimeout);
if (t_err) {
return mcc_deduce_error<error_t>(t_err, MccSimpleTrackingModelErrorCode::ERROR_GET_TELEMETRY);
}
}
if (*_stopTracking) {
break;
if (*_stopTracking) {
break;
}
// control prohibited zones
if (mcc_is_near_pzones(controls, tdata, _currentParams.minTimeToPZone, pz_err)) {
return MccSimpleTrackingModelErrorCode::ERROR_NEAR_PZONE;
}
if (pz_err) {
return mcc_deduce_error<error_t>(pz_err,
MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
if (*_stopTracking) {
break;
}
{
std::lock_guard lock{*_currentParamsMutex};
auto now = std::chrono::steady_clock::now();
if ((now - last_corr_tp) < _currentParams.trackingCycleInterval) {
continue;
}
if (no_intersects) {
if ((intsc_coords.HA - tdata.HA) < 10.0_mins) { // recompute target point
intsc_coords.X += 11.0_hours;
hw_err = hardware->hardwareGetState(&hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err,
MccSimpleTrackingModelErrorCode::ERROR_HW_GETSTATE);
}
pcm_err = pcm->computeInversePCM(intsc_coords, &pcm_inv_res, &hw_state);
if (pcm_err) {
return mcc_deduce_error<error_t>(pcm_err,
MccSimpleTrackingModelErrorCode::ERROR_PCM_COMP);
}
// just set sideral rate once
mcc_tp2tp(tdata.time_point, hw_state.time_point);
{
std::lock_guard lock{*_currentTrackParamsMutex};
hw_state.speedX = _currentTrackParams.trackSpeedX;
hw_state.speedY = _currentTrackParams.trackSpeedY;
}
hw_state.moving_type = HardwareT::hardware_moving_state_t::HW_MOVE_TRACKING;
// start tracking
hw_err = hardware->hardwareSetState(std::move(hw_state));
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err,
MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
}
// update prohibited zones intersection point
if ((now - last_ipzone_update_tp) < _currentParams.updatingPZoneInterval) {
pz_err = update_pzones_ipoint();
if (pz_err) {
return mcc_deduce_error<error_t>(
pz_err, MccSimpleTrackingModelErrorCode::ERROR_PZONE_CONTAINER_COMP);
}
}
// compute new target-in-future point
auto ccte_err = target_point(&target_in_future_pt);
if (ccte_err) {
return mcc_deduce_error(ccte_err, MccSimpleTrackingModelErrorCode::ERROR_CCTE);
}
}
return MccSimpleTrackingModelErrorCode::ERROR_OK;
} else if constexpr (mccIsAltAzMount(PcmT::mountType)) {
static_assert(false, "NOT IMPLEMENTED!");
} else {
static_assert(false, "UNKNOW MOUNT TYPE!");
// send corrections
hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_Tracking;
hw_err = controls->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error<error_t>(hw_err, MccSimpleTrackingModelErrorCode::ERROR_HW_SETSTATE);
}
}
return MccSimpleTrackingModelErrorCode::ERROR_OK;
};
}
MccSimpleTrackingModel(MccSimpleTrackingModel&&) = default;
MccSimpleTrackingModel& operator=(MccSimpleTrackingModel&&) = default;
MccSimpleTrackingModel(const MccSimpleTrackingModel&) = delete;
MccSimpleTrackingModel& operator=(const MccSimpleTrackingModel&) = delete;
error_t trackMount()
error_t trackTarget()
{
*_stopTracking = false;
return _trackingFunc();
}
error_t stopTracking()
error_t stoptTracking()
{
*_stopTracking = true;
return MccSimpleTrackingModelErrorCode::ERROR_OK;
}
error_t setTrackingParams(tracking_params_t params)
{
std::lock_guard lock{*_currentTrackParamsMutex};
std::lock_guard lock{*_currentParamsMutex};
_currentTrackParams = std::move(params);
_currentParams = std::move(params);
return MccSimpleTrackingModelErrorCode::ERROR_OK;
}
tracking_params_t getTrackingParams() const
{
std::lock_guard lock{*_currentTrackParamsMutex};
std::lock_guard lock{*_currentParamsMutex};
return _currentTrackParams;
return _currentParams;
}
protected:
std::function<error_t()> _trackingFunc{};
std::unique_ptr<std::atomic_bool> _stopTracking{};
std::unique_ptr<std::atomic_bool> _stopTracking;
tracking_params_t _currentTrackParams;
std::unique_ptr<std::mutex> _currentTrackParamsMutex;
tracking_params_t _currentParams{};
std::unique_ptr<std::mutex> _currentParamsMutex{};
};
} // namespace mcc