...
This commit is contained in:
parent
3c0c719e37
commit
255c34dbb2
@ -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<PZoneContT>(std::move(pzcont_ctor_args))),
|
||||
SlewModelT(std::make_from_tuple<SlewModelT>(std::move(smodel_ctor_args))),
|
||||
TrackModelT(std::make_from_tuple<TrackModelT>(std::move(tmodel_ctor_args))),
|
||||
_mountStatus(new std::atomic<mount_status_t>{})
|
||||
_mountStatus(new std::atomic<mount_status_t>{}),
|
||||
_lastMountError(new std::atomic<error_t>{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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
bool in_zone;
|
||||
std::vector<bool> 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);
|
||||
}
|
||||
|
||||
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 {
|
||||
s_err = trackTarget();
|
||||
*_lastMountError = trackTarget();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return s_err;
|
||||
);
|
||||
|
||||
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<std::atomic<MccGenericMount::mount_status_t>> _mountStatus;
|
||||
|
||||
std::future<void> _slewingFuture{};
|
||||
|
||||
std::unique_ptr<std::atomic<error_t>> _lastMountError;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -212,19 +212,23 @@ public:
|
||||
|
||||
|
||||
auto target_point = [&, this](MccCelestialPoint* point) -> std::error_code {
|
||||
auto dt = std::chrono::duration<double>{tdata.HA} +
|
||||
_currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds
|
||||
double dha =
|
||||
std::chrono::duration<double>(_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<double>{tdata.HA} +
|
||||
// _currentParams.timeShiftToTargetPoint * mcc_sideral_to_UT1_ratio; // hour seconds
|
||||
|
||||
auto tp_dt = std::chrono::duration_cast<typename decltype(tdata.time_point)::duration>(
|
||||
_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<MccAngle::NORM_KIND_0_360>(),
|
||||
.X = MccAngle(dt.count() * std::numbers::pi / 3600.0 / 15.0)
|
||||
.normalize<MccAngle::NORM_KIND_180_180>(),
|
||||
.Y = tdata.DEC_APP};
|
||||
.X = MccAngle(target_ha).normalize<MccAngle::NORM_KIND_180_180>(),
|
||||
.Y = tdata.target.DEC_APP};
|
||||
mcc_tp2tp(tdata.time_point + tp_dt, pt.time_point);
|
||||
|
||||
point->time_point = pt.time_point;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user