diff --git a/include/mcc/mcc_coordinate.h b/include/mcc/mcc_coordinate.h index 5908b75..6e5389e 100644 --- a/include/mcc/mcc_coordinate.h +++ b/include/mcc/mcc_coordinate.h @@ -196,14 +196,18 @@ protected: { if constexpr (pairKind != MccCoordPairKind::COORDS_KIND_GENERIC && pairKind != MccCoordPairKind::COORDS_KIND_XY) { - // if constexpr (pairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP || - // pairKind == MccCoordPairKind::COORDS_KIND_HADEC_OBS) { - // _x = (double)MccAngle(_x).normalize(); - // } else { // RA, AZ - // _x = (double)MccAngle(_x).normalize(); - // } + if constexpr (pairKind == MccCoordPairKind::COORDS_KIND_HADEC_APP || + pairKind == MccCoordPairKind::COORDS_KIND_HADEC_OBS) { + _x = (double)MccAngle(_x).normalize(); + } else { // RA, AZ + _x = (double)MccAngle(_x).normalize(); + } - _x = (double)MccAngle(_x).normalize(); + // _x = (double)MccAngle(_x).normalize(); + + // if (utils::isEqual((double)_x, MCC_TWO_PI)) { + // _x = 0.0; + // } // DEC and ALT is [-90,90] degrees if constexpr (pairKind != MccCoordPairKind::COORDS_KIND_AZZD) { diff --git a/include/mcc/mcc_generic_movecontrols.h b/include/mcc/mcc_generic_movecontrols.h index 537a42a..32eefc1 100644 --- a/include/mcc/mcc_generic_movecontrols.h +++ b/include/mcc/mcc_generic_movecontrols.h @@ -388,12 +388,6 @@ public: self._wakeupRequest->clear(); - log_str = std::format("\n{:*^80}\n", " UNLOCKED "); - if constexpr (mcc_generic_mount_c) { - self.logTrace(log_str); - } else { - std::println("{}", log_str); - } // if (stoken.stop_requested()) { // break; @@ -401,6 +395,17 @@ public: do_state = self._fsmState->load(); + log_str = std::format(" UNLOCKED WITH STATE (STOP = {} SLEW = {} TRACK = {}) ", + do_state & STATE_STOP, do_state & STATE_SLEW, do_state & STATE_TRACK); + + log_str = std::format("\n{:*^80}\n", log_str); + if constexpr (mcc_generic_mount_c) { + self.logTrace(log_str); + } else { + std::println("{}", log_str); + } + + if (do_state & STATE_STOP) { // if (_fsmState->load() & STATE_STOP) { *self._stopMovementRequest = true; diff --git a/include/mcc/mcc_pcm.h b/include/mcc/mcc_pcm.h index 27400a3..671251a 100644 --- a/include/mcc/mcc_pcm.h +++ b/include/mcc/mcc_pcm.h @@ -334,10 +334,19 @@ public: } dfx, dfy, df; // partial derivatives ret = _computeFuncDeriv(x, y, res, true, &dfx, &dfy); - if (!ret) { + if (ret) { return ret; } + // are all geometry coeffs equal to 0? + if (utils::isEqual(dfx.pcmX, 0.0) && utils::isEqual(dfy.pcmX, 0.0) && utils::isEqual(dfx.pcmY, 0.0) && + utils::isEqual(dfy.pcmY, 0.0)) { + if constexpr (mcc_coord_pair_c) { + *hw_pt = MccCoordPair{x, y, obs_skycoord.epoch()}; + } + + return MccDefaultPCMErrorCode::ERROR_OK; + } dfx.pcmX += 1.0; // a dfx.pcmY += 1.0; // b @@ -356,15 +365,26 @@ public: // -1 | d -b | // A = 1/detA * | -c a | // + // and + // + // -1 | pcmX | | d*pcmX - b*pcmY | + // A * | pcmY | = | -c*pcmX + a*pcmY | + // - df.pcmX = dfy.pcmY * res->pcmX - dfx.pcmY * res->pcmY; - df.pcmY = -dfy.pcmX * res->pcmX + dfx.pcmX * res->pcmY; + df.pcmX = (dfy.pcmY * res->pcmX - dfx.pcmY * res->pcmY) / detJ; + df.pcmY = (-dfy.pcmX * res->pcmX + dfx.pcmX * res->pcmY) / detJ; - res->pcmX -= df.pcmX; - res->pcmY -= df.pcmY; + res->pcmX = -df.pcmX; + res->pcmY = -df.pcmY; + // res->pcmX -= df.pcmX; + // res->pcmY -= df.pcmY; + // if constexpr (mcc_coord_pair_c) { + // *hw_pt = MccCoordPair{res->pcmX, res->pcmY, obs_skycoord.epoch()}; + // } if constexpr (mcc_coord_pair_c) { - *hw_pt = MccCoordPair{res->pcmX, res->pcmY, obs_skycoord.epoch()}; + *hw_pt = + MccCoordPair{x + res->pcmX, y + res->pcmY, obs_skycoord.epoch()}; } } else { // for B-splines the result is computed directly from inverse B-spline coefficients ret = _computeFuncDeriv(x, y, res);