1060 lines
42 KiB
C++
1060 lines
42 KiB
C++
#pragma once
|
|
|
|
|
|
|
|
/****************************************************************************************
|
|
* *
|
|
* MOUNT CONTROL COMPONENTS LIBRARY *
|
|
* *
|
|
* *
|
|
* IMPLEMENTATION OF VERY SIMPLE MOUNT MOVEMENT CONTROLS CLASS *
|
|
* *
|
|
****************************************************************************************/
|
|
|
|
|
|
|
|
/*
|
|
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>
|
|
#include <thread>
|
|
|
|
#include "mcc/mcc_coordinate.h"
|
|
#include "mcc_concepts.h"
|
|
#include "mcc_error.h"
|
|
|
|
namespace mcc::impl
|
|
{
|
|
|
|
namespace details
|
|
{
|
|
// just auxiliary class instance
|
|
static MccNullLogger NullLogger{};
|
|
} // namespace details
|
|
|
|
|
|
enum class MccSimpleMovementControlsErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_HW_GETSTATE,
|
|
ERROR_HW_SETSTATE,
|
|
ERROR_HW_ERROR,
|
|
ERROR_PCM_COMP,
|
|
ERROR_CCTE_COMP,
|
|
ERROR_GET_TELEMETRY,
|
|
ERROR_DIST_TELEMETRY,
|
|
ERROR_PZONE_CONTAINER_COMP,
|
|
ERROR_TARGET_IN_PZONE,
|
|
ERROR_NEAR_PZONE,
|
|
ERROR_TIMEOUT,
|
|
ERROR_ALREADY_SLEW,
|
|
ERROR_ALREADY_STOPPED,
|
|
ERROR_STOPPED
|
|
};
|
|
|
|
} // namespace mcc::impl
|
|
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::impl::MccSimpleMovementControlsErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
|
|
namespace mcc::impl
|
|
{
|
|
|
|
// error category
|
|
struct MccSimpleMovementControlsCategory : public std::error_category {
|
|
MccSimpleMovementControlsCategory() : std::error_category() {}
|
|
|
|
const char* name() const noexcept
|
|
{
|
|
return "SIMPLE-SLEWING-MODEL";
|
|
}
|
|
|
|
std::string message(int ec) const
|
|
{
|
|
MccSimpleMovementControlsErrorCode err = static_cast<MccSimpleMovementControlsErrorCode>(ec);
|
|
|
|
switch (err) {
|
|
case MccSimpleMovementControlsErrorCode::ERROR_OK:
|
|
return "OK";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_HW_GETSTATE:
|
|
return "cannot get hardware state";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE:
|
|
return "cannot set hardware state";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_HW_ERROR:
|
|
return "hardware error occured";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_PCM_COMP:
|
|
return "PCM computation error";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP:
|
|
return "celestial coordinate transformation error";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY:
|
|
return "cannot get telemetry";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_DIST_TELEMETRY:
|
|
return "cannot get target-to-mount-position distance";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_PZONE_CONTAINER_COMP:
|
|
return "pzone container computation error";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_TARGET_IN_PZONE:
|
|
return "target is in prohibited zone";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE:
|
|
return "near prohibited zone";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_TIMEOUT:
|
|
return "a timeout occured while slewing";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_ALREADY_SLEW:
|
|
return "already slewing";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_ALREADY_STOPPED:
|
|
return "slewing is already stopped";
|
|
case MccSimpleMovementControlsErrorCode::ERROR_STOPPED:
|
|
return "slewing was stopped";
|
|
default:
|
|
return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
static const MccSimpleMovementControlsCategory& get()
|
|
{
|
|
static const MccSimpleMovementControlsCategory constInst;
|
|
return constInst;
|
|
}
|
|
};
|
|
|
|
|
|
inline std::error_code make_error_code(MccSimpleMovementControlsErrorCode ec)
|
|
{
|
|
return std::error_code(static_cast<int>(ec), MccSimpleMovementControlsCategory::get());
|
|
}
|
|
|
|
|
|
|
|
struct MccSimpleMovementControlsParameters {
|
|
// ******* common for all modes *******
|
|
|
|
// mean celestial rate
|
|
static constexpr double sideralRate = 15.0410686_arcsecs; // in radians per second
|
|
|
|
// timeout to telemetry updating
|
|
std::chrono::milliseconds telemetryTimeout{3000};
|
|
|
|
// minimal time to prohibited zone (at current speed in slewing mode). if it is lesser then exit with error
|
|
std::chrono::seconds minTimeToPZone{10};
|
|
|
|
// time interval to update prohibited zones related quantities (e.g. intersection points)
|
|
std::chrono::milliseconds updatingPZoneInterval{5000};
|
|
|
|
|
|
// ******* slewing mode *******
|
|
|
|
bool slewAndStop{false}; // slew to target and stop mount
|
|
|
|
// coordinates difference to stop slewing (in radians)
|
|
double slewToleranceRadius{5.0_arcsecs};
|
|
|
|
// telemetry request interval
|
|
std::chrono::milliseconds slewingTelemetryInterval{100};
|
|
|
|
// target-mount coordinate difference to start adjusting of slewing (in radians)
|
|
double adjustCoordDiff{slewToleranceRadius * 10.0};
|
|
|
|
// slew process timeout
|
|
std::chrono::seconds slewTimeout{3600};
|
|
|
|
double slewRateX{0.0}; // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
|
|
double slewRateY{0.0}; // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
|
|
|
|
std::chrono::milliseconds adjustCycleInterval{500}; // minimum time between two successive adjustments
|
|
|
|
double adjustRateX{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
|
|
double adjustRateY{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
|
|
|
|
// braking acceleration after execution of mount stopping command (in rads/s^2)
|
|
// it must be given as non-negative value!!!
|
|
double brakingAccelX{0.0};
|
|
double brakingAccelY{0.0};
|
|
|
|
// slewing trajectory file. if empty - just skip saving
|
|
std::string slewingPathFilename{};
|
|
// ******* tracking mode *******
|
|
|
|
// telemetry request interval
|
|
std::chrono::milliseconds trackingTelemetryInterval{100};
|
|
|
|
double trackSpeedX{};
|
|
double trackSpeedY{};
|
|
std::chrono::milliseconds trackingCycleInterval{500}; // minimum time between two successive tracking corrections
|
|
bool dualAxisTracking{true}; // mount must be of an equatorial type: false means guiding along only HA-axis
|
|
|
|
// time shift into future to compute target position in future (UT1-scale time duration)
|
|
std::chrono::milliseconds timeShiftToTargetPoint{10000};
|
|
// maximal target-to-mount difference for tracking process (in arcsecs)
|
|
// it it is greater then the current mount coordinates are used as target one
|
|
double trackingMaxCoordDiff{20.0};
|
|
|
|
// tracking trajectory file. if empty - just skip saving
|
|
std::string trackingPathFilename{};
|
|
};
|
|
|
|
|
|
|
|
class MccSimpleMovementControls
|
|
{
|
|
protected:
|
|
// a simple class for movement path saving
|
|
class PathFile
|
|
{
|
|
public:
|
|
PathFile(const std::string& filename = "") : _filename(filename), _st() {}
|
|
|
|
PathFile(const PathFile&) = delete;
|
|
PathFile(PathFile&&) = default;
|
|
|
|
~PathFile()
|
|
{
|
|
save();
|
|
}
|
|
|
|
void setFilename(const std::string& filename)
|
|
{
|
|
_filename = filename;
|
|
}
|
|
|
|
std::string getFilename() const
|
|
{
|
|
return _filename;
|
|
}
|
|
|
|
friend PathFile& operator<<(PathFile& pf, auto&& v)
|
|
{
|
|
pf._st << std::forward<decltype(v)>(v);
|
|
|
|
return pf;
|
|
}
|
|
|
|
bool save()
|
|
{
|
|
std::fstream fst;
|
|
|
|
if (_filename.empty()) {
|
|
return false;
|
|
}
|
|
|
|
if (_st.str().empty()) { // nothing to save
|
|
return true;
|
|
}
|
|
|
|
fst.open(_filename, std::ios::app);
|
|
|
|
if (!fst.is_open()) {
|
|
return false;
|
|
}
|
|
|
|
fst << _st.str();
|
|
_st.str("");
|
|
_filename.clear();
|
|
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
std::string _filename;
|
|
std::ostringstream _st;
|
|
};
|
|
|
|
|
|
public:
|
|
typedef MccError error_t;
|
|
|
|
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)
|
|
std::invocable<typename MOUNT_T::mount_status_t const&> CallbackFuncT =
|
|
decltype([](typename MOUNT_T::mount_status_t const&) {})>
|
|
MccSimpleMovementControls(
|
|
HARDWARE_T* hardware,
|
|
MOUNT_T* mount,
|
|
CallbackFuncT&& mode_switch_callback = [](typename MOUNT_T::mount_status_t const&) {})
|
|
: MccSimpleMovementControls(hardware, mount, mount, std::forward<CallbackFuncT>(mode_switch_callback), mount)
|
|
{
|
|
}
|
|
|
|
template <mcc_hardware_c HARDWARE_T,
|
|
mcc_telemetry_c TELEMETRY_T,
|
|
mcc_pzone_container_c PZONE_CONT_T,
|
|
typename CallbackFuncT,
|
|
mcc_logger_c LOGGER_T = MccNullLogger>
|
|
MccSimpleMovementControls(HARDWARE_T* hardware,
|
|
TELEMETRY_T* telemetry,
|
|
PZONE_CONT_T* pzone_cont,
|
|
CallbackFuncT&& mode_switch_callback,
|
|
LOGGER_T* logger = &details::NullLogger)
|
|
{
|
|
static_assert(traits::mcc_func_traits<CallbackFuncT>::arity == 1 &&
|
|
mcc_mount_status_c<std::decay_t<traits::mcc_func_arg1_t<CallbackFuncT>>>,
|
|
"INVALID 'CallbackFuncT' CALLABLE SIGNATURE!");
|
|
|
|
using STATUS_T = std::decay_t<traits::mcc_func_arg1_t<CallbackFuncT>>;
|
|
|
|
auto send_to_hardware = [hardware, logger](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t {
|
|
auto tp =
|
|
std::chrono::duration_cast<std::chrono::milliseconds>(hw_state.XY.epoch().UTC().time_since_epoch());
|
|
|
|
if constexpr (std::derived_from<decltype(hw_state.XY), MccCoordPair<MccAngleX, MccAngleY>>) {
|
|
logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})",
|
|
hw_state.XY.x().degrees(), hw_state.XY.y().degrees(), tp));
|
|
} else { // user defined mcc_coord_pair_c
|
|
logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})",
|
|
MccAngle((double)hw_state.XY.x()).degrees(),
|
|
MccAngle((double)hw_state.XY.y()).degrees(), tp));
|
|
}
|
|
|
|
auto hw_err = hardware->hardwareSetState(hw_state);
|
|
if (hw_err) {
|
|
mcc_deduced_err(hw_err, MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE);
|
|
}
|
|
|
|
logger->logDebug(" the 'hardwareSetState' method performed successfully!");
|
|
|
|
|
|
return MccSimpleMovementControlsErrorCode::ERROR_OK;
|
|
};
|
|
|
|
|
|
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;
|
|
|
|
// calculate the distances along the X and Y axes that the mount will travel at the current speed in a given
|
|
// time, taking into account the braking acceleration
|
|
double speedX = tdata.hwState.speedXY.x();
|
|
double speedY = tdata.hwState.speedXY.y();
|
|
|
|
// time to stop mount with given current speed and constant braking acceleration
|
|
double tx_stop = std::abs(speedX) / braking_accelX;
|
|
double ty_stop = std::abs(speedY) / braking_accelY;
|
|
|
|
double tx = min_time_to_pzone_in_secs;
|
|
double ty = min_time_to_pzone_in_secs;
|
|
|
|
if (std::isfinite(tx_stop) && (min_time_to_pzone_in_secs > tx_stop)) {
|
|
tx = tx_stop;
|
|
}
|
|
if (std::isfinite(ty_stop) && (min_time_to_pzone_in_secs > ty_stop)) {
|
|
ty = ty_stop;
|
|
}
|
|
|
|
// the distance:
|
|
// here, one must take into account the sign of the speed!!!
|
|
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;
|
|
|
|
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)));
|
|
|
|
|
|
// calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
|
|
// and check them for getting into the prohibited zones
|
|
|
|
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");
|
|
|
|
mount_pos.setX((double)tdata.hwState.XY.x() + tdata.pcmCorrection.pcmX + dx);
|
|
mount_pos.setY((double)tdata.hwState.XY.y() + tdata.pcmCorrection.pcmY + dy);
|
|
mount_pos.setEpoch(tdata.hwState.XY.epoch());
|
|
|
|
logger->logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s", MccAngleFancyString(speedX),
|
|
MccAngleFancyString(speedY)));
|
|
|
|
auto pz_err = pzone_cont->inPZone(MccSkyPoint(mount_pos), &in_zone, &in_zone_vec);
|
|
if (pz_err) {
|
|
return mcc_deduced_err(pz_err, MccSimpleMovementControlsErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
|
|
if (in_zone) {
|
|
size_t i = 0;
|
|
for (; i < in_zone_vec.size(); ++i) {
|
|
if (in_zone_vec[i]) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
auto names = pzone_cont->pzoneNames();
|
|
auto it = names.begin();
|
|
std::ranges::advance(it, i);
|
|
|
|
logger->logError(
|
|
"target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i,
|
|
*it);
|
|
|
|
MccSkyHADEC_OBS hadec;
|
|
MccSkyRADEC_OBS radec;
|
|
MccSkyAZZD azzd;
|
|
MccSkyAZALT azalt;
|
|
MccAngle lst;
|
|
|
|
auto ccte_err = tdata.mountPos.appSideralTime(&lst, true);
|
|
if (ccte_err) {
|
|
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
|
|
}
|
|
|
|
ccte_err = tdata.mountPos.toAtSameEpoch(radec, hadec, azzd, azalt);
|
|
if (ccte_err) {
|
|
return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
|
|
}
|
|
|
|
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()));
|
|
|
|
logger->logError(std::format(" hardware X, Y: {}, {}", MccAngle{tdata.hwState.XY.x()}.sexagesimal(),
|
|
MccAngle{tdata.hwState.XY.y()}.sexagesimal()));
|
|
|
|
return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE;
|
|
}
|
|
|
|
|
|
return MccSimpleMovementControlsErrorCode::ERROR_OK;
|
|
};
|
|
|
|
|
|
auto log_pos = [logger, this](typename TELEMETRY_T::telemetry_data_t const& tdata,
|
|
std::tuple<double, double, double>& dist) -> error_t {
|
|
double x_mnt, y_mnt, x_tag, y_tag;
|
|
|
|
std::conditional_t<
|
|
mccIsEquatorialMount(HARDWARE_T::hwMountType), MccSkyHADEC_OBS,
|
|
std::conditional_t<mccIsAltAzMount(HARDWARE_T::hwMountType), MccSkyAZALT, 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::hwMountType) ? "HA"
|
|
: mccIsAltAzMount(HARDWARE_T::hwMountType) ? "AZ"
|
|
: "GEN_X";
|
|
const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "DEC"
|
|
: mccIsAltAzMount(HARDWARE_T::hwMountType) ? "ALT"
|
|
: "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 = coord_pair.x();
|
|
y_tag = coord_pair.y();
|
|
|
|
logger->logTrace(std::format(" current target: {} = {}, {} = {} (encoders: {} {})", x_str,
|
|
coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal(),
|
|
tdata.targetXY.x().sexagesimal(), tdata.targetXY.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: {} = {}, {} = {} (encoders: {} {})", x_str,
|
|
coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal(),
|
|
tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal()));
|
|
|
|
x_mnt = coord_pair.x();
|
|
y_mnt = coord_pair.y();
|
|
|
|
dist = utils::distanceOnSphere(x_tag, y_tag, x_mnt, y_mnt);
|
|
|
|
_pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << " "
|
|
<< x_mnt << " " << y_mnt << " " << std::get<0>(dist) << " " << std::get<1>(dist) << " "
|
|
<< std::get<2>(dist) << " " << (int)tdata.hwState.movementState << "\n";
|
|
// _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";
|
|
|
|
logger->logTrace(std::format(" target-to-mount distance: {} (dx = {}, dy = {})",
|
|
MccAngleFancyString(std::get<2>(dist)), MccAngleFancyString(std::get<0>(dist)),
|
|
MccAngleFancyString(std::get<1>(dist))));
|
|
|
|
return MccSimpleMovementControlsErrorCode::ERROR_OK;
|
|
};
|
|
|
|
auto cb_sptr =
|
|
std::make_shared<std::function<void(STATUS_T const&)>>(std::forward<CallbackFuncT>(mode_switch_callback));
|
|
|
|
|
|
/* stop moving function */
|
|
|
|
_stopMovingFunc = [send_to_hardware, hardware, cb_sptr, this]() {
|
|
typename HARDWARE_T::hardware_state_t hw_state;
|
|
hw_state.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPING;
|
|
|
|
*_stopMoving = true;
|
|
|
|
*_lastError = send_to_hardware(hw_state);
|
|
|
|
if (_lastError->load()) {
|
|
(*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
|
|
logger->logInfo(std::format("Set slewing path filename to {}", _currentParams.slewingPathFilename));
|
|
_pathFile.setFilename(_currentParams.slewingPathFilename);
|
|
} else {
|
|
logger->logWarn("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}> <taget-mount-distance> <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;
|
|
}
|
|
|
|
using ep_t = decltype(hw_state.XY.epoch());
|
|
|
|
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());
|
|
hw_state.XY.setEpoch(ep_t::now());
|
|
|
|
// 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 = std::chrono::steady_clock::now();
|
|
auto last_hw_time = tdata.hwState.XY.epoch().UTC();
|
|
|
|
mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp;
|
|
|
|
std::tuple<double, double, double> dist;
|
|
|
|
*_stopMoving = false;
|
|
|
|
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_ADJUSTING;
|
|
|
|
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;
|
|
}
|
|
|
|
hw_state.XY.setX(tdata.targetXY.x());
|
|
hw_state.XY.setY(tdata.targetXY.y());
|
|
hw_state.XY.setEpoch(ep_t::now());
|
|
|
|
log_pos(tdata, dist);
|
|
|
|
// logger->logInfo("\tMNT.XY {} {}; MNT.HADEC {} {}", (double)tdata.hwState.XY.x(),
|
|
// (double)tdata.hwState.XY.y(), tdata.mountPos.co_lon(), tdata.mountPos.co_lat());
|
|
// logger->logInfo("\tTAG.XY {} {}; TAG.HADEC {} {}", (double)tdata.targetXY.x(),
|
|
// (double)tdata.targetXY.y(), tdata.targetPos.co_lon(), tdata.targetPos.co_lat());
|
|
|
|
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 (std::get<2>(dist) <= _currentParams.slewToleranceRadius) {
|
|
logger->logInfo("target-to-mount distance is lesser than slew acceptable radius - exit!");
|
|
|
|
break;
|
|
}
|
|
|
|
|
|
// start adjusting (precise pointing) the slewing
|
|
if (std::get<2>(dist) <= _currentParams.adjustCoordDiff) {
|
|
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ADJUSTING);
|
|
*_lastError = send_to_hardware(hw_state);
|
|
if (_lastError->load()) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) {
|
|
// the mount stopped but still to far from target position!!!
|
|
if (std::get<2>(dist) > _currentParams.slewToleranceRadius) {
|
|
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ADJUSTING);
|
|
|
|
*_lastError = send_to_hardware(hw_state);
|
|
if (_lastError->load()) {
|
|
break;
|
|
}
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_ERROR) {
|
|
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_HW_ERROR;
|
|
break;
|
|
}
|
|
|
|
|
|
/*
|
|
if (slew_and_stop) { // just wait until the mount stops
|
|
if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) {
|
|
// if (std::get<2>(dist) > _currentParams.slewToleranceRadius) {
|
|
// // 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());
|
|
// hw_state.XY.setEpoch(ep_t::now());
|
|
|
|
// *_lastError = send_to_hardware(hw_state);
|
|
// if (_lastError->load()) {
|
|
// break;
|
|
// }
|
|
// } else {
|
|
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();
|
|
|
|
// stop slewing and exit from the cycle?
|
|
if (std::get<2>(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) {
|
|
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());
|
|
hw_state.XY.setEpoch(ep_t::now());
|
|
|
|
*_lastError = send_to_hardware(hw_state);
|
|
if (_lastError->load()) {
|
|
break;
|
|
}
|
|
}
|
|
*/
|
|
|
|
// sleep here
|
|
std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
|
|
}
|
|
|
|
if (_stopMoving->load()) { // external stop
|
|
logger->logWarn("Slewing was stopped!");
|
|
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
|
|
_pathFile.save();
|
|
|
|
return;
|
|
}
|
|
|
|
*_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;
|
|
}
|
|
|
|
log_pos(tdata, dist);
|
|
|
|
if (!slew_and_stop) { // start tracking
|
|
_trackingFunc();
|
|
} else {
|
|
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
/* tracking function */
|
|
|
|
_trackingFunc = [log_pos, check_pzones, send_to_hardware, telemetry, cb_sptr, logger, this]() {
|
|
double braking_accelX, braking_accelY;
|
|
double min_time_to_pzone_in_secs;
|
|
|
|
|
|
logger->logInfo("Start tracking");
|
|
{
|
|
// 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
|
|
logger->logInfo(
|
|
std::format(" Set tracking filename to {}", _currentParams.trackingPathFilename));
|
|
_pathFile.setFilename(_currentParams.trackingPathFilename);
|
|
} else {
|
|
logger->logWarn(" Tracking path filename is empty! Do not save it!");
|
|
}
|
|
}
|
|
|
|
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));
|
|
logger->logInfo(
|
|
std::format(" max target-to-mount distance: {} arcseconds", _currentParams.trackingMaxCoordDiff));
|
|
|
|
|
|
_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}> <taget-mount-distance> <moving state>\n";
|
|
|
|
|
|
typename TELEMETRY_T::telemetry_data_t tdata;
|
|
typename HARDWARE_T::hardware_state_t hw_state;
|
|
std::tuple<double, double, double> dist;
|
|
|
|
|
|
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;
|
|
}
|
|
|
|
log_pos(tdata, dist);
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
if (_currentParams.trackingMaxCoordDiff < std::get<2>(dist)) {
|
|
logger->logInfo(
|
|
std::format("The target-to-mount distance {} is greater than allowed one ({})! Track current "
|
|
"mount position!",
|
|
MccAngleFancyString(std::get<2>(dist)),
|
|
MccAngleFancyString(_currentParams.trackingMaxCoordDiff)));
|
|
|
|
telemetry->setPointingTarget(tdata.mountPos);
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
*_stopMoving = false;
|
|
|
|
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, dist);
|
|
|
|
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();
|
|
|
|
|
|
// 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());
|
|
hw_state.XY.setEpoch(tdata.hwState.XY.epoch());
|
|
|
|
*_lastError = send_to_hardware(hw_state);
|
|
if (_lastError->load()) {
|
|
break;
|
|
}
|
|
|
|
if (*_stopMoving) {
|
|
break;
|
|
}
|
|
|
|
// sleep here
|
|
std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval);
|
|
}
|
|
|
|
if (_stopMoving->load()) { // external stop
|
|
logger->logWarn("Tracking was stopped!");
|
|
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
|
|
} else {
|
|
*_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;
|
|
|
|
MccSimpleMovementControls(const MccSimpleMovementControls&) = delete;
|
|
MccSimpleMovementControls(MccSimpleMovementControls&&) = default;
|
|
|
|
error_t slewToTarget(bool slew_and_stop = false)
|
|
{
|
|
_slewingFunc(slew_and_stop);
|
|
|
|
return *_lastError;
|
|
}
|
|
|
|
error_t trackTarget()
|
|
{
|
|
_trackingFunc();
|
|
|
|
return *_lastError;
|
|
}
|
|
|
|
|
|
error_t stopMount()
|
|
{
|
|
if (*_stopMoving) {
|
|
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_ALREADY_STOPPED;
|
|
} else {
|
|
_stopMovingFunc();
|
|
}
|
|
|
|
return *_lastError;
|
|
}
|
|
|
|
error_t setMovementParams(movement_params_t const& params)
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
_currentParams = params;
|
|
|
|
return MccSimpleMovementControlsErrorCode::ERROR_OK;
|
|
}
|
|
|
|
movement_params_t getMovementParams() const
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
return _currentParams;
|
|
}
|
|
|
|
error_t mountMovementLastError() const
|
|
{
|
|
return _lastError->load();
|
|
}
|
|
|
|
protected:
|
|
std::function<void(bool)> _slewingFunc{};
|
|
std::function<void()> _trackingFunc{};
|
|
std::function<void()> _stopMovingFunc{};
|
|
|
|
std::unique_ptr<std::atomic_bool> _stopMoving{new std::atomic_bool{false}};
|
|
|
|
std::unique_ptr<std::mutex> _currentParamsMutex{new std::mutex{}};
|
|
movement_params_t _currentParams{};
|
|
|
|
std::unique_ptr<std::atomic<error_t>> _lastError{
|
|
new std::atomic<error_t>{MccSimpleMovementControlsErrorCode::ERROR_OK}};
|
|
|
|
PathFile _pathFile{};
|
|
};
|
|
|
|
} // namespace mcc::impl
|