diff --git a/include/mcc/mcc_movement_controls.h b/include/mcc/mcc_movement_controls.h index 635c8f1..14f945b 100644 --- a/include/mcc/mcc_movement_controls.h +++ b/include/mcc/mcc_movement_controls.h @@ -257,7 +257,7 @@ protected: return true; } - fst.open(_filename); + fst.open(_filename, std::ios::app); if (!fst.is_open()) { return false; @@ -312,13 +312,16 @@ public: using STATUS_T = std::decay_t>; auto send_to_hardware = [hardware, logger](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t { + auto tp = + std::chrono::duration_cast(hw_state.XY.epoch().UTC().time_since_epoch()); + if constexpr (std::derived_from>) { - logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", hw_state.XY.x().degrees(), - hw_state.XY.y().degrees())); + logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})", + hw_state.XY.x().degrees(), hw_state.XY.y().degrees(), tp)); } else { // user defined mcc_coord_pair_c - logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", + logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})", MccAngle((double)hw_state.XY.x()).degrees(), - MccAngle((double)hw_state.XY.y()).degrees())); + MccAngle((double)hw_state.XY.y()).degrees(), tp)); } auto hw_err = hardware->hardwareSetState(hw_state); @@ -438,11 +441,13 @@ public: }; - auto log_pos = [logger, this](typename TELEMETRY_T::telemetry_data_t const& tdata) -> error_t { + auto log_pos = [logger, this](typename TELEMETRY_T::telemetry_data_t const& tdata, + std::tuple& dist) -> error_t { double x_mnt, y_mnt, x_tag, y_tag; - std::conditional_t> + std::conditional_t< + mccIsEquatorialMount(HARDWARE_T::hwMountType), MccSkyHADEC_OBS, + std::conditional_t> coord_pair; static_assert(!std::is_null_pointer_v, "UNKNOWN MOUNT TYPE!"); @@ -451,7 +456,7 @@ public: : mccIsAltAzMount(HARDWARE_T::hwMountType) ? "AZ" : "GEN_X"; const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "DEC" - : mccIsAltAzMount(HARDWARE_T::hwMountType) ? "ZD" + : mccIsAltAzMount(HARDWARE_T::hwMountType) ? "ALT" : "GEN_Y"; ; @@ -477,9 +482,22 @@ public: coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal(), tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal())); + x_mnt = coord_pair.x(); + y_mnt = coord_pair.y(); + + dist = utils::distanceOnSphere(x_tag, y_tag, x_mnt, y_mnt); + _pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << " " - << x_mnt << " " << y_mnt << " " << (x_tag - x_mnt) << " " << (y_tag - y_mnt) << " " - << (int)tdata.hwState.movementState << "\n"; + << x_mnt << " " << y_mnt << " " << std::get<0>(dist) << " " << std::get<1>(dist) << " " + << std::get<2>(dist) << " " << (int)tdata.hwState.movementState << "\n"; + // _pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << " + // " + // << x_mnt << " " << y_mnt << " " << (x_tag - x_mnt) << " " << (y_tag - y_mnt) << " " + // << (int)tdata.hwState.movementState << "\n"; + + logger->logTrace(std::format(" target-to-mount distance: {} (dx = {}, dy = {})", + MccAngleFancyString(std::get<2>(dist)), MccAngleFancyString(std::get<0>(dist)), + MccAngleFancyString(std::get<1>(dist)))); return MccSimpleMovementControlsErrorCode::ERROR_OK; }; @@ -530,6 +548,7 @@ public: std::chrono::duration_cast>(_currentParams.minTimeToPZone).count(); if (!_currentParams.slewingPathFilename.empty()) { // open slewing trajectory file + logger->logInfo(std::format("Set slewing path filename to {}", _currentParams.slewingPathFilename)); _pathFile.setFilename(_currentParams.slewingPathFilename); } else { logger->logWarn("Slewing path filename is empty! Do not save it!"); @@ -559,7 +578,7 @@ public: _pathFile << "# \n"; _pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n"; _pathFile << "# " - " \n"; + " \n"; typename TELEMETRY_T::telemetry_data_t tdata; @@ -584,9 +603,12 @@ public: return; } + using ep_t = decltype(hw_state.XY.epoch()); + hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING; hw_state.XY.setX(tdata.targetXY.x()); hw_state.XY.setY(tdata.targetXY.y()); + hw_state.XY.setEpoch(ep_t::now()); // start slewing ... @@ -607,6 +629,8 @@ public: mcc_deduced_coord_pair_t tag_cp, mnt_cp; + std::tuple dist; + *_stopMoving = false; while (!*_stopMoving) { @@ -618,7 +642,12 @@ public: return; } - log_pos(tdata); + log_pos(tdata, dist); + + // logger->logInfo("\tMNT.XY {} {}; MNT.HADEC {} {}", (double)tdata.hwState.XY.x(), + // (double)tdata.hwState.XY.y(), tdata.mountPos.co_lon(), tdata.mountPos.co_lat()); + // logger->logInfo("\tTAG.XY {} {}; TAG.HADEC {} {}", (double)tdata.targetXY.x(), + // (double)tdata.targetXY.y(), tdata.targetPos.co_lon(), tdata.targetPos.co_lat()); if (*_stopMoving) { *_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED; @@ -643,8 +672,21 @@ public: if (slew_and_stop) { // just wait until the mount stops if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) { - logger->logInfo("mount movement state is STOPPED! Exit from slewing process!"); - break; + if (std::get<2>(dist) > _currentParams.slewToleranceRadius) { + // resend new position since target coordinates are changed in time + hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING; + hw_state.XY.setX(tdata.targetXY.x()); + hw_state.XY.setY(tdata.targetXY.y()); + hw_state.XY.setEpoch(ep_t::now()); + + *_lastError = send_to_hardware(hw_state); + if (_lastError->load()) { + break; + } + } else { + logger->logInfo("mount movement state is STOPPED! Exit from slewing process!"); + break; + } } } else { if (last_hw_time == tdata.hwState.XY.epoch().UTC()) { @@ -654,20 +696,21 @@ public: last_hw_time = tdata.hwState.XY.epoch().UTC(); - auto ccte_err = tdata.targetPos.to(tag_cp); - if (ccte_err) { - *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); - } - ccte_err = tdata.mountPos.to(mnt_cp); - if (ccte_err) { - *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); - } + // auto ccte_err = tdata.targetPos.toAtSameEpoch(tag_cp); + // if (ccte_err) { + // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + // } + // ccte_err = tdata.mountPos.toAtSameEpoch(mnt_cp); + // if (ccte_err) { + // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + // } - auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); + // auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); - logger->logTrace(std::format( - " target-to-mount distance: {} (dx = {}, dy = {})", MccAngleFancyString(std::get<2>(dist)), - MccAngleFancyString(std::get<0>(dist)), MccAngleFancyString(std::get<1>(dist)))); + // logger->logTrace(std::format( + // " target-to-mount distance: {} (dx = {}, dy = {})", + // MccAngleFancyString(std::get<2>(dist)), MccAngleFancyString(std::get<0>(dist)), + // MccAngleFancyString(std::get<1>(dist)))); // stop slewing and exit from the cycle? @@ -694,20 +737,24 @@ public: hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING; hw_state.XY.setX(tdata.targetXY.x()); hw_state.XY.setY(tdata.targetXY.y()); + hw_state.XY.setEpoch(ep_t::now()); *_lastError = send_to_hardware(hw_state); if (_lastError->load()) { break; } - - // sleep here - std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval); } + + // sleep here + std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval); } if (_stopMoving->load()) { // external stop logger->logWarn("Slewing was stopped!"); *_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK; + _pathFile.save(); + + return; } *_stopMoving = true; @@ -729,27 +776,27 @@ public: return; } - auto ccte_err = tdata.targetPos.to(tag_cp); - if (ccte_err) { - *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); - (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR); + // auto ccte_err = tdata.targetPos.to(tag_cp); + // if (ccte_err) { + // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + // (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR); - return; - } - ccte_err = tdata.mountPos.to(mnt_cp); - if (ccte_err) { - *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); - (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR); + // return; + // } + // ccte_err = tdata.mountPos.to(mnt_cp); + // if (ccte_err) { + // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + // (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR); - return; - } + // return; + // } - auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); + // auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); - log_pos(tdata); + log_pos(tdata, dist); - logger->logDebug( - std::format(" target-to-mount distance {}", MccAngleFancyString(std::get<2>(dist)))); + // logger->logDebug( + // std::format(" target-to-mount distance {}", MccAngleFancyString(std::get<2>(dist)))); if (!slew_and_stop) { // start tracking _trackingFunc(); @@ -760,11 +807,14 @@ public: }; + /* tracking function */ + _trackingFunc = [log_pos, check_pzones, send_to_hardware, telemetry, cb_sptr, logger, this]() { double braking_accelX, braking_accelY; double min_time_to_pzone_in_secs; + logger->logInfo("Start tracking"); { // std::lock_guard lock{*_currentParamsMutex}; if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) { @@ -783,13 +833,14 @@ public: std::chrono::duration_cast>(_currentParams.minTimeToPZone).count(); if (!_currentParams.trackingPathFilename.empty()) { // open slewing trajectory file + logger->logInfo( + std::format(" Set tracking filename to {}", _currentParams.trackingPathFilename)); _pathFile.setFilename(_currentParams.trackingPathFilename); } else { - logger->logWarn("Tracking path filename is empty! Do not save it!"); + logger->logWarn(" Tracking path filename is empty! Do not save it!"); } } - logger->logInfo("Start tracking"); logger->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)", MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX)); logger->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)", @@ -802,7 +853,7 @@ public: _pathFile << "# \n"; _pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n"; _pathFile << "# " - " \n"; + " \n"; typename TELEMETRY_T::telemetry_data_t tdata; @@ -832,6 +883,8 @@ public: mcc_deduced_coord_pair_t tag_cp, mnt_cp; + std::tuple dist; + *_stopMoving = false; while (!_stopMoving->load()) { @@ -843,7 +896,7 @@ public: return; } - log_pos(tdata); + log_pos(tdata, dist); if (*_stopMoving) { *_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED; @@ -863,25 +916,26 @@ public: last_hw_time = tdata.hwState.XY.epoch().UTC(); - auto ccte_err = tdata.targetPos.to(tag_cp); - if (ccte_err) { - *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); - } - ccte_err = tdata.mountPos.to(mnt_cp); - if (ccte_err) { - *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); - } + // auto ccte_err = tdata.targetPos.to(tag_cp); + // if (ccte_err) { + // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + // } + // ccte_err = tdata.mountPos.to(mnt_cp); + // if (ccte_err) { + // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); + // } - auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); + // auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); - logger->logTrace(std::format( - " target-to-mount distance: {} (dx = {}, dy = {})", MccAngleFancyString(std::get<2>(dist)), - MccAngleFancyString(std::get<0>(dist)), MccAngleFancyString(std::get<1>(dist)))); + // logger->logTrace(std::format( + // " target-to-mount distance: {} (dx = {}, dy = {})", MccAngleFancyString(std::get<2>(dist)), + // MccAngleFancyString(std::get<0>(dist)), MccAngleFancyString(std::get<1>(dist)))); // resend new position since target coordinates are changed in time hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_TRACKING; hw_state.XY.setX(tdata.targetXY.x()); hw_state.XY.setY(tdata.targetXY.y()); + hw_state.XY.setEpoch(tdata.hwState.XY.epoch()); *_lastError = send_to_hardware(hw_state); if (_lastError->load()) { @@ -899,14 +953,15 @@ public: if (_stopMoving->load()) { // external stop logger->logWarn("Tracking was stopped!"); *_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK; + } else { + *_stopMoving = true; + + logger->logInfo("Tracking finished"); + auto err = _lastError->load(); + logger->logInfo( + std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message())); } - *_stopMoving = true; - - logger->logInfo("Tracking finished"); - auto err = _lastError->load(); - logger->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message())); - _pathFile.save(); }; } diff --git a/include/mcc/mcc_serializer.h b/include/mcc/mcc_serializer.h index d148522..71d321f 100644 --- a/include/mcc/mcc_serializer.h +++ b/include/mcc/mcc_serializer.h @@ -518,7 +518,7 @@ struct MccSerializer : MccSerializerBase { MccSerializerBase::addElemDelimiter(output, pars_h); - err = ang_sr(output, rd_obs.y(), pars_h); + err = ang_sr(output, rd_obs.y(), pars_d); if (err) { return mcc_deduced_err(err, MccSerializerErrorCode::ERROR_UNDERLYING_SERIALIZER); } diff --git a/include/mcc/mcc_utils.h b/include/mcc/mcc_utils.h index 4827e4c..bcbb7b4 100644 --- a/include/mcc/mcc_utils.h +++ b/include/mcc/mcc_utils.h @@ -395,16 +395,20 @@ static std::tuple distanceOnSphere(double co_lon1, std::get<0>(res) = co_lon1 - co_lon2; std::get<1>(res) = co_lat1 - co_lat2; - double cosDco_lon = cos(std::get<0>(res)); - double cos1 = cos(co_lat1); - double sin1 = sin(co_lat1); - double cos2 = cos(co_lat2); - double sin2 = sin(co_lat2); + double cosDco_lon = std::cos(std::abs(std::get<0>(res))); - double term1 = cos1 * sin(std::get<0>(res)); - double term2 = cos2 * sin1 - sin2 * cos1 * cosDco_lon; + double cos1 = std::cos(co_lat1); + double sin1 = std::sin(co_lat1); - std::get<2>(res) = atan2(sqrt(term1 * term1 + term2 * term2), sin2 * sin1 + cos2 * cos1 * cosDco_lon); + double cos2 = std::cos(co_lat2); + double sin2 = std::sin(co_lat2); + + double term1 = cos2 * std::sin(std::abs(std::get<0>(res))); + double term2 = cos1 * sin2 - sin1 * cos2 * cosDco_lon; + + std::get<2>(res) = std::atan2(sqrt(term1 * term1 + term2 * term2), sin1 * sin2 + cos1 * cos2 * cosDco_lon); + + // std::get<2>(res) = acos(sin1 * sin2 + cos1 * cos2 * cosDco_lon); return res; }