Files
mcc/include/mcc/mcc_generic_movecontrols.h

599 lines
21 KiB
C++

#pragma once
/****************************************************************************************
* *
* MOUNT CONTROL COMPONENTS LIBRARY *
* *
* *
* GENERIC IMPLEMENTATION OF MOUNT MOVEMENT CONTROLS *
* *
****************************************************************************************/
#include <atomic>
#include <fstream>
#include <future>
#include <thread>
#include <type_traits>
#include "mcc_coordinate.h"
#include "mcc_error.h"
namespace mcc::impl
{
// mount movement-related generic errors
enum class MccGenericMovementControlsErrorCode : int {
ERROR_OK,
ERROR_IN_PZONE,
ERROR_NEAR_PZONE,
ERROR_SLEW_TIMEOUT,
ERROR_STOP_TIMEOUT,
ERROR_HARDWARE,
ERROR_TELEMETRY_TIMEOUT
};
} // namespace mcc::impl
namespace std
{
template <>
class is_error_code_enum<mcc::impl::MccGenericMovementControlsErrorCode> : public true_type
{
};
} // namespace std
namespace mcc::impl
{
// error category
struct MccGenericMovementControlsErrorCategory : std::error_category {
const char* name() const noexcept
{
return "MCC-GENERIC-MOVECONTRL";
}
std::string message(int ec) const
{
MccGenericMovementControlsErrorCode err = static_cast<MccGenericMovementControlsErrorCode>(ec);
switch (err) {
case MccGenericMovementControlsErrorCode::ERROR_OK:
return "OK";
case MccGenericMovementControlsErrorCode::ERROR_IN_PZONE:
return "target is in zone";
case MccGenericMovementControlsErrorCode::ERROR_NEAR_PZONE:
return "mount is near zone";
case MccGenericMovementControlsErrorCode::ERROR_SLEW_TIMEOUT:
return "a timeout occured while slewing";
case MccGenericMovementControlsErrorCode::ERROR_STOP_TIMEOUT:
return "a timeout occured while mount stopping";
case MccGenericMovementControlsErrorCode::ERROR_HARDWARE:
return "a hardware error occured";
case MccGenericMovementControlsErrorCode::ERROR_TELEMETRY_TIMEOUT:
return "telemetry data timeout";
default:
return "unknown";
}
}
static const MccGenericMovementControlsErrorCategory& get()
{
static const MccGenericMovementControlsErrorCategory constInst;
return constInst;
}
};
inline std::error_code make_error_code(MccGenericMovementControlsErrorCode ec)
{
return std::error_code(static_cast<int>(ec), MccGenericMovementControlsErrorCategory::get());
}
struct MccGenericMovementControlsParams {
// timeout to telemetry updating
std::chrono::milliseconds telemetryTimeout{3000};
// 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 mode *******
// coordinates difference to stop slewing (in radians)
double slewToleranceRadius{5.0_arcsecs};
// slewing trajectory file. if empty - just skip saving
std::string slewingPathFilename{};
// ******* tracking mode *******
// 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{};
};
/* UTILITY CLASS TO HOLD AND SAVE MOUNT MOVING TRAJECTORY */
struct MccMovementPathFile {
static constexpr std::string_view commentSeq{"# "};
static constexpr std::string_view lineDelim{"\n"};
void setCommentSeq(traits::mcc_input_char_range auto const& s)
{
_commentSeq.clear();
std::ranges::copy(s, std::back_inserter(_commentSeq));
}
void setLineDelim(traits::mcc_input_char_range auto const& s)
{
_lineDelim.clear();
std::ranges::copy(s, std::back_inserter(_lineDelim));
}
// add comment string/strings
template <traits::mcc_input_char_range RT, traits::mcc_input_char_range... RTs>
void addComment(RT const& r, RTs const&... rs)
{
std::ranges::copy(_commentSeq, std::back_inserter(_buffer));
std::ranges::copy(r, std::back_inserter(_buffer));
std::ranges::copy(lineDelim, std::back_inserter(_buffer));
if constexpr (sizeof...(RTs)) {
addComment(rs...);
}
}
// comment corresponded to addToPath(mcc_telemetry_data_c auto const& tdata)
void addDefaultComment()
{
addComment("Format (time is in milliseconds, coordinates are in degrees):");
addComment(
" <UNIXTIME> <mount X> <mount Y> <target X> <target Y> <dX_{mount-target}> "
"<dY_{mount-target}> <mount-to-target-distance> <moving state>");
}
// general purpose method
// template <std::formattable<char>... ArgTs>
// void addToPath(std::format_string<ArgTs...> fmt, ArgTs&&... args)
// {
// std::format_to(std::back_inserter(_buffer), fmt, std::forward<ArgTs>(args)...);
// std::ranges::copy(lineDelim, std::back_inserter(_buffer));
// }
// general purpose method
template <std::formattable<char>... ArgTs>
void addToPath(std::string_view fmt, ArgTs&... args)
{
std::vformat_to(std::back_inserter(_buffer), fmt, std::make_format_args(args...));
std::ranges::copy(lineDelim, std::back_inserter(_buffer));
}
// default-implemented method
void addToPath(mcc_telemetry_data_c auto const& tdata)
{
// UNIX-time millisecs, mount X, mount Y, target X, target Y, dX(mount-target), dY(mount-target), dist, state
auto dist = tdata.mountPos.distance(tdata.targetPos);
using d_t = std::chrono::milliseconds;
auto tp = std::chrono::duration_cast<d_t>(tdata.mountPos.epoch().UTC().time_since_epoch());
const std::string_view d_fmt = "{:14.8f}";
const auto v = std::views::repeat(d_fmt, 7) | std::views::join_with(' ');
std::string fmt = "{} " + std::string(v.begin(), v.end()) + " {}";
int state = (int)tdata.hwState.movementState;
auto tp_val = tp.count();
double mnt_x = MccAngle(tdata.mountPos.co_lon()).degrees(), mnt_y = MccAngle(tdata.mountPos.co_lon()).degrees(),
tag_x = dist.x2.degrees(), tag_y = dist.y2.degrees(), dx = dist.dy.degrees(), dy = dist.dy.degrees(),
dd = dist.dist.degrees();
addToPath(std::string_view(fmt.begin(), fmt.end()), tp_val, mnt_x, mnt_y, tag_x, tag_y, dx, dy, dd, state);
}
void clearPath()
{
_buffer.clear();
}
bool saveToFile(std::string const& filename, std::ios_base::openmode mode = std::ios::out | std::ios::trunc)
{
if (filename.empty()) {
return true;
}
std::ofstream fst(filename, mode);
if (fst.is_open()) {
fst << _buffer;
return true;
} else {
return false;
}
}
protected:
std::string _buffer{};
std::string _commentSeq{commentSeq};
std::string _lineDelim{lineDelim};
};
enum class MccGenericMovementControlsPolicy : int { POLICY_ASYNC, POLICY_BLOCKING };
template <std::movable PARAMS_T,
MccGenericMovementControlsPolicy EXEC_POLICY = MccGenericMovementControlsPolicy::POLICY_ASYNC>
class MccGenericMovementControls
{
enum { STATE_SLEW, STATE_TRACK, STATE_IDLE } _currentState;
public:
static constexpr MccGenericMovementControlsPolicy executePolicy = EXEC_POLICY;
static constexpr std::chrono::seconds defaultWaitTimeout{3};
typedef MccError error_t;
typedef PARAMS_T movement_params_t;
template <std::invocable<bool> SLEW_FUNC_T, std::invocable<> TRACK_FUNC_T, std::invocable<> STOP_FUNC_T>
MccGenericMovementControls(SLEW_FUNC_T&& slew_func, TRACK_FUNC_T&& track_func, STOP_FUNC_T&& stop_func)
: _slewFunc(std::forward<SLEW_FUNC_T>(slew_func)),
_trackFunc(std::forward<TRACK_FUNC_T>(track_func)),
_stopFunc(std::forward<STOP_FUNC_T>(stop_func))
{
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
*_doSlew = false;
*_doTrack = false;
*_stopMovementRequest = false;
_currentState = STATE_IDLE;
_fstFuture = std::async(
[&, this](std::stop_token stoken) {
while (!stoken.stop_requested()) {
if (_stopMovementRequest->load()) {
_currentState = STATE_IDLE;
_stopFunc();
}
if (_doSlew->load()) {
_currentState = STATE_SLEW;
_slewFunc(_slewAndStop->load());
_currentState = STATE_IDLE;
}
if (_doTrack->load()) {
_currentState = STATE_TRACK;
_trackFunc();
_currentState = STATE_IDLE;
}
// wait here ...
_wakeupRequest->wait(false, std::memory_order_relaxed);
_wakeupRequest->clear();
}
},
_fstStopSource.get_token());
}
}
MccGenericMovementControls(const MccGenericMovementControls&) = delete;
MccGenericMovementControls(MccGenericMovementControls&& other) = default;
MccGenericMovementControls& operator=(const MccGenericMovementControls&) = delete;
MccGenericMovementControls& operator=(MccGenericMovementControls&&) = default;
virtual ~MccGenericMovementControls()
{
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
// if (_slewFuncFuture.valid()) {
// auto status = _slewFuncFuture.wait_for(_waitTimeout->load());
// }
// if (_trackFuncFuture.valid()) {
// auto status = _trackFuncFuture.wait_for(_waitTimeout->load());
// }
// if (_stopFuncFuture.valid()) {
// auto status = _stopFuncFuture.wait_for(_waitTimeout->load());
// }
*_doSlew = false;
*_doTrack = false;
// *_stopMovementRequest = false;
_fstStopSource.request_stop();
_wakeupRequest->test_and_set();
_wakeupRequest->notify_one();
std::this_thread::sleep_for(std::chrono::milliseconds(200));
if (_fstFuture.valid()) {
auto status = _fstFuture.wait_for(_waitTimeout->load());
}
}
}
error_t slewToTarget(bool slew_and_stop)
{
*_stopMovementRequest = false;
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
// if (_slewFuncFuture.valid()) { // already slewing
// _slewFuncFuture = std::async(
// std::launch::async,
// [this](bool st) {
// auto err = _stopFunc(); // first, stop mount
// if (!err) {
// *_stopMovementRequest = false;
// return _slewFunc(st);
// } else {
// return err;
// }
// },
// slew_and_stop);
// } else {
// _slewFuncFuture = std::async(std::launch::async, _slewFunc, slew_and_stop);
// }
if (_currentState == STATE_SLEW || _currentState == STATE_TRACK) { // first, stop mount
*_stopMovementRequest = true;
}
*_doTrack = false;
*_doSlew = true;
*_slewAndStop = slew_and_stop;
_wakeupRequest->test_and_set();
_wakeupRequest->notify_one();
return MccGenericMovementControlsErrorCode::ERROR_OK;
} else if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_BLOCKING) {
return _slewFunc(slew_and_stop);
} else {
static_assert(false, "UNKNOWN EXECUTION POLICY!");
}
}
error_t trackTarget()
{
*_stopMovementRequest = false;
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
// if (!_trackFuncFuture.valid()) {
// _trackFuncFuture = std::async(std::launch::async, _trackFunc);
// } // already tracking, just ignore
if (_currentState != STATE_TRACK) {
if (_currentState == STATE_SLEW) { // first, stop mount
*_stopMovementRequest = true;
}
*_doSlew = false;
*_doTrack = true;
_wakeupRequest->test_and_set();
_wakeupRequest->notify_one();
} // already tracking, just ignore
return MccGenericMovementControlsErrorCode::ERROR_OK;
} else if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_BLOCKING) {
return _trackFunc();
} else {
static_assert(false, "UNKNOWN EXECUTION POLICY!");
}
}
error_t stopMount()
{
*_stopMovementRequest = true;
if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
// if future is valid then stop is already called
// if (!_stopFuncFuture.valid()) {
// _stopFuncFuture = std::async(std::launch::async, _stopFunc);
// }
*_doSlew = false;
*_doTrack = false;
_wakeupRequest->test_and_set();
_wakeupRequest->notify_one();
return MccGenericMovementControlsErrorCode::ERROR_OK;
} else if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_BLOCKING) {
return _stopFunc();
} else {
static_assert(false, "UNKNOWN EXECUTION POLICY!");
}
}
error_t setMovementParams(movement_params_t const& pars)
{
std::lock_guard lock{*_currentMovementParamsMutex};
_currentMovementParams = pars;
return MccGenericMovementControlsErrorCode::ERROR_OK;
}
movement_params_t getMovementParams() const
{
std::lock_guard lock{*_currentMovementParamsMutex};
return _currentMovementParams;
}
protected:
std::unique_ptr<std::mutex> _currentMovementParamsMutex{new std::mutex{}};
PARAMS_T _currentMovementParams{};
std::unique_ptr<std::atomic_bool> _stopMovementRequest{new std::atomic_bool{false}};
std::function<error_t(bool)> _slewFunc{};
std::function<error_t()> _trackFunc{};
std::function<error_t()> _stopFunc{};
std::conditional_t<executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC,
std::future<error_t>,
std::nullptr_t>
_slewFuncFuture{};
std::conditional_t<executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC,
std::future<error_t>,
std::nullptr_t>
_trackFuncFuture{};
std::conditional_t<executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC,
std::future<error_t>,
std::nullptr_t>
_stopFuncFuture{};
std::unique_ptr<std::atomic<std::chrono::nanoseconds>> _waitTimeout{
new std::atomic<std::chrono::nanoseconds>{defaultWaitTimeout}};
std::conditional_t<executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC,
std::future<void>,
std::nullptr_t>
_fstFuture{};
std::stop_source _fstStopSource{};
std::unique_ptr<std::atomic_flag> _wakeupRequest{new std::atomic_flag};
std::unique_ptr<std::atomic_bool> _doSlew{new std::atomic_bool{false}};
std::unique_ptr<std::atomic_bool> _doTrack{new std::atomic_bool{false}};
std::unique_ptr<std::atomic_bool> _slewAndStop{new std::atomic_bool{false}};
// utility methods
// the method calculates the change in coordinates of a point over a given time given the current speed and braking
// acceleration. a position after given 'time' interval is returned
auto coordsAfterTime(mcc_coord_pair_c auto const& cp,
mcc_coord_pair_c auto const& speedXY, // in radians per seconds
mcc_coord_pair_c auto const& braking_accelXY, // in radians per seconds in square
traits::mcc_time_duration_c auto const& time,
mcc_coord_pair_c auto* dxy = nullptr)
{
// time to stop mount with given current speed and constant braking acceleration
double tx_stop = std::abs(speedXY.x()) / braking_accelXY.x();
double ty_stop = std::abs(speedXY.y()) / braking_accelXY.y();
using secs_t = std::chrono::duration<double>; // seconds as double
double tx = std::chrono::duration_cast<secs_t>(time).count();
double ty = std::chrono::duration_cast<secs_t>(time).count();
if (std::isfinite(tx_stop) && (tx > tx_stop)) {
tx = tx_stop;
}
if (std::isfinite(ty_stop) && (ty > ty_stop)) {
ty = ty_stop;
}
// the distance:
// here, one must take into account the sign of the speed!!!
double dx = speedXY.x() * tx - std::copysign(braking_accelXY.x(), speedXY.x()) * tx * tx / 2.0;
double dy = speedXY.y() * ty - std::copysign(braking_accelXY.y(), speedXY.y()) * ty * ty / 2.0;
std::remove_cvref_t<decltype(cp)> cp_res{};
cp_res.setEpoch(cp.epoch() + time);
cp_res.setX((double)cp.x() + dx);
cp_res.setY((double)cp.y() + dy);
if (dxy) {
dxy->setX(dx);
dxy->setY(dy);
dxy->setEpoch(cp_res.epoch());
}
return cp_res;
}
auto coordsAfterTime(mcc_skypoint_c auto const& sp,
mcc_coord_pair_c auto const& speedXY, // in radians per seconds
mcc_coord_pair_c auto const& braking_accelXY, // in radians per seconds in square
traits::mcc_time_duration_c auto const& time,
mcc_coord_pair_c auto* dxy = nullptr)
{
auto run_func = [&, this](auto& cp) {
sp.toAtSameEpoch(cp);
auto new_cp = coordsAfterTime(cp, speedXY, braking_accelXY, time, dxy);
std::remove_cvref_t<decltype(sp)> sp_res{};
sp_res.from(cp);
return sp_res;
};
switch (sp.pairKind()) {
case MccCoordPairKind::COORDS_KIND_RADEC_ICRS: {
MccSkyRADEC_ICRS rd;
return run_func(rd);
};
case MccCoordPairKind::COORDS_KIND_RADEC_OBS: {
MccSkyRADEC_OBS rd;
return run_func(rd);
};
case MccCoordPairKind::COORDS_KIND_RADEC_APP: {
MccSkyRADEC_APP rd;
return run_func(rd);
};
case MccCoordPairKind::COORDS_KIND_HADEC_OBS: {
MccSkyHADEC_OBS hd;
return run_func(hd);
};
case MccCoordPairKind::COORDS_KIND_HADEC_APP: {
MccSkyHADEC_APP hd;
return run_func(hd);
};
case MccCoordPairKind::COORDS_KIND_AZZD: {
MccSkyAZZD azzd;
return run_func(azzd);
};
case MccCoordPairKind::COORDS_KIND_AZALT: {
MccSkyAZALT azalt;
return run_func(azalt);
};
case MccCoordPairKind::COORDS_KIND_GENERIC:
case MccCoordPairKind::COORDS_KIND_XY: {
MccGenXY xy;
return run_func(xy);
};
default:
return sp;
}
}
};
template <std::default_initializable PARAMS_T>
using MccGenericBlockingMovementControls =
MccGenericMovementControls<PARAMS_T, MccGenericMovementControlsPolicy::POLICY_BLOCKING>;
template <std::default_initializable PARAMS_T>
using MccGenericAsyncMovementControls =
MccGenericMovementControls<PARAMS_T, MccGenericMovementControlsPolicy::POLICY_ASYNC>;
static_assert(mcc_movement_controls_c<MccGenericAsyncMovementControls<MccGenericMovementControlsParams>>, "!!!");
} // namespace mcc::impl