diff --git a/mcc/mcc_generic_mount.h b/mcc/mcc_generic_mount.h index aab9ae4..2d394c6 100644 --- a/mcc/mcc_generic_mount.h +++ b/mcc/mcc_generic_mount.h @@ -20,7 +20,11 @@ enum class MccGenericMountErrorCode : int { ERROR_HW_GETSTATE, ERROR_SET_TARGET, ERROR_MOUNT_SLEW, - ERROR_MOUNT_TRACK + ERROR_MOUNT_TRACK, + ERROR_GET_TELEMETRY, + ERROR_UNSUPPORTED_TARGET_COORDPAIR, + ERROR_PZONE_COMP, + ERROR_TARGET_IN_ZONE }; enum class MccGenericFsmMountErrorCode : int { ERROR_OK, ERROR_INVALID_OPERATION, ERROR_UNKNOWN_EVENT }; @@ -70,6 +74,20 @@ struct MccGenericMountCategory : public std::error_category { return "an error occured while stopping mount"; case MccGenericMountErrorCode::ERROR_HW_GETSTATE: return "cannot get state of hardware"; + case MccGenericMountErrorCode::ERROR_SET_TARGET: + return "cannot set target coordinates"; + case MccGenericMountErrorCode::ERROR_MOUNT_SLEW: + return "slewing error"; + case MccGenericMountErrorCode::ERROR_MOUNT_TRACK: + return "tracking error"; + case MccGenericMountErrorCode::ERROR_GET_TELEMETRY: + return "cannot get telemetry data"; + case MccGenericMountErrorCode::ERROR_UNSUPPORTED_TARGET_COORDPAIR: + return "unsupported coordinate pair of target"; + case MccGenericMountErrorCode::ERROR_PZONE_COMP: + return "an error occured while computing prohibited zone"; + case MccGenericMountErrorCode::ERROR_TARGET_IN_ZONE: + return "target coordinates are in prohibitted zone"; default: return "UNKNOWN"; } @@ -186,7 +204,8 @@ public: PZoneContT(std::make_from_tuple(std::move(pzcont_ctor_args))), SlewModelT(std::make_from_tuple(std::move(smodel_ctor_args))), TrackModelT(std::make_from_tuple(std::move(tmodel_ctor_args))), - _mountStatus(new std::atomic{}) + _mountStatus(new std::atomic{}), + _lastMountError(new std::atomic{MccGenericMountErrorCode::ERROR_OK}) { *_mountStatus = mount_status_t::IDLE; } @@ -200,12 +219,18 @@ public: virtual ~MccGenericMount() { stopMount(); + + if (_slewingFuture.valid()) { + _slewingFuture.wait_for(std::chrono::seconds(3)); + } }; error_t stopMount() { logInfo("Stop any movements ..."); + *_lastMountError = MccGenericMountErrorCode::ERROR_OK; + this->stopTracking(); this->stopSlewing(); @@ -213,34 +238,36 @@ public: if (hw_err) { *_mountStatus = mount_status_t::ERROR; - return mcc_deduce_error_code(hw_err, MccGenericMountErrorCode::ERROR_HW_STOP); + *_lastMountError = mcc_deduce_error_code(hw_err, MccGenericMountErrorCode::ERROR_HW_STOP); + } else { + logInfo("Stop command was sent"); + + *_mountStatus = mount_status_t::STOPPED; } - logInfo("Stop command was sent"); - - *_mountStatus = mount_status_t::STOPPED; - - return MccGenericMountErrorCode::ERROR_OK; + return *_lastMountError; } error_t initMount() { logInfo("Start generic mount initialization ..."); + *_lastMountError = MccGenericMountErrorCode::ERROR_OK; + *_mountStatus = mount_status_t::INITIALIZATION; auto hw_err = this->hardwareInit(); if (hw_err) { *_mountStatus = mount_status_t::ERROR; - return mcc_deduce_error_code(hw_err, MccGenericMountErrorCode::ERROR_HW_STOP); + *_lastMountError = mcc_deduce_error_code(hw_err, MccGenericMountErrorCode::ERROR_HW_STOP); + } else { + logInfo("Generic mount initialization was performed"); + + *_mountStatus = mount_status_t::IDLE; } - logInfo("Generic mount initialization was performed"); - - *_mountStatus = mount_status_t::IDLE; - - return MccGenericMountErrorCode::ERROR_OK; + return *_lastMountError; } // re-implements TelemetryT::setPointingTarget to hold target coordinates @@ -268,6 +295,47 @@ public: // re-implements SlewModelT::slewToTarget to fetch input target coordinates from intermediate buffer error_t slewToTarget(bool slew_and_stop = false) { + *_lastMountError = MccGenericMountErrorCode::ERROR_OK; + + // save current target + MccTelemetryData tdata; + + auto sl_params = this->getSlewingParams(); + + auto t_err = this->telemetryData(&tdata); + if (t_err) { + return *_lastMountError = mcc_deduce_error_code(t_err, MccGenericMountErrorCode::ERROR_GET_TELEMETRY); + } + + MccCelestialPoint curr_target{.pair_kind = tdata.target.pair_kind, .time_point = tdata.target.time_point}; + if (curr_target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_ICRS) { + curr_target.X = tdata.target.RA_ICRS; + curr_target.Y = tdata.target.DEC_ICRS; + } else if (curr_target.pair_kind == MccCoordPairKind::COORDS_KIND_RADEC_APP) { + curr_target.X = tdata.target.RA_APP; + curr_target.Y = tdata.target.DEC_APP; + } else if (curr_target.pair_kind == MccCoordPairKind::COORDS_KIND_HADEC_APP) { + curr_target.X = tdata.target.HA; + curr_target.Y = tdata.target.DEC_APP; + } else if (curr_target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) { + curr_target.X = tdata.target.AZ; + curr_target.Y = tdata.target.ZD; + } else if (curr_target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT) { + curr_target.X = tdata.target.AZ; + curr_target.Y = tdata.target.ALT; + } else if (curr_target.pair_kind == MccCoordPairKind::COORDS_KIND_XY) { + curr_target.X = tdata.target.X; + curr_target.Y = tdata.target.Y; + } else { // it should not be! + logError( + std::format("Unsupported coordinate pair kind ({}) was read from telemetry data! Is mount " + "initialized?! Cannot start slewing!", + (int)curr_target.pair_kind)); + return *_lastMountError = MccGenericMountErrorCode::ERROR_UNSUPPORTED_TARGET_COORDPAIR; + } + + // set new target coordinates and check it + _enteredTargetCoordiniates.time_point = std::chrono::system_clock::now(); auto err = TelemetryT::setPointingTarget(_enteredTargetCoordiniates); if (err) { @@ -276,23 +344,67 @@ public: return mcc_deduce_error_code(err, MccGenericMountErrorCode::ERROR_SET_TARGET); } - *_mountStatus = mount_status_t::SLEWING; - - error_t s_err = - mcc_deduce_error_code(SlewModelT::slewToTarget(slew_and_stop), MccGenericMountErrorCode::ERROR_MOUNT_SLEW); - - if (s_err) { - *_mountStatus = mount_status_t::ERROR; - return s_err; + t_err = this->waitForTelemetryData(&tdata, sl_params.telemetryTimeout); + if (t_err) { + return mcc_deduce_error_code(t_err, MccGenericMountErrorCode::ERROR_GET_TELEMETRY); } - if (slew_and_stop) { - *_mountStatus = mount_status_t::IDLE; - } else { - s_err = trackTarget(); + bool in_zone; + std::vector in_zone_vec; + auto pz_err = this->inPZone(tdata.target, &in_zone, &in_zone_vec); + if (pz_err) { + return *_lastMountError = mcc_deduce_error_code(pz_err, MccGenericMountErrorCode::ERROR_PZONE_COMP); } - return s_err; + if (in_zone) { + size_t i = 0; + for (; i < in_zone_vec.size(); ++i) { + if (in_zone_vec[i]) { + break; + } + } + + logError("target point is in prohibited zone (zone index: {})! Entered target coordinates:", i); + logError(std::format( + " RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", mcc::MccAngle{tdata.target.RA_APP}.sexagesimal(true), + mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal(), mcc::MccAngle{tdata.target.HA}.sexagesimal(true), + mcc::MccAngle{tdata.LST}.sexagesimal(true))); + logError(std::format(" AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.target.AZ}.sexagesimal(), + mcc::MccAngle{tdata.target.ZD}.sexagesimal(), + mcc::MccAngle{tdata.target.ALT}.sexagesimal())); + + logError(std::format(" hardware X, Y: {}, {}", mcc::MccAngle{tdata.target.X}.sexagesimal(), + mcc::MccAngle{tdata.target.Y}.sexagesimal())); + + return *_lastMountError = mcc_deduce_error_code(pz_err, MccGenericMountErrorCode::ERROR_TARGET_IN_ZONE); + } + + + + // start slewing (asunchronous operation) + + _slewingFuture = std::async(std::launch::async, + [slew_and_stop, this]() { + *_mountStatus = mount_status_t::SLEWING; + + auto err = SlewModelT::slewToTarget(slew_and_stop); + if (err) { + *_lastMountError = + mcc_deduce_error_code(err, MccGenericMountErrorCode::ERROR_MOUNT_SLEW); + + *_mountStatus = mount_status_t::ERROR; + } else { + if (slew_and_stop) { + *_mountStatus = mount_status_t::IDLE; + } else { + *_lastMountError = trackTarget(); + } + } + } + + ); + + return MccGenericMountErrorCode::ERROR_OK; } error_t trackTarget() @@ -321,8 +433,7 @@ public: error_t stopTracking() { - // *_mountStatus = mount_status_t::IDLE; - _mountStatus->store(mount_status_t::IDLE); + *_mountStatus = mount_status_t::IDLE; TrackModelT::stopTracking(); @@ -335,10 +446,21 @@ public: return *_mountStatus; } + + // returns last mount-action error code + error_t mountLastError() const + { + return *_lastMountError; + } + protected: MccCelestialPoint _enteredTargetCoordiniates{}; std::unique_ptr> _mountStatus; + + std::future _slewingFuture{}; + + std::unique_ptr> _lastMountError; }; diff --git a/mcc/mcc_slewing_model.h b/mcc/mcc_slewing_model.h index e0ea08e..d4361bc 100644 --- a/mcc/mcc_slewing_model.h +++ b/mcc/mcc_slewing_model.h @@ -623,6 +623,9 @@ public: *_stopSlewing = true; logger.logInfo("Slewing finished"); + error_t err = *_lastError; + logger.logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message())); + // get final position @@ -700,12 +703,12 @@ public: return *_lastError; } - // asynchronous slewing process - _slewFuncFuture = std::async(std::launch::async, _slewingFunc, slew_and_stop); + // // asynchronous slewing process + // _slewFuncFuture = std::async(std::launch::async, _slewingFunc, slew_and_stop); - return MccSimpleSlewingModelErrorCode::ERROR_OK; + // return MccSimpleSlewingModelErrorCode::ERROR_OK; - // return _slewingFunc(slew_and_stop); + return _slewingFunc(slew_and_stop); } diff --git a/mcc/mcc_tracking_model.h b/mcc/mcc_tracking_model.h index 588cc20..ee33f6a 100644 --- a/mcc/mcc_tracking_model.h +++ b/mcc/mcc_tracking_model.h @@ -212,19 +212,23 @@ public: auto target_point = [&, this](MccCelestialPoint* point) -> std::error_code { - auto dt = std::chrono::duration{tdata.HA} + - _currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds + double dha = + std::chrono::duration(_currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio) + .count(); // sideral hour seconds + dha *= std::numbers::pi / 43200.0; // radians + + auto target_ha = tdata.target.HA + dha; + + // auto dt = std::chrono::duration{tdata.HA} + + // _currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds auto tp_dt = std::chrono::duration_cast( _currentParams.timeShiftToTargetPoint); // point in +time_dist future MccCelestialPoint pt{.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP, - // .X = MccAngle(dt.count() * std::numbers::pi / 3600.0 - // / 15.0).normalize(), - .X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0) - .normalize(), - .Y = tdata.DEC_APP}; + .X = MccAngle(target_ha).normalize(), + .Y = tdata.target.DEC_APP}; mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point); point->time_point = pt.time_point;