Hamlib/spid/spid.c

573 wiersze
15 KiB
C

/*
* Hamlib Rotator backend - SPID Rot1Prog & Rot2Prog
* Copyright (c) 2009-2011 by Norvald H. Ryeng, LA6YKA
*
*
* 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
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include "hamlib/rotator.h"
#include "serial.h"
#include "misc.h"
#include "register.h"
#include "spid.h"
#define TOK_AZRES 1
#define TOK_ELRES 2
struct spid_rot2prog_priv_data
{
int az_resolution;
int el_resolution;
};
static int spid_rot_init(ROT *rot)
{
rig_debug(RIG_DEBUG_TRACE, "%s called\n", __func__);
if (!rot || !rot->caps)
{
return -RIG_EINVAL;
}
if (rot->caps->rot_model == ROT_MODEL_SPID_ROT2PROG ||
rot->caps->rot_model == ROT_MODEL_SPID_MD01_ROT2PROG)
{
struct spid_rot2prog_priv_data *priv;
priv = (struct spid_rot2prog_priv_data *)malloc(sizeof(struct
spid_rot2prog_priv_data));
if (!priv)
{
return -RIG_ENOMEM;
}
rot->state.priv = (void *)priv;
priv->az_resolution = 0;
priv->el_resolution = 0;
}
return RIG_OK;
}
static int spid_rot_cleanup(ROT *rot)
{
rig_debug(RIG_DEBUG_TRACE, "%s called\n", __func__);
if (!rot)
{
return -RIG_EINVAL;
}
if (rot->state.priv && (rot->caps->rot_model == ROT_MODEL_SPID_ROT2PROG ||
rot->caps->rot_model == ROT_MODEL_SPID_MD01_ROT2PROG))
{
free(rot->state.priv);
}
rot->state.priv = NULL;
return RIG_OK;
}
static int spid_get_conf(ROT *rot, token_t token, char *val)
{
struct spid_rot2prog_priv_data *priv = (struct spid_rot2prog_priv_data *)
rot->state.priv;
rig_debug(RIG_DEBUG_TRACE, "%s called %d\n", __func__, (int)token);
if (rot->caps->rot_model != ROT_MODEL_SPID_ROT2PROG &&
rot->caps->rot_model != ROT_MODEL_SPID_MD01_ROT2PROG)
{
return -RIG_EINVAL;
}
switch (token)
{
case TOK_AZRES:
sprintf(val, "%d", priv->az_resolution);
break;
case TOK_ELRES:
sprintf(val, "%d", priv->el_resolution);
break;
default:
return -RIG_EINVAL;
}
return RIG_OK;
}
static int spid_set_conf(ROT *rot, token_t token, const char *val)
{
struct spid_rot2prog_priv_data *priv = (struct spid_rot2prog_priv_data *)
rot->state.priv;
rig_debug(RIG_DEBUG_TRACE, "%s: called %d=%s\n", __func__, (int)token, val);
if (rot->caps->rot_model != ROT_MODEL_SPID_ROT2PROG &&
rot->caps->rot_model != ROT_MODEL_SPID_MD01_ROT2PROG)
{
return -RIG_EINVAL;
}
switch (token)
{
case TOK_AZRES:
priv->az_resolution = atoi(val);
break;
case TOK_ELRES:
priv->el_resolution = atoi(val);
break;
default:
return -RIG_EINVAL;
}
return RIG_OK;
}
static int spid_rot1prog_rot_set_position(ROT *rot, azimuth_t az,
elevation_t el)
{
struct rot_state *rs = &rot->state;
int retval;
char cmdstr[13];
unsigned int u_az;
rig_debug(RIG_DEBUG_TRACE, "%s called: %f %f\n", __func__, az, el);
u_az = 360 + az;
cmdstr[0] = 0x57; /* S */
cmdstr[1] = 0x30 + u_az / 100; /* H1 */
cmdstr[2] = 0x30 + (u_az % 100) / 10; /* H2 */
cmdstr[3] = 0x30 + (u_az % 10); /* H3 */
cmdstr[4] = 0x30; /* H4 */
cmdstr[5] = 0x00; /* PH */
cmdstr[6] = 0x00; /* V1 */
cmdstr[7] = 0x00; /* V2 */
cmdstr[8] = 0x00; /* V3 */
cmdstr[9] = 0x00; /* V4 */
cmdstr[10] = 0x00; /* PV */
cmdstr[11] = 0x2F; /* K */
cmdstr[12] = 0x20; /* END */
retval = write_block(&rs->rotport, cmdstr, 13);
if (retval != RIG_OK)
{
return retval;
}
return RIG_OK;
}
static int spid_rot2prog_rot_set_position(ROT *rot, azimuth_t az,
elevation_t el)
{
struct rot_state *rs = &rot->state;
struct spid_rot2prog_priv_data *priv = (struct spid_rot2prog_priv_data *)
rs->priv;
int retval;
int retry_read = 0;
char cmdstr[13];
unsigned int u_az, u_el;
rig_debug(RIG_DEBUG_TRACE, "%s called: %f %f\n", __func__, az, el);
if (!priv->az_resolution || !priv->el_resolution)
{
do
{
retval = write_block(&rs->rotport,
"\x57\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1F\x20", 13);
if (retval != RIG_OK)
{
return retval;
}
memset(cmdstr, 0, 12);
retval = read_block(&rs->rotport, cmdstr, 12);
}
while (retval < 0 && retry_read++ < rot->state.rotport.retry);
if (retval < 0)
{
return retval;
}
}
else
{
cmdstr[5] = priv->az_resolution; /* PH */
cmdstr[10] = priv->el_resolution; /* PV */
}
u_az = cmdstr[5] * (360 + az);
u_el = cmdstr[10] * (360 + el);
cmdstr[0] = 0x57; /* S */
cmdstr[1] = 0x30 + u_az / 1000; /* H1 */
cmdstr[2] = 0x30 + (u_az % 1000) / 100; /* H2 */
cmdstr[3] = 0x30 + (u_az % 100) / 10; /* H3 */
cmdstr[4] = 0x30 + (u_az % 10); /* H4 */
cmdstr[6] = 0x30 + u_el / 1000; /* V1 */
cmdstr[7] = 0x30 + (u_el % 1000) / 100; /* V2 */
cmdstr[8] = 0x30 + (u_el % 100) / 10; /* V3 */
cmdstr[9] = 0x30 + (u_el % 10); /* V4 */
cmdstr[11] = 0x2F; /* K */
cmdstr[12] = 0x20; /* END */
retval = write_block(&rs->rotport, cmdstr, 13);
if (retval != RIG_OK)
{
return retval;
}
/* Unlike the original Rot2Prog, MD-01 and MD-02 return the position
after receiving the set position command. */
if (rot->caps->rot_model == ROT_MODEL_SPID_MD01_ROT2PROG)
{
retry_read = 0;
do
{
retval = read_block(&rs->rotport, cmdstr, 12);
}
while ((retval < 0) && (retry_read++ < rot->state.rotport.retry));
}
return RIG_OK;
}
static int spid_rot_get_position(ROT *rot, azimuth_t *az, elevation_t *el)
{
struct rot_state *rs = &rot->state;
int retval;
int retry_read = 0;
char posbuf[12];
rig_debug(RIG_DEBUG_TRACE, "%s called\n", __func__);
do
{
retval = write_block(&rs->rotport,
"\x57\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1F\x20", 13);
if (retval != RIG_OK)
{
return retval;
}
memset(posbuf, 0, 12);
if (rot->caps->rot_model == ROT_MODEL_SPID_ROT1PROG)
{
retval = read_block(&rs->rotport, posbuf, 5);
}
else if (rot->caps->rot_model == ROT_MODEL_SPID_ROT2PROG ||
rot->caps->rot_model == ROT_MODEL_SPID_MD01_ROT2PROG)
{
retval = read_block(&rs->rotport, posbuf, 12);
}
else
{
retval = -RIG_EINVAL;
}
}
while (retval < 0 && retry_read++ < rot->state.rotport.retry);
if (retval < 0)
{
return retval;
}
*az = posbuf[1] * 100;
*az += posbuf[2] * 10;
*az += posbuf[3];
if (rot->caps->rot_model == ROT_MODEL_SPID_ROT2PROG ||
rot->caps->rot_model == ROT_MODEL_SPID_MD01_ROT2PROG)
{
*az += posbuf[4] / 10.0;
}
*az -= 360;
*el = 0.0;
if (rot->caps->rot_model == ROT_MODEL_SPID_ROT2PROG ||
rot->caps->rot_model == ROT_MODEL_SPID_MD01_ROT2PROG)
{
*el = posbuf[6] * 100;
*el += posbuf[7] * 10;
*el += posbuf[8];
*el += posbuf[9] / 10.0;
*el -= 360;
}
rig_debug(RIG_DEBUG_TRACE, "%s: (az, el) = (%.1f, %.1f)\n",
__func__, *az, *el);
return RIG_OK;
}
static int spid_rot_stop(ROT *rot)
{
struct rot_state *rs = &rot->state;
int retval;
int retry_read = 0;
char posbuf[12];
rig_debug(RIG_DEBUG_TRACE, "%s called\n", __func__);
do
{
retval = write_block(&rs->rotport,
"\x57\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x0F\x20", 13);
if (retval != RIG_OK)
{
return retval;
}
memset(posbuf, 0, 12);
if (rot->caps->rot_model == ROT_MODEL_SPID_ROT1PROG)
{
retval = read_block(&rs->rotport, posbuf, 5);
}
else if (rot->caps->rot_model == ROT_MODEL_SPID_ROT2PROG ||
rot->caps->rot_model == ROT_MODEL_SPID_MD01_ROT2PROG)
{
retval = read_block(&rs->rotport, posbuf, 12);
}
}
while (retval < 0 && retry_read++ < rot->state.rotport.retry);
if (retval < 0)
{
return retval;
}
return RIG_OK;
}
static int spid_md01_rot2prog_rot_move(ROT *rot, int direction, int speed)
{
struct rot_state *rs = &rot->state;
char dir = 0x00;
int retval;
char cmdstr[13];
rig_debug(RIG_DEBUG_TRACE, "%s called\n", __func__);
switch (direction)
{
case ROT_MOVE_UP:
dir = 0x04;
break;
case ROT_MOVE_DOWN:
dir = 0x08;
break;
case ROT_MOVE_LEFT:
dir = 0x01;
break;
case ROT_MOVE_RIGHT:
dir = 0x02;
break;
}
cmdstr[0] = 0x57; /* S */
cmdstr[1] = dir; /* H1 */
cmdstr[2] = 0x00; /* H2 */
cmdstr[3] = 0x00; /* H3 */
cmdstr[4] = 0x00; /* H4 */
cmdstr[6] = 0x00; /* V1 */
cmdstr[7] = 0x00; /* V2 */
cmdstr[8] = 0x00; /* V3 */
cmdstr[9] = 0x00; /* V4 */
cmdstr[11] = 0x14; /* K */
cmdstr[12] = 0x20; /* END */
/* The rotator must be stopped before changing directions. Since
we don't know which direction we're already moving in (if
moving at all), always send the stop command first. */
spid_rot_stop(rot);
retval = write_block(&rs->rotport, cmdstr, 13);
return retval;
}
const struct confparams spid_cfg_params[] =
{
{
TOK_AZRES, "az_resolution", "Azimuth resolution", "Number of pulses per degree, 0 = auto sense",
"0", RIG_CONF_NUMERIC, { .n = { 0, 0xff, 1 } }
},
{
TOK_ELRES, "el_resolution", "Eleveation resolution", "Number of pulses per degree, 0 = auto sense",
"0", RIG_CONF_NUMERIC, { .n = { 0, 0xff, 1 } }
},
{ RIG_CONF_END, NULL, }
};
const struct rot_caps spid_rot1prog_rot_caps =
{
.rot_model = ROT_MODEL_SPID_ROT1PROG,
.model_name = "Rot1Prog",
.mfg_name = "SPID",
.version = "1.0",
.copyright = "LGPL",
.status = RIG_STATUS_STABLE,
.rot_type = ROT_TYPE_AZIMUTH,
.port_type = RIG_PORT_SERIAL,
.serial_rate_min = 1200,
.serial_rate_max = 1200,
.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 = 400,
.retry = 3,
.min_az = -180.0,
.max_az = 540.0,
.min_el = 0.0,
.max_el = 0.0,
.cfgparams = spid_cfg_params,
.get_conf = spid_get_conf,
.set_conf = spid_set_conf,
.rot_init = spid_rot_init,
.rot_cleanup = spid_rot_cleanup,
.get_position = spid_rot_get_position,
.set_position = spid_rot1prog_rot_set_position,
.stop = spid_rot_stop,
};
const struct rot_caps spid_rot2prog_rot_caps =
{
.rot_model = ROT_MODEL_SPID_ROT2PROG,
.model_name = "Rot2Prog",
.mfg_name = "SPID",
.version = "1.0",
.copyright = "LGPL",
.status = RIG_STATUS_STABLE,
.rot_type = ROT_TYPE_AZEL,
.port_type = RIG_PORT_SERIAL,
.serial_rate_min = 600,
.serial_rate_max = 600,
.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 = 400,
.retry = 3,
.min_az = -180.0,
.max_az = 540.0,
.min_el = -20.0,
.max_el = 210.0,
.cfgparams = spid_cfg_params,
.get_conf = spid_get_conf,
.set_conf = spid_set_conf,
.rot_init = spid_rot_init,
.rot_cleanup = spid_rot_cleanup,
.get_position = spid_rot_get_position,
.set_position = spid_rot2prog_rot_set_position,
.stop = spid_rot_stop,
};
const struct rot_caps spid_md01_rot2prog_rot_caps =
{
.rot_model = ROT_MODEL_SPID_MD01_ROT2PROG,
.model_name = "MD-01/02 (ROT2 mode)",
.mfg_name = "SPID",
.version = "1.0",
.copyright = "LGPL",
.status = RIG_STATUS_STABLE,
.rot_type = ROT_TYPE_AZEL,
.port_type = RIG_PORT_SERIAL,
.serial_rate_min = 600,
.serial_rate_max = 460800,
.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 = 400,
.retry = 3,
.min_az = -180.0,
.max_az = 540.0,
.min_el = -20.0,
.max_el = 210.0,
.cfgparams = spid_cfg_params,
.get_conf = spid_get_conf,
.set_conf = spid_set_conf,
.rot_init = spid_rot_init,
.rot_cleanup = spid_rot_cleanup,
.get_position = spid_rot_get_position,
.set_position = spid_rot2prog_rot_set_position,
.move = spid_md01_rot2prog_rot_move,
.stop = spid_rot_stop,
};
DECLARE_INITROT_BACKEND(spid)
{
rig_debug(RIG_DEBUG_VERBOSE, "%s called\n", __func__);
rot_register(&spid_rot1prog_rot_caps);
rot_register(&spid_rot2prog_rot_caps);
rot_register(&spid_md01_rot2prog_rot_caps);
return RIG_OK;
}