250 lines
10 KiB
C++
250 lines
10 KiB
C++
#include "asibfm700_mount.h"
|
|
|
|
#include <mcc_pzone.h>
|
|
|
|
namespace asibfm700
|
|
{
|
|
|
|
|
|
/* CONSTRUCTOR AND DESTRUCTOR */
|
|
|
|
Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger)
|
|
: mcc::ccte::erfa::MccCCTE_ERFA({.meteo{.temperature = 10.0, .humidity = 0.5, .pressure = 1010.0},
|
|
.wavelength = config.refractWavelength(),
|
|
.lat = config.siteLatitude(),
|
|
.lon = config.siteLongitude(),
|
|
.elev = config.siteElevation()}),
|
|
Asibfm700PCM(config.pcmData()),
|
|
base_gm_class_t(Asibfm700StartState{},
|
|
std::make_tuple(config.servoControllerConfig()),
|
|
std::make_tuple(this),
|
|
std::make_tuple(),
|
|
std::make_tuple(this),
|
|
std::make_tuple(this),
|
|
std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)),
|
|
_mountConfig(config),
|
|
_mountConfigMutex(new std::mutex)
|
|
{
|
|
addMarkToPatternIdx("[ASIB-MOUNT]");
|
|
|
|
logDebug("Create Asibfm700Mount class instance ({})", this->getThreadId());
|
|
|
|
initMount();
|
|
}
|
|
// Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger)
|
|
// : mcc::ccte::erfa::MccCCTE_ERFA({.meteo{.temperature = 10.0, .humidity = 0.5, .pressure = 1010.0},
|
|
// .wavelength = config.refractWavelength(),
|
|
// .lat = config.siteLatitude(),
|
|
// .lon = config.siteLongitude(),
|
|
// .elev = config.siteElevation()}),
|
|
// Asibfm700PCM(config.pcmData()),
|
|
// base_gm_class_t(
|
|
// gm_class_t{AsibFM700ServoController{config.servoControllerConfig()}, mcc::MccTelemetry{this},
|
|
// Asibfm700PZoneContainer{}, mcc::MccSimpleSlewingModel{this}, mcc::MccSimpleTrackingModel{this},
|
|
// Asibfm700Logger{std::move(logger), Asibfm700Logger::LOGGER_DEFAULT_FORMAT}},
|
|
// Asibfm700StartState{}),
|
|
// _mountConfig(config),
|
|
// _mountConfigMutex(new std::mutex)
|
|
// {
|
|
// 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("Stop telemetry data updating");
|
|
stopInternalTelemetryDataUpdating();
|
|
|
|
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::MccAltLimitPZ<mcc::MccAltLimitKind::MIN_ALT_LIMIT>{_mountConfig.pzMinAltitude(),
|
|
_mountConfig.siteLatitude(), this});
|
|
|
|
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::MccAxisLimitSwitchPZ<mcc::MccCoordKind::COORDS_KIND_HA>{
|
|
_mountConfig.pzLimitSwitchHAMin(), _mountConfig.pzLimitSwitchHAMax(), this});
|
|
|
|
logInfo("{} prohibited zones were added successfully", pz_num);
|
|
|
|
auto mpars = _mountConfig.movingModelParams();
|
|
|
|
logInfo("");
|
|
logInfo("Setup slewing and tracking parameters ...");
|
|
mpars.slewRateX = _mountConfig.getValue<mcc::MccAngle>("hwMaxRateHA").value_or(0.0);
|
|
mpars.slewRateY = _mountConfig.getValue<mcc::MccAngle>("hwMaxRateDEC").value_or(0.0);
|
|
auto st_err = setSlewingParams(mpars);
|
|
if (st_err) {
|
|
errorLogging(" An error occured while setting slewing parameters: ", st_err);
|
|
} else {
|
|
logInfo(" Max HA-axis speed: {} degs/s", mcc::MccAngle(mpars.slewRateX).degrees());
|
|
logInfo(" Max DEC-axis speed: {} degs/s", mcc::MccAngle(mpars.slewRateY).degrees());
|
|
}
|
|
st_err = setTrackingParams(_mountConfig.movingModelParams());
|
|
if (st_err) {
|
|
errorLogging(" An error occured while setting tracking parameters: ", st_err);
|
|
}
|
|
logInfo("Slewing and tracking parameters have been set successfully");
|
|
|
|
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(" 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);
|
|
|
|
// auto hw_err = hardwareInit();
|
|
// if (hw_err) {
|
|
// errorLogging("", hw_err);
|
|
// }
|
|
|
|
// call base class initMount method
|
|
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 ...");
|
|
|
|
|
|
// 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);
|
|
}
|
|
|
|
setStateERFA(std::move(ccte_state));
|
|
|
|
setTelemetryUpdateTimeout(_mountConfig.movingModelParams().telemetryTimeout);
|
|
startInternalTelemetryDataUpdating();
|
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
|
bool ok = isInternalTelemetryDataUpdating();
|
|
if (ok) {
|
|
logInfo("Start updating telemetry data");
|
|
} else {
|
|
auto err = lastUpdateError();
|
|
logError("Cannot update telemetry data (err = {} [{}, {}])!", err.message(), err.value(),
|
|
err.category().name());
|
|
}
|
|
|
|
return mcc::MccGenericMountErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
Asibfm700Mount::error_t Asibfm700Mount::updateMountConfig(const Asibfm700MountConfig& cfg)
|
|
{
|
|
std::lock_guard lock{*_mountConfigMutex};
|
|
|
|
_mountConfig = cfg;
|
|
|
|
auto hw_cfg = _mountConfig.servoControllerConfig();
|
|
hardwareUpdateConfig(hw_cfg.devConfig);
|
|
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
|