...
This commit is contained in:
parent
c71cf98dd9
commit
2ff2c24e52
@ -252,9 +252,6 @@ concept mcc_mount_hardware_c = !std::copyable<T> && std::movable<T> && requires(
|
||||
|
||||
{ t.stop() } -> std::same_as<typename T::error_t>; // stop any moving
|
||||
{ t.init() } -> std::same_as<typename T::error_t>; // initialize hardware
|
||||
|
||||
{ t.slewTo(std::declval<typename T::axes_pos_t>()) } -> std::same_as<typename T::error_t>;
|
||||
{ t.correctTo(std::declval<typename T::axes_pos_t>()) } -> std::same_as<typename T::error_t>;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -114,17 +114,32 @@ public:
|
||||
|
||||
typedef std::error_code error_t;
|
||||
|
||||
static constexpr size_t defaultAdjustSuccessCycles = 5;
|
||||
|
||||
struct slew_point_t : MccCelestialPoint {
|
||||
// target-mount coordinate difference to start adjusting slewing (in radians)
|
||||
// target-mount coordinate difference to start adjusting of slewing (in radians)
|
||||
coord_t adjustCoordDiff{(double)MccAngle{10.0_degs}};
|
||||
|
||||
// coordinates difference to stop slewing (in radians)
|
||||
coord_t slewPrecision{(double)MccAngle{5.0_arcsecs}};
|
||||
coord_t slewToleranceRadius{(double)MccAngle{5.0_arcsecs}};
|
||||
|
||||
// coordinates polling interval in seconds
|
||||
std::chrono::duration<double> coordPollingInterval{0.1};
|
||||
bool stopAfterSlew{false};
|
||||
std::chrono::seconds slewTimeout{3600};
|
||||
|
||||
coord_t slewXRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
|
||||
coord_t slewYRate{0.0}; // maximal slewing rate (0 means move with maximal allowed rate)
|
||||
|
||||
coord_t adjustXRate{
|
||||
(double)MccAngle{5.0_arcmins}}; // maximal adjusting rate (a rate at the final slewing stage)
|
||||
coord_t adjustYRate{
|
||||
(double)MccAngle{5.0_arcmins}}; // maximal adjusting rate (a rate at the final slewing stage)
|
||||
|
||||
// number of consecutive measurements within slewPrecision radius to stop adjusting of slewing
|
||||
size_t withinToleranceCycleNumber{10};
|
||||
// maximal allowed number of adjusting cycles
|
||||
size_t maxAdjustingCycleNumber{100};
|
||||
};
|
||||
|
||||
|
||||
@ -198,11 +213,15 @@ protected:
|
||||
|
||||
coord_t ra_icrs, dec_icrs;
|
||||
|
||||
if (slew_point.adjustSuccessCycles == 0) {
|
||||
slew_point.adjustSuccessCycles = 5;
|
||||
}
|
||||
|
||||
// first, compute encoder coordinates
|
||||
ax_pos.time_point = astrom_engine_t::timePointNow();
|
||||
t_err = telemetry.toHardware(slew_point, ax_pos.time_point, ax_pos.x, ax_pos.y);
|
||||
if (!t_err) {
|
||||
// setup target sky point
|
||||
// SETUP TARGET SKY POINT
|
||||
t_err = telemetry.setTarget(slew_point);
|
||||
}
|
||||
|
||||
@ -218,16 +237,19 @@ protected:
|
||||
}
|
||||
}
|
||||
|
||||
// move mount (it is assumed this is asynchronous operation!!!)
|
||||
typename hardware_t::error_t err = hardware->setPos(ax_pos);
|
||||
// start moving the mount (it is assumed this is asynchronous operation!!!)
|
||||
ax_pos.xrate = slew_point.slewXRate;
|
||||
ax_pos.yrate = slew_point.slewYRate;
|
||||
typename hardware_t::error_t hw_err = hardware->setPos(ax_pos);
|
||||
|
||||
if (err) {
|
||||
if constexpr (std::same_as<decltype(err), error_t>) {
|
||||
logError(std::format("An hardware error occured: code = {} ({})", err.value(), err.message()));
|
||||
return err;
|
||||
if (hw_err) {
|
||||
if constexpr (std::same_as<decltype(hw_err), error_t>) {
|
||||
logError(
|
||||
std::format("An hardware error occured: code = {} ({})", hw_err.value(), hw_err.message()));
|
||||
return hw_err;
|
||||
} else {
|
||||
if constexpr (traits::mcc_formattable<decltype(err)>) {
|
||||
logError(std::format("An hardware error occured: code = {}", err));
|
||||
if constexpr (traits::mcc_formattable<decltype(hw_err)>) {
|
||||
logError(std::format("An hardware error occured: code = {}", hw_err));
|
||||
}
|
||||
return MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
|
||||
}
|
||||
@ -235,36 +257,23 @@ protected:
|
||||
|
||||
|
||||
typename hardware_t::axes_pos_t::time_point_t prev_time_point{};
|
||||
// typename telemetry_t::mount_telemetry_data_t::time_point_t prev_time_point{};
|
||||
|
||||
auto adj_ax_pos = ax_pos; // to prevent possible effects in hardware 'setPos' method
|
||||
adj_ax_pos.xrate = slew_point.adjustXRate;
|
||||
adj_ax_pos.yrate = slew_point.adjustYRate;
|
||||
|
||||
typename telemetry_t::mount_telemetry_data_t::coord_t xr, yr, coord_diff2,
|
||||
adjRad2 = slew_point.adjustCoordDiff * slew_point.adjustCoordDiff;
|
||||
adj_rad2 = slew_point.adjustCoordDiff * slew_point.adjustCoordDiff,
|
||||
tol_rad2 = slew_point.slewToleranceRadius * slew_point.slewToleranceRadius;
|
||||
|
||||
std::array<bool, Nzones> in_zone_flag;
|
||||
auto start_poll_tm = std::chrono::steady_clock::now();
|
||||
|
||||
size_t i_adj_cycle = 0;
|
||||
size_t i_in_tol_cycle = 0;
|
||||
bool in_adj_mode = false;
|
||||
|
||||
auto iter = telemetry.addCallbackFunc([slew_point = std::move(slew_point), this](auto t_data) {
|
||||
// check prohibited zones
|
||||
|
||||
std::array<bool, Nzones> in_zone_flag;
|
||||
auto t_err = mccCheckInZonePZTuple(*telemetry, p_mount_controls->prohibitedZones, in_zone_flag);
|
||||
|
||||
if (t_err) {
|
||||
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
||||
logError(
|
||||
std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message()));
|
||||
return t_err;
|
||||
} else {
|
||||
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
||||
logError(std::format("An telemetry error occured: code = {}", t_err));
|
||||
}
|
||||
return MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
|
||||
}
|
||||
}
|
||||
|
||||
typename telemetry_t::mount_telemetry_data_t::coord_t xr, yr, coord_diff2,
|
||||
adjRad2 = slew_point.adjustCoordDiff * slew_point.adjustCoordDiff;
|
||||
|
||||
auto coord_diff_func = [](auto& t_data) {
|
||||
typename telemetry_t::mount_telemetry_data_t::coord_t xr, yr;
|
||||
if constexpr (mccIsEquatorialMount(pec_t::mountType)) {
|
||||
xr = t_data.tagRA - t_data.mntHA;
|
||||
yr = t_data.tagDEC - t_data.mntDEC;
|
||||
@ -275,11 +284,76 @@ protected:
|
||||
static_assert(false, "UNSUPPORTED MOUNT TYPE!");
|
||||
}
|
||||
|
||||
coord_diff2 = xr * xr + yr * yr;
|
||||
return xr * xr + yr * yr;
|
||||
};
|
||||
|
||||
if (coord_diff2 < adjRad2) { // switch to adjusting mode
|
||||
auto cycle_func = [&](auto t_data) mutable {
|
||||
// check for prohibited zones
|
||||
auto t_err = mccCheckInZonePZTuple(*telemetry, p_mount_controls->prohibitedZones, in_zone_flag);
|
||||
|
||||
if (t_err) {
|
||||
if constexpr (std::same_as<decltype(t_err), error_t>) {
|
||||
logError(
|
||||
std::format("An telemetry error occured: code = {} ({})", t_err.value(), t_err.message()));
|
||||
res_err = t_err;
|
||||
} else {
|
||||
if constexpr (traits::mcc_formattable<decltype(t_err)>) {
|
||||
logError(std::format("An telemetry error occured: code = {}", t_err));
|
||||
}
|
||||
res_err = MccSimpleSlewModelErrorCode::ERROR_TELEMETRY_DATA;
|
||||
}
|
||||
return;
|
||||
}
|
||||
});
|
||||
|
||||
// t_data was updated in caller!!!
|
||||
coord_diff2 = coord_diff_func(t_data);
|
||||
|
||||
if (coord_diff2 < adj_rad2) { // adjusting mode
|
||||
in_adj_mode = true;
|
||||
hw_err = hardware->setPos(adj_ax_pos);
|
||||
|
||||
if (!hw_err) {
|
||||
++i_adj_cycle;
|
||||
if (coord_diff2 < tol_rad2) {
|
||||
++i_in_tol_cycle;
|
||||
|
||||
if (i_in_tol_cycle == slew_point.withinToleranceCycleNumber) {
|
||||
res_err = MccSimpleSlewModelErrorCode::ERROR_OK;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (i_adj_cycle == slew_point.maxAdjustingCycleNumber) {
|
||||
// res_err = max iter namber was exceeded
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
if constexpr (std::same_as<decltype(hw_err), error_t>) {
|
||||
logError(std::format("An hardware error occured: code = {} ({})", hw_err.value(),
|
||||
hw_err.message()));
|
||||
res_err = hw_err;
|
||||
} else {
|
||||
if constexpr (traits::mcc_formattable<decltype(hw_err)>) {
|
||||
logError(std::format("An hardware error occured: code = {}", hw_err));
|
||||
}
|
||||
res_err = MccSimpleSlewModelErrorCode::ERROR_HARDWARE_SETPOS;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (in_adj_mode) { // ?!!!!!!!!!!!!!
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
auto start_poll_tm = std::chrono::steady_clock::now();
|
||||
|
||||
// NOTE: TARGET COORDINATES WILL BE UPDATED FOR CURRENT TIME-POINT IN TELEMETRY-CLASS!!!
|
||||
|
||||
auto iter = telemetry.addCallbackFunc(cycle_func);
|
||||
|
||||
|
||||
while (true) {
|
||||
@ -328,7 +402,7 @@ protected:
|
||||
|
||||
coord_diff2 = xr * xr + yr * yr;
|
||||
|
||||
if (coord_diff2 < adjRad2) { // switch to adjusting mode
|
||||
if (coord_diff2 < adj_rad2) { // switch to adjusting mode
|
||||
}
|
||||
|
||||
// if (prev_time_point == t_data.time_point) {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user