#include "asibfm700_mount.h" #include #include #include "mcc/mcc_serializer.h" namespace asibfm700 { /* CONSTRUCTOR AND DESTRUCTOR */ Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr 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{_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{ _mountConfig.pzLimitSwitchHAMin(), _mountConfig.pzLimitSwitchHAMax(), &_pcm}); logInfo("{} prohibited zones were added successfully", pz_num); auto mpars = _mountConfig.movingModelParams(); using secs_t = std::chrono::duration; auto to_msecs = [](double secs) { auto s = secs_t{secs}; return std::chrono::duration_cast(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("hwMaxRateDEC").value_or(0.0); eeprom_cfg.Yslewrate = _mountConfig.getValue("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("hwMaxRateHA").value_or(0.0); mpars.slewRateY = _mountConfig.getValue("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("PIDMaxDt").value_or({}); auto min_dt_intvl = _mountConfig.getValue("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; } 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(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 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 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("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("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(Mount.emergStop()); if (_lastMountError->load()) { errorLogging("An error occured while trying emergency stop: ", _lastMountError->load()); } } return _lastMountError->load(); } } // namespace asibfm700