295 lines
9.1 KiB
C++
295 lines
9.1 KiB
C++
|
|
#pragma once
|
|
|
|
|
|
/* MOUNT CONTROL COMPONENTS LIBRARY */
|
|
|
|
/* AN REFERENCE "PERIODIC-ERROR-CORRECTION" CLASS IMPLEMENTATION */
|
|
|
|
|
|
#include <mutex>
|
|
#include "fitpack/fitpack.h"
|
|
#include "mcc_mount_concepts.h"
|
|
#include "mcc_mount_coord.h"
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
enum class MccMountDefaultPECErrorCode : int { ERROR_OK, ERROR_INVALID_INPUTS_BISPLEV, ERROR_EXCEED_MAX_ITERS };
|
|
|
|
/* error category definition */
|
|
|
|
// error category
|
|
struct MccMountDefaultPECCategory : public std::error_category {
|
|
MccMountDefaultPECCategory() : std::error_category() {}
|
|
|
|
const char* name() const noexcept { return "ADC_GENERIC_DEVICE"; }
|
|
|
|
std::string message(int ec) const
|
|
{
|
|
MccMountDefaultPECErrorCode err = static_cast<MccMountDefaultPECErrorCode>(ec);
|
|
|
|
switch (err) {
|
|
case MccMountDefaultPECErrorCode::ERROR_OK:
|
|
return "OK";
|
|
case MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV:
|
|
return "invalid input arguments for bispev";
|
|
case MccMountDefaultPECErrorCode::ERROR_EXCEED_MAX_ITERS:
|
|
return "exceed maximum of iterations number";
|
|
default:
|
|
return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
static const MccMountDefaultPECCategory& get()
|
|
{
|
|
static const MccMountDefaultPECCategory constInst;
|
|
return constInst;
|
|
}
|
|
};
|
|
|
|
|
|
inline std::error_code make_error_code(MccMountDefaultPECErrorCode ec)
|
|
{
|
|
return std::error_code(static_cast<int>(ec), MccMountDefaultPECCategory::get());
|
|
}
|
|
} // namespace mcc
|
|
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::MccMountDefaultPECErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
|
|
namespace mcc
|
|
{
|
|
|
|
|
|
// type of PEC corrections (algorithm used):
|
|
// PEC_TYPE_GEOMETRY - "classic" geometry-based correction coefficients
|
|
// PEC_TYPE_GEOMETRY_BSPLINE - previous one and additional 2D B-spline corrections
|
|
// PEC_TYPE_BSPLINE - pure 2D B-spline corrections
|
|
enum class MccMountDefaultPECType { PEC_TYPE_GEOMETRY, PEC_TYPE_GEOMETRY_BSPLINE, PEC_TYPE_BSPLINE };
|
|
|
|
template <MccMountType MOUNT_TYPE>
|
|
class MccMountDefaultPEC final
|
|
{
|
|
public:
|
|
static constexpr MccMountType mountType = MOUNT_TYPE;
|
|
|
|
typedef std::error_code error_t;
|
|
typedef MccAngle coord_t;
|
|
|
|
struct pec_result_t {
|
|
coord_t dx, dy;
|
|
};
|
|
|
|
// "classic" geometric PEC coefficients
|
|
struct pec_geom_coeffs_t {
|
|
typedef double coeff_t;
|
|
|
|
coeff_t zeroPointX;
|
|
coeff_t zeroPointY;
|
|
coeff_t collimationErr; // tube collimation error
|
|
coeff_t nonperpendErr; // X-Y axes nonperpendicularity
|
|
coeff_t misalignErr1; // misalignment of hour-angle/azimuth axis: left-right for equatorial, East-West for
|
|
// alt-azimuthal
|
|
coeff_t misalignErr2; // misalignment of hour-angle/azimuth axis: vertical for equatorial, North-South for
|
|
// alt-azimuthal
|
|
|
|
coeff_t tubeFlexure;
|
|
coeff_t forkFlexure;
|
|
coeff_t DECaxisFlexure; // declination axis flexure
|
|
};
|
|
|
|
// B-splines coefficients
|
|
struct pec_bspline_coeffs_t {
|
|
typedef double knot_t;
|
|
typedef double coeff_t;
|
|
|
|
size_t bsplDegreeX = 3;
|
|
size_t bsplDegreeY = 3;
|
|
|
|
std::vector<knot_t> knotsX{};
|
|
std::vector<knot_t> knotsY{};
|
|
|
|
std::vector<coeff_t> coeffsX{};
|
|
std::vector<coeff_t> coeffsY{};
|
|
};
|
|
|
|
struct pec_data_t {
|
|
MccMountDefaultPECType type{MccMountDefaultPECType::PEC_TYPE_GEOMETRY};
|
|
double siteLatitude{0.0}; // in radians
|
|
pec_geom_coeffs_t geomCoefficients{};
|
|
pec_bspline_coeffs_t bsplineCoefficients{};
|
|
};
|
|
|
|
// constructors
|
|
|
|
MccMountDefaultPEC(pec_data_t pdata)
|
|
: _pecData(std::move(pdata)),
|
|
_phi(_pecData.siteLatitude),
|
|
_geomCoeffs(_pecData.geomCoefficients),
|
|
_bsplCoeffs(_pecData.bsplineCoefficients),
|
|
_pecDataMutex(new std::mutex)
|
|
{
|
|
}
|
|
|
|
MccMountDefaultPEC(const MccMountDefaultPEC&) = delete;
|
|
MccMountDefaultPEC& operator=(const MccMountDefaultPEC&) = delete;
|
|
|
|
MccMountDefaultPEC(MccMountDefaultPEC&& other)
|
|
: _pecData(std::move(other._pecData)),
|
|
_phi(_pecData.siteLatitude),
|
|
_geomCoeffs(_pecData.geomCoefficients),
|
|
_bsplCoeffs(_pecData.bsplineCoefficients),
|
|
_pecDataMutex(std::move(other._pecDataMutex))
|
|
{
|
|
}
|
|
|
|
MccMountDefaultPEC& operator=(MccMountDefaultPEC&& other)
|
|
{
|
|
if (this == &other) {
|
|
return *this;
|
|
}
|
|
|
|
_pecData = std::move(other._pecData);
|
|
_phi = _pecData.siteLatitude;
|
|
_geomCoeffs = _pecData.geomCoefficients;
|
|
_bsplCoeffs = _pecData.bsplineCoefficients;
|
|
_pecDataMutex = std::move(other._pecDataMutex);
|
|
|
|
return *this;
|
|
}
|
|
|
|
void setData(pec_data_t pdata)
|
|
{
|
|
std::lock_guard lock(*_pecDataMutex);
|
|
|
|
_pecData = std::move(pdata);
|
|
_phi = _pecData.siteLatitude;
|
|
_geomCoeffs = _pecData.geomCoefficients;
|
|
_bsplCoeffs = _pecData.bsplineCoefficients;
|
|
}
|
|
|
|
|
|
pec_data_t getData() const
|
|
{
|
|
std::lock_guard lock(*_pecDataMutex);
|
|
|
|
return _pecData;
|
|
}
|
|
|
|
void setType(MccMountDefaultPECType type)
|
|
{
|
|
std::lock_guard lock(*_pecDataMutex);
|
|
|
|
_pecData.type = type;
|
|
}
|
|
|
|
MccMountDefaultPECType getType() const
|
|
{
|
|
std::lock_guard lock(*_pecDataMutex);
|
|
|
|
return _pecData.type;
|
|
}
|
|
|
|
// The computed PEC quantities must be interpretated as:
|
|
// apparent_X = encoder_X + pec_result_t.dx
|
|
// apparent_Y = encoder_Y + pec_result_t.dy
|
|
// so, input x and y are assumed to be mount axis encoder coordinates
|
|
error_t compute(const coord_t& x, const coord_t& y, pec_result_t& res)
|
|
{
|
|
std::lock_guard lock(*_pecDataMutex);
|
|
|
|
if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) { // equatorial
|
|
if (_pecData.type == MccMountDefaultPECType::PEC_TYPE_GEOMETRY) {
|
|
const auto cosPhi = std::cos(_phi);
|
|
const auto sinPhi = std::sin(_phi);
|
|
const auto tanY = std::tan(y);
|
|
const auto sinX = std::sin(x);
|
|
const auto cosX = std::cos(x);
|
|
const auto cosY = std::cos(y);
|
|
|
|
|
|
if (utils::isEqual(cosY, 0.0)) {
|
|
res.dx = _geomCoeffs.zeroPointX;
|
|
} else {
|
|
res.dx = _geomCoeffs.zeroPointX + _geomCoeffs.collimationErr / cosY +
|
|
_geomCoeffs.nonperpendErr * tanY - _geomCoeffs.misalignErr1 * cosX * tanY +
|
|
_geomCoeffs.misalignErr2 * sinX * tanY + _geomCoeffs.tubeFlexure * cosPhi * sinX / cosY -
|
|
_geomCoeffs.DECaxisFlexure * (cosPhi * cosX + sinPhi * tanY);
|
|
}
|
|
|
|
res.dy = _geomCoeffs.zeroPointY + _geomCoeffs.misalignErr1 * sinX + _geomCoeffs.misalignErr2 * cosX +
|
|
_geomCoeffs.tubeFlexure * (cosPhi * cosX * std::sin(y) - sinPhi * cosY);
|
|
|
|
if constexpr (mountType == MccMountType::FORK_TYPE) {
|
|
if (!utils::isEqual(cosX, 0.0)) {
|
|
res.dy += _geomCoeffs.forkFlexure / cosX;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (_pecData.type == MccMountDefaultPECType::PEC_TYPE_BSPLINE ||
|
|
_pecData.type == MccMountDefaultPECType::PEC_TYPE_GEOMETRY_BSPLINE) {
|
|
double spl_valX, spl_valY;
|
|
|
|
int ret = fitpack::fitpack_eval_spl2d(_bsplCoeffs.knotsX, _bsplCoeffs.knotsY, _bsplCoeffs.coeffsX, x, y,
|
|
spl_valX, _bsplCoeffs.bsplDegreeX, _bsplCoeffs.bsplDegreeY);
|
|
|
|
if (ret) {
|
|
res.dx = std::numeric_limits<double>::quiet_NaN();
|
|
res.dy = std::numeric_limits<double>::quiet_NaN();
|
|
return MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
|
|
}
|
|
|
|
|
|
ret = fitpack::fitpack_eval_spl2d(_bsplCoeffs.knotsX, _bsplCoeffs.knotsY, _bsplCoeffs.coeffsY, x, y,
|
|
spl_valY, _bsplCoeffs.bsplDegreeX, _bsplCoeffs.bsplDegreeY);
|
|
|
|
if (ret) {
|
|
res.dx = std::numeric_limits<double>::quiet_NaN();
|
|
res.dy = std::numeric_limits<double>::quiet_NaN();
|
|
return MccMountDefaultPECErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
|
|
}
|
|
|
|
|
|
res.dx += spl_valX;
|
|
res.dy += spl_valY;
|
|
}
|
|
} else if constexpr (mcc_is_altaz_mount<MOUNT_TYPE>) {
|
|
} else {
|
|
static_assert(false, "UNSUPPORTED");
|
|
}
|
|
|
|
return MccMountDefaultPECErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
private:
|
|
pec_data_t _pecData;
|
|
double& _phi;
|
|
pec_geom_coeffs_t& _geomCoeffs;
|
|
pec_bspline_coeffs_t& _bsplCoeffs;
|
|
|
|
std::unique_ptr<std::mutex> _pecDataMutex;
|
|
};
|
|
|
|
|
|
typedef MccMountDefaultPEC<MccMountType::ALTAZ_TYPE> MccMountDefaultAltAzPec;
|
|
typedef MccMountDefaultPEC<MccMountType::FORK_TYPE> MccMountDefaultForkPec;
|
|
|
|
static_assert(traits::mcc_mount_pec_c<MccMountDefaultForkPec>, "");
|
|
static_assert(std::movable<MccMountDefaultForkPec>);
|
|
|
|
|
|
} // namespace mcc
|