diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..9160a76 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "C_Cpp.default.compilerPath": "/usr/bin/g++-15" +} \ No newline at end of file diff --git a/asibfm700_mount.cpp b/asibfm700_mount.cpp index 3e31c9b..491de37 100644 --- a/asibfm700_mount.cpp +++ b/asibfm700_mount.cpp @@ -377,6 +377,8 @@ void Asibfm700Mount::logMountPos(telemetry_t::telemetry_data_t const& tdata) // tdata.mountPos and tdata.targetPos are coordinates in the HA-DEC // equathorial system + logTrace("Current telemetry data:"); + 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(), @@ -387,6 +389,10 @@ void Asibfm700Mount::logMountPos(telemetry_t::telemetry_data_t const& tdata) mcc::impl::MccAngle(tdata.mountPos.co_lat()).sexagesimal(), tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal()); + logTrace(" current mount speed: HA = {}/s, DEC = {}/s", + mcc::impl::MccAngleFancyString(tdata.hwState.speedXY.x()), + mcc::impl::MccAngleFancyString(tdata.hwState.speedXY.y())); + _pathFile.addToPath(tdata); auto dist = tdata.mountPos.distance(tdata.targetPos); @@ -472,7 +478,7 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) { _pathFile.clearPath(); - _enteredBackupCoordinates = getPointingTarget(); + _enteredBackupCoordinates = telemetry_t::getPointingTarget(); typename telemetry_t::telemetry_data_t tdata; mcc::impl::MccSkyPoint start_target; @@ -560,11 +566,14 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) mcc::impl::MccSkyPoint::dist_result_t dist; while (!_stopMovementRequest->load()) { + // check for slewing timeout if ((std::chrono::steady_clock::now() - start_point) > getMovementParams().slewTimeout) { *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_SLEW_TIMEOUT; break; } + + // get current mount and target coordinates *_lastMountError = telemetryData(&tdata); if (_lastMountError->load()) { @@ -573,11 +582,21 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) break; } + // detect hardware errors + if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_ERROR) { + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_HARDWARE; + break; + } + + // check for prohibited zones proximity + *_lastMountError = checkPZone(tdata, true); + if (_lastMountError->load()) { + break; + } + if (last_hw_time >= tdata.hwState.XY.epoch().UTC()) { logTrace("\nSame hardware timepoint! Just continue to polling!\n"); - last_hw_time = tdata.hwState.XY.epoch().UTC(); - auto d = getMovementParams().slewingTelemetryInterval / 3; if (d.count()) { @@ -589,18 +608,20 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) continue; } + last_hw_time = tdata.hwState.XY.epoch().UTC(); hw_state.XY.setEpoch(ep_t::now()); - if (!slew_and_stop) { // update target coordinates only in the "slew-and-track" mode - hw_state.XY.setX(tdata.targetXY.x()); - hw_state.XY.setY(tdata.targetXY.y()); - } else { // restore initial target coordinate + // update target coordinates and corresponded encoder ones only in the "slew-and-track" mode + if (slew_and_stop) { // restore initial target coordinate init_target.setEpoch(tdata.mountPos.epoch()); tdata.targetPos.from(init_target); tdata.targetXY = start_targetXY; tdata.targetXY.setEpoch(tdata.mountPos.epoch()); + } else { + hw_state.XY.setX(tdata.targetXY.x()); + hw_state.XY.setY(tdata.targetXY.y()); } @@ -610,51 +631,46 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) break; } - *_lastMountError = checkPZone(tdata, true); - if (_lastMountError->load()) { - break; - // return _lastMountError->load(); - } - - dist = tdata.mountPos.distance(tdata.targetPos); - - if (dist.dist <= getMovementParams().slewToleranceRadius) { // start adjusting (precise pointing) - logInfo("target-to-mount distance is lesser than slew acceptable radius - exit!"); - - break; - } - - if (dist.dist <= getMovementParams().adjustCoordDiff) { // start adjusting (precise pointing) - *_mountStatus = mount_status_t::MOUNT_STATUS_ADJUSTING; - - // LibSidServo.correctTo - *_lastMountError = sendToHardware(hw_state); - - if (_lastMountError->load()) { + if (slew_and_stop) { // just wait for axes stopping + if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_STOPPED) { break; } - } + } else { + dist = tdata.mountPos.distance(tdata.targetPos); - 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) { + if (dist.dist <= getMovementParams().slewToleranceRadius) { + logInfo("target-to-mount distance is lesser than slew acceptable radius - exit!"); + + break; + } + + if (dist.dist <= getMovementParams().adjustCoordDiff) { // start adjusting (precise pointing) *_mountStatus = mount_status_t::MOUNT_STATUS_ADJUSTING; + // LibSidServo.correctTo *_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; - } + // 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; + // } + // } // sleep here std::this_thread::sleep_for(getMovementParams().slewingTelemetryInterval); @@ -753,7 +769,8 @@ Asibfm700Mount::error_t Asibfm700Mount::trackingImpl() mcc::impl::MccAngleFancyString(dist.dist), mcc::impl::MccAngleFancyString(_currentMovementParams.trackingMaxCoordDiff)); - _enteredBackupCoordinates = getPointingTarget(); + _enteredBackupCoordinates = telemetry_t::getPointingTarget(); + setPointingTarget(tdata.mountPos); *_lastMountError = telemetryData(&tdata); @@ -796,6 +813,12 @@ Asibfm700Mount::error_t Asibfm700Mount::trackingImpl() break; } + // detect hardware errors + if (tdata.hwState.movementState == hardware_t::hardware_movement_state_t::HW_MOVE_ERROR) { + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_HARDWARE; + break; + } + hw_state.XY.setX(tdata.targetXY.x()); hw_state.XY.setY(tdata.targetXY.y()); hw_state.XY.setEpoch(ep_t::now()); @@ -865,13 +888,16 @@ Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl() if (!_lastMountError->load()) { *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE; + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK; } else { *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR; errorLogging("An error occured while stoppping mount: ", _lastMountError->load()); - } - if (!_lastMountError->load()) { - *_lastMountError = mcc::impl::MccGenericMountErrorCode::ERROR_OK; + logError("Try to perform emergency stop!"); + *_lastMountError = static_cast(Mount.emergStop()); + if (_lastMountError->load()) { + errorLogging("An error occured while trying emergency stop: ", _lastMountError->load()); + } } return _lastMountError->load(); diff --git a/asibfm700_mount.h b/asibfm700_mount.h index cec66f5..9ae28b7 100644 --- a/asibfm700_mount.h +++ b/asibfm700_mount.h @@ -63,6 +63,12 @@ public: error_t updateMountConfig(std::string const&); Asibfm700MountConfig currentMountConfig(); + // redefined method + auto getPointingTarget() const + { + return _enteredBackupCoordinates; + } + protected: Asibfm700MountConfig _mountConfig; std::unique_ptr _mountConfigMutex;