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

...

Browse Source
This commit is contained in:
Timur A. Fatkhullin 2025-12-15 17:26:26 +03:00
parent a30729fb37
commit a8dd511366
4 changed files with 703 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
asibfm700/asibfm700_servocontroller.cpp
View File

@ -108,6 +108,38 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(har
{
std::lock_guard lock{*_setStateMutex};
if (state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) { // stop!
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.stop());
if (err) {
return err;
}
hardware_state_t hw_state;
auto start_tp = std::chrono::steady_clock::now();
// poll hardware till stopped-state detected ...
while (true) {
err = hardwareGetState(&hw_state);
if (err) {
return err;
}
if (hw_state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) {
break;
}
if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) {
err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT;
break;
}
std::this_thread::sleep_for(_hardwareConfig.pollingInterval);
}
return err;
}
// static thread_local coordval_pair_t cvalpair{.X{0.0, 0.0}, .Y{0.0, 0.0}};
// static thread_local coordpair_t cpair{.X = 0.0, .Y = 0.0};

43
mcc/mcc_angle.h
View File

@ -459,11 +459,31 @@ class MccAngleDEC_APP : public MccAngle
using MccAngle::MccAngle;
};
class MccAngleRA_OBS : public MccAngle
{
using MccAngle::MccAngle;
};
class MccAngleDEC_OBS : public MccAngle
{
using MccAngle::MccAngle;
};
class MccAngleHA : public MccAngle
{
using MccAngle::MccAngle;
};
class MccAngleHA_APP : public MccAngle
{
using MccAngle::MccAngle;
};
class MccAngleHA_OBS : public MccAngle
{
using MccAngle::MccAngle;
};
class MccAngleAZ : public MccAngle
{
using MccAngle::MccAngle;
@ -516,7 +536,11 @@ enum class MccCoordKind : size_t {
COORDS_KIND_DEC_ICRS = traits::mcc_type_hash<MccAngleDEC_ICRS>,
COORDS_KIND_RA_APP = traits::mcc_type_hash<MccAngleRA_APP>,
COORDS_KIND_DEC_APP = traits::mcc_type_hash<MccAngleDEC_APP>,
COORDS_KIND_RA_OBS = traits::mcc_type_hash<MccAngleRA_OBS>,
COORDS_KIND_DEC_OBS = traits::mcc_type_hash<MccAngleDEC_OBS>,
COORDS_KIND_HA = traits::mcc_type_hash<MccAngleHA>,
COORDS_KIND_HA_APP = traits::mcc_type_hash<MccAngleHA_APP>,
COORDS_KIND_HA_OBS = traits::mcc_type_hash<MccAngleHA_OBS>,
COORDS_KIND_AZ = traits::mcc_type_hash<MccAngleAZ>,
COORDS_KIND_ZD = traits::mcc_type_hash<MccAngleZD>,
COORDS_KIND_ALT = traits::mcc_type_hash<MccAngleALT>,
@ -531,18 +555,23 @@ enum class MccCoordPairKind : size_t {
COORDS_KIND_GENERIC = traits::mcc_type_pair_hash<MccAngle, MccAngle>(),
COORDS_KIND_RADEC_ICRS = traits::mcc_type_pair_hash<MccAngleRA_ICRS, MccAngleDEC_ICRS>(),
COORDS_KIND_RADEC_APP = traits::mcc_type_pair_hash<MccAngleRA_APP, MccAngleDEC_APP>(),
COORDS_KIND_RADEC_OBS = traits::mcc_type_pair_hash<MccAngleRA_OBS, MccAngleDEC_OBS>(),
COORDS_KIND_HADEC_APP = traits::mcc_type_pair_hash<MccAngleHA, MccAngleDEC_APP>(),
// COORDS_KIND_HADEC_APP = traits::mcc_type_pair_hash<MccAngleHA_APP, MccAngleDEC_APP>(),
COORDS_KIND_HADEC_OBS = traits::mcc_type_pair_hash<MccAngleHA_OBS, MccAngleDEC_OBS>(),
COORDS_KIND_AZZD = traits::mcc_type_pair_hash<MccAngleAZ, MccAngleZD>(),
COORDS_KIND_AZALT = traits::mcc_type_pair_hash<MccAngleAZ, MccAngleALT>(),
COORDS_KIND_XY = traits::mcc_type_pair_hash<MccAngleX, MccAngleY>(),
COORDS_KIND_LATLON = traits::mcc_type_pair_hash<MccAngleLAT, MccAngleLON>(),
COORDS_KIND_LONLAT = traits::mcc_type_pair_hash<MccAngleLON, MccAngleLAT>(),
COORDS_KIND_UNKNOWN = traits::mcc_type_pair_hash<MccAngleUnknown, MccAngleUnknown>()
};
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_ICRS_STR = "RADEC-IRCS";
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_APP_STR = "RADEC-APP";
static constexpr std::string_view MCC_COORDPAIR_KIND_RADEC_OBS_STR = "RADEC-OBS";
static constexpr std::string_view MCC_COORDPAIR_KIND_HADEC_APP_STR = "HADEC-APP";
static constexpr std::string_view MCC_COORDPAIR_KIND_HADEC_OBS_STR = "HADEC-OBS";
static constexpr std::string_view MCC_COORDPAIR_KIND_AZALT_STR = "AZALT";
static constexpr std::string_view MCC_COORDPAIR_KIND_AZZD_STR = "AZZD";
static constexpr std::string_view MCC_COORDPAIR_KIND_XY_STR = "XY";
@ -554,11 +583,13 @@ template <MccCoordPairKind KIND>
static constexpr std::string_view MccCoordPairKindStr =
KIND == MccCoordPairKind::COORDS_KIND_RADEC_ICRS ? MCC_COORDPAIR_KIND_RADEC_ICRS_STR
: KIND == MccCoordPairKind::COORDS_KIND_RADEC_APP ? MCC_COORDPAIR_KIND_RADEC_APP_STR
: KIND == MccCoordPairKind::COORDS_KIND_RADEC_OBS ? MCC_COORDPAIR_KIND_RADEC_OBS_STR
: KIND == MccCoordPairKind::COORDS_KIND_HADEC_APP ? MCC_COORDPAIR_KIND_HADEC_APP_STR
: KIND == MccCoordPairKind::COORDS_KIND_HADEC_OBS ? MCC_COORDPAIR_KIND_HADEC_OBS_STR
: KIND == MccCoordPairKind::COORDS_KIND_AZALT ? MCC_COORDPAIR_KIND_AZALT_STR
: KIND == MccCoordPairKind::COORDS_KIND_AZZD ? MCC_COORDPAIR_KIND_AZZD_STR
: KIND == MccCoordPairKind::COORDS_KIND_XY ? MCC_COORDPAIR_KIND_XY_STR
: KIND == MccCoordPairKind::COORDS_KIND_LATLON ? MCC_COORDPAIR_KIND_LATLON_STR
: KIND == MccCoordPairKind::COORDS_KIND_LONLAT ? MCC_COORDPAIR_KIND_LATLON_STR
: KIND == MccCoordPairKind::COORDS_KIND_GENERIC ? MCC_COORDPAIR_KIND_GENERIC_STR
: MCC_COORDPAIR_KIND_UNKNOWN_STR;
@ -567,11 +598,13 @@ static constexpr std::string_view MccCoordPairKindToStr(MccCoordPairKind KIND)
{
return KIND == MccCoordPairKind::COORDS_KIND_RADEC_ICRS ? MCC_COORDPAIR_KIND_RADEC_ICRS_STR
: KIND == MccCoordPairKind::COORDS_KIND_RADEC_APP ? MCC_COORDPAIR_KIND_RADEC_APP_STR
: KIND == MccCoordPairKind::COORDS_KIND_RADEC_OBS ? MCC_COORDPAIR_KIND_RADEC_OBS_STR
: KIND == MccCoordPairKind::COORDS_KIND_HADEC_APP ? MCC_COORDPAIR_KIND_HADEC_APP_STR
: KIND == MccCoordPairKind::COORDS_KIND_HADEC_OBS ? MCC_COORDPAIR_KIND_HADEC_OBS_STR
: KIND == MccCoordPairKind::COORDS_KIND_AZALT ? MCC_COORDPAIR_KIND_AZALT_STR
: KIND == MccCoordPairKind::COORDS_KIND_AZZD ? MCC_COORDPAIR_KIND_AZZD_STR
: KIND == MccCoordPairKind::COORDS_KIND_XY ? MCC_COORDPAIR_KIND_XY_STR
: KIND == MccCoordPairKind::COORDS_KIND_LATLON ? MCC_COORDPAIR_KIND_LATLON_STR
: KIND == MccCoordPairKind::COORDS_KIND_LONLAT ? MCC_COORDPAIR_KIND_LATLON_STR
: KIND == MccCoordPairKind::COORDS_KIND_GENERIC ? MCC_COORDPAIR_KIND_GENERIC_STR
: MCC_COORDPAIR_KIND_UNKNOWN_STR;
}
@ -588,11 +621,13 @@ static constexpr MccCoordPairKind MccCoordStrToPairKind(R&& spair)
return hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_RADEC_ICRS_STR) ? MccCoordPairKind::COORDS_KIND_RADEC_ICRS
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_RADEC_APP_STR) ? MccCoordPairKind::COORDS_KIND_RADEC_APP
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_RADEC_OBS_STR) ? MccCoordPairKind::COORDS_KIND_RADEC_OBS
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_HADEC_APP_STR) ? MccCoordPairKind::COORDS_KIND_HADEC_APP
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_HADEC_OBS_STR) ? MccCoordPairKind::COORDS_KIND_HADEC_OBS
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_AZALT_STR) ? MccCoordPairKind::COORDS_KIND_AZALT
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_AZZD_STR) ? MccCoordPairKind::COORDS_KIND_AZZD
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_XY_STR) ? MccCoordPairKind::COORDS_KIND_XY
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_LATLON_STR) ? MccCoordPairKind::COORDS_KIND_LATLON
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_LATLON_STR) ? MccCoordPairKind::COORDS_KIND_LONLAT
: hash == mcc::utils::FNV1aHash(MCC_COORDPAIR_KIND_GENERIC_STR) ? MccCoordPairKind::COORDS_KIND_GENERIC
: MccCoordPairKind::COORDS_KIND_UNKNOWN;
}

118
mcc/mcc_coord.h
View File

@ -14,7 +14,51 @@ public:
typedef CO_LON_T x_t;
typedef CO_LAT_T y_t;
MccCoordPair(CO_LON_T const& x, CO_LAT_T const& y) : _x(x), _y(y) {}
static constexpr MccCoordPairKind pairKind =
!(std::derived_from<CO_LON_T, MccAngle> ||
std::derived_from<CO_LAT_T, MccAngle>) // unknown type (possibly just double or float)
? MccCoordPairKind::COORDS_KIND_GENERIC
: (std::same_as<CO_LON_T, MccAngle> || std::same_as<CO_LAT_T, MccAngle>) // one of the types is MccAngle
? MccCoordPairKind::COORDS_KIND_GENERIC
// ICRS RA and DEC
: (std::same_as<CO_LON_T, MccAngleRA_ICRS> && std::same_as<CO_LAT_T, MccAngleDEC_ICRS>)
? MccCoordPairKind::COORDS_KIND_RADEC_ICRS
// apparent RA and DEC
: (std::same_as<CO_LON_T, MccAngleRA_APP> && std::same_as<CO_LAT_T, MccAngleDEC_APP>)
? MccCoordPairKind::COORDS_KIND_RADEC_APP
// observed RA and DEC
: (std::same_as<CO_LON_T, MccAngleRA_OBS> && std::same_as<CO_LAT_T, MccAngleDEC_OBS>)
? MccCoordPairKind::COORDS_KIND_RADEC_APP
// apparent HA and DEC
: (std::same_as<CO_LON_T, MccAngleHA_APP> && std::same_as<CO_LAT_T, MccAngleDEC_APP>)
? MccCoordPairKind::COORDS_KIND_HADEC_APP
// observed HA and DEC
: (std::same_as<CO_LON_T, MccAngleHA_OBS> && std::same_as<CO_LAT_T, MccAngleDEC_OBS>)
? MccCoordPairKind::COORDS_KIND_HADEC_APP
// apparent AZ and ZD
: (std::same_as<CO_LON_T, MccAngleAZ> && std::same_as<CO_LAT_T, MccAngleZD>)
? MccCoordPairKind::COORDS_KIND_AZZD
// apparent AZ and ALT
: (std::same_as<CO_LON_T, MccAngleAZ> && std::same_as<CO_LAT_T, MccAngleALT>)
? MccCoordPairKind::COORDS_KIND_AZZD
// general purpose X and Y
: (std::same_as<CO_LON_T, MccAngleX> && std::same_as<CO_LAT_T, MccAngleY>)
? MccCoordPairKind::COORDS_KIND_XY
// geographical longitude and latitude
: (std::same_as<CO_LON_T, MccAngleLON> && std::same_as<CO_LAT_T, MccAngleLAT>)
? MccCoordPairKind::COORDS_KIND_LONLAT
: MccCoordPairKind::COORDS_KIND_UNKNOWN;
template <mcc_coord_epoch_c EpT = MccCelestialCoordEpoch>
MccCoordPair(CO_LON_T const& x, CO_LAT_T const& y, EpT const& epoch = EpT::now()) : _x(x), _y(y), _mjd(epoch.MJD)
{
}
MccCoordPair(const MccCoordPair&) = default;
MccCoordPair(MccCoordPair&&) = default;
MccCoordPair& operator=(const MccCoordPair&) = default;
MccCoordPair& operator=(MccCoordPair&&) = default;
virtual ~MccCoordPair() = default;
@ -28,9 +72,16 @@ public:
return _y;
}
operator std::tuple<CO_LON_T, CO_LAT_T>() const
double MJD() const
{
return {_x, _y};
return _mjd;
}
// for something like:
// auto [ra, dec, mjd] = coord_pair;
operator std::tuple<CO_LON_T, CO_LAT_T, double>() const
{
return {_x, _y, _mjd};
}
void setX(const CO_LON_T& x)
@ -43,9 +94,16 @@ public:
_y = y;
}
void setMJD(double mjd)
{
_mjd = mjd;
}
protected:
CO_LON_T _x;
CO_LAT_T _y;
double _mjd;
};
@ -58,7 +116,50 @@ concept mcc_coord_pair_c = requires {
};
/* predefined coordinate pairs */
template <mcc_angle_c CO_LON_T, mcc_angle_c CO_LAT_T>
requires(std::derived_from<CO_LON_T, MccAngle> && std::derived_from<CO_LAT_T, MccAngle>)
class MccNamedCoordPair : public MccCoordPair<CO_LON_T, CO_LAT_T>
{
public:
template <typename CxT, typename CyT, mcc_coord_epoch_c EpT = MccCelestialCoordEpoch>
requires(std::is_arithmetic_v<CxT> && std::is_arithmetic_v<CyT>)
MccNamedCoordPair(CxT const& x, CyT const& y, EpT const& epoch = EpT::now())
: MccCoordPair<CO_LON_T, CO_LAT_T>(CO_LON_T{x}, CO_LAT_T{y}, epoch)
{
}
template <mcc_coord_epoch_c EpT = MccCelestialCoordEpoch>
MccNamedCoordPair(MccAngle const& x, MccAngle const& y, EpT const& epoch = EpT::now())
: MccCoordPair<CO_LON_T, CO_LAT_T>(CO_LON_T{(double)x}, CO_LAT_T{(double)y}, epoch)
{
}
MccNamedCoordPair(const MccNamedCoordPair&) = default;
MccNamedCoordPair(MccNamedCoordPair&&) = default;
MccNamedCoordPair& operator=(const MccNamedCoordPair&) = default;
MccNamedCoordPair& operator=(MccNamedCoordPair&&) = default;
virtual ~MccNamedCoordPair() = default;
};
using MccSkyRADEC_ICRS = MccNamedCoordPair<MccAngleRA_ICRS, MccAngleDEC_ICRS>;
using MccSkyRADEC_APP = MccNamedCoordPair<MccAngleRA_APP, MccAngleDEC_APP>;
using MccSkyRADEC_OBS = MccNamedCoordPair<MccAngleRA_OBS, MccAngleDEC_OBS>;
using MccSkyHADEC_APP = MccNamedCoordPair<MccAngleHA_APP, MccAngleDEC_APP>;
using MccSkyHADEC_OBS = MccNamedCoordPair<MccAngleHA_OBS, MccAngleDEC_OBS>;
using MccSkyAZZD = MccNamedCoordPair<MccAngleAZ, MccAngleZD>;
using MccSkyAZALT = MccNamedCoordPair<MccAngleAZ, MccAngleALT>;
using MccGenXY = MccNamedCoordPair<MccAngleX, MccAngleY>;
using MccGeoLONLAT = MccNamedCoordPair<MccAngleLON, MccAngleLAT>;
struct mcc_skycoord_interface_t {
virtual ~mcc_skycoord_interface_t() = default;
template <std::derived_from<mcc_skycoord_interface_t> SelfT, mcc_angle_c XT, mcc_angle_c YT>
SelfT& from(this SelfT&& self, XT&& x, YT&& y)
{
@ -81,7 +182,7 @@ struct mcc_skycoord_interface_t {
template <std::derived_from<mcc_skycoord_interface_t> SelfT, mcc_coord_pair_c PT, mcc_coord_pair_c... PTs>
SelfT& to(this SelfT&& self, PT& cpair, PTs&... cpairs)
{
return std::forward<SelfT>(self).to(cpair, cpairs);
return std::forward<SelfT>(self).to(cpair, cpairs...);
}
template <std::derived_from<mcc_skycoord_interface_t> SelfT, mcc_coord_pair_c PT>
@ -103,4 +204,11 @@ concept mcc_skycoord_c = std::derived_from<T, mcc_skycoord_interface_t> && requi
{ T::meteo(std::declval<typename T::meteo_t const&>()) };
};
} // end namespace mcc
class MccSkyPoint : public mcc_skycoord_interface_t
{
public:
};
} // end namespace mcc

582
mcc/mcc_moving_controls.h
View File

@ -105,6 +105,62 @@ inline std::error_code make_error_code(MccSimpleMovingControlsErrorCode ec)
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;
}
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;
@ -125,30 +181,10 @@ public:
CallbackFuncT&& mode_switch_calback = [](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,
MccTelemetryData const& tdata) {
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 start_point = tdata.time_point; // needed for trajectory file
if constexpr (mccIsEquatorialMount(MountT::mountType)) {
mount->logDebug(std::format(" entered target: HA = {}, DEC = {}",
mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal()));
mount->logDebug(std::format(" current mount: HA = {}, DEC = {}",
mcc::MccAngle{tdata.HA}.sexagesimal(true),
mcc::MccAngle{tdata.DEC_APP}.sexagesimal()));
} else if constexpr (mccIsAltAzMount(MountT::mountType)) {
mount->logDebug(std::format(" entered target: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
mcc::MccAngle{tdata.target.ZD}.sexagesimal()));
mount->logDebug(std::format(" current mount: AZ = {}, ZD = {}",
mcc::MccAngle{tdata.AZ}.sexagesimal(),
mcc::MccAngle{tdata.ZD}.sexagesimal()));
}
auto hw_err = mount->hardwareSetState(hw_state);
if (hw_err) {
return mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE);
@ -160,19 +196,134 @@ public:
};
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_calback)));
_slewingFunc = [mount, cb_sptr, this](bool slew_and_stop) {
/* 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;
bool store_path = false;
std::ofstream fst;
using path_tp_t = std::chrono::duration<double>; // seconds represented as double
{
// std::lock_guard lock{*_currentParamsMutex};
@ -192,13 +343,9 @@ public:
std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
if (!_currentParams.slewingPathFilename.empty()) { // open slewing trajectory file
fst.open(_currentParams.slewingPathFilename);
if (fst.is_open()) {
store_path = true;
} else {
mount->logError(std::format("Cannot open slewing path file: {}! Do not save it!",
_currentParams.slewingPathFilename));
}
_pathFile.setFilename(_currentParams.slewingPathFilename);
} else {
mount->logError("Slewing path filename is empty! Do not save it!");
}
}
@ -216,18 +363,16 @@ public:
mount->logInfo(std::format(" min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));
if (store_path) {
fst << "# \n";
fst << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n";
fst << "# Config:\n";
fst << "# slewing tolerance radius: " << mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs()
<< " arcsecs\n";
fst << "# slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n";
fst << "# \n";
fst << "# Format (time is in seconds, coordinates are in radians): \n";
fst << "# <time-since-start> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
"<dY_{target-mount}> <moving state>\n";
}
_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;
@ -238,32 +383,33 @@ public:
t_err = mount->telemetryData(&tdata);
if (t_err) {
return *_lastError =
mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
*_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;
// bool in_zone;
// std::vector<bool> in_zone_vec;
MccCelestialPoint cpt;
// 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!");
}
// 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;
return *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
*_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
return;
}
@ -278,14 +424,321 @@ public:
}
hw_state.moving_state = MountT::hardware_moving_state_t::HW_MOVE_SLEWING;
if (*_stopMoving) {
mount->logDebug("slewing was stopped!");
return *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
// 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;
}
}
};
_trackingFunc = [mount, cb_sptr, this]() {
/* 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();
};
}
@ -305,12 +758,12 @@ public:
}
error_t stopMountMoving()
error_t stopMount()
{
if (*_stopMoving) {
*_lastError = MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED;
} else {
*_stopMoving = true;
_stopMovingFunc();
}
return *_lastError;
@ -340,6 +793,7 @@ public:
protected:
std::function<void(bool)> _slewingFunc{};
std::function<void()> _trackingFunc{};
std::function<void()> _stopMovingFunc{};
std::unique_ptr<std::atomic_bool> _stopMoving;
@ -347,6 +801,8 @@ protected:
MccSimpleMovingModelParams _currentParams{};
std::unique_ptr<std::atomic<error_t>> _lastError;
PathFile _pathFile{};
};
} // namespace mcc