diff --git a/asibfm700/asibfm700_servocontroller.cpp b/asibfm700/asibfm700_servocontroller.cpp index 21bc75e..dfcf710 100644 --- a/asibfm700/asibfm700_servocontroller.cpp +++ b/asibfm700/asibfm700_servocontroller.cpp @@ -114,7 +114,8 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(har // according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis coordval_pair_t cvalpair{.X{.val = state.Y, .t = tp}, .Y{.val = state.X, .t = tp}}; - coordpair_t cpair{.X = state.Y, .Y = state.X + mcc::MccAngle(10.0_arcsecs)}; + coordpair_t cpair{.X = state.Y, .Y = state.X}; + // coordpair_t cpair{.X = state.Y, .Y = state.X + mcc::MccAngle(10.0_arcsecs)}; // correctTo is asynchronous function!!! // diff --git a/mcc/mcc_angle.h b/mcc/mcc_angle.h index 1a05a2e..dd10373 100644 --- a/mcc/mcc_angle.h +++ b/mcc/mcc_angle.h @@ -598,4 +598,23 @@ static constexpr MccCoordPairKind MccCoordStrToPairKind(R&& spair) } +std::string MccAngleFancyString(std::convertible_to auto const& ang, + std::format_string val_fmt = "{}") +{ + std::string s; + + if (ang < 1.0_arcmins) { + std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcsecs()); + s += " arcsecs"; + } else if (ang < 1.0_degs) { + std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.arcmins()); + s += " arcmins"; + } else { + std::format_to(std::back_inserter(s), val_fmt, MccAngle{ang}.degrees()); + s += " degs"; + } + + return s; +} + } // namespace mcc diff --git a/mcc/mcc_generics.h b/mcc/mcc_generics.h index 73195ed..fedd455 100644 --- a/mcc/mcc_generics.h +++ b/mcc/mcc_generics.h @@ -504,12 +504,12 @@ concept mcc_hardware_c = requires(T t, const T t_const) { // HW_MOVE_ADJUSTING, HW_MOVE_TRACKING, HW_MOVE_GUIDING} // // struct hardware_moving_state_t { - // uint16_t HW_MOVE_STOPPED = 0; - // uint16_t HW_MOVE_SLEWING = 111; - // uint16_t HW_MOVE_ADJUSTING = 222; - // uint16_t HW_MOVE_TRACKING = 333; - // uint16_t HW_MOVE_GUIDING = 444; - // uint16_t HW_MOVE_ERROR = 555; + // static constexpr uint16_t HW_MOVE_STOPPED = 0; + // static constexpr uint16_t HW_MOVE_SLEWING = 111; + // static constexpr uint16_t HW_MOVE_ADJUSTING = 222; + // static constexpr uint16_t HW_MOVE_TRACKING = 333; + // static constexpr uint16_t HW_MOVE_GUIDING = 444; + // static constexpr uint16_t HW_MOVE_ERROR = 555; // } requires requires(typename T::hardware_moving_state_t type) { []() { diff --git a/mcc/mcc_moving_model_common.h b/mcc/mcc_moving_model_common.h index cd0d180..c9f4d56 100644 --- a/mcc/mcc_moving_model_common.h +++ b/mcc/mcc_moving_model_common.h @@ -92,23 +92,26 @@ std::pair mcc_compute_distance(mcc_telemetry_data_c auto const& { std::pair res; - // first, check if the mount will stop after given time_in_secs to prevent traveled path to be - // negative + // first, check if the mount stops before time_in_secs + // + // the time to stop mount: (V_ini - a*t) = 0 => t = V_ini/a // // the traveled path: s = V_ini*t - a*t*t/2, V_ini - initial speed, a - braking accel, t - the time - // then, for s>=0, t <= 2*V_ini/a // - double term_x = 2.0 * std::abs(tdata.speedX) / braking_accelX; - double term_y = 2.0 * std::abs(tdata.speedY) / braking_accelY; + + // time to stop mount with given current speed and braking acceleration + double tx_stop = std::abs(tdata.speedX) / braking_accelX; + double ty_stop = std::abs(tdata.speedY) / braking_accelY; + double tx = time_in_secs; double ty = time_in_secs; - if (std::isfinite(term_x) && (time_in_secs > term_x)) { - tx = term_x; + if (std::isfinite(tx_stop) && (time_in_secs > tx_stop)) { + tx = tx_stop; } - if (std::isfinite(term_y) && (time_in_secs > term_y)) { - ty = term_y; + if (std::isfinite(ty_stop) && (time_in_secs > ty_stop)) { + ty = ty_stop; } // here, one must take into account the sign of the speed!!! diff --git a/mcc/mcc_slewing_model.h b/mcc/mcc_slewing_model.h index 2b8dd65..64a4e59 100644 --- a/mcc/mcc_slewing_model.h +++ b/mcc/mcc_slewing_model.h @@ -239,13 +239,20 @@ public: } // start slewing - logger.logDebug(std::format("Send to hardware: X = {}, Y = {}", hw_state.X, hw_state.Y)); + logger.logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", + mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees())); if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { - logger.logDebug(std::format(" target: HA = {}, DEC = {}", tdata.target.HA, tdata.target.DEC_APP)); - logger.logDebug(std::format(" mount: HA = {}, DEC = {}", tdata.HA, tdata.DEC_APP)); + logger.logDebug(std::format(" target: HA = {}, DEC = {}", + mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(), + mcc::MccAngle{tdata.target.HA}.sexagesimal(true))); + logger.logDebug(std::format(" mount: HA = {}, DEC = {}", mcc::MccAngle{tdata.DEC_APP}.sexagesimal(), + mcc::MccAngle{tdata.HA}.sexagesimal(true))); } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { - logger.logDebug(std::format(" target: AZ = {}, ZD = {}", tdata.target.AZ, tdata.target.ZD)); - logger.logDebug(std::format(" mount: AZ = {}, ZD = {}", tdata.AZ, tdata.ZD)); + logger.logDebug(std::format(" target: AZ = {}, ZD = {}", + mcc::MccAngle{tdata.target.AZ}.sexagesimal(), + mcc::MccAngle{tdata.target.ZD}.sexagesimal())); + logger.logDebug(std::format(" mount: AZ = {}, ZD = {}", mcc::MccAngle{tdata.AZ}.sexagesimal(), + mcc::MccAngle{tdata.ZD}.sexagesimal())); } hw_err = controls->hardwareSetState(hw_state); @@ -294,7 +301,8 @@ public: logger.logTrace( std::format(" the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}", - min_time_to_pzone_in_secs, distXY.first, distXY.second)); + min_time_to_pzone_in_secs, mcc::MccAngleFancyString(distXY.first), + mcc::MccAngleFancyString(distXY.second))); // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead // and check them for getting into the prohibited zones @@ -329,12 +337,17 @@ public: cpt.Y = std::numbers::pi; } - logger.logTrace(std::format(" target: AZ = {}, ZD = {}", tdata.target.AZ, tdata.target.ZD)); - logger.logTrace(std::format(" mount: AZ = {}, ZD = {}", tdata.AZ, tdata.ZD)); + logger.logTrace(std::format(" target: AZ = {}, ZD = {}", + mcc::MccAngle(tdata.target.AZ).sexagesimal(), + mcc::MccAngle(tdata.target.ZD).sexagesimal())); + logger.logTrace(std::format(" mount: AZ = {}, ZD = {}", mcc::MccAngle(tdata.AZ).sexagesimal(), + mcc::MccAngle(tdata.ZD).sexagesimal())); } mcc_tp2tp(tdata.time_point, cpt.time_point); - logger.logTrace(std::format(" mount: speedX = {}, speedY = {}", tdata.speedX, tdata.speedY)); + logger.logTrace(std::format(" mount: speedX = {}/s, speedY = {}/s", + mcc::MccAngleFancyString(tdata.speedX), + mcc::MccAngleFancyString(tdata.speedY))); pz_err = controls->inPZone(cpt, &in_zone); if (pz_err) { @@ -376,13 +389,7 @@ public: return mcc_deduce_error_code(t_err, MccSimpleSlewingModelErrorCode::ERROR_DIST_TELEMETRY); } - logger.logTrace(std::format(" target-to-mount distance: {} {}", - dist < 1.0_arcmins ? mcc::MccAngle(dist).arcsecs() - : dist < 1.0_degs ? mcc::MccAngle(dist).arcmins() - : mcc::MccAngle(dist).degrees(), - dist < 1.0_arcmins ? "arcsecs" - : dist < 1.0_degs ? "arcmins" - : "degs")); + logger.logTrace(std::format(" target-to-mount distance: {}", mcc::MccAngleFancyString(dist))); if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle logger.logInfo("target-to-mount distance is lesser than slew tolerance radius - exit!"); @@ -394,90 +401,15 @@ public: hw_state.X = (double)tdata.target.X; hw_state.Y = (double)tdata.target.Y; - logger.logTrace(std::format("Send to hardware: X = {}, Y = {}", hw_state.X, hw_state.Y)); + logger.logTrace(std::format("Send to hardware: X = {} degs, Y = {} degs", + mcc::MccAngle{hw_state.X}.degrees(), + mcc::MccAngle{hw_state.Y}.degrees())); hw_err = controls->hardwareSetState(hw_state); if (hw_err) { *_stopSlewing = true; return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); } } - - - - // if (*_stopSlewing) { - // return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; - // } - - // { - // std::lock_guard lock{*_currentParamsMutex}; - - // // if (adjust_mode && !_currentParams.slewAndStop) { - // if (adjust_mode && !slew_and_stop) { - // // do not allow mount speed fall below sideral - // if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) { - // // turn on sideral rate only if the current position point catches up with the target - // if ((tdata.target.HA - tdata.HA) <= 0.0 && tdata.speedX < slewing_params_t::sideralRate) - // { - // hw_state.X = (double)tdata.target.X; - // hw_state.Y = (double)tdata.target.Y; - - // hw_state.speedX = slewing_params_t::sideralRate; - - // hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_TRACKING; - - // hw_err = controls->hardwareSetState(hw_state); - // if (hw_err) { - // *_stopSlewing = true; - // return mcc_deduce_error_code(hw_err, - // MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); - // } - // } - // } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) { - // } else { - // static_assert(false, "UNKNOWN MOUNT TYPE!!"); - // } - // } - - // if (dist <= _currentParams.slewToleranceRadius) { // stop slewing and exit from cycle - // if (hw_state.moving_state == - // CONTROLS_T::hardware_moving_state_t::HW_MOVE_STOPPED) { // mount was stopped - // *_stopSlewing = true; - // break; - // } - // } - - // if (dist <= _currentParams.adjustCoordDiff) { // adjust mount pointing - // auto now = std::chrono::steady_clock::now(); - // if ((now - last_adjust_tp) < _currentParams.adjustCycleInterval) { - // continue; - // } - - // hw_state.X = (double)tdata.target.X; - // hw_state.Y = (double)tdata.target.Y; - - // hw_state.speedX = _currentParams.adjustRateX; - // hw_state.speedY = _currentParams.adjustRateY; - - // hw_state.moving_state = CONTROLS_T::hardware_moving_state_t::HW_MOVE_ADJUSTING; - - // hw_err = controls->hardwareSetState(hw_state); - // if (hw_err) { - // *_stopSlewing = true; - // return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_SETSTATE); - // } - - // last_adjust_tp = now; - - // adjust_mode = true; - - // } else { - // adjust_mode = false; - // } - // } - - // if (*_stopSlewing) { - // return MccSimpleSlewingModelErrorCode::ERROR_STOPPED; - // } } *_stopSlewing = true; diff --git a/mcc/mcc_telemetry.h b/mcc/mcc_telemetry.h index 86b1372..16627d5 100644 --- a/mcc/mcc_telemetry.h +++ b/mcc/mcc_telemetry.h @@ -378,40 +378,32 @@ public: }; - /* - _setTargetFunc = [controls, this](MccCelestialPoint const& pt) { - // in the case of apparent input coordinates - // one must ensure the same time points + // arm internal update loop + _internalUpdatingStopSource = std::stop_source{}; - _data.target.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS; - _data.target.time_point = - std::chrono::time_point_cast(pt.time_point); + _internalUpdatingLoopFuture = std::async( + std::launch::async, + [this](std::stop_token stoken) { + while (!(*_internalUpdatingLoopStop)) { + { + std::unique_lock ulock(*_internalUpdatingLoopMutex); + _internalUpdatingLoopCondVar->wait(ulock, [this]() -> bool { return *_dataUpdatingRequested; }); + } - auto ret = controls->transformCoordinates(pt, &_data.target); + { + std::lock_guard lock_update(*_updateMutex); - if (!ret) { - if (pt.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { - _data.target.RA_ICRS = _data.target.X; - _data.target.DEC_ICRS = _data.target.Y; + *_isDataUpdated = false; + _lastUpdateError = _updateFunc(stoken); + } - // update apparent coordinates - ret = _updateTargetFunc(false, {}); - } else { // apparent coordinates were computed above - // compute ICRS coordinates - MccCelestialPoint cpt{.pair_kind = MccCoordPairKind::COORDS_KIND_RADEC_ICRS}; - ret = controls->transformCoordinates(pt, &cpt); + *_isDataUpdated = true; - _data.target.RA_ICRS = cpt.X; - _data.target.DEC_ICRS = cpt.Y; - - // compute only hardware coordinates - ret = _updateTargetFunc(true, {}); + // unlock all waiting threads + _updateCondVar->notify_all(); } - } - - return mcc_deduce_error_code(ret, MccTelemetryErrorCode::ERROR_COORD_TRANSFORM); - }; -*/ + }, + _internalUpdatingStopSource.get_token()); } @@ -424,6 +416,8 @@ public: virtual ~MccTelemetry() { + *_internalUpdatingLoopStop = true; + stopInternalTelemetryDataUpdating(); if (_internalUpdatingFuture.valid()) { @@ -552,33 +546,50 @@ public: error_t updateTelemetryData(traits::mcc_time_duration_c auto const& timeout) { { - std::lock_guard thread_lock{*_updateMutex}; + std::lock_guard lock(*_internalUpdatingLoopMutex); + *_dataUpdatingRequested = true; + } + std::unique_lock ulock(*_updateMutex); - std::stop_source stop_source; + _internalUpdatingLoopCondVar->notify_one(); + *_dataUpdatingRequested = false; - *_isDataUpdated = false; - - std::future update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token()); - // std::future update_ft = - // std::async(std::launch::async, _updateFunc, _internalUpdatingStopSource.get_token()); - auto status = update_ft.wait_for(timeout); - - if (status == std::future_status::ready) { - *_isDataUpdated = true; - _lastUpdateError = update_ft.get(); - } else if (status == std::future_status::deferred) { // std::async was invoked in this thread, get result - _lastUpdateError = update_ft.get(); - if (!_lastUpdateError) { - *_isDataUpdated = true; - } - } else { // timeout - stop_source.request_stop(); - _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; - } + bool ok = _updateCondVar->wait_for(ulock, timeout, [this]() -> bool { return *_isDataUpdated; }); + if (!ok) { + _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; } - // unblock waiting threads even in the case of timeout! - _updateCondVar->notify_all(); + // { + // std::lock_guard thread_lock{*_updateMutex}; + + // std::stop_source stop_source; + + // *_isDataUpdated = false; + + // // std::future update_ft = std::async(std::launch::async, _updateFunc, stop_source.get_token()); + // // // std::future update_ft = + // // // std::async(std::launch::async, _updateFunc, _internalUpdatingStopSource.get_token()); + // // auto status = update_ft.wait_for(timeout); + + // // if (status == std::future_status::ready) { + // // *_isDataUpdated = true; + // // _lastUpdateError = update_ft.get(); + // // } else if (status == std::future_status::deferred) { // std::async was invoked in this thread, get + // // result + // // _lastUpdateError = update_ft.get(); + // // if (!_lastUpdateError) { + // // *_isDataUpdated = true; + // // } + // // } else { // timeout + // // stop_source.request_stop(); + // // _lastUpdateError = MccTelemetryErrorCode::ERROR_DATA_TIMEOUT; + // // } + + // *_isDataUpdated = true; + // } + + // // unblock waiting threads even in the case of timeout! + // _updateCondVar->notify_all(); // *_isDataUpdated = false; @@ -731,6 +742,12 @@ protected: std::unique_ptr _updateMutex; std::unique_ptr _updateCondVar; + std::future _internalUpdatingLoopFuture{}; + std::unique_ptr _internalUpdatingLoopMutex{new std::mutex()}; + std::unique_ptr _internalUpdatingLoopCondVar{new std::condition_variable()}; + std::unique_ptr _internalUpdatingLoopStop{new std::atomic_bool{false}}; + std::unique_ptr _dataUpdatingRequested{new std::atomic_bool{false}}; + error_t _lastUpdateError{MccTelemetryErrorCode::ERROR_OK}; };