350 lines
15 KiB
C++
350 lines
15 KiB
C++
#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;
|
|
}
|
|
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::updateMountConfig(const std::string& cfg_filename)
|
|
{
|
|
Asibfm700MountConfig new_config;
|
|
auto err = new_config.load(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());
|
|
}
|
|
}
|
|
|
|
} // namespace asibfm700
|