Timur A. Fatkhullin
1c774d2d69
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)
225 lines
8.3 KiB
C++
225 lines
8.3 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)
|
|
{
|
|
// 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();
|
|
|
|
std::lock_guard lock{*_setStateMutex};
|
|
|
|
// 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.Y, .Y = state.X + mcc::MccAngle(10.0_arcsecs)};
|
|
|
|
// 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));
|
|
|
|
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};
|
|
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.Xstate;
|
|
state->stateY = mdata.Ystate;
|
|
|
|
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 if (mdata.Ystate == AXIS_ERROR) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_ERROR;
|
|
} 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 if (mdata.Xstate == AXIS_ERROR) {
|
|
state->moving_state = hardware_moving_state_t::HW_MOVE_ERROR;
|
|
} 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
|