...

2026-02-26 12:13:14 +03:00
parent 69e49283dd
commit b14b20e069
3 changed files with 350 additions and 141 deletions
--- a/asibfm700_mount.cpp
+++ b/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();
 }