Asibfm700Mount is now MccGenericMount (not MccGenericFsmMount)
fix mount initialization (add EEPROM reading, assign correponded mount config items) rewrite computing distance to pzones in slewing mode (add braking aceleration) add more informative errors description for serialization (network protocol)
This commit is contained in:
@@ -827,6 +827,17 @@ public:
|
||||
MccGenericFsmMount(StartStateT start_state, MountCtorArgTs&&... mount_ctor_args)
|
||||
: MOUNT_T(std::forward<MountCtorArgTs>(mount_ctor_args)...), fsm::MccFiniteStateMachine(std::move(start_state))
|
||||
{
|
||||
if constexpr (mcc_generic_log_mount_c<MOUNT_T>) {
|
||||
this->logDebug(std::format("Create MccGenericFsmMount class instance ({})", this->getThreadId()));
|
||||
this->logDebug("MccGenericFsmMount finite-state machine states:");
|
||||
for (auto& el : stateIDs()) {
|
||||
this->logDebug(std::format(" {}", el));
|
||||
}
|
||||
this->logDebug("MccGenericFsmMount finite-state machine events:");
|
||||
for (auto& el : eventIDs()) {
|
||||
this->logDebug(std::format(" {}", el));
|
||||
}
|
||||
}
|
||||
}
|
||||
// template <fsm::traits::fsm_state_c ERROR_STATE_T = MccGenericFsmMountErrorState>
|
||||
// MccGenericFsmMount(MOUNT_T mount,
|
||||
@@ -835,6 +846,19 @@ public:
|
||||
// {
|
||||
// }
|
||||
|
||||
MccGenericFsmMount(MccGenericFsmMount&&) = default;
|
||||
MccGenericFsmMount(const MccGenericFsmMount&) = delete;
|
||||
|
||||
MccGenericFsmMount& operator=(MccGenericFsmMount&&) = default;
|
||||
MccGenericFsmMount& operator=(const MccGenericFsmMount&) = delete;
|
||||
|
||||
virtual ~MccGenericFsmMount()
|
||||
{
|
||||
if constexpr (mcc_generic_log_mount_c<MOUNT_T>) {
|
||||
this->logDebug(std::format("Delete MccGenericFsmMount class instance ({})", this->getThreadId()));
|
||||
}
|
||||
}
|
||||
|
||||
// reimplementation of base-class methods to adapt it to FSM-behavior
|
||||
|
||||
error_t initMount()
|
||||
|
||||
@@ -51,6 +51,11 @@ struct MccSimpleMovingModelParams {
|
||||
double adjustRateX{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
|
||||
double adjustRateY{5.0_arcmins}; // maximal adjusting rate (a rate at the final slewing stage)
|
||||
|
||||
// braking acceleration after execution of mount stopping command (in rads/s^2)
|
||||
// it must be given as non-negative value!!!
|
||||
double brakingAccelX{0.0};
|
||||
double brakingAccelY{0.0};
|
||||
|
||||
|
||||
// ******* tracking mode *******
|
||||
|
||||
@@ -73,6 +78,46 @@ struct MccSimpleMovingModelParams {
|
||||
};
|
||||
|
||||
|
||||
// calculate the distances along the X and Y axes that the mount will travel at the current speed in a given time,
|
||||
// taking into account the braking acceleration
|
||||
//
|
||||
// WARNING:
|
||||
// It is assumed that the given braking accelerations are non-negative,
|
||||
// while speeds may have a sign according to motion direction.
|
||||
// The latter means that returned distances can be negative!
|
||||
std::pair<double, double> mcc_compute_distance(mcc_telemetry_data_c auto const& tdata,
|
||||
double time_in_secs,
|
||||
mcc_angle_c auto const& braking_accelX,
|
||||
mcc_angle_c auto const& braking_accelY)
|
||||
{
|
||||
std::pair<double, double> res;
|
||||
|
||||
// first, check if the mount will stop after given time_in_secs to prevent traveled path to be
|
||||
// negative
|
||||
//
|
||||
// the traveled path: s = V_ini*t - a*t*t/2, V_ini - initial speed, a - braking accel, t - the time
|
||||
// then, for s>=0, t <= 2*V_ini/a
|
||||
//
|
||||
|
||||
double term_x = 2.0 * std::abs(tdata.speedX) / braking_accelX;
|
||||
double term_y = 2.0 * std::abs(tdata.speedY) / braking_accelY;
|
||||
double tx = time_in_secs;
|
||||
double ty = time_in_secs;
|
||||
|
||||
if (std::isfinite(term_x) && (time_in_secs > term_x)) {
|
||||
tx = term_x;
|
||||
}
|
||||
if (std::isfinite(term_y) && (time_in_secs > term_y)) {
|
||||
ty = term_y;
|
||||
}
|
||||
|
||||
// here, one must take into account the sign of the speed!!!
|
||||
res.first = tdata.speedX * tx - std::copysign(braking_accelX, tdata.speedX) * tx * tx / 2.0;
|
||||
res.second = tdata.speedY * ty - std::copysign(braking_accelY, tdata.speedY) * ty * ty / 2.0;
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
template <mcc_pzone_container_c PZoneContT>
|
||||
bool mcc_is_near_pzones(PZoneContT* pz_cont,
|
||||
mcc_telemetry_data_c auto const& tdata,
|
||||
|
||||
@@ -589,7 +589,7 @@ public:
|
||||
std::expected<T, std::error_code> paramValue(size_t idx, DeserFuncT&& deser_func) const
|
||||
{
|
||||
if (idx >= _params.size()) {
|
||||
return std::unexpected{std::make_error_code(std::errc::argument_out_of_domain)};
|
||||
return std::unexpected{std::make_error_code(std::errc::value_too_large)};
|
||||
}
|
||||
|
||||
T val;
|
||||
@@ -655,7 +655,7 @@ public:
|
||||
|
||||
auto r = valid_keys_t::isKeywordValid(key);
|
||||
if (!r) {
|
||||
return std::make_error_code(std::errc::argument_out_of_domain);
|
||||
return std::make_error_code(std::errc::not_supported);
|
||||
}
|
||||
|
||||
_keywordHash = *r;
|
||||
|
||||
@@ -212,10 +212,28 @@ public:
|
||||
bool adjust_mode = false;
|
||||
static constexpr auto sideral_rate2 = slewing_params_t::sideralRate * slewing_params_t::sideralRate;
|
||||
|
||||
double braking_accelX, braking_accelY;
|
||||
{
|
||||
std::lock_guard lock{*_currentParamsMutex};
|
||||
if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
|
||||
braking_accelX = std::numeric_limits<double>::min();
|
||||
} else {
|
||||
braking_accelX = std::abs(_currentParams.brakingAccelX);
|
||||
}
|
||||
|
||||
if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
|
||||
braking_accelY = std::numeric_limits<double>::min();
|
||||
} else {
|
||||
braking_accelY = std::abs(_currentParams.brakingAccelY);
|
||||
}
|
||||
}
|
||||
|
||||
std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
|
||||
start_slewing_tp = std::chrono::steady_clock::now();
|
||||
last_adjust_tp = start_slewing_tp;
|
||||
|
||||
std::pair<double, double> distXY;
|
||||
|
||||
// main loop (simply monitors the current position taking into account the prohibited zones, as well as the
|
||||
// timeout of the entire process)
|
||||
while (!*_stopSlewing) {
|
||||
@@ -235,11 +253,16 @@ public:
|
||||
return MccSimpleSlewingModelErrorCode::ERROR_STOPPED;
|
||||
}
|
||||
|
||||
distXY = mcc_compute_distance(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
|
||||
|
||||
// calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
|
||||
// and check them for getting into the prohibited zones
|
||||
if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
|
||||
cpt.X = tdata.HA + tdata.speedX * min_time_to_pzone_in_secs;
|
||||
cpt.Y = tdata.DEC_APP + tdata.speedY * min_time_to_pzone_in_secs;
|
||||
cpt.X = tdata.HA + distXY.first;
|
||||
cpt.Y = tdata.DEC_APP + distXY.second;
|
||||
|
||||
// cpt.X = tdata.HA + tdata.speedX * min_time_to_pzone_in_secs;
|
||||
// cpt.Y = tdata.DEC_APP + tdata.speedY * min_time_to_pzone_in_secs;
|
||||
if (cpt.Y > DEG90INRADS) {
|
||||
cpt.Y = DEG90INRADS;
|
||||
}
|
||||
@@ -247,8 +270,16 @@ public:
|
||||
cpt.Y = -DEG90INRADS;
|
||||
}
|
||||
} else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
|
||||
cpt.X = tdata.AZ + tdata.speedX * min_time_to_pzone_in_secs;
|
||||
cpt.Y = tdata.ZD + tdata.speedY * min_time_to_pzone_in_secs;
|
||||
cpt.X = tdata.AZ + distXY.first;
|
||||
cpt.Y = tdata.ZD + distXY.second;
|
||||
// cpt.X = tdata.AZ + tdata.speedX * min_time_to_pzone_in_secs;
|
||||
// cpt.Y = tdata.ZD + tdata.speedY * min_time_to_pzone_in_secs;
|
||||
if (cpt.Y < 0.0) {
|
||||
cpt.Y = 0.0;
|
||||
}
|
||||
if (cpt.Y > std::numbers::pi) {
|
||||
cpt.Y = std::numbers::pi;
|
||||
}
|
||||
}
|
||||
mcc_tp2tp(tdata.time_point, cpt.time_point);
|
||||
|
||||
@@ -384,6 +415,8 @@ public:
|
||||
// }
|
||||
}
|
||||
|
||||
*_stopSlewing = true;
|
||||
|
||||
return MccSimpleSlewingModelErrorCode::ERROR_OK;
|
||||
};
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user