1000 lines
40 KiB
C++
1000 lines
40 KiB
C++
#include "asibfm700_mount.h"
|
|
|
|
#include <mcc/mcc_coordinate.h>
|
|
#include <mcc/mcc_pzone.h>
|
|
#include "mcc/mcc_serializer.h"
|
|
|
|
namespace asibfm700
|
|
{
|
|
|
|
|
|
/* CONSTRUCTOR AND DESTRUCTOR */
|
|
|
|
Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger)
|
|
: _servolController(config.servoControllerConfig()),
|
|
_pcm(config.pcmData()),
|
|
gm_class_t(std::make_tuple(&_servolController, &_pcm),
|
|
std::make_tuple(),
|
|
// std::make_tuple(&_servolController,
|
|
// this,
|
|
// [this](Asibfm700Mount::mount_status_t const& status) { *_mountStatus = status; }),
|
|
std::make_tuple([this](bool sl) { return slewingImpl(sl); },
|
|
[this]() { return trackingImpl(); },
|
|
[this]() { return stoppingImpl(); }),
|
|
std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)),
|
|
_mountConfig(config),
|
|
_mountConfigMutex(new std::mutex)
|
|
{
|
|
gm_class_t::addMarkToPatternIdx("[ASIB-MOUNT]");
|
|
|
|
logDebug("Create Asibfm700Mount class instance ({})", this->getThreadId());
|
|
|
|
initMount();
|
|
}
|
|
|
|
Asibfm700Mount::~Asibfm700Mount()
|
|
{
|
|
*_lastMountError = stoppingImpl();
|
|
if (_lastMountError->load()) {
|
|
errorLogging("an error occured while stopping mount: ", _lastMountError->load());
|
|
}
|
|
|
|
_servolController.hardwareShutdown();
|
|
|
|
logDebug("Delete Asibfm700Mount class instance ({})", this->getThreadId());
|
|
}
|
|
|
|
|
|
|
|
/* PUBIC METHODS */
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::initMount()
|
|
{
|
|
std::lock_guard lock{*_mountConfigMutex};
|
|
|
|
logInfo("Init AstroSib FM-700 mount with configuration:");
|
|
logInfo(" site latitude: {}", _mountConfig.siteLatitude().sexagesimal());
|
|
logInfo(" site longitude: {}", _mountConfig.siteLongitude().sexagesimal());
|
|
logInfo(" site elevation: {} meters", _mountConfig.siteElevation());
|
|
logInfo(" refraction wavelength: {} mkm", _mountConfig.refractWavelength());
|
|
logInfo(" leap seconds filename: {}", _mountConfig.leapSecondFilename());
|
|
logInfo(" IERS Bulletin A filename: {}", _mountConfig.bulletinAFilename());
|
|
|
|
logInfo("{:-^80}", "Initializing ERFA-engine with values above:");
|
|
auto ccte_state = mcc::impl::MccSkyPoint::cctEngine.getStateERFA();
|
|
mcc::impl::MccSkyPoint::cctEngine.setStateERFA({.meteo = ccte_state.meteo, // just use of previous values
|
|
.wavelength = _mountConfig.refractWavelength(),
|
|
.lat = _mountConfig.siteLatitude(),
|
|
.lon = _mountConfig.siteLongitude(),
|
|
.elev = _mountConfig.siteElevation()});
|
|
|
|
if (_mountConfig.leapSecondFilename().size()) { // load leap seconds file
|
|
logInfo("Loading leap second file: '{}' ...", _mountConfig.leapSecondFilename());
|
|
bool ok = ccte_state._leapSeconds.load(_mountConfig.leapSecondFilename());
|
|
if (ok) {
|
|
logInfo("Leap second file was loaded successfully (expire date: {})", ccte_state._leapSeconds.expireDate());
|
|
} else {
|
|
logError("Leap second file loading failed! Using hardcoded defauls (expire date: {})",
|
|
ccte_state._leapSeconds.expireDate());
|
|
}
|
|
} else {
|
|
logInfo("Using hardcoded leap seconds defauls (expire date: {})", ccte_state._leapSeconds.expireDate());
|
|
}
|
|
|
|
if (_mountConfig.bulletinAFilename().size()) { // load IERS Bulletin A file
|
|
logInfo("Loading IERS Bulletin A file: '{}' ...", _mountConfig.bulletinAFilename());
|
|
bool ok = ccte_state._bulletinA.load(_mountConfig.bulletinAFilename());
|
|
if (ok) {
|
|
logInfo("IERS Bulletin A file was loaded successfully (date range: {} - {})",
|
|
ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
|
|
} else {
|
|
logError("IERS Bulletin A file loading failed! Using hardcoded defauls (date range: {} - {})",
|
|
ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
|
|
}
|
|
} else {
|
|
logInfo("Using hardcoded IERS Bulletin A defauls (date range: {} - {})",
|
|
ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
|
|
}
|
|
|
|
logInfo("{:-^80}", "ERFA-engine was initialized");
|
|
|
|
|
|
logInfo("");
|
|
logDebug("Delete previously defined prohibited zones");
|
|
clearPZones();
|
|
logInfo("Add prohibited zones ...");
|
|
|
|
logInfo(" Add MccAltLimitPZ zone: min alt = {}, lat = {} (pzone type: '{}')",
|
|
_mountConfig.pzMinAltitude().degrees(), _mountConfig.siteLatitude().degrees(),
|
|
"Minimal altitude prohibited zone");
|
|
addPZone(mcc::impl::MccAltLimitPZ<mcc::impl::MccAltLimitKind::MIN_ALT_LIMIT>{_mountConfig.pzMinAltitude(),
|
|
_mountConfig.siteLatitude()});
|
|
|
|
logInfo(" Add MccAxisLimitSwitchPZ zone: min value = {}, max value = {} (pzone type: '{}')",
|
|
_mountConfig.pzLimitSwitchHAMin().degrees(), _mountConfig.pzLimitSwitchHAMax().degrees(),
|
|
"HA-axis limit switch");
|
|
size_t pz_num = addPZone(mcc::impl::MccAxisLimitSwitchPZ<mcc::impl::MccCoordKind::COORDS_KIND_HA_OBS>{
|
|
_mountConfig.pzLimitSwitchHAMin(), _mountConfig.pzLimitSwitchHAMax(), &_pcm});
|
|
|
|
logInfo("{} prohibited zones were added successfully", pz_num);
|
|
|
|
auto mpars = _mountConfig.movingModelParams();
|
|
|
|
using secs_t = std::chrono::duration<double>;
|
|
auto to_msecs = [](double secs) {
|
|
auto s = secs_t{secs};
|
|
return std::chrono::duration_cast<std::chrono::milliseconds>(s);
|
|
};
|
|
|
|
auto hw_cfg = _mountConfig.servoControllerConfig();
|
|
auto eeprom_cfg = hw_cfg.hwConfig;
|
|
|
|
logInfo("");
|
|
logInfo("Hardware initialization ...");
|
|
logInfo(" set encoder device paths, PIDs configuration and rough encoder zero-points:");
|
|
logInfo(" RunModel: {}", hw_cfg.devConfig.RunModel == 1 ? "MODEL-MODE" : "REAL-MODE");
|
|
logInfo(" mount dev path: {}", hw_cfg.MountDevPath);
|
|
logInfo(" encoder dev path: {}", hw_cfg.EncoderDevPath);
|
|
logInfo(" encoder X-dev path: {}", hw_cfg.EncoderXDevPath);
|
|
logInfo(" encoder Y-dev path: {}", hw_cfg.EncoderYDevPath);
|
|
|
|
logInfo(" EncoderDevSpeed: {}", hw_cfg.devConfig.EncoderDevSpeed);
|
|
logInfo(" SepEncoder: {}", hw_cfg.devConfig.SepEncoder);
|
|
logInfo(" MountReqInterval: {}", to_msecs(hw_cfg.devConfig.MountReqInterval));
|
|
logInfo(" EncoderReqInterval: {}", to_msecs(hw_cfg.devConfig.EncoderReqInterval));
|
|
logInfo(" EncoderSpeedInterval: {}", to_msecs(hw_cfg.devConfig.EncoderSpeedInterval));
|
|
logInfo(" PIDMaxDt: {}", to_msecs(hw_cfg.devConfig.PIDMaxDt));
|
|
logInfo(" PIDRefreshDt: {}", to_msecs(hw_cfg.devConfig.PIDRefreshDt));
|
|
logInfo(" PIDCycleDt: {}", to_msecs(hw_cfg.devConfig.PIDCycleDt));
|
|
|
|
logInfo(" XPIDC: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.XPIDC.P, hw_cfg.devConfig.XPIDC.I,
|
|
hw_cfg.devConfig.XPIDC.D);
|
|
logInfo(" XPIDV: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.XPIDV.P, hw_cfg.devConfig.XPIDV.I,
|
|
hw_cfg.devConfig.XPIDV.D);
|
|
logInfo(" YPIDC: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.YPIDC.P, hw_cfg.devConfig.YPIDC.I,
|
|
hw_cfg.devConfig.YPIDC.D);
|
|
logInfo(" YPIDV: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.YPIDV.P, hw_cfg.devConfig.YPIDV.I,
|
|
hw_cfg.devConfig.YPIDV.D);
|
|
logInfo(" XEncZero: {}", hw_cfg.devConfig.XEncZero);
|
|
logInfo(" YEncZero: {}", hw_cfg.devConfig.YEncZero);
|
|
|
|
// actually, only set _servolController._hardwareConfig.devConfig part and paths!!!
|
|
_servolController.hardwareUpdateConfig(hw_cfg.devConfig);
|
|
|
|
auto hw_ini_err = _servolController.hardwareInit();
|
|
if (hw_ini_err) {
|
|
errorLogging(" cannot initialize hardware (paths, PIDs, zeros): ", hw_ini_err);
|
|
return hw_ini_err;
|
|
}
|
|
|
|
logInfo(" paths, PIDs, zeros were initialized successfully!");
|
|
|
|
logInfo("");
|
|
logInfo(" current hardware EEPROM data:");
|
|
|
|
mcc::impl::MccAngle ang;
|
|
if (hw_cfg.devConfig.RunModel != 1) { // load EEPROM only in REAL HARDWARE mode
|
|
// load EEPROM part
|
|
auto cfg_err = _servolController.hardwareUpdateConfig();
|
|
if (cfg_err) {
|
|
errorLogging("Cannot load EEPROM data:", cfg_err);
|
|
return cfg_err;
|
|
}
|
|
|
|
hw_cfg = _servolController.getHardwareConfig();
|
|
|
|
ang = hw_cfg.hwConfig.Yconf.accel; // Sidereal defines HA-axis as Y-axis
|
|
|
|
logInfo(" HA-axis accel: {} degs/s^2", ang.degrees());
|
|
ang = hw_cfg.hwConfig.Xconf.accel; // Sidereal defines DEC-axis as X-axis
|
|
logInfo(" DEC-axis accel: {} degs/s^2", ang.degrees());
|
|
logInfo(" HA-axis backlash: {}", (double)hw_cfg.hwConfig.Yconf.backlash);
|
|
logInfo(" DEC-axis backlash: {}", (double)hw_cfg.hwConfig.Xconf.backlash);
|
|
|
|
logInfo(" HA-axis encoder ticks per revolution: {}",
|
|
hw_cfg.hwConfig.Ysetpr); // Sidereal defines HA-axis as Y-axis
|
|
logInfo(" DEC-axis encoder ticks per revolution: {}",
|
|
hw_cfg.hwConfig.Xsetpr); // Sidereal defines DEC-axis as X-axis
|
|
logInfo(" HA-motor encoder ticks per revolution: {}",
|
|
hw_cfg.hwConfig.Ymetpr); // Sidereal defines HA-axis as Y-axis
|
|
logInfo(" DEC-motor encoder ticks per revolution: {}",
|
|
hw_cfg.hwConfig.Xmetpr); // Sidereal defines DEC-axis as X-axis
|
|
|
|
ang = hw_cfg.hwConfig.Yslewrate; // Sidereal defines HA-axis as Y-axis
|
|
logInfo(" HA-axis slew rate: {} degs/s", ang.degrees());
|
|
ang = hw_cfg.hwConfig.Xslewrate; // Sidereal defines DEC-axis as X-axis
|
|
logInfo(" DEC-axis slew rate: {} degs/s", ang.degrees());
|
|
} else {
|
|
logWarn(" MODEL-MODE, no EEPROM data!");
|
|
}
|
|
|
|
logInfo("");
|
|
logInfo(" new hardware EEPROM data:");
|
|
|
|
|
|
eeprom_cfg.Xconf.accel = hw_cfg.hwConfig.Xconf.accel;
|
|
eeprom_cfg.Yconf.accel = hw_cfg.hwConfig.Yconf.accel;
|
|
|
|
eeprom_cfg.Xslewrate = _mountConfig.getValue<mcc::impl::MccAngle>("hwMaxRateDEC").value_or(0.0);
|
|
eeprom_cfg.Yslewrate = _mountConfig.getValue<mcc::impl::MccAngle>("hwMaxRateHA").value_or(0.0);
|
|
|
|
ang = eeprom_cfg.Yconf.accel; // Sidereal defines HA-axis as Y-axis
|
|
|
|
logInfo(" HA-axis accel: {} degs/s^2", ang.degrees());
|
|
ang = eeprom_cfg.Xconf.accel; // Sidereal defines DEC-axis as X-axis
|
|
logInfo(" DEC-axis accel: {} degs/s^2", ang.degrees());
|
|
logInfo(" HA-axis backlash: {}", (double)eeprom_cfg.Yconf.backlash);
|
|
logInfo(" DEC-axis backlash: {}", (double)eeprom_cfg.Xconf.backlash);
|
|
|
|
logInfo(" HA-axis encoder ticks per revolution: {}",
|
|
eeprom_cfg.Ysetpr); // Sidereal defines HA-axis as Y-axis
|
|
logInfo(" DEC-axis encoder ticks per revolution: {}",
|
|
eeprom_cfg.Xsetpr); // Sidereal defines DEC-axis as X-axis
|
|
logInfo(" HA-motor encoder ticks per revolution: {}",
|
|
eeprom_cfg.Ymetpr); // Sidereal defines HA-axis as Y-axis
|
|
logInfo(" DEC-motor encoder ticks per revolution: {}",
|
|
eeprom_cfg.Xmetpr); // Sidereal defines DEC-axis as X-axis
|
|
|
|
ang = eeprom_cfg.Yslewrate; // Sidereal defines HA-axis as Y-axis
|
|
logInfo(" HA-axis slew rate: {} degs/s", ang.degrees());
|
|
ang = eeprom_cfg.Xslewrate; // Sidereal defines DEC-axis as X-axis
|
|
logInfo(" DEC-axis slew rate: {} degs/s", ang.degrees());
|
|
|
|
// 03.03.2026: still do not save EEPROM!!!
|
|
// hw_ini_err = _servolController.hardwareUpdateConfig(eeprom_cfg);
|
|
// if (hw_ini_err) {
|
|
// errorLogging(" cannot initialize hardware (EEPROM data)!");
|
|
|
|
// return hw_ini_err;
|
|
// }
|
|
|
|
logInfo("{:-^80}", "Hardware initialization was performed successfully!");
|
|
|
|
logInfo("");
|
|
logInfo("Setup slewing and tracking parameters ...");
|
|
mpars.slewRateX = _mountConfig.getValue<mcc::impl::MccAngle>("hwMaxRateHA").value_or(0.0);
|
|
mpars.slewRateY = _mountConfig.getValue<mcc::impl::MccAngle>("hwMaxRateDEC").value_or(0.0);
|
|
if (hw_cfg.devConfig.RunModel != 1) {
|
|
mpars.brakingAccelX = eeprom_cfg.Yconf.accel; // Sidereal defines HA-axis as Y-axis
|
|
mpars.brakingAccelY = eeprom_cfg.Xconf.accel; // Sidereal defines DEC-axis as X-axis
|
|
//
|
|
} else { // set model's default values
|
|
mpars.brakingAccelX = 0.165806; // Sidereal defines HA-axis as Y-axis
|
|
mpars.brakingAccelY = 0.219911; // Sidereal defines DEC-axis as X-axis
|
|
|
|
hw_cfg.hwConfig.Xslewrate = mpars.slewRateY;
|
|
hw_cfg.hwConfig.Yslewrate = mpars.slewRateX;
|
|
hw_cfg.hwConfig.Xconf.accel = mpars.brakingAccelY;
|
|
hw_cfg.hwConfig.Yconf.accel = mpars.brakingAccelX;
|
|
}
|
|
|
|
auto max_dt_intvl = _mountConfig.getValue<std::chrono::milliseconds>("PIDMaxDt").value_or({});
|
|
auto min_dt_intvl = _mountConfig.getValue<std::chrono::milliseconds>("PIDRefreshDt").value_or({});
|
|
|
|
// check for polling interval consistency
|
|
auto intvl = mpars.slewingTelemetryInterval;
|
|
if (intvl > max_dt_intvl) {
|
|
mpars.slewingTelemetryInterval = max_dt_intvl;
|
|
logWarn(
|
|
" slewingTelemetryInterval user value ({} ms) is greater than allowed! Set it to maximal "
|
|
"allowed one: {} ms",
|
|
intvl.count(), max_dt_intvl.count());
|
|
}
|
|
if (intvl < min_dt_intvl) {
|
|
mpars.slewingTelemetryInterval = min_dt_intvl;
|
|
logWarn(
|
|
" slewingTelemetryInterval user value ({} ms) is lesser than allowed! Set it to minimal allowed "
|
|
"one: {} ms",
|
|
intvl.count(), min_dt_intvl.count());
|
|
}
|
|
|
|
intvl = mpars.trackingTelemetryInterval;
|
|
if (intvl > max_dt_intvl) {
|
|
mpars.trackingTelemetryInterval = max_dt_intvl;
|
|
logWarn(
|
|
" trackingTelemetryInterval user value ({} ms) is greater than allowed! Set it to maximal "
|
|
"allowed one: {} ms",
|
|
intvl.count(), max_dt_intvl.count());
|
|
}
|
|
if (intvl < min_dt_intvl) {
|
|
mpars.trackingTelemetryInterval = min_dt_intvl;
|
|
logWarn(
|
|
" trackingTelemetryInterval user value ({} ms) is lesser than allowed! Set it to minimal "
|
|
"allowed one: {} ms",
|
|
intvl.count(), min_dt_intvl.count());
|
|
}
|
|
|
|
|
|
auto st_err = setMovementParams(mpars);
|
|
if (st_err) {
|
|
errorLogging(" An error occured while setting slewing parameters: ", st_err);
|
|
} else {
|
|
logInfo(" Max HA-axis speed: {} degs/s", mcc::impl::MccAngle(mpars.slewRateX).degrees());
|
|
logInfo(" Max DEC-axis speed: {} degs/s", mcc::impl::MccAngle(mpars.slewRateY).degrees());
|
|
logInfo(" HA-axis stop acceleration braking: {} degs/s^2",
|
|
mcc::impl::MccAngle(mpars.brakingAccelX).degrees());
|
|
logInfo(" DEC-axis stop acceleration braking: {} degs/s^2",
|
|
mcc::impl::MccAngle(mpars.brakingAccelY).degrees());
|
|
|
|
logInfo(" Slewing telemetry polling interval: {} millisecs", mpars.slewingTelemetryInterval.count());
|
|
}
|
|
// st_err = setMovementParams(_mountConfig.movingModelParams());
|
|
// if (st_err) {
|
|
// errorLogging(" An error occured while setting tracking parameters: ", st_err);
|
|
// } else {
|
|
// logInfo(" Tracking telemetry polling interval: {} millisecs", mpars.trackingTelemetryInterval.count());
|
|
// }
|
|
logInfo("Slewing and tracking parameters have been set successfully");
|
|
|
|
|
|
coordpair_t cp;
|
|
Mount.getMaxSpeed(&cp);
|
|
logInfo("Check mount max speed: {} {}", cp.Y, cp.X);
|
|
|
|
// call base class initMount method
|
|
auto hw_err = gm_class_t::initMount();
|
|
// auto hw_err = base_gm_class_t::initMount();
|
|
if (hw_err) {
|
|
errorLogging("", hw_err);
|
|
return hw_err;
|
|
} else {
|
|
logInfo("Hardware initialization was performed sucessfully!");
|
|
}
|
|
|
|
setTelemetryDataTimeout(_mountConfig.movingModelParams().telemetryTimeout);
|
|
|
|
telemetry_data_t tdata;
|
|
auto t_err = telemetryData(&tdata);
|
|
if (t_err) {
|
|
logError(gm_class_t::formatError(t_err, "Cannot get initial telemetry data: "));
|
|
|
|
return t_err;
|
|
}
|
|
|
|
startAsyncMovementCycle();
|
|
|
|
return mcc::impl::MccGenericMountErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::updateMountConfig(const Asibfm700MountConfig& cfg)
|
|
{
|
|
std::lock_guard lock{*_mountConfigMutex};
|
|
|
|
_mountConfig = cfg;
|
|
|
|
auto hw_cfg = _mountConfig.servoControllerConfig();
|
|
_servolController.hardwareUpdateConfig(hw_cfg.devConfig);
|
|
_servolController.hardwareUpdateConfig(hw_cfg.hwConfig);
|
|
|
|
return AsibFM700ServoControllerErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::updateMountConfig(const std::string& cfg_filename)
|
|
{
|
|
Asibfm700MountConfig new_config;
|
|
auto err =
|
|
new_config.load(cfg_filename.empty() ? _mountConfig.configFilename() : std::filesystem::path{cfg_filename});
|
|
if (err) {
|
|
return mcc::mcc_deduced_err(err, std::make_error_code(std::errc::file_exists));
|
|
}
|
|
|
|
return updateMountConfig(new_config);
|
|
}
|
|
|
|
|
|
/* PROTECTED METHODS */
|
|
|
|
void Asibfm700Mount::errorLogging(const std::string& msg, const std::error_code& err)
|
|
{
|
|
if (msg.empty()) {
|
|
logError("{}::{} ({})", err.category().name(), err.value(), err.message());
|
|
} else {
|
|
logError("{}: {}::{} ({})", msg, err.category().name(), err.value(), err.message());
|
|
}
|
|
}
|
|
|
|
|
|
/* MOVEMENT METHODS */
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::sendToHardware(AsibFM700ServoController::hardware_state_t const& hw_state)
|
|
{
|
|
auto tp = std::chrono::duration_cast<std::chrono::milliseconds>(hw_state.XY.epoch().UTC().time_since_epoch());
|
|
|
|
logDebug("Send to hardware: X = {} degs, Y = {} degs, sX = {}/s, sY = {}/s, STATE = {} (timepoint: {})",
|
|
hw_state.XY.x().degrees(), hw_state.XY.y().degrees(),
|
|
mcc::impl::MccAngleFancyString(hw_state.speedXY.x().degrees()),
|
|
mcc::impl::MccAngleFancyString(hw_state.speedXY.y().degrees()), hw_state.movementState, tp);
|
|
|
|
*_lastMountError = _servolController.hardwareSetState(hw_state);
|
|
|
|
if (_lastMountError->load()) {
|
|
errorLogging("An error occured while send command to servo controller: ", _lastMountError->load());
|
|
} else {
|
|
logDebug(" the 'hardwareSetState' method performed successfully!");
|
|
}
|
|
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
|
|
void Asibfm700Mount::logMountPos(telemetry_t::telemetry_data_t const& tdata)
|
|
{
|
|
// NOTE: the implementation of MccTelemetry class guarantees that
|
|
// tdata.mountPos and tdata.targetPos are coordinates in the HA-DEC
|
|
// equathorial system
|
|
|
|
logTrace("Current telemetry data:");
|
|
|
|
logTrace(" current target: HA = {}, DEC = {} (encoders: {} {})",
|
|
mcc::impl::MccAngle(tdata.targetPos.co_lon()).sexagesimal(true),
|
|
mcc::impl::MccAngle(tdata.targetPos.co_lat()).sexagesimal(), tdata.targetXY.x().sexagesimal(),
|
|
tdata.targetXY.y().sexagesimal());
|
|
|
|
logTrace(" current mount: HA = {}, DEC = {} (encoders: {} {})",
|
|
mcc::impl::MccAngle(tdata.mountPos.co_lon()).sexagesimal(true),
|
|
mcc::impl::MccAngle(tdata.mountPos.co_lat()).sexagesimal(), tdata.hwState.XY.x().sexagesimal(),
|
|
tdata.hwState.XY.y().sexagesimal());
|
|
|
|
logTrace(" current mount speed: HA = {}/s, DEC = {}/s",
|
|
mcc::impl::MccAngleFancyString(tdata.hwState.speedXY.x()),
|
|
mcc::impl::MccAngleFancyString(tdata.hwState.speedXY.y()));
|
|
|
|
_pathFile.addToPath(tdata);
|
|
|
|
auto dist = tdata.mountPos.distance(tdata.targetPos);
|
|
|
|
logTrace(" target-to-mount distance: {} (dx = {}, dy = {})", mcc::impl::MccAngleFancyString(dist.dist),
|
|
mcc::impl::MccAngleFancyString(dist.dx), mcc::impl::MccAngleFancyString(dist.dy));
|
|
}
|
|
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemetry_data_t const& tdata, bool predicted)
|
|
{
|
|
bool in_zone;
|
|
std::vector<bool> in_zone_vec;
|
|
|
|
if (predicted) {
|
|
mcc::impl::MccGenXY braking_accel{getMovementParams().brakingAccelX, getMovementParams().brakingAccelY}, dxy{};
|
|
|
|
auto new_pos = this->coordsAfterTime(tdata.mountPos, tdata.hwState.speedXY, braking_accel,
|
|
getMovementParams().minTimeToPZone, &dxy);
|
|
|
|
logTrace(" the distance that will be covered in the next {} seconds: HA-axis: {}, DEC-axis: {}",
|
|
getMovementParams().minTimeToPZone.count(), mcc::impl::MccAngleFancyString(dxy.x()),
|
|
mcc::impl::MccAngleFancyString(dxy.y()));
|
|
|
|
*_lastMountError = this->inPZone(new_pos, &in_zone, &in_zone_vec);
|
|
} else {
|
|
*_lastMountError = this->inPZone(tdata.mountPos, &in_zone, &in_zone_vec);
|
|
}
|
|
|
|
if (_lastMountError->load()) {
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
if (in_zone) {
|
|
size_t i = 0;
|
|
for (; i < in_zone_vec.size(); ++i) {
|
|
if (in_zone_vec[i]) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
auto names = this->pzoneNames();
|
|
auto it = names.begin();
|
|
std::ranges::advance(it, i);
|
|
|
|
logError("target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i,
|
|
*it);
|
|
|
|
mcc::impl::MccSkyHADEC_OBS hadec;
|
|
mcc::impl::MccSkyRADEC_OBS radec;
|
|
mcc::impl::MccSkyAZZD azzd;
|
|
mcc::impl::MccSkyAZALT azalt;
|
|
mcc::impl::MccAngle lst;
|
|
|
|
*_lastMountError = tdata.mountPos.appSideralTime(&lst, true);
|
|
if (_lastMountError->load()) {
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
*_lastMountError = tdata.mountPos.toAtSameEpoch(radec, hadec, azzd, azalt);
|
|
if (_lastMountError->load()) {
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
logError(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", radec.x().sexagesimal(true), radec.y().sexagesimal(),
|
|
hadec.x().sexagesimal(true), lst.sexagesimal(true));
|
|
logError(" AZ, ZD, ALT: {}, {}, {}", azzd.x().sexagesimal(), azzd.y().sexagesimal(),
|
|
azalt.y().sexagesimal());
|
|
|
|
logError(" hardware X, Y: {}, {}", tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal());
|
|
|
|
if (predicted) {
|
|
return *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_NEAR_PZONE;
|
|
} else {
|
|
return *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_IN_PZONE;
|
|
}
|
|
}
|
|
|
|
return *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
|
|
}
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop)
|
|
{
|
|
// if mount is already slewing then stop and re-slew
|
|
if (_mountStatus->load() == mount_status_t::MOUNT_STATUS_SLEWING ||
|
|
_mountStatus->load() == mount_status_t::MOUNT_STATUS_ADJUSTING) {
|
|
*_lastMountError = stoppingImpl();
|
|
|
|
if (_lastMountError->load()) {
|
|
return _lastMountError->load();
|
|
}
|
|
}
|
|
|
|
*_stopMovementRequest = false;
|
|
|
|
_pathFile.clearPath();
|
|
|
|
_enteredBackupCoordinates = telemetry_t::getPointingTarget();
|
|
|
|
typename telemetry_t::telemetry_data_t tdata;
|
|
mcc::impl::MccSkyPoint start_target;
|
|
mcc::impl::MccSkyHADEC_OBS init_target;
|
|
decltype(tdata.targetXY) start_targetXY;
|
|
|
|
*_lastMountError = telemetryData(&tdata);
|
|
if (_lastMountError->load()) {
|
|
errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
*_lastMountError = checkPZone(tdata, false);
|
|
if (_lastMountError->load()) {
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
if (slew_and_stop) {
|
|
// store initial target coordinates in the "slew-and-stop" mode
|
|
// and use it below!
|
|
start_target = tdata.targetPos;
|
|
|
|
init_target.setX(tdata.targetPos.co_lon());
|
|
init_target.setY(tdata.targetPos.co_lat());
|
|
|
|
start_targetXY = tdata.targetXY;
|
|
}
|
|
|
|
|
|
logInfo("Start slewing in mode '{}'", slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK");
|
|
logInfo(" slewing process timeout: {} secs", _currentMovementParams.slewTimeout.count());
|
|
logInfo(" slewing tolerance radius: {} arcsecs",
|
|
mcc::impl::MccAngle{_currentMovementParams.slewToleranceRadius}.arcsecs());
|
|
|
|
logInfo(" braking acceleration X: {} degs/s^2",
|
|
mcc::impl::MccAngle(_currentMovementParams.brakingAccelX).degrees());
|
|
logInfo(" braking acceleration Y: {} degs/s^2",
|
|
mcc::impl::MccAngle(_currentMovementParams.brakingAccelY).degrees());
|
|
logInfo(" min time to prohibited zone: {} seconds", _currentMovementParams.minTimeToPZone.count());
|
|
|
|
if (!_currentMovementParams.slewingPathFilename.empty()) { // slewing trajectory file
|
|
logInfo("Set slewing path filename to {}", _currentMovementParams.slewingPathFilename);
|
|
} else {
|
|
logWarn("Slewing path filename is empty! Do not save it!");
|
|
}
|
|
|
|
|
|
_pathFile.addComment("");
|
|
_pathFile.addComment(std::format("Slewing trajectory, {}", std::chrono::system_clock::now()));
|
|
_pathFile.addComment("Config:");
|
|
_pathFile.addComment(std::format(" slewing tolerance radius: {} arcsecs",
|
|
mcc::impl::MccAngle{_currentMovementParams.slewToleranceRadius}.arcsecs()));
|
|
_pathFile.addComment(
|
|
std::format(" slewing process timeout: {} secs", _currentMovementParams.slewTimeout.count()));
|
|
_pathFile.addComment("");
|
|
_pathFile.addDefaultComment();
|
|
|
|
typename hardware_t::hardware_state_t hw_state;
|
|
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 (LibSidServo.moveTo) ...
|
|
|
|
*_lastMountError = sendToHardware(hw_state);
|
|
|
|
if (_lastMountError->load()) {
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
|
|
_pathFile.clearPath();
|
|
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_SLEWING;
|
|
|
|
auto start_point = std::chrono::steady_clock::now();
|
|
auto last_hw_time = tdata.hwState.XY.epoch().UTC();
|
|
|
|
hw_state.movementState = hardware_t::hardware_movement_state_t::HW_MOVE_ADJUSTING;
|
|
|
|
mcc::impl::MccSkyPoint::dist_result_t dist;
|
|
|
|
while (!_stopMovementRequest->load()) {
|
|
// check for slewing timeout
|
|
if ((std::chrono::steady_clock::now() - start_point) > getMovementParams().slewTimeout) {
|
|
*_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_SLEW_TIMEOUT;
|
|
break;
|
|
}
|
|
|
|
|
|
// get current mount and target coordinates
|
|
*_lastMountError = telemetryData(&tdata);
|
|
|
|
if (_lastMountError->load()) {
|
|
errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
|
|
// return _lastMountError->load();
|
|
break;
|
|
}
|
|
|
|
// detect hardware errors
|
|
if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_ERROR) {
|
|
*_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_HARDWARE;
|
|
break;
|
|
}
|
|
|
|
// check for prohibited zones proximity
|
|
*_lastMountError = checkPZone(tdata, true);
|
|
if (_lastMountError->load()) {
|
|
break;
|
|
}
|
|
|
|
if (last_hw_time >= tdata.hwState.XY.epoch().UTC()) {
|
|
logTrace("\nSame hardware timepoint! Just continue to polling!\n");
|
|
|
|
auto d = getMovementParams().slewingTelemetryInterval / 3;
|
|
|
|
if (d.count()) {
|
|
std::this_thread::sleep_for(d);
|
|
} else {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
|
|
continue;
|
|
}
|
|
|
|
last_hw_time = tdata.hwState.XY.epoch().UTC();
|
|
|
|
hw_state.XY.setEpoch(ep_t::now());
|
|
|
|
// update target coordinates and corresponded encoder ones only in the "slew-and-track" mode
|
|
if (slew_and_stop) { // restore initial target coordinate
|
|
init_target.setEpoch(tdata.mountPos.epoch());
|
|
tdata.targetPos.from(init_target);
|
|
|
|
tdata.targetXY = start_targetXY;
|
|
tdata.targetXY.setEpoch(tdata.mountPos.epoch());
|
|
} else {
|
|
hw_state.XY.setX(tdata.targetXY.x());
|
|
hw_state.XY.setY(tdata.targetXY.y());
|
|
}
|
|
|
|
|
|
logMountPos(tdata);
|
|
|
|
if (_stopMovementRequest->load()) { // external stop
|
|
break;
|
|
}
|
|
|
|
if (slew_and_stop) { // just wait for axes stopping
|
|
if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_STOPPED) {
|
|
break;
|
|
}
|
|
} else {
|
|
dist = tdata.mountPos.distance(tdata.targetPos);
|
|
|
|
if (dist.dist <= getMovementParams().slewToleranceRadius) {
|
|
logInfo("target-to-mount distance is lesser than slew acceptable radius - exit!");
|
|
|
|
break;
|
|
}
|
|
|
|
if (dist.dist <= getMovementParams().adjustCoordDiff) { // start adjusting (precise pointing)
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_ADJUSTING;
|
|
|
|
// LibSidServo.correctTo
|
|
*_lastMountError = sendToHardware(hw_state);
|
|
|
|
if (_lastMountError->load()) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_STOPPED &&
|
|
// !_stopMovementRequest->load()) {
|
|
// // the mount stopped but still to far from target position!!!
|
|
// if (dist.dist > getMovementParams().slewToleranceRadius) {
|
|
// *_mountStatus = mount_status_t::MOUNT_STATUS_ADJUSTING;
|
|
|
|
// *_lastMountError = sendToHardware(hw_state);
|
|
|
|
// if (_lastMountError->load()) {
|
|
// break;
|
|
// }
|
|
// } else { // just exit
|
|
// break;
|
|
// }
|
|
// }
|
|
|
|
// sleep here
|
|
std::this_thread::sleep_for(getMovementParams().slewingTelemetryInterval);
|
|
}
|
|
|
|
if (_stopMovementRequest->load()) { // external stop
|
|
logWarn("Slewing was stopped!");
|
|
*_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_STOPPED;
|
|
} else {
|
|
if (_lastMountError->load()) {
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
|
|
auto err = _lastMountError->load();
|
|
logError("Slewing finished with error: {} ({}::{})", err.message(), err.category().name(), err.value());
|
|
} else {
|
|
logInfo("Slewing finished without errors!");
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_IDLE;
|
|
}
|
|
}
|
|
|
|
if (!_lastMountError->load()) { // get final position
|
|
*_lastMountError = telemetryData(&tdata);
|
|
|
|
if (_lastMountError->load()) {
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
|
|
errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
|
|
} else {
|
|
logInfo("Final mount slewing position:");
|
|
|
|
if (slew_and_stop) { // restore initial target position
|
|
init_target.setEpoch(tdata.mountPos.epoch());
|
|
tdata.targetPos.from(init_target);
|
|
|
|
tdata.targetXY = start_targetXY;
|
|
}
|
|
logMountPos(tdata);
|
|
}
|
|
}
|
|
|
|
bool ok = _pathFile.saveToFile(_currentMovementParams.slewingPathFilename);
|
|
if (!ok && !_currentMovementParams.slewingPathFilename.empty()) {
|
|
logError("Cannot save trajectory file!");
|
|
}
|
|
_pathFile.clearPath();
|
|
|
|
if (!_lastMountError->load() && !_stopMovementRequest->load()) { // do not start tracking if an error occured
|
|
if (!slew_and_stop) { // or stop requested
|
|
// return trackTarget();
|
|
return trackingImpl();
|
|
}
|
|
} else {
|
|
*_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
|
|
}
|
|
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::trackingImpl()
|
|
{
|
|
// if (_mountStatus->load() == mount_status_t::MOUNT_STATUS_TRACKING) { // just ignore
|
|
// return mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
|
|
// }
|
|
|
|
*_stopMovementRequest = false;
|
|
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_TRACKING;
|
|
_pathFile.clearPath();
|
|
|
|
logInfo("Start tracking");
|
|
logInfo(" braking acceleration X: {} degs/s^2",
|
|
mcc::impl::MccAngle(_currentMovementParams.brakingAccelX).degrees());
|
|
logInfo(" braking acceleration Y: {} degs/s^2",
|
|
mcc::impl::MccAngle(_currentMovementParams.brakingAccelY).degrees());
|
|
logInfo(" min time to prohibited zone: {} seconds", _currentMovementParams.minTimeToPZone.count());
|
|
logInfo(" max target-to-mount distance: {} arcseconds",
|
|
mcc::impl::MccAngle(_currentMovementParams.trackingMaxCoordDiff).arcsecs());
|
|
|
|
if (!_currentMovementParams.trackingPathFilename.empty()) { // tracking trajectory file
|
|
logInfo("Set tracking path filename to {}", _currentMovementParams.trackingPathFilename);
|
|
} else {
|
|
logWarn("Tracking path filename is empty! Do not save it!");
|
|
}
|
|
|
|
telemetry_data_t tdata;
|
|
|
|
*_lastMountError = telemetryData(&tdata);
|
|
|
|
if (_lastMountError->load()) {
|
|
errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
|
|
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
|
|
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
|
|
mcc::impl::MccSkyPoint::dist_result_t dist;
|
|
dist = tdata.mountPos.distance(tdata.targetPos);
|
|
|
|
if (dist.dist > _currentMovementParams.trackingMaxCoordDiff) {
|
|
logInfo(
|
|
"The target-to-mount distance {} is greater than allowed one ({})! Track current "
|
|
"mount position!",
|
|
mcc::impl::MccAngleFancyString(dist.dist),
|
|
mcc::impl::MccAngleFancyString(_currentMovementParams.trackingMaxCoordDiff));
|
|
|
|
_enteredBackupCoordinates = telemetry_t::getPointingTarget();
|
|
|
|
setPointingTarget(tdata.mountPos);
|
|
|
|
std::string s;
|
|
mcc::impl::MccSerializer<mcc::impl::MccSkyPoint> ser;
|
|
ser(s, tdata.mountPos);
|
|
logTrace("The target point is set to {}", s);
|
|
|
|
*_lastMountError = telemetryData(&tdata);
|
|
|
|
if (_lastMountError->load()) {
|
|
errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
|
|
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
|
|
|
|
return _lastMountError->load();
|
|
}
|
|
}
|
|
|
|
*_lastMountError = checkPZone(tdata, true);
|
|
if (_lastMountError->load()) {
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
_pathFile.addComment("");
|
|
_pathFile.addComment(std::format("Tracking trajectory, {}", std::chrono::system_clock::now()));
|
|
_pathFile.addDefaultComment();
|
|
|
|
typename hardware_t::hardware_state_t hw_state;
|
|
using ep_t = decltype(hw_state.XY.epoch());
|
|
|
|
hw_state.movementState = hardware_t::hardware_movement_state_t::HW_MOVE_TRACKING;
|
|
|
|
logTrace("Start main tracking cycle ...");
|
|
|
|
while (!_stopMovementRequest->load()) {
|
|
*_lastMountError = telemetryData(&tdata);
|
|
|
|
if (_lastMountError->load()) {
|
|
errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
|
|
break;
|
|
}
|
|
|
|
logMountPos(tdata);
|
|
|
|
*_lastMountError = checkPZone(tdata, true);
|
|
if (_lastMountError->load()) {
|
|
break;
|
|
}
|
|
|
|
// detect hardware errors
|
|
if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_ERROR) {
|
|
*_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_HARDWARE;
|
|
break;
|
|
}
|
|
|
|
hw_state.XY.setX(tdata.targetXY.x());
|
|
hw_state.XY.setY(tdata.targetXY.y());
|
|
hw_state.XY.setEpoch(ep_t::now());
|
|
|
|
*_lastMountError = sendToHardware(hw_state);
|
|
if (_lastMountError->load()) {
|
|
break;
|
|
}
|
|
|
|
if (_stopMovementRequest->load()) {
|
|
break;
|
|
}
|
|
|
|
// sleep here
|
|
std::this_thread::sleep_for(_currentMovementParams.trackingTelemetryInterval);
|
|
}
|
|
|
|
logWarn("Tracking was stopped!");
|
|
|
|
if (_lastMountError->load()) {
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
|
|
auto err = _lastMountError->load();
|
|
logError("Tracking finished with error: {} ({}::{})", err.message(), err.category().name(), err.value());
|
|
} else { // ???!!!!!
|
|
*_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
|
|
logInfo("Tracking finished without errors!");
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_IDLE;
|
|
}
|
|
|
|
bool ok = _pathFile.saveToFile(_currentMovementParams.trackingPathFilename, std::ios::out | std::ios::app);
|
|
if (!ok && !_currentMovementParams.trackingPathFilename.empty()) {
|
|
logError("Cannot save trajectory file!");
|
|
}
|
|
_pathFile.clearPath();
|
|
|
|
return *_lastMountError;
|
|
}
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl()
|
|
{
|
|
logInfo("Stop mount requested");
|
|
|
|
*_stopMovementRequest = true;
|
|
|
|
typename AsibFM700ServoController::hardware_state_t hw_state;
|
|
|
|
hw_state.movementState = AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPING;
|
|
|
|
*_lastMountError = sendToHardware(hw_state);
|
|
|
|
if (!_lastMountError->load()) {
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_STOPPING;
|
|
|
|
auto start_tp = std::chrono::steady_clock::now();
|
|
|
|
*_lastMountError = _servolController.hardwareGetState(&hw_state);
|
|
|
|
logDebug(" wait for mount stopping (timeout is {} seconds) ...",
|
|
_mountConfig.getValue<std::chrono::seconds>("stopTimeout").value().count());
|
|
while (hw_state.movementState != AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPED &&
|
|
!_lastMountError->load()) {
|
|
if ((std::chrono::steady_clock::now() - start_tp) >
|
|
_mountConfig.getValue<std::chrono::seconds>("stopTimeout").value()) {
|
|
*_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_STOP_TIMEOUT;
|
|
break;
|
|
}
|
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(300));
|
|
|
|
*_lastMountError = _servolController.hardwareGetState(&hw_state);
|
|
}
|
|
}
|
|
|
|
if (!_lastMountError->load()) {
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_IDLE;
|
|
*_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
|
|
logInfo("The mount is stopped!");
|
|
|
|
telemetry_data_t tdata;
|
|
*_lastMountError = telemetryData(&tdata);
|
|
if (_lastMountError->load()) {
|
|
errorLogging("An error occured while trying to get telemetry data: ", _lastMountError->load());
|
|
} else {
|
|
logDebug("Mount stopped at HA = {}, DEC = {} (encoder: {} {})",
|
|
mcc::impl::MccAngle(tdata.mountPos.co_lon()).sexagesimal(true),
|
|
mcc::impl::MccAngle(tdata.mountPos.co_lat()).sexagesimal(), tdata.hwState.XY.x().sexagesimal(),
|
|
tdata.hwState.XY.y().sexagesimal());
|
|
}
|
|
} else {
|
|
*_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
|
|
errorLogging("An error occured while stoppping mount: ", _lastMountError->load());
|
|
|
|
logError("Try to perform emergency stop!");
|
|
*_lastMountError = static_cast<AsibFM700ServoControllerErrorCode>(Mount.emergStop());
|
|
if (_lastMountError->load()) {
|
|
errorLogging("An error occured while trying emergency stop: ", _lastMountError->load());
|
|
}
|
|
}
|
|
|
|
return _lastMountError->load();
|
|
}
|
|
|
|
} // namespace asibfm700
|