timur/ASIBFM700
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

...

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2026-02-20 08:51:09 +03:00
parent ca5618789e
commit a4d4e15f18
109 changed files with 11460 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

337
asibfm700_mount.cpp Normal file
View File

@@ -0,0 +1,337 @@
#include "asibfm700_mount.h"
#include <mcc/mcc_coordinate.h>
#include <mcc/mcc_pzone.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(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()
{
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("");
logDebug("Delete previously defined prohobited 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();
logInfo("");
logInfo("Hardware initialization ...");
logInfo(" set hardware configuration:");
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 this->_hardwareConfig.devConfig part and paths!!!
// this->_hardwareConfig = hw_cfg;
_servolController.hardwareUpdateConfig(hw_cfg.devConfig);
logInfo("");
logInfo(" EEPROM data:");
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();
mcc::impl::MccAngle 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("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 = hw_cfg.hwConfig.Yconf.accel; // Sidereal defines HA-axis as Y-axis
mpars.brakingAccelY = hw_cfg.hwConfig.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");
// update Eddy's LibSidServo internal config
_servolController.hardwareUpdateConfig(hw_cfg.hwConfig);
coordpair_t cp;
Mount.getMaxSpeed(&cp);
logInfo("Check mount max speed: {} {}", cp.Y, cp.X);
_servolController.hardwareInit();
// 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!");
}
logInfo("ERFA engine initialization ...");
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()});
// set ERFA state
// Asibfm700CCTE::engine_state_t ccte_state{
// .meteo = Asibfm700CCTE::_currentState.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);
}
// setTelemetryDataUpdateInterval(_mountConfig.hardwarePollingPeriod());
setTelemetryDataTimeout(_mountConfig.movingModelParams().telemetryTimeout);
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
telemetry_data_t tdata;
auto t_err = telemetryData(&tdata);
if (t_err) {
logError(gm_class_t::formatError(t_err, "Cannot update 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;
}
/* 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());
}
}
} // namespace asibfm700