...

asibfm700_common.h

@@ -33,6 +33,11 @@ struct movement_pars_t {
     // time interval to update prohibited zones related quantities (e.g. intersection points)
     std::chrono::milliseconds updatingPZoneInterval{5000};
 
+    // 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 mode    *******
 
@@ -58,11 +63,6 @@ struct movement_pars_t {
     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{};
 

asibfm700_mount.cpp

@@ -396,21 +396,26 @@ void Asibfm700Mount::logMountPos(telemetry_t::telemetry_data_t const& tdata)
 }
 
 
-Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemetry_data_t const& tdata)
+Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemetry_data_t const& tdata, bool predicted)
 {
-    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 (predicted) {
+        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()));
+
+        *_lastMountError = this->inPZone(new_pos, &in_zone, &in_zone_vec);
+    } else {
+        *_lastMountError = this->inPZone(tdata.mountPos, &in_zone, &in_zone_vec);
+    }
+
     if (_lastMountError->load()) {
         return _lastMountError->load();
     }
@@ -453,11 +458,325 @@ Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemet
 
         logError("    hardware X, Y: {}, {}", tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal());
 
-        return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE;
+        if (predicted) {
+            return *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_NEAR_PZONE;
+        } else {
+            return *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_IN_PZONE;
+        }
     }
+
+    return *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
 }
 
-Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) {}
+Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop)
+{
+    _pathFile.clearPath();
+
+    _enteredBackupCoordinates = getPointingTarget();
+
+    typename telemetry_t::telemetry_data_t tdata;
+    *_lastMountError = telemetryData(&tdata);
+    if (_lastMountError->load()) {
+        errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
+        return _lastMountError->load();
+    }
+
+    *_lastMountError = checkPZone(tdata, false);
+    if (_lastMountError->load()) {
+        return _lastMountError->load();
+    }
+
+    logInfo("Start slewing in mode '{}'", slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK");
+    logInfo("    slewing process timeout: {} secs", _currentMovementParams.slewTimeout.count());
+    logInfo("    slewing tolerance radius: {} arcsecs",
+            mcc::impl::MccAngle{_currentMovementParams.slewToleranceRadius}.arcsecs());
+
+    logInfo("    braking acceleration X: {} degs/s^2",
+            mcc::impl::MccAngle(_currentMovementParams.brakingAccelX).degrees());
+    logInfo("    braking acceleration Y: {} degs/s^2",
+            mcc::impl::MccAngle(_currentMovementParams.brakingAccelY).degrees());
+    logInfo("    min time to prohibited zone: {} seconds", _currentMovementParams.minTimeToPZone.count());
+
+    if (!_currentMovementParams.slewingPathFilename.empty()) {  // slewing trajectory file
+        logInfo("Set slewing path filename to {}", _currentMovementParams.slewingPathFilename);
+    } else {
+        logWarn("Slewing path filename is empty! Do not save it!");
+    }
+
+
+    _pathFile.addComment("");
+    _pathFile.addComment(std::format("Slewing trajectory, {}", std::chrono::system_clock::now()));
+    _pathFile.addComment("Config:");
+    _pathFile.addComment(std::format("    slewing tolerance radius: {} arcsecs",
+                                     mcc::impl::MccAngle{_currentMovementParams.slewToleranceRadius}.arcsecs()));
+    _pathFile.addComment(
+        std::format("    slewing process timeout: {} secs", _currentMovementParams.slewTimeout.count()));
+    _pathFile.addComment("");
+    _pathFile.addDefaultComment();
+
+    typename hardware_t::hardware_state_t hw_state;
+    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 (LibSidServo.moveTo) ...
+
+    *_lastMountError = sendToHardware(hw_state);
+
+    if (_lastMountError->load()) {
+        *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
+        _pathFile.clearPath();
+
+        return _lastMountError->load();
+    }
+
+    *_mountStatus = mount_status_t::MOUNT_STATUS_SLEWING;
+
+    auto start_point = std::chrono::steady_clock::now();
+    auto last_hw_time = tdata.hwState.XY.epoch().UTC();
+
+    hw_state.movementState = hardware_t::hardware_movement_state_t::HW_MOVE_ADJUSTING;
+
+    mcc::impl::MccSkyPoint::dist_result_t dist;
+
+    while (!_stopMovementRequest->load()) {
+        *_lastMountError = telemetryData(&tdata);
+
+        if (_lastMountError->load()) {
+            errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
+            // return _lastMountError->load();
+            break;
+        }
+
+        hw_state.XY.setX(tdata.targetXY.x());
+        hw_state.XY.setY(tdata.targetXY.y());
+        hw_state.XY.setEpoch(ep_t::now());
+
+        logMountPos(tdata);
+
+        if (_stopMovementRequest->load()) {  // external stop
+            break;
+        }
+
+        *_lastMountError = checkPZone(tdata, true);
+        if (_lastMountError->load()) {
+            break;
+            // return _lastMountError->load();
+        }
+
+        if ((std::chrono::steady_clock::now() - start_point) > getMovementParams().slewTimeout) {
+            *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_SLEW_TIMEOUT;
+            break;
+        }
+
+        dist = tdata.mountPos.distance(tdata.targetPos);
+
+        if (dist.dist <= getMovementParams().adjustCoordDiff) {  // start adjusting (precise pointing)
+            *_mountStatus = mount_status_t::MOUNT_STATUS_ADJUSTING;
+
+            *_lastMountError = sendToHardware(hw_state);
+
+            if (_lastMountError->load()) {
+                break;
+            }
+        }
+
+        if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_STOPPED &&
+            !_stopMovementRequest->load()) {
+            // the mount stopped but still to far from target position!!!
+            if (dist.dist > getMovementParams().slewToleranceRadius) {
+                *_mountStatus = mount_status_t::MOUNT_STATUS_ADJUSTING;
+
+                *_lastMountError = sendToHardware(hw_state);
+
+                if (_lastMountError->load()) {
+                    break;
+                }
+            } else {  // just exit
+                break;
+            }
+        }
+
+        if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_ERROR) {
+            *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_HARDWARE;
+            break;
+        }
+
+        // sleep here
+        std::this_thread::sleep_for(getMovementParams().slewingTelemetryInterval);
+    }
+
+    if (_stopMovementRequest->load()) {  // external stop
+        logWarn("Slewing was stopped!");
+        *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
+        *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE;
+    } else {
+        if (_lastMountError->load()) {
+            *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
+            auto err = _lastMountError->load();
+            logError("Slewing finished with error: {} ({}::{})", err.message(), err.category().name(), err.value());
+        } else {
+            logInfo("Slewing finished without errors!");
+        }
+    }
+
+    if (!_lastMountError->load()) {  // get final position
+        *_lastMountError = telemetryData(&tdata);
+
+        if (_lastMountError->load()) {
+            *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
+            errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
+        } else {
+            logInfo("Final mount slewing position:");
+            logMountPos(tdata);
+        }
+    }
+
+    bool ok = _pathFile.saveToFile(_currentMovementParams.slewingPathFilename);
+    if (!ok && !_currentMovementParams.slewingPathFilename.empty()) {
+        logError("Cannot save trajectory file!");
+    }
+    _pathFile.clearPath();
+
+    if (!_lastMountError->load()) {  // do not start tracking if an error occured
+        if (!slew_and_stop) {
+            return trackTarget();
+        }
+    } else {
+        *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
+    }
+
+    return _lastMountError->load();
+}
+
+Asibfm700Mount::error_t Asibfm700Mount::trackingImpl()
+{
+    *_mountStatus = mount_status_t::MOUNT_STATUS_TRACKING;
+    _pathFile.clearPath();
+
+    logInfo("Start tracking");
+    logInfo("    braking acceleration X: {} degs/s^2",
+            mcc::impl::MccAngle(_currentMovementParams.brakingAccelX).degrees());
+    logInfo("    braking acceleration Y: {} degs/s^2",
+            mcc::impl::MccAngle(_currentMovementParams.brakingAccelY).degrees());
+    logInfo("    min time to prohibited zone: {} seconds", _currentMovementParams.minTimeToPZone.count());
+    logInfo("    max target-to-mount distance: {} arcseconds", _currentMovementParams.trackingMaxCoordDiff);
+
+    if (!_currentMovementParams.trackingPathFilename.empty()) {  // tracking trajectory file
+        logInfo("Set tracking path filename to {}", _currentMovementParams.trackingPathFilename);
+    } else {
+        logWarn("Tracking path filename is empty! Do not save it!");
+    }
+
+    telemetry_data_t tdata;
+
+    *_lastMountError = telemetryData(&tdata);
+
+    if (_lastMountError->load()) {
+        errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
+
+        *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
+
+        return _lastMountError->load();
+    }
+
+
+    mcc::impl::MccSkyPoint::dist_result_t dist;
+    dist = tdata.mountPos.distance(tdata.targetPos);
+
+    if (dist.dist > _currentMovementParams.trackingMaxCoordDiff) {
+        logInfo(
+            "The target-to-mount distance {} is greater than allowed one ({})! Track current "
+            "mount position!",
+            mcc::impl::MccAngleFancyString(dist.dist),
+            mcc::impl::MccAngleFancyString(_currentMovementParams.trackingMaxCoordDiff));
+
+        _enteredBackupCoordinates = getPointingTarget();
+        setPointingTarget(tdata.mountPos);
+
+        *_lastMountError = telemetryData(&tdata);
+
+        if (_lastMountError->load()) {
+            errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
+
+            *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
+
+            return _lastMountError->load();
+        }
+    }
+
+    *_lastMountError = checkPZone(tdata, true);
+    if (_lastMountError->load()) {
+        return _lastMountError->load();
+    }
+
+    _pathFile.addComment("");
+    _pathFile.addComment(std::format("Tracking trajectory, {}", std::chrono::system_clock::now()));
+    _pathFile.addDefaultComment();
+
+    typename hardware_t::hardware_state_t hw_state;
+    using ep_t = decltype(hw_state.XY.epoch());
+
+    hw_state.movementState = hardware_t::hardware_movement_state_t::HW_MOVE_TRACKING;
+
+    while (!_stopMovementRequest->load()) {
+        *_lastMountError = telemetryData(&tdata);
+
+        if (_lastMountError->load()) {
+            errorLogging("An error occured while telemetry requesting: ", _lastMountError->load());
+            break;
+        }
+
+        logMountPos(tdata);
+
+        *_lastMountError = checkPZone(tdata, true);
+        if (_lastMountError->load()) {
+            break;
+        }
+
+        hw_state.XY.setX(tdata.targetXY.x());
+        hw_state.XY.setY(tdata.targetXY.y());
+        hw_state.XY.setEpoch(ep_t::now());
+
+        *_lastMountError = sendToHardware(hw_state);
+        if (_lastMountError->load()) {
+            break;
+        }
+
+        if (_stopMovementRequest->load()) {
+            break;
+        }
+
+        // sleep here
+        std::this_thread::sleep_for(_currentMovementParams.trackingTelemetryInterval);
+    }
+
+    if (_stopMovementRequest->load()) {  // external stop
+        logWarn("Tracking was stopped!");
+        *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK;
+        *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE;
+    } else {
+        if (_lastMountError->load()) {
+            *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
+            auto err = _lastMountError->load();
+            logError("Tracking finished with error: {} ({}::{})", err.message(), err.category().name(), err.value());
+        } else {  // ???!!!!!
+            logInfo("Tracking finished without errors!");
+        }
+    }
+
+    bool ok = _pathFile.saveToFile(_currentMovementParams.trackingPathFilename, std::ios_base::app);
+    if (!ok && !_currentMovementParams.trackingPathFilename.empty()) {
+        logError("Cannot save trajectory file!");
+    }
+    _pathFile.clearPath();
+
+    return *_lastMountError;
+}
 
 Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl()
 {
@@ -478,8 +797,7 @@ Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl()
                !_lastMountError->load()) {
             if ((std::chrono::steady_clock::now() - start_tp) >
                 _mountConfig.getValue<std::chrono::seconds>("stopTimeout").value()) {
-                // set timeout error!
-                // *_lastMountError =
+                *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_STOP_TIMEOUT;
                 break;
             }
 
@@ -496,6 +814,10 @@ Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl()
         errorLogging("An error occured while stoppping mount: ", _lastMountError->load());
     }
 
+    if (!_lastMountError->load()) {
+        *_lastMountError = mcc::impl::MccGenericMountErrorCode::ERROR_OK;
+    }
+
     return _lastMountError->load();
 }
 

asibfm700_mount.h

@@ -2,7 +2,7 @@
 
 #include <mcc/mcc_generic_mount.h>
 #include <mcc/mcc_generic_movecontrols.h>
-#include <mcc/mcc_movement_controls.h>
+// #include <mcc/mcc_movement_controls.h>
 #include <mcc/mcc_pzone_container.h>
 #include <mcc/mcc_spdlog.h>
 #include <mcc/mcc_telemetry.h>
@@ -39,21 +39,11 @@ public:
     using typename movement_controls_t::movement_params_t;
 
 
-    // using Asibfm700CCTE::setStateERFA;
-    // using Asibfm700CCTE::updateBulletinA;
-    // using Asibfm700CCTE::updateLeapSeconds;
-    // using Asibfm700CCTE::updateMeteoERFA;
-
     using gm_class_t::logCritical;
     using gm_class_t::logDebug;
     using gm_class_t::logError;
     using gm_class_t::logInfo;
     using gm_class_t::logWarn;
-    // using Asibfm700Logger::logCritical;
-    // using Asibfm700Logger::logDebug;
-    // using Asibfm700Logger::logError;
-    // using Asibfm700Logger::logInfo;
-    // using Asibfm700Logger::logWarn;
 
     // using Asibfm700PZoneContainer::addPZone;
 
@@ -84,126 +74,23 @@ protected:
 
     // movement methods
     mcc::impl::MccMovementPathFile _pathFile{};
+    decltype(telemetry_data_t::targetPos) _enteredBackupCoordinates;
 
     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);
+
+    // if predicted == true then the method checks the coordinates calculated for a point in time in the future, i.e.
+    //                 the predicted position if the movement is immediately stopped.
+    // if predicted == false then the method checks the current mount coordinates
+    error_t checkPZone(typename telemetry_t::telemetry_data_t const& tdata, bool predicted = true);
+
     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}};
 };
 
 
-/*
-class Asibfm700Mount : public Asibfm700CCTE,
-                       public Asibfm700PCM,
-                       public mcc::MccGenericFsmMount<mcc::MccGenericMount<AsibFM700ServoController,
-                                                                           mcc::MccTelemetry,
-                                                                           Asibfm700PZoneContainer,
-                                                                           mcc::MccSimpleSlewingModel,
-                                                                           mcc::MccSimpleTrackingModel,
-                                                                           Asibfm700Logger>>
-{
-    typedef mcc::MccGenericMount<AsibFM700ServoController,
-                                 mcc::MccTelemetry,
-                                 Asibfm700PZoneContainer,
-                                 mcc::MccSimpleSlewingModel,
-                                 mcc::MccSimpleTrackingModel,
-                                 Asibfm700Logger>
-        gm_class_t;
-
-    typedef mcc::MccGenericFsmMount<mcc::MccGenericMount<AsibFM700ServoController,
-                                                         mcc::MccTelemetry,
-                                                         Asibfm700PZoneContainer,
-                                                         mcc::MccSimpleSlewingModel,
-                                                         mcc::MccSimpleTrackingModel,
-                                                         Asibfm700Logger>>
-        base_gm_class_t;
-
-protected:
-    struct Asibfm700ErrorState : base_gm_class_t::MccGenericFsmMountBaseState {
-        static constexpr std::string_view ID{"ASIBFM700-MOUNT-ERROR-STATE"};
-
-        // void exit(MccGenericFsmMountErrorEvent& event)
-        // {
-        //     event.mount()->logWarn("The mount already in error state!");
-        // }
-
-        void enter(MccGenericFsmMountErrorEvent& event)
-        {
-            enterLog(event);
-
-            // event.mount()->logWarn("The mount already in error state!");
-            auto err = event.eventData();
-            event.mount()->logError("An error occured: {} [{} {}]", err.message(), err.value(), err.category().name());
-        }
-
-        void exit(mcc::fsm::traits::fsm_event_c auto& event)
-        {
-            exitLog(event);
-        }
-
-        void enter(mcc::fsm::traits::fsm_event_c auto& event)
-        {
-            enterLog(event);
-
-            // ...
-        }
-
-        using transition_t = mcc::fsm::fsm_transition_table_t<
-            std::pair<MccGenericFsmMountErrorEvent, Asibfm700ErrorState>,
-            std::pair<MccGenericFsmMountInitEvent, MccGenericFsmMountInitState<Asibfm700ErrorState>>,
-            std::pair<MccGenericFsmMountIdleEvent, MccGenericFsmMountIdleState<Asibfm700ErrorState>>>;
-    };
-
-
-    typedef base_gm_class_t::MccGenericFsmMountStartState<Asibfm700ErrorState> Asibfm700StartState;
-
-public:
-    using base_gm_class_t::error_t;
-
-    using Asibfm700CCTE::setStateERFA;
-    using Asibfm700CCTE::updateBulletinA;
-    using Asibfm700CCTE::updateLeapSeconds;
-    using Asibfm700CCTE::updateMeteoERFA;
-
-    using Asibfm700Logger::logCritical;
-    using Asibfm700Logger::logDebug;
-    using Asibfm700Logger::logError;
-    using Asibfm700Logger::logInfo;
-    using Asibfm700Logger::logWarn;
-
-    // using Asibfm700PZoneContainer::addPZone;
-
-    Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger);
-
-    ~Asibfm700Mount();
-
-    Asibfm700Mount(Asibfm700Mount&&) = default;
-    Asibfm700Mount& operator=(Asibfm700Mount&&) = default;
-
-    Asibfm700Mount(const Asibfm700Mount&) = delete;
-    Asibfm700Mount& operator=(const Asibfm700Mount&) = delete;
-
-    error_t initMount();
-
-    error_t updateMountConfig(Asibfm700MountConfig const&);
-    Asibfm700MountConfig currentMountConfig();
-
-protected:
-    Asibfm700MountConfig _mountConfig;
-    std::unique_ptr<std::mutex> _mountConfigMutex;
-
-    void errorLogging(const std::string&, const std::error_code&);
-};
-*/
-
-// static_assert(mcc::mcc_position_controls_c<Asibfm700Mount>, "");
-// static_assert(mcc::mcc_all_controls_c<Asibfm700Mount>, "");
 
 static_assert(mcc::mcc_generic_mount_c<Asibfm700Mount>, "");