diff --git a/LibSidServo/main.c b/LibSidServo/main.c index a6f04b0..52d5200 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -47,10 +47,10 @@ static movemodel_t *Xmodel, *Ymodel; // accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2 limits_t Xlimits = { - .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, + .min = {.coord = -3.1241, .speed = -1e-10, .accel = 1e-6}, .max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}}, Ylimits = { - .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, + .min = {.coord = -3.1241, .speed = -1e-10, .accel = 1e-6}, .max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}} ; static mcc_errcodes_t shortcmd(short_command_t *cmd); @@ -93,7 +93,7 @@ static int initstarttime(){ // return difference (in seconds) between time1 and time0 double timediff(const struct timespec *time1, const struct timespec *time0){ if(!time1 || !time0) return -1.; - return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9; + return (double)(time1->tv_sec - time0->tv_sec) + (double)(time1->tv_nsec - time0->tv_nsec) / 1e9; } // difference between given time and last initstarttime() call double timediff0(const struct timespec *time1){ @@ -319,8 +319,8 @@ static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){ */ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){ if(!target || !speed) return MCC_E_BADFORMAT; - if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; - if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT; + // if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; + // if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT; // updateMotorPos() here can make a problem; TODO: remove? if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; short_command_t cmd = {0}; diff --git a/asibfm700_mount.cpp b/asibfm700_mount.cpp index af90516..3e31c9b 100644 --- a/asibfm700_mount.cpp +++ b/asibfm700_mount.cpp @@ -475,6 +475,10 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) _enteredBackupCoordinates = getPointingTarget(); typename telemetry_t::telemetry_data_t tdata; + mcc::impl::MccSkyPoint start_target; + mcc::impl::MccSkyHADEC_OBS init_target; + decltype(tdata.targetXY) start_targetXY; + *_lastMountError = telemetryData(&tdata); if (_lastMountError->load()) { errorLogging("An error occured while telemetry requesting: ", _lastMountError->load()); @@ -486,6 +490,18 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) return _lastMountError->load(); } + if (slew_and_stop) { + // store initial target coordinates in the "slew-and-stop" mode + // and use it below! + start_target = tdata.targetPos; + + init_target.setX(tdata.targetPos.co_lon()); + init_target.setY(tdata.targetPos.co_lat()); + + start_targetXY = tdata.targetXY; + } + + 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", @@ -544,6 +560,11 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) mcc::impl::MccSkyPoint::dist_result_t dist; while (!_stopMovementRequest->load()) { + if ((std::chrono::steady_clock::now() - start_point) > getMovementParams().slewTimeout) { + *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_SLEW_TIMEOUT; + break; + } + *_lastMountError = telemetryData(&tdata); if (_lastMountError->load()) { @@ -552,10 +573,37 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) break; } - hw_state.XY.setX(tdata.targetXY.x()); - hw_state.XY.setY(tdata.targetXY.y()); + 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()) { + std::this_thread::sleep_for(d); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + continue; + } + + 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 + init_target.setEpoch(tdata.mountPos.epoch()); + tdata.targetPos.from(init_target); + + tdata.targetXY = start_targetXY; + tdata.targetXY.setEpoch(tdata.mountPos.epoch()); + } + + logMountPos(tdata); if (_stopMovementRequest->load()) { // external stop @@ -568,16 +616,18 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) // return _lastMountError->load(); } - if ((std::chrono::steady_clock::now() - start_point) > getMovementParams().slewTimeout) { - *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_SLEW_TIMEOUT; + 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; } - dist = tdata.mountPos.distance(tdata.targetPos); - 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()) { @@ -613,7 +663,7 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) if (_stopMovementRequest->load()) { // external stop logWarn("Slewing was stopped!"); *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK; - *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE; + *_mountStatus = mount_status_t::MOUNT_STATUS_STOPPED; } else { if (_lastMountError->load()) { *_mountStatus = mount_status_t::MOUNT_STATUS_ERROR; @@ -621,6 +671,7 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) logError("Slewing finished with error: {} ({}::{})", err.message(), err.category().name(), err.value()); } else { logInfo("Slewing finished without errors!"); + *_mountStatus = mount_status_t::MOUNT_STATUS_IDLE; } } @@ -632,6 +683,13 @@ Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) errorLogging("An error occured while telemetry requesting: ", _lastMountError->load()); } else { logInfo("Final mount slewing position:"); + + if (slew_and_stop) { // restore initial target position + init_target.setEpoch(tdata.mountPos.epoch()); + tdata.targetPos.from(init_target); + + tdata.targetXY = start_targetXY; + } logMountPos(tdata); } } @@ -755,21 +813,19 @@ Asibfm700Mount::error_t Asibfm700Mount::trackingImpl() std::this_thread::sleep_for(_currentMovementParams.trackingTelemetryInterval); } - if (_stopMovementRequest->load()) { // external stop - logWarn("Tracking was stopped!"); + logWarn("Tracking was stopped!"); + + 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 { // ???!!!!! *_lastMountError = mcc::impl::MccGenericMovementControlsErrorCode::ERROR_OK; + logInfo("Tracking finished without errors!"); *_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); + bool ok = _pathFile.saveToFile(_currentMovementParams.trackingPathFilename, std::ios::out | std::ios::app); if (!ok && !_currentMovementParams.trackingPathFilename.empty()) { logError("Cannot save trajectory file!"); } diff --git a/asibfm700_servocontroller.cpp b/asibfm700_servocontroller.cpp index d0c8076..7ed192f 100644 --- a/asibfm700_servocontroller.cpp +++ b/asibfm700_servocontroller.cpp @@ -144,6 +144,7 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(har ns -= secs; std::timespec tp{.tv_sec = secs.count(), .tv_nsec = ns.count()}; + Mount.currentT(&tp); if (state.movementState == hardware_movement_state_t::HW_MOVE_SLEWING) { // according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis