815 lines
30 KiB
C++
815 lines
30 KiB
C++
#pragma once
|
|
|
|
#include <fstream>
|
|
|
|
#include "mcc_defaults.h"
|
|
#include "mcc_generics.h"
|
|
#include "mcc_moving_model_common.h"
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
enum class MccSimpleMovingControlsErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_HW_GETSTATE,
|
|
ERROR_HW_SETSTATE,
|
|
ERROR_PCM_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
|
|
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::MccSimpleMovingControlsErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
// error category
|
|
struct MccSimpleMovingControlsCategory : public std::error_category {
|
|
MccSimpleMovingControlsCategory() : std::error_category() {}
|
|
|
|
const char* name() const noexcept
|
|
{
|
|
return "SIMPLE-SLEWING-MODEL";
|
|
}
|
|
|
|
std::string message(int ec) const
|
|
{
|
|
MccSimpleMovingControlsErrorCode err = static_cast<MccSimpleMovingControlsErrorCode>(ec);
|
|
|
|
switch (err) {
|
|
case MccSimpleMovingControlsErrorCode::ERROR_OK:
|
|
return "OK";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE:
|
|
return "cannot get hardware state";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE:
|
|
return "cannot set hardware state";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_PCM_COMP:
|
|
return "PCM computation error";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY:
|
|
return "cannot get telemetry";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY:
|
|
return "cannot get target-to-mount-position distance";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_PZONE_CONTAINER_COMP:
|
|
return "pzone container computation error";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_TARGET_IN_PZONE:
|
|
return "target is in prohibited zone";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_NEAR_PZONE:
|
|
return "near prohibited zone";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_TIMEOUT:
|
|
return "a timeout occured while slewing";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_ALREADY_SLEW:
|
|
return "already slewing";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED:
|
|
return "slewing is already stopped";
|
|
case MccSimpleMovingControlsErrorCode::ERROR_STOPPED:
|
|
return "slewing was stopped";
|
|
default:
|
|
return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
static const MccSimpleMovingControlsCategory& get()
|
|
{
|
|
static const MccSimpleMovingControlsCategory constInst;
|
|
return constInst;
|
|
}
|
|
};
|
|
|
|
|
|
inline std::error_code make_error_code(MccSimpleMovingControlsErrorCode ec)
|
|
{
|
|
return std::error_code(static_cast<int>(ec), MccSimpleMovingControlsCategory::get());
|
|
}
|
|
|
|
|
|
|
|
class MccSimpleMovingControls
|
|
{
|
|
static constexpr auto DEG90INRADS = std::numbers::pi / 2.0;
|
|
|
|
class PathFile
|
|
{
|
|
public:
|
|
PathFile(const std::string& filename = "") : _filename(filename), _st() {}
|
|
|
|
void setFilename(const std::string& filename)
|
|
{
|
|
_filename = filename;
|
|
}
|
|
|
|
std::string getFilename() const
|
|
{
|
|
return _filename;
|
|
}
|
|
|
|
~PathFile()
|
|
{
|
|
save();
|
|
}
|
|
|
|
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);
|
|
|
|
if (!fst.is_open()) {
|
|
return false;
|
|
}
|
|
|
|
fst << _st.str();
|
|
_st.str("");
|
|
|
|
_filename.clear();
|
|
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
std::string _filename;
|
|
std::istringstream _st;
|
|
};
|
|
|
|
public:
|
|
typedef std::error_code error_t;
|
|
|
|
typedef MccSimpleMovingModelParams moving_params_t;
|
|
|
|
enum Mode { MOVING_MODE_SLEW, MOVING_MODE_TRACK, MOVING_MODE_ERROR };
|
|
|
|
// typedef std::CallbackFuncTion<void(Mode mode)> mode_switch_callback_t;
|
|
|
|
// protected:
|
|
// constexpr static auto defaultModeSwitchCallback = [](Mode) {};
|
|
|
|
// public:
|
|
|
|
template <mcc_generic_mount_c MountT,
|
|
std::invocable<typename MountT::mount_status_t> CallbackFuncT =
|
|
decltype([](typename MountT::mount_status_t) {})>
|
|
MccSimpleMovingControls(
|
|
MountT* mount,
|
|
CallbackFuncT&& mode_switch_callback = [](typename MountT::mount_status_t) {})
|
|
: _stopMoving(new std::atomic_bool), _currentParamsMutex(new std::mutex), _lastError(new std::atomic<error_t>)
|
|
{
|
|
auto send_to_hardware = [mount](typename MountT::hardware_state_t const& hw_state) {
|
|
mount->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
|
|
mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));
|
|
|
|
auto hw_err = mount->hardwareSetState(hw_state);
|
|
if (hw_err) {
|
|
return mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE);
|
|
}
|
|
|
|
mount->logDebug(" the 'hardwareSetState' method performed successfully!");
|
|
|
|
return MccSimpleMovingControlsErrorCode::ERROR_OK;
|
|
};
|
|
|
|
|
|
auto check_pzones = [mount, this](MccTelemetryData const& tdata, double min_time_to_pzone_in_secs,
|
|
double braking_accelX, double braking_accelY) {
|
|
bool in_zone;
|
|
std::vector<bool> in_zone_vec;
|
|
MccCelestialPoint cpt;
|
|
|
|
auto distXY = mcc_compute_distance(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
|
|
|
|
mount->logTrace(
|
|
std::format(" the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}",
|
|
min_time_to_pzone_in_secs, mcc::MccAngleFancyString(distXY.first),
|
|
mcc::MccAngleFancyString(distXY.second)));
|
|
|
|
// calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
|
|
// and check them for getting into the prohibited zones
|
|
if constexpr (mccIsEquatorialMount(MountT::mountType)) {
|
|
cpt.X = tdata.HA + distXY.first;
|
|
cpt.Y = tdata.DEC_APP + distXY.second;
|
|
|
|
if (cpt.Y > DEG90INRADS) {
|
|
cpt.Y = DEG90INRADS;
|
|
}
|
|
if (cpt.Y < -DEG90INRADS) {
|
|
cpt.Y = -DEG90INRADS;
|
|
}
|
|
|
|
} else if constexpr (mccIsAltAzMount(MountT::mountType)) {
|
|
cpt.X = tdata.AZ + distXY.first;
|
|
cpt.Y = tdata.ZD + distXY.second;
|
|
if (cpt.Y < 0.0) {
|
|
cpt.Y = 0.0;
|
|
}
|
|
if (cpt.Y > std::numbers::pi) {
|
|
cpt.Y = std::numbers::pi;
|
|
}
|
|
}
|
|
mcc_tp2tp(tdata.time_point, cpt.time_point);
|
|
|
|
mount->logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s",
|
|
mcc::MccAngleFancyString(tdata.speedX),
|
|
mcc::MccAngleFancyString(tdata.speedY)));
|
|
|
|
in_zone_vec.clear();
|
|
auto pz_err = mount->inPZone(cpt, &in_zone, &in_zone_vec);
|
|
if (pz_err) {
|
|
return mcc_deduce_error_code(pz_err, MccSimpleMovingControlsErrorCode::ERROR_PZONE_CONTAINER_COMP);
|
|
}
|
|
|
|
if (in_zone) {
|
|
size_t i = 0;
|
|
for (; i < in_zone_vec.size(); ++i) {
|
|
if (in_zone_vec[i]) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
mount->logError("target point is near prohibited zone (zone index: {})! Entered target coordinates:",
|
|
i);
|
|
mount->logError(std::format(
|
|
" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", mcc::MccAngle{tdata.RA_APP}.sexagesimal(true),
|
|
mcc::MccAngle{tdata.DEC_APP}.sexagesimal(), mcc::MccAngle{tdata.HA}.sexagesimal(true),
|
|
mcc::MccAngle{tdata.LST}.sexagesimal(true)));
|
|
mount->logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.AZ}.sexagesimal(),
|
|
mcc::MccAngle{tdata.ZD}.sexagesimal(),
|
|
mcc::MccAngle{tdata.ALT}.sexagesimal()));
|
|
|
|
mount->logError(std::format(" hardware X, Y: {}, {}", mcc::MccAngle{tdata.X}.sexagesimal(),
|
|
mcc::MccAngle{tdata.Y}.sexagesimal()));
|
|
|
|
return MccSimpleMovingControlsErrorCode::ERROR_NEAR_PZONE;
|
|
}
|
|
|
|
return MccSimpleMovingControlsErrorCode::ERROR_OK;
|
|
};
|
|
|
|
auto log_pos = [mount, this](typename MountT::hardware_state_t const& hw_state, MccTelemetryData const& tdata) {
|
|
if constexpr (mccIsEquatorialMount(MountT::mountType)) {
|
|
mount->logTrace(std::format(" current target: HA = {}, DEC = {}",
|
|
mcc::MccAngle(tdata.target.HA).sexagesimal(true),
|
|
mcc::MccAngle(tdata.target.DEC_APP).sexagesimal()));
|
|
mount->logTrace(std::format(" current mount: HA = {}, DEC = {}",
|
|
mcc::MccAngle(tdata.HA).sexagesimal(true),
|
|
mcc::MccAngle(tdata.DEC_APP).sexagesimal()));
|
|
|
|
_pathFile << tdata.time_point.time_since_epoch().count() << " " << tdata.target.HA << " "
|
|
<< tdata.target.DEC_APP << " " << tdata.HA << " " << tdata.DEC_APP << " "
|
|
<< (tdata.target.HA - tdata.HA) << " " << (tdata.target.DEC_APP - tdata.DEC_APP) << " "
|
|
<< (int)hw_state.moving_state << "\n";
|
|
|
|
} else if constexpr (mccIsAltAzMount(MountT::mountType)) {
|
|
mount->logTrace(std::format(" target: AZ = {}, ZD = {}",
|
|
mcc::MccAngle(tdata.target.AZ).sexagesimal(),
|
|
mcc::MccAngle(tdata.target.ZD).sexagesimal()));
|
|
mount->logTrace(std::format(" mount: AZ = {}, ZD = {}", mcc::MccAngle(tdata.AZ).sexagesimal(),
|
|
mcc::MccAngle(tdata.ZD).sexagesimal()));
|
|
|
|
_pathFile << tdata.time_point.time_since_epoch().count() << " " << tdata.target.AZ << " "
|
|
<< tdata.target.ZD << " " << tdata.AZ << " " << tdata.ZD << " "
|
|
<< (tdata.target.AZ - tdata.AZ) << " " << (tdata.target.ZD - tdata.ZD) << " "
|
|
<< (int)hw_state.moving_state << "\n";
|
|
}
|
|
};
|
|
|
|
*_stopMoving = true;
|
|
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_OK;
|
|
|
|
using cb_func_t = std::function<void(typename MountT::mount_status_t)>;
|
|
auto cb_sptr = std::shared_ptr<cb_func_t>(new cb_func_t(std::forward<CallbackFuncT>(mode_switch_callback)));
|
|
|
|
|
|
/* stop moving function */
|
|
|
|
_stopMovingFunc = [mount, this]() {
|
|
typename MountT::hardware_state_t hw_state;
|
|
hw_state.moving_state == MountT::hardware_moving_state_t::HW_MOVE_STOPPED;
|
|
|
|
*_stopMoving = true;
|
|
|
|
*_lastError = send_to_hardware(hw_state);
|
|
};
|
|
|
|
|
|
/* slewing function */
|
|
|
|
_slewingFunc = [mount, cb_sptr, send_to_hardware, check_pzones, log_pos, 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
|
|
_pathFile.setFilename(_currentParams.slewingPathFilename);
|
|
} else {
|
|
mount->logError("Slewing path filename is empty! Do not save it!");
|
|
}
|
|
}
|
|
|
|
mount->logInfo(
|
|
std::format("Start slewing in mode '{}'", (slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK")));
|
|
mount->logInfo(std::format(" slewing process timeout: {} secs", _currentParams.slewTimeout.count()));
|
|
if (!slew_and_stop) {
|
|
mount->logInfo(std::format(" slewing tolerance radius: {} arcsecs",
|
|
mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs()));
|
|
}
|
|
mount->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
|
|
mcc::MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
|
|
mount->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
|
|
mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
|
|
mount->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: "
|
|
<< mcc::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}> <moving state>\n";
|
|
|
|
|
|
typename MountT::error_t t_err;
|
|
MccTelemetryData tdata;
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
t_err = mount->telemetryData(&tdata);
|
|
|
|
if (t_err) {
|
|
*_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
|
|
return;
|
|
}
|
|
}
|
|
|
|
auto last_hw_time = tdata.time_point;
|
|
|
|
// bool in_zone;
|
|
// std::vector<bool> in_zone_vec;
|
|
|
|
// MccCelestialPoint cpt;
|
|
|
|
// if constexpr (mccIsEquatorialMount(MountT::mountType)) {
|
|
// cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
|
// } else if constexpr (mccIsAltAzMount(MountT::mountType)) {
|
|
// cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
|
// } else {
|
|
// static_assert(false, "UNKNOWN MOUNT TYPE!");
|
|
// }
|
|
|
|
typename MountT::hardware_state_t hw_state;
|
|
|
|
auto hw_err = mount->hardwareGetState(&hw_state);
|
|
if (hw_err) {
|
|
*_stopMoving = true;
|
|
*_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
|
|
return;
|
|
}
|
|
|
|
|
|
hw_state.X = (double)tdata.target.X;
|
|
hw_state.Y = (double)tdata.target.Y;
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
hw_state.speedX = _currentParams.slewRateX;
|
|
hw_state.speedY = _currentParams.slewRateY;
|
|
}
|
|
hw_state.moving_state = MountT::hardware_moving_state_t::HW_MOVE_SLEWING;
|
|
|
|
// start slewing ...
|
|
error_t err = send_to_hardware(hw_state);
|
|
if (err) {
|
|
*_lastError = err;
|
|
mount->logError(std::format("start slewing: an error occured while sending hardware state: {} {} {}",
|
|
err.value(), err.category().name(), err.message()));
|
|
return;
|
|
}
|
|
|
|
*cb_sptr(MountT::mount_status_t::SLEWING); // send the status to the mount
|
|
|
|
|
|
double dist;
|
|
|
|
|
|
std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
|
|
start_slewing_tp = std::chrono::steady_clock::now();
|
|
last_adjust_tp = start_slewing_tp;
|
|
|
|
std::pair<double, double> distXY;
|
|
bool tag_var_coord = true;
|
|
|
|
if (tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT ||
|
|
tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
|
|
tag_var_coord = false;
|
|
}
|
|
|
|
auto start_point = tdata.time_point; // needed for trajectory file
|
|
|
|
// main loop (simply monitors the current position taking into account the prohibited zones, as well as the
|
|
// timeout of the entire process)
|
|
while (!*_stopMoving) {
|
|
// wait for updated telemetry data
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
|
|
|
|
if (t_err) {
|
|
*_lastError =
|
|
mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
|
|
break;
|
|
}
|
|
|
|
last_hw_time = tdata.time_point;
|
|
}
|
|
|
|
hw_err = mount->hardwareGetState(&hw_state);
|
|
if (hw_err) {
|
|
*_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
|
|
break;
|
|
}
|
|
|
|
log_pos(hw_state, tdata);
|
|
|
|
if (*_stopMoving) {
|
|
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
|
|
break;
|
|
}
|
|
|
|
err = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
|
|
if (err) {
|
|
*_lastError = err;
|
|
break;
|
|
}
|
|
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
if ((std::chrono::steady_clock::now() - start_slewing_tp) > _currentParams.slewTimeout) {
|
|
mount->logError("slewing process timeout!");
|
|
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_TIMEOUT;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (slew_and_stop && !tag_var_coord) { // just wait for mount to be stopped
|
|
if (hw_state.moving_state == MountT::hardware_moving_state_t::HW_MOVE_STOPPED) {
|
|
mount->logInfo("mount moving state is STOPPED - exit!");
|
|
break;
|
|
}
|
|
} else {
|
|
if (last_hw_time == tdata.time_point) {
|
|
mount->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
|
|
continue;
|
|
}
|
|
|
|
last_hw_time = tdata.time_point;
|
|
|
|
t_err = mount->targetToMountDist(&dist);
|
|
if (t_err) {
|
|
*_lastError =
|
|
mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY);
|
|
break;
|
|
}
|
|
|
|
mount->logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist)));
|
|
|
|
if ((dist <= _currentParams.slewToleranceRadius) &&
|
|
(hw_state.moving_state ==
|
|
MountT::hardware_moving_state_t::HW_MOVE_GUIDING)) { // stop slewing and exit from
|
|
// cycle
|
|
mount->logInfo("target-to-mount distance is lesser than slew tolerance radius - exit!");
|
|
|
|
if (slew_and_stop) {
|
|
stopMount();
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
|
|
if (*_stopMoving) {
|
|
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
|
|
break;
|
|
// return MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
|
|
}
|
|
|
|
|
|
// resend new position since target coordinates are changed in time
|
|
hw_state.X = (double)tdata.target.X;
|
|
hw_state.Y = (double)tdata.target.Y;
|
|
|
|
err = send_to_hardware(hw_state);
|
|
if (err) {
|
|
*_lastError = err;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (*_stopMoving) {
|
|
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
|
|
break;
|
|
}
|
|
|
|
// sleep here
|
|
std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
|
|
}
|
|
|
|
*_stopMoving = true;
|
|
|
|
mount->logInfo("Slewing finished");
|
|
err = *_lastError;
|
|
mount->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message()));
|
|
|
|
_pathFile.save();
|
|
|
|
// get final position
|
|
|
|
if (!err) {
|
|
// wait for updated telemetry data
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
|
|
|
|
if (t_err) {
|
|
*_lastError =
|
|
mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
|
|
return;
|
|
}
|
|
}
|
|
|
|
t_err = mount->targetToMountDist(&dist);
|
|
if (t_err) {
|
|
*_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY);
|
|
return;
|
|
}
|
|
|
|
log_pos(hw_state, tdata);
|
|
|
|
mount->logDebug(std::format(" target-to-mount distance {}", mcc::MccAngleFancyString(dist)));
|
|
|
|
if (!slew_and_stop) { // start tracking
|
|
_trackingFunc();
|
|
} else {
|
|
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_OK;
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
/* tracking function */
|
|
|
|
_trackingFunc = [mount, cb_sptr, check_pzones, send_to_hardware, log_pos, 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 {
|
|
mount->logError("Tracking path filename is empty! Do not save it!");
|
|
}
|
|
}
|
|
|
|
mount->logInfo("Start tracking");
|
|
mount->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
|
|
mcc::MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
|
|
mount->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
|
|
mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
|
|
mount->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 MountT::hardware_state_t hw_state;
|
|
|
|
error_t err;
|
|
MccTelemetryData tdata;
|
|
double dist;
|
|
|
|
auto last_hw_time = tdata.time_point;
|
|
|
|
*cb_sptr(MountT::mount_status_t::TRACKING); // send the status to the mount
|
|
|
|
while (!*_stopMoving) {
|
|
// wait for updated telemetry data
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
auto t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);
|
|
|
|
if (t_err) {
|
|
*_lastError =
|
|
mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
|
|
break;
|
|
}
|
|
|
|
last_hw_time = tdata.time_point;
|
|
}
|
|
|
|
auto hw_err = mount->hardwareGetState(&hw_state);
|
|
if (hw_err) {
|
|
*_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
|
|
break;
|
|
}
|
|
|
|
log_pos(hw_state, tdata);
|
|
|
|
if (*_stopMoving) {
|
|
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
|
|
break;
|
|
}
|
|
|
|
err = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
|
|
if (err) {
|
|
*_lastError = err;
|
|
break;
|
|
}
|
|
|
|
if (last_hw_time == tdata.time_point) {
|
|
mount->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
|
|
continue;
|
|
}
|
|
|
|
last_hw_time = tdata.time_point;
|
|
|
|
auto t_err = mount->targetToMountDist(&dist);
|
|
if (t_err) {
|
|
*_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY);
|
|
break;
|
|
}
|
|
|
|
mount->logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist)));
|
|
|
|
// resend new position since target coordinates are changed in time
|
|
hw_state.X = (double)tdata.target.X;
|
|
hw_state.Y = (double)tdata.target.Y;
|
|
|
|
err = send_to_hardware(hw_state);
|
|
if (err) {
|
|
*_lastError = err;
|
|
break;
|
|
}
|
|
|
|
if (*_stopMoving) {
|
|
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
|
|
break;
|
|
}
|
|
|
|
// sleep here
|
|
std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval);
|
|
}
|
|
|
|
*_stopMoving = true;
|
|
|
|
mount->logInfo("Tracking finished");
|
|
err = *_lastError;
|
|
mount->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message()));
|
|
|
|
_pathFile.save();
|
|
};
|
|
}
|
|
|
|
virtual ~MccSimpleMovingControls()
|
|
{
|
|
*_stopMoving = true;
|
|
}
|
|
|
|
error_t slewToTarget(bool slew_and_stop = false)
|
|
{
|
|
return *_lastError;
|
|
}
|
|
|
|
error_t trackTarget()
|
|
{
|
|
return *_lastError;
|
|
}
|
|
|
|
|
|
error_t stopMount()
|
|
{
|
|
if (*_stopMoving) {
|
|
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED;
|
|
} else {
|
|
_stopMovingFunc();
|
|
}
|
|
|
|
return *_lastError;
|
|
}
|
|
|
|
error_t setMovingParams(moving_params_t params)
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
_currentParams = std::move(params);
|
|
|
|
return MccSimpleMovingControlsErrorCode::ERROR_OK;
|
|
}
|
|
|
|
moving_params_t getMovingParams() const
|
|
{
|
|
std::lock_guard lock{*_currentParamsMutex};
|
|
|
|
return _currentParams;
|
|
}
|
|
|
|
error_t mountMovingLastError() const
|
|
{
|
|
return *_lastError;
|
|
}
|
|
|
|
protected:
|
|
std::function<void(bool)> _slewingFunc{};
|
|
std::function<void()> _trackingFunc{};
|
|
std::function<void()> _stopMovingFunc{};
|
|
|
|
std::unique_ptr<std::atomic_bool> _stopMoving;
|
|
|
|
std::unique_ptr<std::mutex> _currentParamsMutex;
|
|
moving_params_t _currentParams{};
|
|
|
|
std::unique_ptr<std::atomic<error_t>> _lastError;
|
|
|
|
PathFile _pathFile{};
|
|
};
|
|
|
|
} // namespace mcc
|