...

.vscode/c_cpp_properties.json

@@ -0,0 +1,16 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "intelliSenseMode": "linux-gcc-x64",
+            "cppStandard": "c++26",
+            "cStandard": "gnu99"
+        }
+    ],
+    "version": 4
+}

asibfm700_common.h

@@ -15,6 +15,81 @@
 namespace asibfm700
 {
 
+namespace details
+{
+
+struct movement_pars_t {
+    // *******    common for all modes    *******
+
+    // mean celestial rate
+    static constexpr double sideralRate = 15.0410686_arcsecs;  // in radians per second
+
+    // timeout to telemetry updating
+    std::chrono::milliseconds telemetryTimeout{3000};
+
+    // minimal time to prohibited zone (at current speed in slewing mode). if it is lesser then exit with error
+    std::chrono::seconds minTimeToPZone{10};
+
+    // time interval to update prohibited zones related quantities (e.g. intersection points)
+    std::chrono::milliseconds updatingPZoneInterval{5000};
+
+
+    // *******    slewing mode    *******
+
+    bool slewAndStop{false};  // slew to target and stop mount
+
+    // coordinates difference to stop slewing (in radians)
+    double slewToleranceRadius{5.0_arcsecs};
+
+    // telemetry request interval
+    std::chrono::milliseconds slewingTelemetryInterval{100};
+
+    // target-mount coordinate difference to start adjusting of slewing (in radians)
+    double adjustCoordDiff{slewToleranceRadius * 10.0};
+
+    // slew process timeout
+    std::chrono::seconds slewTimeout{3600};
+
+    double slewRateX{0.0};  // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
+    double slewRateY{0.0};  // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
+
+    std::chrono::milliseconds adjustCycleInterval{500};  // minimum time between two successive adjustments
+
+    double adjustRateX{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
+    double adjustRateY{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
+
+    // braking acceleration after execution of mount stopping command (in rads/s^2)
+    // it must be given as non-negative value!!!
+    double brakingAccelX{0.0};
+    double brakingAccelY{0.0};
+
+    // slewing trajectory file. if empty - just skip saving
+    std::string slewingPathFilename{};
+
+
+    // *******    tracking mode    *******
+
+    // telemetry request interval
+    std::chrono::milliseconds trackingTelemetryInterval{100};
+
+    double trackSpeedX{};
+    double trackSpeedY{};
+    std::chrono::milliseconds trackingCycleInterval{500};  // minimum time between two successive tracking corrections
+    bool dualAxisTracking{true};  // mount must be of an equatorial type: false means guiding along only HA-axis
+
+    // time shift into future to compute target position in future (UT1-scale time duration)
+    std::chrono::milliseconds timeShiftToTargetPoint{10000};
+    // maximal target-to-mount difference for tracking process (in arcsecs)
+    // it it is greater then the current mount coordinates are used as target one
+    double trackingMaxCoordDiff{20.0};
+
+    // tracking trajectory file. if empty - just skip saving
+    std::string trackingPathFilename{};
+};
+
+}  // namespace details
+
+
 static constexpr mcc::MccMountType asibfm700MountType = mcc::MccMountType::FORK_TYPE;
 
 

asibfm700_configfile.h

@@ -156,6 +156,9 @@ static auto Asibfm700MountConfigDefaults = std::make_tuple(
     // slew process timeout in seconds
     simple_config_record_t{"slewTimeout", std::chrono::seconds(3600), {"slew process timeout in seconds"}},
 
+    // mount stopping process timeout in seconds
+    simple_config_record_t{"stopTimeout", std::chrono::seconds(30), {"mount stopping process timeout in seconds"}},
+
     // a time shift into future to compute target position in future (UT1-scale time duration, millisecs)
     simple_config_record_t{
         "timeShiftToTargetPoint",
@@ -581,11 +584,11 @@ public:
     }
 
 
-    mcc::impl::MccSimpleMovementControlsParameters movingModelParams() const
+    details::movement_pars_t movingModelParams() const
     {
         static constexpr double arcsecs2rad = std::numbers::pi / 180.0 / 3600.0;  // arcseconds to radians
 
-        mcc::impl::MccSimpleMovementControlsParameters pars;
+        details::movement_pars_t pars;
 
         auto get_value = [this]<typename VT>(std::string_view name, VT& val) {
             val = getValue<VT>(name).value_or(val);

asibfm700_mount.cpp

@@ -14,9 +14,12 @@ Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_p
       _pcm(config.pcmData()),
       gm_class_t(std::make_tuple(&_servolController, &_pcm),
                  std::make_tuple(),
-                 std::make_tuple(&_servolController,
+                 // std::make_tuple(&_servolController,
-                                 this,
+                 //                 this,
-                                 [this](Asibfm700Mount::mount_status_t const& status) { *_mountStatus = status; }),
+                 //                 [this](Asibfm700Mount::mount_status_t const& status) { *_mountStatus = status; }),
+                 std::make_tuple([this](bool sl) { return slewingImpl(sl); },
+                                 [this]() { return trackingImpl(); },
+                                 [this]() { return stoppingImpl(); }),
                  std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)),
       _mountConfig(config),
       _mountConfigMutex(new std::mutex)
@@ -346,4 +349,154 @@ void Asibfm700Mount::errorLogging(const std::string& msg, const std::error_code&
     }
 }
 
+
+/*  MOVEMENT METHODS  */
+
+Asibfm700Mount::error_t Asibfm700Mount::sendToHardware(AsibFM700ServoController::hardware_state_t const& hw_state)
+{
+    auto tp = std::chrono::duration_cast<std::chrono::milliseconds>(hw_state.XY.epoch().UTC().time_since_epoch());
+
+    logDebug("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})", hw_state.XY.x().degrees(),
+             hw_state.XY.y().degrees(), tp);
+
+    *_lastMountError = _servolController.hardwareSetState(hw_state);
+
+    if (_lastMountError->load()) {
+        errorLogging("An error occured while send command to servo controller: ", _lastMountError->load());
+    } else {
+        logDebug("    the 'hardwareSetState' method performed successfully!");
+    }
+
+    return _lastMountError->load();
+}
+
+
+void Asibfm700Mount::logMountPos(telemetry_t::telemetry_data_t const& tdata)
+{
+    // NOTE: the implementation of MccTelemetry class guarantees that
+    //       tdata.mountPos and tdata.targetPos are coordinates in the HA-DEC
+    //       equathorial system
+
+    logTrace("    current target: HA = {}, DEC = {} (encoders: {} {})",
+             mcc::impl::MccAngle(tdata.targetPos.co_lon()).sexagesimal(true),
+             mcc::impl::MccAngle(tdata.targetPos.co_lat()).sexagesimal(), tdata.targetXY.x().sexagesimal(),
+             tdata.targetXY.y().sexagesimal());
+
+    logTrace("    current mount: HA = {}, DEC = {} (encoders: {} {})",
+             mcc::impl::MccAngle(tdata.mountPos.co_lon()).sexagesimal(true),
+             mcc::impl::MccAngle(tdata.mountPos.co_lat()).sexagesimal(), tdata.hwState.XY.x().sexagesimal(),
+             tdata.hwState.XY.y().sexagesimal());
+
+    _pathFile.addToPath(tdata);
+
+    auto dist = tdata.mountPos.distance(tdata.targetPos);
+
+    logTrace("    target-to-mount distance: {} (dx = {}, dy = {})", mcc::impl::MccAngleFancyString(dist.dist),
+             mcc::impl::MccAngleFancyString(dist.dx), mcc::impl::MccAngleFancyString(dist.dy));
+}
+
+
+Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemetry_data_t const& tdata)
+{
+    mcc::impl::MccGenXY braking_accel{getMovementParams().brakingAccelX, getMovementParams().brakingAccelY}, dxy{};
+
+    auto new_pos = this->coordsAfterTime(tdata.mountPos, tdata.hwState.speedXY, braking_accel,
+                                         getMovementParams().minTimeToPZone, &dxy);
+
+    logTrace("    the distance that will be covered in the next {} seconds: HA-axis: {}, DEC-axis: {}",
+             getMovementParams().minTimeToPZone.count(), mcc::impl::MccAngleFancyString(dxy.x()),
+             mcc::impl::MccAngleFancyString(dxy.y()));
+
+    bool in_zone;
+    std::vector<bool> in_zone_vec;
+
+    *_lastMountError = this->inPZone(new_pos, &in_zone, &in_zone_vec);
+    if (_lastMountError->load()) {
+        return _lastMountError->load();
+    }
+
+    if (in_zone) {
+        size_t i = 0;
+        for (; i < in_zone_vec.size(); ++i) {
+            if (in_zone_vec[i]) {
+                break;
+            }
+        }
+
+        auto names = this->pzoneNames();
+        auto it = names.begin();
+        std::ranges::advance(it, i);
+
+        logError("target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i,
+                 *it);
+
+        mcc::impl::MccSkyHADEC_OBS hadec;
+        mcc::impl::MccSkyRADEC_OBS radec;
+        mcc::impl::MccSkyAZZD azzd;
+        mcc::impl::MccSkyAZALT azalt;
+        mcc::impl::MccAngle lst;
+
+        *_lastMountError = tdata.mountPos.appSideralTime(&lst, true);
+        if (_lastMountError->load()) {
+            return _lastMountError->load();
+        }
+
+        *_lastMountError = tdata.mountPos.toAtSameEpoch(radec, hadec, azzd, azalt);
+        if (_lastMountError->load()) {
+            return _lastMountError->load();
+        }
+
+        logError("    RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", radec.x().sexagesimal(true), radec.y().sexagesimal(),
+                 hadec.x().sexagesimal(true), lst.sexagesimal(true));
+        logError("    AZ, ZD, ALT: {}, {}, {}", azzd.x().sexagesimal(), azzd.y().sexagesimal(),
+                 azalt.y().sexagesimal());
+
+        logError("    hardware X, Y: {}, {}", tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal());
+
+        return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE;
+    }
+}
+
+Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) {}
+
+Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl()
+{
+    typename AsibFM700ServoController::hardware_state_t hw_state;
+
+    hw_state.movementState == AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPING;
+
+    *_lastMountError = sendToHardware(hw_state);
+
+    if (!_lastMountError->load()) {
+        *_mountStatus = mount_status_t::MOUNT_STATUS_STOPPING;
+
+        auto start_tp = std::chrono::steady_clock::now();
+
+        *_lastMountError = _servolController.hardwareGetState(&hw_state);
+
+        while (hw_state.movementState != AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPED &&
+               !_lastMountError->load()) {
+            if ((std::chrono::steady_clock::now() - start_tp) >
+                _mountConfig.getValue<std::chrono::seconds>("stopTimeout").value()) {
+                // set timeout error!
+                // *_lastMountError =
+                break;
+            }
+
+            std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+            *_lastMountError = _servolController.hardwareGetState(&hw_state);
+        }
+    }
+
+    if (!_lastMountError->load()) {
+        *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE;
+    } else {
+        *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
+        errorLogging("An error occured while stoppping mount: ", _lastMountError->load());
+    }
+
+    return _lastMountError->load();
+}
+
 }  // namespace asibfm700

asibfm700_mount.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <mcc/mcc_generic_mount.h>
+#include <mcc/mcc_generic_movecontrols.h>
 #include <mcc/mcc_movement_controls.h>
 #include <mcc/mcc_pzone_container.h>
 #include <mcc/mcc_spdlog.h>
@@ -14,19 +15,29 @@ namespace asibfm700
 
 
 
-class Asibfm700Mount : public mcc::impl::MccGenericMount<mcc::impl::MccTelemetry<AsibFM700ServoController>,
+class Asibfm700Mount
-                                                         Asibfm700PZoneContainer,
+    : public mcc::impl::MccGenericMount<mcc::impl::MccTelemetry<AsibFM700ServoController>,
-                                                         mcc::impl::MccSimpleMovementControls,
+                                        Asibfm700PZoneContainer,
-                                                         Asibfm700Logger>
+                                        // mcc::impl::MccSimpleMovementControls,
+                                        mcc::impl::MccGenericAsyncMovementControls<details::movement_pars_t>,
+                                        Asibfm700Logger>
 {
     typedef mcc::impl::MccGenericMount<mcc::impl::MccTelemetry<AsibFM700ServoController>,
                                        Asibfm700PZoneContainer,
-                                       mcc::impl::MccSimpleMovementControls,
+                                       // mcc::impl::MccSimpleMovementControls,
+                                       mcc::impl::MccGenericAsyncMovementControls<details::movement_pars_t>,
                                        Asibfm700Logger>
         gm_class_t;
 
+
 public:
-    using gm_class_t::error_t;
+    typedef mcc::impl::MccTelemetry<AsibFM700ServoController> telemetry_t;
+
+    typedef mcc::impl::MccGenericAsyncMovementControls<details::movement_pars_t> movement_controls_t;
+
+    using typename gm_class_t::error_t;
+    using typename movement_controls_t::movement_params_t;
+
 
     // using Asibfm700CCTE::setStateERFA;
     // using Asibfm700CCTE::updateBulletinA;
@@ -70,6 +81,20 @@ protected:
     Asibfm700PCM _pcm;
 
     void errorLogging(const std::string&, const std::error_code&);
+
+    // movement methods
+    mcc::impl::MccMovementPathFile _pathFile{};
+
+    error_t slewingImpl(bool);
+    error_t trackingImpl();
+    error_t stoppingImpl();
+
+    error_t sendToHardware(AsibFM700ServoController::hardware_state_t const& hw_state);
+    error_t checkPZone(typename telemetry_t::telemetry_data_t const& tdata);
+    void logMountPos(telemetry_t::telemetry_data_t const& tdata);
+
+    std::unique_ptr<std::atomic<error_t>> _lastMountError{
+        new std::atomic<error_t>{mcc::impl::MccGenericMountErrorCode::ERROR_OK}};
 };