kopia lustrzana https://github.com/Hamlib/Hamlib
271 wiersze
8.7 KiB
C
271 wiersze
8.7 KiB
C
/*
|
|
* Hamlib Rotator backend - SkyWatcher
|
|
* Copyright (c) 2024 by Andrey Rodionov <dernasherbrezon@gmail.com>
|
|
*
|
|
*
|
|
* This library is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2.1 of the License, or (at your option) any later version.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* Lesser General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public
|
|
* License along with this library; if not, write to the Free Software
|
|
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*
|
|
*/
|
|
|
|
#include <stdlib.h>
|
|
#include "skywatcher.h"
|
|
#include "register.h"
|
|
#include "serial.h"
|
|
|
|
#define SKYWATCHER_ERROR_CODES_LENGTH 9
|
|
#define ERROR_CHECK(x) \
|
|
do { \
|
|
int __err_rc = (x); \
|
|
if (__err_rc != RIG_OK) { \
|
|
return __err_rc; \
|
|
} \
|
|
} while (0)
|
|
|
|
static const char *skywatcher_error_codes[SKYWATCHER_ERROR_CODES_LENGTH] = {
|
|
"Unknown Command",
|
|
"Command Length Error",
|
|
"Motor not Stopped",
|
|
"Invalid Character",
|
|
"Not Initialized",
|
|
"Driver Sleeping",
|
|
"Unknown",
|
|
"PEC Training is running",
|
|
"No Valid PEC data"
|
|
};
|
|
|
|
typedef struct {
|
|
uint32_t cpr[2];
|
|
} skywatcher_priv_data;
|
|
|
|
static int skywatcher_cmd(ROT *rot, const char *cmd, char *response, size_t response_len) {
|
|
hamlib_port_t *port = ROTPORT(rot);
|
|
rig_flush(port);
|
|
size_t cmd_len = strlen(cmd);
|
|
ERROR_CHECK(write_block(port, (unsigned char *) cmd, cmd_len));
|
|
// echo from the device
|
|
int code = read_string(port, (unsigned char *) response, response_len, "\r", 1, 0, 1);
|
|
if (code < 0) {
|
|
return -code;
|
|
}
|
|
// the actual response
|
|
code = read_string(port, (unsigned char *) response, response_len, "\r", 1, 0, 1);
|
|
if (code < 0) {
|
|
return -code;
|
|
}
|
|
// nullify last \r
|
|
response[strlen(response) - 1] = '\0';
|
|
if (response[0] == '!') {
|
|
code = atoi(&response[1]);
|
|
if (code < SKYWATCHER_ERROR_CODES_LENGTH) {
|
|
rig_debug(RIG_DEBUG_ERR, "Error code: %d Message: '%s'\n", code, skywatcher_error_codes[code]);
|
|
} else {
|
|
rig_debug(RIG_DEBUG_ERR, "Error response: '%s'\n", response);
|
|
}
|
|
return RIG_EPROTO;
|
|
}
|
|
// remove leading '='
|
|
memmove(response, response + 1, strlen(response) - 1);
|
|
response[strlen(response) - 1] = '\0';
|
|
return RIG_OK;
|
|
}
|
|
|
|
uint32_t skywatcher_convert24bit(long input) {
|
|
return ((input & 0x0000FF) << 16) | (input & 0x00FF00) | ((input & 0xFF0000) >> 16);
|
|
}
|
|
|
|
static const char *skywatcher_get_info(ROT *rot) {
|
|
static char info[32];
|
|
char str[16];
|
|
|
|
rig_debug(RIG_DEBUG_TRACE, "%s called\n", __func__);
|
|
|
|
// Read from 1st motor only. In practice both motors have the same version
|
|
if (skywatcher_cmd(rot, ":e1\r", str, sizeof(str)) != RIG_OK) {
|
|
return NULL;
|
|
}
|
|
|
|
SNPRINTF(info, sizeof(info), "V%s", str);
|
|
|
|
return info;
|
|
}
|
|
|
|
int skywatcher_get_spr(ROT *rot, int motor_index, uint32_t *result) {
|
|
skywatcher_priv_data *priv = ROTSTATE(rot)->priv;
|
|
if (priv->cpr[motor_index - 1] != 0) {
|
|
*result = priv->cpr[motor_index - 1];
|
|
return RIG_OK;
|
|
}
|
|
char str[16];
|
|
char req[16];
|
|
SNPRINTF(req, sizeof(req), ":a%d\r", motor_index);
|
|
ERROR_CHECK(skywatcher_cmd(rot, req, str, sizeof(str)));
|
|
priv->cpr[motor_index - 1] = skywatcher_convert24bit(strtol(str, NULL, 16));
|
|
*result = priv->cpr[motor_index - 1];
|
|
return RIG_OK;
|
|
}
|
|
|
|
int skywatcher_get_motor_position(ROT *rot, int motor_index, float *result) {
|
|
char str[16];
|
|
char req[16];
|
|
SNPRINTF(req, sizeof(req), ":j%d\r", motor_index);
|
|
ERROR_CHECK(skywatcher_cmd(rot, req, str, sizeof(str)));
|
|
long motor_ticks = skywatcher_convert24bit(strtol(str, NULL, 16)) ^ 0x800000;
|
|
uint32_t cpr;
|
|
ERROR_CHECK(skywatcher_get_spr(rot, motor_index, &cpr));
|
|
double ticks_per_angle = (double) cpr / 360.0;
|
|
*result = (float) ((double) motor_ticks / ticks_per_angle);
|
|
return RIG_OK;
|
|
}
|
|
|
|
static int skywatcher_get_position(ROT *rot, azimuth_t *az, elevation_t *el) {
|
|
rig_debug(RIG_DEBUG_TRACE, "%s called\n", __func__);
|
|
|
|
ERROR_CHECK(skywatcher_get_motor_position(rot, 1, (float *) az));
|
|
// normalize to 0 ~ 360
|
|
if (*az < 0.0f) {
|
|
*az = 360.0f + *az;
|
|
}
|
|
ERROR_CHECK(skywatcher_get_motor_position(rot, 2, (float *) el));
|
|
|
|
rig_debug(RIG_DEBUG_TRACE, "%s: (az, el) = (%.1f, %.1f)\n", __func__, *az, *el);
|
|
return RIG_OK;
|
|
}
|
|
|
|
static int skywatcher_stop(ROT *rot) {
|
|
rig_debug(RIG_DEBUG_TRACE, "%s called\n", __func__);
|
|
char str[16];
|
|
// don't wait for actual stop. Operation can be async
|
|
ERROR_CHECK(skywatcher_cmd(rot, ":K1\r", str, sizeof(str)));
|
|
ERROR_CHECK(skywatcher_cmd(rot, ":K2\r", str, sizeof(str)));
|
|
return RIG_OK;
|
|
}
|
|
|
|
int skywatcher_set_motor_position(ROT *rot, int motor_index, float angle) {
|
|
char str[16];
|
|
char req[16];
|
|
hamlib_port_t *rotp = ROTPORT(rot);
|
|
SNPRINTF(req, sizeof(req), ":f%d\r", motor_index);
|
|
int current_retry = 0;
|
|
int stopped = 0;
|
|
while (current_retry < rotp->retry) {
|
|
ERROR_CHECK(skywatcher_cmd(rot, req, str, sizeof(str)));
|
|
int status = str[1] - '0';
|
|
if (status & 0b10) {
|
|
rig_debug(RIG_DEBUG_ERR, "%s: motor is blocked\n", __func__);
|
|
return RIG_EPROTO;
|
|
}
|
|
if (status & 0b1) {
|
|
current_retry++;
|
|
//10ms sleep just to make sure motor is fully stopped
|
|
hl_usleep(10000);
|
|
continue;
|
|
}
|
|
stopped = 1;
|
|
break;
|
|
}
|
|
if (!stopped) {
|
|
return RIG_EPROTO;
|
|
}
|
|
SNPRINTF(req, sizeof(req), ":G%d00\r", motor_index);
|
|
ERROR_CHECK(skywatcher_cmd(rot, req, str, sizeof(str)));
|
|
uint32_t cpr;
|
|
ERROR_CHECK(skywatcher_get_spr(rot, motor_index, &cpr));
|
|
double ticks_per_angle = (double) cpr / 360.0;
|
|
uint32_t value = ((uint32_t) (ticks_per_angle * angle)) & 0xFFFFFF;
|
|
value = value | 0x800000;
|
|
SNPRINTF(req, sizeof(req), ":S%d%02X%02X%02X\r", motor_index, (value & 0xFF), (value & 0xFF00) >> 8, (value & 0xFF0000) >> 16);
|
|
ERROR_CHECK(skywatcher_cmd(rot, req, str, sizeof(str)));
|
|
// start motion, don't wait until it completes
|
|
SNPRINTF(req, sizeof(req), ":J%d\r", motor_index);
|
|
ERROR_CHECK(skywatcher_cmd(rot, req, str, sizeof(str)));
|
|
return RIG_OK;
|
|
}
|
|
|
|
static int skywatcher_set_position(ROT *rot, azimuth_t az, elevation_t el) {
|
|
rig_debug(RIG_DEBUG_TRACE, "%s called: %f %f\n", __func__, az, el);
|
|
ERROR_CHECK(skywatcher_stop(rot));
|
|
// stop is called for 1 then for 2 motor. do motor position in reverse because it awaits "stop motor" state.
|
|
// potentially saving 1 call to :f
|
|
ERROR_CHECK(skywatcher_set_motor_position(rot, 2, el));
|
|
ERROR_CHECK(skywatcher_set_motor_position(rot, 1, az));
|
|
return RIG_OK;
|
|
}
|
|
|
|
static int skywatcher_init(ROT *rot) {
|
|
rig_debug(RIG_DEBUG_VERBOSE, "%s called\n", __func__);
|
|
skywatcher_priv_data *result = calloc(1, sizeof(skywatcher_priv_data));
|
|
if (result == NULL) {
|
|
return -RIG_ENOMEM;
|
|
}
|
|
memset(result->cpr, 0, sizeof(result->cpr));
|
|
ROTSTATE(rot)->priv = result;
|
|
return RIG_OK;
|
|
}
|
|
|
|
static int skywatcher_cleanup(ROT *rot) {
|
|
skywatcher_priv_data *priv = ROTSTATE(rot)->priv;
|
|
if (priv == NULL) {
|
|
return RIG_OK;
|
|
}
|
|
free(priv);
|
|
return RIG_OK;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
/*
|
|
* Protocol documentation:
|
|
* https://inter-static.skywatcher.com/downloads/skywatcher_motor_controller_command_set.pdf
|
|
*/
|
|
const struct rot_caps skywatcher_rot_caps =
|
|
{
|
|
ROT_MODEL(ROT_MODEL_SKYWATCHER),
|
|
.model_name = "Sky-Watcher",
|
|
.mfg_name = "Sky-Watcher",
|
|
.version = "20240825.0",
|
|
.copyright = "LGPL",
|
|
.status = RIG_STATUS_STABLE,
|
|
.rot_type = ROT_TYPE_AZEL,
|
|
.port_type = RIG_PORT_SERIAL,
|
|
.serial_rate_min = 9600,
|
|
.serial_rate_max = 9600,
|
|
.serial_data_bits = 8,
|
|
.serial_stop_bits = 1,
|
|
.serial_parity = RIG_PARITY_NONE,
|
|
.serial_handshake = RIG_HANDSHAKE_NONE,
|
|
.write_delay = 0,
|
|
.post_write_delay = 0,
|
|
.timeout = 3500, /* worst case scenario */
|
|
.retry = 1,
|
|
|
|
.min_az = 0.0f,
|
|
.max_az = 360.0f,
|
|
.min_el = 0.0f,
|
|
.max_el = 180.0f,
|
|
|
|
.rot_init = skywatcher_init,
|
|
.rot_cleanup = skywatcher_cleanup,
|
|
.get_position = skywatcher_get_position,
|
|
.set_position = skywatcher_set_position,
|
|
.stop = skywatcher_stop,
|
|
.get_info = skywatcher_get_info,
|
|
};
|
|
|
|
|
|
DECLARE_INITROT_BACKEND(skywatcher) {
|
|
rig_debug(RIG_DEBUG_VERBOSE, "%s called\n", __func__);
|
|
rot_register(&skywatcher_rot_caps);
|
|
return RIG_OK;
|
|
}
|