238 lines
8.7 KiB
C++
238 lines
8.7 KiB
C++
#include "asibfm700_servocontroller.h"
|
|
|
|
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() {}
|
|
|
|
|
|
constexpr std::string_view AsibFM700ServoController::hardwareName() const
|
|
{
|
|
return "Sidereal-ServoControllerII";
|
|
}
|
|
|
|
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.moving_state == hardware_moving_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::hardwareSetState(hardware_state_t state)
|
|
{
|
|
std::lock_guard lock{*_setStateMutex};
|
|
|
|
// static thread_local coordval_pair_t cvalpair{.X{0.0, 0.0}, .Y{0.0, 0.0}};
|
|
// static thread_local coordpair_t cpair{.X = 0.0, .Y = 0.0};
|
|
|
|
// cvalpair.X = {.val = state.Y, .t = tp};
|
|
// cvalpair.Y = {.val = state.X, .t = tp};
|
|
|
|
// cpair.X = state.tagY;
|
|
// cpair.Y = state.tagX;
|
|
|
|
// time point from sidservo library is 'double' number represented UNIXTIME with
|
|
// microseconds/nanoseconds precision
|
|
double tp = std::chrono::duration<double>(state.time_point.time_since_epoch()).count();
|
|
|
|
// according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
|
|
coordval_pair_t cvalpair{.X{.val = state.Y, .t = tp}, .Y{.val = state.X, .t = tp}};
|
|
// coordpair_t cpair{.X = state.endptY, .Y = state.endptX};
|
|
|
|
|
|
// 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
|
|
// auto err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.correctTo(&cvalpair, &cpair));
|
|
auto 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(hardware_state_t::time_point);
|
|
|
|
mountdata_t mdata;
|
|
|
|
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.getMountData(&mdata));
|
|
if (!err) {
|
|
// time point from sidservo library is 'double' number represented UNIXTIME with
|
|
// microseconds/nanoseconds precision (must be equal for encXposition and encYposition)
|
|
|
|
using secs_t = std::chrono::duration<double>;
|
|
|
|
secs_t secs = secs_t{mdata.encXposition.t};
|
|
state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
|
|
|
|
// if (mcc::utils::isEqual(secs.count(), 0.0)) { // model mode?
|
|
// state->time_point = decltype(state->time_point)::clock::now();
|
|
// } else {
|
|
// state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
|
|
// }
|
|
// WARNING: TEMPORARY (WAIT FOR Eddy fix its implementation of LibSidServo)!!!
|
|
// state->time_point = decltype(state->time_point)::clock::now();
|
|
|
|
// according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis
|
|
state->X = mdata.encYposition.val;
|
|
state->Y = mdata.encXposition.val;
|
|
|
|
state->speedX = mdata.encYspeed.val;
|
|
state->speedY = mdata.encXspeed.val;
|
|
|
|
state->stateX = mdata.Ystate;
|
|
state->stateY = mdata.Xstate;
|
|
|
|
if (mdata.Xstate == AXIS_ERROR || mdata.Ystate == AXIS_ERROR) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_ERROR;
|
|
} else {
|
|
if (mdata.Xstate == AXIS_STOPPED) {
|
|
if (mdata.Ystate == AXIS_STOPPED) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_STOPPED;
|
|
} else if (mdata.Ystate == AXIS_SLEWING) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
|
|
} else if (mdata.Ystate == AXIS_POINTING) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
|
|
} else if (mdata.Ystate == AXIS_GUIDING) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING;
|
|
} else {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN;
|
|
}
|
|
} else if (mdata.Xstate == AXIS_SLEWING) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
|
|
} else if (mdata.Xstate == AXIS_POINTING) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
|
|
} else if (mdata.Xstate == AXIS_GUIDING) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING;
|
|
} else {
|
|
state->moving_state = hardware_moving_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
|