remove guiding model
now it are only slewing and tracking states
This commit is contained in:
189
mcc/mcc_pzone.h
189
mcc/mcc_pzone.h
@@ -12,7 +12,7 @@ namespace mcc
|
||||
{
|
||||
|
||||
|
||||
enum MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM };
|
||||
enum MccAltLimitPZErrorCode : int { ERROR_OK, ERROR_NULLPTR, ERROR_COORD_TRANSFROM, ERROR_PCM_COMP };
|
||||
|
||||
} // namespace mcc
|
||||
|
||||
@@ -56,6 +56,8 @@ struct MccAltLimitPZCategory : public std::error_category {
|
||||
return "input argument os nullptr";
|
||||
case MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM:
|
||||
return "coordinate transformation error";
|
||||
case MccAltLimitPZErrorCode::ERROR_PCM_COMP:
|
||||
return "PCM computation error";
|
||||
default:
|
||||
return "UNKNOWN";
|
||||
}
|
||||
@@ -85,6 +87,8 @@ protected:
|
||||
static constexpr auto pi2 = std::numbers::pi * 2.0;
|
||||
|
||||
public:
|
||||
static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_STOP;
|
||||
|
||||
typedef std::error_code error_t;
|
||||
|
||||
MccAltLimitPZ(mcc_angle_c auto const& alt_limit, mcc_angle_c auto const& latitude, mcc_ccte_c auto* ccte_engine)
|
||||
@@ -551,4 +555,187 @@ protected:
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
/* co-longitude axis (HA or AZ) limit switch prohibited zone */
|
||||
|
||||
template <MccCoordKind AXIS_KIND>
|
||||
class MccAxisLimitSwitchPZ : public mcc_pzone_interface_t<std::error_code>
|
||||
{
|
||||
public:
|
||||
static_assert(AXIS_KIND == MccCoordKind::COORDS_KIND_AZ || AXIS_KIND == MccCoordKind::COORDS_KIND_HA,
|
||||
"UNSUPPORTED AXIS TYPE!");
|
||||
|
||||
typedef std::error_code error_t;
|
||||
|
||||
static constexpr MccCoordKind axisKind = AXIS_KIND;
|
||||
|
||||
static constexpr MccProhibitedZonePolicy pzPolicy = MccProhibitedZonePolicy::PZ_POLICY_FLIP;
|
||||
|
||||
//
|
||||
// min_limit_val and max_limit_val are hardware encoder angles in radians!
|
||||
//
|
||||
MccAxisLimitSwitchPZ(mcc_angle_c auto const& min_limit_val,
|
||||
mcc_angle_c auto const& max_limit_val,
|
||||
mcc_position_controls_c auto* controls)
|
||||
: _minLimit(min_limit_val), _maxLimit(max_limit_val)
|
||||
{
|
||||
_transformCoordinates = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t {
|
||||
if (to_pt == nullptr) {
|
||||
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
auto err = controls->transformCoordinates(from_pt, to_pt);
|
||||
if (!err) {
|
||||
return MccAltLimitPZErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
if (std::same_as<decltype(err), error_t>) {
|
||||
return err;
|
||||
} else {
|
||||
return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM;
|
||||
}
|
||||
};
|
||||
|
||||
_transformCoordinatesEqtHrzCoords = [controls](MccCelestialPoint from_pt, MccEqtHrzCoords* to_pt) -> error_t {
|
||||
if (to_pt == nullptr) {
|
||||
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
auto err = controls->transformCoordinates(from_pt, to_pt);
|
||||
if (!err) {
|
||||
return MccAltLimitPZErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
if (std::same_as<decltype(err), error_t>) {
|
||||
return err;
|
||||
} else {
|
||||
return MccAltLimitPZErrorCode::ERROR_COORD_TRANSFROM;
|
||||
}
|
||||
};
|
||||
|
||||
_computePCM = [controls](MccCelestialPoint from_pt, MccCelestialPoint* to_pt) -> error_t {
|
||||
MccPCMResult inv_res;
|
||||
auto err = controls->computeInversePCM(std::move(from_pt), &inv_res, to_pt);
|
||||
if (err) {
|
||||
return mcc_deduce_error<error_t>(err, MccAltLimitPZErrorCode::ERROR_PCM_COMP);
|
||||
}
|
||||
|
||||
return MccAltLimitPZErrorCode::ERROR_OK;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
consteval std::string_view name()
|
||||
{
|
||||
return axisKind == MccCoordKind::COORDS_KIND_AZ ? "AZ_AXIS-LIMITSWITCH_ZONE"
|
||||
: axisKind == MccCoordKind::COORDS_KIND_HA ? "HA_AXIS-LIMITSWITCH_ZONE"
|
||||
: "UKNOWN";
|
||||
}
|
||||
|
||||
|
||||
template <typename InputT>
|
||||
error_t inPZone(InputT coords, bool* result)
|
||||
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
||||
{
|
||||
if (result == nullptr) {
|
||||
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
|
||||
// assume here .X and are hardware encoder coordinate of corresponding axis
|
||||
*result = (coords.X < _maxLimit) && (coords.X > _minLimit);
|
||||
} else { // mcc_celestial_point_c
|
||||
if (coords.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware
|
||||
*result = (coords.X < _maxLimit) && (coords.X > _minLimit);
|
||||
} else { // here one needs transform input coordinates to hardware encoder ones
|
||||
MccCelestialPoint pt;
|
||||
|
||||
auto ret = getHWCoords(std::move(coords), &pt);
|
||||
if (ret) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
*result = (pt.X < _maxLimit) && (pt.X > _minLimit);
|
||||
}
|
||||
}
|
||||
|
||||
return MccAltLimitPZErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
template <typename InputT>
|
||||
error_t timeToPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
|
||||
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
||||
{
|
||||
using res_t = std::remove_cvref_t<decltype(*res_time)>;
|
||||
using period_t = typename res_t::period;
|
||||
|
||||
double time_ang;
|
||||
|
||||
if (res_time == nullptr) {
|
||||
return MccAltLimitPZErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
if constexpr (mcc_eqt_hrz_coord_c<InputT>) {
|
||||
// assume here .X and are hardware encoder coordinate of corresponding axis
|
||||
if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) {
|
||||
time_ang = (_maxLimit - coords.X) / mcc_sideral_to_UT1_ratio; // to UT1 scale
|
||||
} else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) {
|
||||
}
|
||||
} else { // mcc_celestial_point_c
|
||||
}
|
||||
|
||||
std::chrono::nanoseconds ns{
|
||||
static_cast<std::chrono::nanoseconds::rep>(time_ang * 43200.0 / std::numbers::pi * 1.0E9)};
|
||||
|
||||
period_t rat;
|
||||
*res_time = res_t{static_cast<typename res_t::rep>(time_ang * 43200.0 / std::numbers::pi * rat.den / rat.num)};
|
||||
}
|
||||
|
||||
template <typename InputT>
|
||||
error_t timeFromPZone(InputT coords, traits::mcc_time_duration_c auto* res_time)
|
||||
requires(mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>)
|
||||
{
|
||||
}
|
||||
|
||||
template <typename InputT, typename ResultT>
|
||||
error_t intersectPZone(InputT coords, ResultT* point)
|
||||
requires((mcc_eqt_hrz_coord_c<InputT> || mcc_celestial_point_c<InputT>) &&
|
||||
(mcc_eqt_hrz_coord_c<ResultT> || mcc_celestial_point_c<ResultT>))
|
||||
{
|
||||
}
|
||||
|
||||
protected:
|
||||
double _minLimit, _maxLimit;
|
||||
|
||||
std::function<error_t(MccCelestialPoint, MccCelestialPoint*)> _transformCoordinates{};
|
||||
std::function<error_t(MccCelestialPoint, MccEqtHrzCoords*)> _transformCoordinatesEqtHrzCoords{};
|
||||
|
||||
std::function<error_t(MccCelestialPoint, MccCelestialPoint*)> _computePCM{};
|
||||
|
||||
error_t getHWCoords(MccCelestialPoint from_pt, MccCelestialPoint* to_pt)
|
||||
{
|
||||
error_t ret = MccAltLimitPZErrorCode::ERROR_OK;
|
||||
|
||||
if (from_pt.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { // hardware
|
||||
to_pt->X = from_pt.X;
|
||||
to_pt->Y = from_pt.Y;
|
||||
} else { // here one needs transform input coordinates to hardware encoder ones
|
||||
if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_AZ) {
|
||||
to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
|
||||
|
||||
} else if constexpr (AXIS_KIND == MccCoordKind::COORDS_KIND_HA) {
|
||||
to_pt->pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
}
|
||||
|
||||
mcc_tp2tp(from_pt.time_point, to_pt->time_point);
|
||||
|
||||
ret = _transformCoordinates(std::move(from_pt), to_pt);
|
||||
if (!ret) {
|
||||
ret = _computePCM(*to_pt, to_pt);
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace mcc
|
||||
|
||||
Reference in New Issue
Block a user