timur/ASIBFM700
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

...

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2026-02-25 18:09:46 +03:00
parent dcd935e9a7
commit 69e49283dd
5 changed files with 283 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

159
asibfm700_mount.cpp
View File

@@ -14,9 +14,12 @@ Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_p
_pcm(config.pcmData()),
gm_class_t(std::make_tuple(&_servolController, &_pcm),
std::make_tuple(),
std::make_tuple(&_servolController,
this,
[this](Asibfm700Mount::mount_status_t const& status) { *_mountStatus = status; }),
// std::make_tuple(&_servolController,
// this,
// [this](Asibfm700Mount::mount_status_t const& status) { *_mountStatus = status; }),
std::make_tuple([this](bool sl) { return slewingImpl(sl); },
[this]() { return trackingImpl(); },
[this]() { return stoppingImpl(); }),
std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)),
_mountConfig(config),
_mountConfigMutex(new std::mutex)
@@ -346,4 +349,154 @@ void Asibfm700Mount::errorLogging(const std::string& msg, const std::error_code&
}
}
/* MOVEMENT METHODS */
Asibfm700Mount::error_t Asibfm700Mount::sendToHardware(AsibFM700ServoController::hardware_state_t const& hw_state)
{
auto tp = std::chrono::duration_cast<std::chrono::milliseconds>(hw_state.XY.epoch().UTC().time_since_epoch());
logDebug("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})", hw_state.XY.x().degrees(),
hw_state.XY.y().degrees(), tp);
*_lastMountError = _servolController.hardwareSetState(hw_state);
if (_lastMountError->load()) {
errorLogging("An error occured while send command to servo controller: ", _lastMountError->load());
} else {
logDebug(" the 'hardwareSetState' method performed successfully!");
}
return _lastMountError->load();
}
void Asibfm700Mount::logMountPos(telemetry_t::telemetry_data_t const& tdata)
{
// NOTE: the implementation of MccTelemetry class guarantees that
// tdata.mountPos and tdata.targetPos are coordinates in the HA-DEC
// equathorial system
logTrace(" current target: HA = {}, DEC = {} (encoders: {} {})",
mcc::impl::MccAngle(tdata.targetPos.co_lon()).sexagesimal(true),
mcc::impl::MccAngle(tdata.targetPos.co_lat()).sexagesimal(), tdata.targetXY.x().sexagesimal(),
tdata.targetXY.y().sexagesimal());
logTrace(" current mount: HA = {}, DEC = {} (encoders: {} {})",
mcc::impl::MccAngle(tdata.mountPos.co_lon()).sexagesimal(true),
mcc::impl::MccAngle(tdata.mountPos.co_lat()).sexagesimal(), tdata.hwState.XY.x().sexagesimal(),
tdata.hwState.XY.y().sexagesimal());
_pathFile.addToPath(tdata);
auto dist = tdata.mountPos.distance(tdata.targetPos);
logTrace(" target-to-mount distance: {} (dx = {}, dy = {})", mcc::impl::MccAngleFancyString(dist.dist),
mcc::impl::MccAngleFancyString(dist.dx), mcc::impl::MccAngleFancyString(dist.dy));
}
Asibfm700Mount::error_t Asibfm700Mount::checkPZone(typename telemetry_t::telemetry_data_t const& tdata)
{
mcc::impl::MccGenXY braking_accel{getMovementParams().brakingAccelX, getMovementParams().brakingAccelY}, dxy{};
auto new_pos = this->coordsAfterTime(tdata.mountPos, tdata.hwState.speedXY, braking_accel,
getMovementParams().minTimeToPZone, &dxy);
logTrace(" the distance that will be covered in the next {} seconds: HA-axis: {}, DEC-axis: {}",
getMovementParams().minTimeToPZone.count(), mcc::impl::MccAngleFancyString(dxy.x()),
mcc::impl::MccAngleFancyString(dxy.y()));
bool in_zone;
std::vector<bool> in_zone_vec;
*_lastMountError = this->inPZone(new_pos, &in_zone, &in_zone_vec);
if (_lastMountError->load()) {
return _lastMountError->load();
}
if (in_zone) {
size_t i = 0;
for (; i < in_zone_vec.size(); ++i) {
if (in_zone_vec[i]) {
break;
}
}
auto names = this->pzoneNames();
auto it = names.begin();
std::ranges::advance(it, i);
logError("target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i,
*it);
mcc::impl::MccSkyHADEC_OBS hadec;
mcc::impl::MccSkyRADEC_OBS radec;
mcc::impl::MccSkyAZZD azzd;
mcc::impl::MccSkyAZALT azalt;
mcc::impl::MccAngle lst;
*_lastMountError = tdata.mountPos.appSideralTime(&lst, true);
if (_lastMountError->load()) {
return _lastMountError->load();
}
*_lastMountError = tdata.mountPos.toAtSameEpoch(radec, hadec, azzd, azalt);
if (_lastMountError->load()) {
return _lastMountError->load();
}
logError(" RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", radec.x().sexagesimal(true), radec.y().sexagesimal(),
hadec.x().sexagesimal(true), lst.sexagesimal(true));
logError(" AZ, ZD, ALT: {}, {}, {}", azzd.x().sexagesimal(), azzd.y().sexagesimal(),
azalt.y().sexagesimal());
logError(" hardware X, Y: {}, {}", tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal());
return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE;
}
}
Asibfm700Mount::error_t Asibfm700Mount::slewingImpl(bool slew_and_stop) {}
Asibfm700Mount::error_t Asibfm700Mount::stoppingImpl()
{
typename AsibFM700ServoController::hardware_state_t hw_state;
hw_state.movementState == AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPING;
*_lastMountError = sendToHardware(hw_state);
if (!_lastMountError->load()) {
*_mountStatus = mount_status_t::MOUNT_STATUS_STOPPING;
auto start_tp = std::chrono::steady_clock::now();
*_lastMountError = _servolController.hardwareGetState(&hw_state);
while (hw_state.movementState != AsibFM700ServoController::hardware_movement_state_t::HW_MOVE_STOPPED &&
!_lastMountError->load()) {
if ((std::chrono::steady_clock::now() - start_tp) >
_mountConfig.getValue<std::chrono::seconds>("stopTimeout").value()) {
// set timeout error!
// *_lastMountError =
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(300));
*_lastMountError = _servolController.hardwareGetState(&hw_state);
}
}
if (!_lastMountError->load()) {
*_mountStatus = mount_status_t::MOUNT_STATUS_IDLE;
} else {
*_mountStatus = mount_status_t::MOUNT_STATUS_ERROR;
errorLogging("An error occured while stoppping mount: ", _lastMountError->load());
}
return _lastMountError->load();
}
} // namespace asibfm700