273 lines
10 KiB
C++
273 lines
10 KiB
C++
#include "asibfm700_servocontroller.h"
|
|
#include <thread>
|
|
|
|
namespace asibfm700
|
|
{
|
|
|
|
const char* AsibFM700ServoControllerErrorCategory::name() const noexcept
|
|
{
|
|
return "ASIBFM700-SERVOCONTROLLER-ERROR-CATEGORY";
|
|
}
|
|
|
|
std::string AsibFM700ServoControllerErrorCategory::message(int ec) const
|
|
{
|
|
AsibFM700ServoControllerErrorCode err = static_cast<AsibFM700ServoControllerErrorCode>(ec);
|
|
|
|
switch (err) {
|
|
case AsibFM700ServoControllerErrorCode::ERROR_OK:
|
|
return "OK";
|
|
case AsibFM700ServoControllerErrorCode::ERROR_FATAL:
|
|
return "LibSidServo fatal error";
|
|
case AsibFM700ServoControllerErrorCode::ERROR_BADFORMAT:
|
|
return "LibSidServo wrong arguments of function";
|
|
case AsibFM700ServoControllerErrorCode::ERROR_ENCODERDEV:
|
|
return "LibSidServo encoder device error or can't open";
|
|
case AsibFM700ServoControllerErrorCode::ERROR_MOUNTDEV:
|
|
return "LibSidServo mount device error or can't open";
|
|
case AsibFM700ServoControllerErrorCode::ERROR_FAILED:
|
|
return "LibSidServo failed to run command";
|
|
case AsibFM700ServoControllerErrorCode::ERROR_NULLPTR:
|
|
return "nullptr argument";
|
|
case AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT:
|
|
return "polling timeout";
|
|
|
|
default:
|
|
return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
|
|
const AsibFM700ServoControllerErrorCategory& AsibFM700ServoControllerErrorCategory::get()
|
|
{
|
|
static const AsibFM700ServoControllerErrorCategory constInst;
|
|
return constInst;
|
|
}
|
|
|
|
|
|
|
|
AsibFM700ServoController::AsibFM700ServoController() : _hardwareConfig(), _setStateMutex(new std::mutex) {}
|
|
|
|
AsibFM700ServoController::AsibFM700ServoController(hardware_config_t config) : AsibFM700ServoController()
|
|
{
|
|
_hardwareConfig = std::move(config);
|
|
|
|
_hardwareConfig.devConfig.MountDevPath = const_cast<char*>(_hardwareConfig.MountDevPath.c_str());
|
|
_hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
|
|
_hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
|
|
_hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
|
|
}
|
|
|
|
|
|
AsibFM700ServoController::~AsibFM700ServoController() {}
|
|
|
|
|
|
// AsibFM700ServoController::error_t AsibFM700ServoController::hardwareStop()
|
|
// {
|
|
// error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.stop());
|
|
// if (err) {
|
|
// return err;
|
|
// }
|
|
|
|
// hardware_state_t hw_state;
|
|
|
|
// auto start_tp = std::chrono::steady_clock::now();
|
|
|
|
// // poll hardware till stopped-state detected ...
|
|
// while (true) {
|
|
// err = hardwareGetState(&hw_state);
|
|
// if (err) {
|
|
// return err;
|
|
// }
|
|
|
|
// if (hw_state.movementState == hardware_movement_state_t::HW_MOVE_STOPPED) {
|
|
// break;
|
|
// }
|
|
|
|
// if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) {
|
|
// err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT;
|
|
// break;
|
|
// }
|
|
|
|
// std::this_thread::sleep_for(_hardwareConfig.pollingInterval);
|
|
// }
|
|
|
|
// return err;
|
|
// }
|
|
|
|
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareInit()
|
|
{
|
|
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.init(&_hardwareConfig.devConfig));
|
|
}
|
|
|
|
|
|
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareShutdown()
|
|
{
|
|
Mount.quit();
|
|
|
|
return AsibFM700ServoControllerErrorCode::ERROR_OK;
|
|
}
|
|
|
|
|
|
|
|
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(hardware_state_t const& state)
|
|
{
|
|
std::lock_guard lock{*_setStateMutex};
|
|
|
|
error_t err;
|
|
|
|
if (state.movementState == hardware_movement_state_t::HW_MOVE_STOPPING) { // stop!
|
|
err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.stop());
|
|
if (err) {
|
|
return err;
|
|
}
|
|
|
|
hardware_state_t hw_state;
|
|
|
|
auto start_tp = std::chrono::steady_clock::now();
|
|
|
|
// poll hardware till stopped-state detected ...
|
|
while (true) {
|
|
err = hardwareGetState(&hw_state);
|
|
if (err) {
|
|
return err;
|
|
}
|
|
|
|
if (hw_state.movementState == hardware_movement_state_t::HW_MOVE_STOPPED) {
|
|
break;
|
|
}
|
|
|
|
if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) {
|
|
err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT;
|
|
break;
|
|
}
|
|
|
|
std::this_thread::sleep_for(_hardwareConfig.pollingInterval);
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
|
|
auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(state.XY.epoch().UTC().time_since_epoch());
|
|
auto secs = std::chrono::floor<std::chrono::seconds>(ns);
|
|
ns -= secs;
|
|
std::timespec tp{.tv_sec = secs.count(), .tv_nsec = ns.count()};
|
|
|
|
Mount.currentT(&tp);
|
|
|
|
if (state.movementState == hardware_movement_state_t::HW_MOVE_SLEWING) {
|
|
// according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
|
|
coordpair_t cp{.X = state.XY.y(), .Y = state.XY.x()};
|
|
|
|
err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.moveTo(&cp));
|
|
} else if (state.movementState == hardware_movement_state_t::HW_MOVE_ADJUSTING ||
|
|
state.movementState == hardware_movement_state_t::HW_MOVE_TRACKING ||
|
|
state.movementState == hardware_movement_state_t::HW_MOVE_GUIDING) {
|
|
// according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
|
|
coordval_pair_t cvalpair{.X{.val = state.XY.y(), .t = tp}, .Y{.val = state.XY.x(), .t = tp}};
|
|
|
|
// correctTo is asynchronous function!!!
|
|
//
|
|
// according to the Eddy's implementation of the LibSidServo library it is safe
|
|
// to pass the addresses of 'cvalpair' and 'cpair' automatic variables
|
|
err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.correctTo(&cvalpair));
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(hardware_state_t* state)
|
|
{
|
|
if (state == nullptr) {
|
|
return AsibFM700ServoControllerErrorCode::ERROR_NULLPTR;
|
|
}
|
|
|
|
using tp_t = decltype(state->XY.epoch().UTC());
|
|
using d_t = tp_t::duration;
|
|
|
|
mountdata_t mdata;
|
|
|
|
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.getMountData(&mdata));
|
|
if (!err) {
|
|
auto dr = std::chrono::duration_cast<d_t>(std::chrono::seconds(mdata.encXposition.t.tv_sec) +
|
|
std::chrono::nanoseconds(mdata.encXposition.t.tv_nsec));
|
|
|
|
mcc::impl::MccCelestialCoordEpoch ep;
|
|
ep.fromTimePoint(tp_t{dr});
|
|
|
|
// according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis
|
|
state->XY = mcc::impl::MccGenXY{mdata.encYposition.val, mdata.encXposition.val, ep};
|
|
state->speedXY = mcc::impl::MccGenXY{mdata.encYspeed.val, mdata.encXspeed.val, ep};
|
|
|
|
if (mdata.Xstate == AXIS_ERROR || mdata.Ystate == AXIS_ERROR) {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_ERROR;
|
|
} else {
|
|
if (mdata.Xstate == AXIS_STOPPED) {
|
|
if (mdata.Ystate == AXIS_STOPPED) {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_STOPPED;
|
|
} else if (mdata.Ystate == AXIS_SLEWING) {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_SLEWING;
|
|
} else if (mdata.Ystate == AXIS_POINTING) {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_ADJUSTING;
|
|
} else if (mdata.Ystate == AXIS_GUIDING) {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_GUIDING;
|
|
} else {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_UNKNOWN;
|
|
}
|
|
} else if (mdata.Xstate == AXIS_SLEWING) {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_SLEWING;
|
|
} else if (mdata.Xstate == AXIS_POINTING) {
|
|
if (mdata.Ystate == AXIS_SLEWING) {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_SLEWING;
|
|
} else {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_ADJUSTING;
|
|
}
|
|
} else if (mdata.Xstate == AXIS_GUIDING) {
|
|
if (mdata.Ystate == AXIS_SLEWING) {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_SLEWING;
|
|
} else if (mdata.Ystate == AXIS_POINTING) {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_ADJUSTING;
|
|
} else {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_GUIDING;
|
|
}
|
|
} else {
|
|
state->movementState = hardware_movement_state_t::HW_MOVE_UNKNOWN;
|
|
}
|
|
}
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
|
|
void AsibFM700ServoController::hardwareUpdateConfig(conf_t cfg)
|
|
{
|
|
_hardwareConfig.devConfig = std::move(cfg);
|
|
|
|
_hardwareConfig.devConfig.MountDevPath = const_cast<char*>(_hardwareConfig.MountDevPath.c_str());
|
|
_hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
|
|
_hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
|
|
_hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
|
|
}
|
|
|
|
|
|
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig(hardware_configuration_t cfg)
|
|
{
|
|
_hardwareConfig.hwConfig = std::move(cfg);
|
|
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.saveHWconfig(&_hardwareConfig.hwConfig));
|
|
}
|
|
|
|
|
|
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig()
|
|
{
|
|
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.getHWconfig(&_hardwareConfig.hwConfig));
|
|
}
|
|
|
|
|
|
AsibFM700ServoController::hardware_config_t AsibFM700ServoController::getHardwareConfig() const
|
|
{
|
|
return _hardwareConfig;
|
|
}
|
|
|
|
} // namespace asibfm700
|