kopia lustrzana https://github.com/Hamlib/Hamlib
asynchronous set_position() using thread
git-svn-id: https://hamlib.svn.sourceforge.net/svnroot/hamlib/trunk@2920 7ae35d74-ebe9-4afe-98af-79ac388436b8Hamlib-1.2.11
Stéphane Fillod, F8CFE
rodzic
efd45500b8
commit
7a42ff1b85
126
ars/ars.c
126
ars/ars.c
|
|
@ -31,6 +31,10 @@
|
|||
#include <sys/ioctl.h>
|
||||
#endif
|
||||
|
||||
#ifdef HAVE_PTHREAD
|
||||
#include <pthread.h>
|
||||
#endif
|
||||
|
||||
#include "hamlib/rotator.h"
|
||||
#include "parallel.h"
|
||||
#include "misc.h"
|
||||
|
|
@ -60,6 +64,12 @@
|
|||
#define PP_IO_PERIOD (25) /* usecs */
|
||||
#define NUM_SAMPLES 3
|
||||
|
||||
/* TODO: take into account ADC res (8 bits -> 1.4 deg at 360 deg)
|
||||
* Rem: at 360 deg/mn, that's 6 deg/sec.
|
||||
*/
|
||||
#define AZ_RANGE 3.
|
||||
#define EL_RANGE 2.
|
||||
|
||||
/* Check return value, with appropriate unlocking upon error.
|
||||
* Assumes "rot" variable is current ROT pointer.
|
||||
*/
|
||||
|
|
@ -73,9 +83,14 @@ static int ars_open(ROT *rot);
|
|||
static int ars_close(ROT *rot);
|
||||
static int ars_stop(ROT *rot);
|
||||
static int ars_move(ROT *rot, int direction, int speed);
|
||||
static int ars_set_position_sync(ROT *rot, azimuth_t az, elevation_t el);
|
||||
static int ars_set_position(ROT *rot, azimuth_t az, elevation_t el);
|
||||
static int ars_get_position(ROT *rot, azimuth_t *az, elevation_t *el);
|
||||
|
||||
#ifdef HAVE_PTHREAD
|
||||
static void *handle_set_position(void*);
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
static int ars_clear_ctrl_pin(ROT *rot, unsigned char pin)
|
||||
|
|
@ -142,7 +157,7 @@ ars_init(ROT *rot)
|
|||
priv->pp_control = 0;
|
||||
priv->pp_data = 0;
|
||||
|
||||
priv->dac_res = 8; /* 8 bits vs. 10 bits DAC */
|
||||
priv->adc_res = 8; /* 8 bits vs. 10 bits ADC */
|
||||
priv->brake_off = 0; /* i.e. brake is ON */
|
||||
priv->curr_move = 0;
|
||||
|
||||
|
|
@ -171,12 +186,41 @@ ars_open(ROT *rot)
|
|||
/* make it idle, and known state */
|
||||
ars_stop(rot);
|
||||
|
||||
#ifdef HAVE_PTHREAD
|
||||
{
|
||||
struct ars_priv_data *priv = (struct ars_priv_data *)rot->state.priv;
|
||||
pthread_attr_t attr;
|
||||
int retcode;
|
||||
|
||||
/* make handle_set_position() quiet at startup */
|
||||
retcode = ars_get_position(rot, &priv->target_az, &priv->target_el);
|
||||
if (retcode != RIG_OK)
|
||||
return retcode;
|
||||
|
||||
pthread_attr_init(&attr);
|
||||
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
|
||||
|
||||
/* create behind-the-scene thread doing the ars_set_position_sync() */
|
||||
retcode = pthread_create(&priv->thread, &attr, handle_set_position, rot);
|
||||
if (retcode != RIG_OK) {
|
||||
rig_debug(RIG_DEBUG_ERR, "%s: pthread_create: %s\n", __func__, strerror(retcode));
|
||||
return -RIG_ENOMEM;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
return RIG_OK;
|
||||
}
|
||||
|
||||
int
|
||||
ars_close(ROT *rot)
|
||||
{
|
||||
#ifdef HAVE_PTHREAD
|
||||
struct ars_priv_data *priv = (struct ars_priv_data *)rot->state.priv;
|
||||
|
||||
pthread_cancel(priv->thread);
|
||||
#endif
|
||||
|
||||
/* leave it in safe state */
|
||||
ars_stop(rot);
|
||||
|
||||
|
|
@ -335,17 +379,61 @@ ars_move(ROT *rot, int direction, int speed)
|
|||
return RIG_OK;
|
||||
}
|
||||
|
||||
|
||||
static int angle_in_range(float angle, float angle_base, float range)
|
||||
{
|
||||
return (angle >= angle_base-range && angle <= angle_base+range);
|
||||
}
|
||||
|
||||
/*
|
||||
* TODO: make set_position asynchronous, with wait loop in background (pthread)
|
||||
* Thread handler
|
||||
*/
|
||||
#ifdef HAVE_PTHREAD
|
||||
static void *handle_set_position(void *arg)
|
||||
{
|
||||
ROT *rot = (ROT*) arg;
|
||||
struct ars_priv_data *priv = (struct ars_priv_data *)rot->state.priv;
|
||||
azimuth_t curr_az;
|
||||
elevation_t curr_el;
|
||||
int retcode;
|
||||
|
||||
while (1) {
|
||||
|
||||
retcode = ars_get_position(rot, &curr_az, &curr_el);
|
||||
if (retcode != RIG_OK) {
|
||||
rig_debug(RIG_DEBUG_WARN, "%s: ars_get_position() failed: %s\n",
|
||||
__func__, rigerror(retcode));
|
||||
usleep(1000*1000);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (angle_in_range(curr_az, priv->target_az, AZ_RANGE) &&
|
||||
(!ars_has_el(rot) || angle_in_range(curr_el, priv->target_el, EL_RANGE)))
|
||||
{
|
||||
/* polling period */
|
||||
usleep(100*1000);
|
||||
continue;
|
||||
}
|
||||
|
||||
retcode = ars_set_position_sync(rot, priv->target_az, priv->target_el);
|
||||
if (retcode != RIG_OK) {
|
||||
rig_debug(RIG_DEBUG_WARN, "%s: ars_set_position_sync() failed: %s\n",
|
||||
__func__, rigerror(retcode));
|
||||
usleep(1000*1000);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* ars_set_position_sync() is synchronous.
|
||||
* See handle_set_position_async() for asynchronous thread handler,
|
||||
* with wait loop in background
|
||||
*/
|
||||
int
|
||||
ars_set_position(ROT *rot, azimuth_t az, elevation_t el)
|
||||
ars_set_position_sync(ROT *rot, azimuth_t az, elevation_t el)
|
||||
{
|
||||
azimuth_t curr_az, prev_az;
|
||||
elevation_t curr_el, prev_el;
|
||||
|
|
@ -353,7 +441,7 @@ ars_set_position(ROT *rot, azimuth_t az, elevation_t el)
|
|||
int az_move, el_move;
|
||||
struct timeval last_pos_az_tv, last_pos_el_tv;
|
||||
|
||||
rig_debug(RIG_DEBUG_TRACE, "%s called: %f %f\n", __func__, az, el);
|
||||
rig_debug(RIG_DEBUG_TRACE, "%s called: %.1f %.1f\n", __func__, az, el);
|
||||
|
||||
ars_stop(rot);
|
||||
|
||||
|
|
@ -367,11 +455,6 @@ ars_set_position(ROT *rot, azimuth_t az, elevation_t el)
|
|||
gettimeofday(&last_pos_az_tv, NULL);
|
||||
last_pos_el_tv = last_pos_az_tv;
|
||||
|
||||
/* TODO: take into account DAC res (8 bits -> 1.4 deg at 360 deg)
|
||||
* Rem: at 360 deg/mn, that's 6 deg/sec.
|
||||
*/
|
||||
#define AZ_RANGE 3.
|
||||
#define EL_RANGE 2.
|
||||
while (!angle_in_range(curr_az, az, AZ_RANGE) ||
|
||||
(ars_has_el(rot) && !angle_in_range(curr_el, el, EL_RANGE))
|
||||
) {
|
||||
|
|
@ -437,6 +520,21 @@ ars_set_position(ROT *rot, azimuth_t az, elevation_t el)
|
|||
return ars_stop(rot);
|
||||
}
|
||||
|
||||
int
|
||||
ars_set_position(ROT *rot, azimuth_t az, elevation_t el)
|
||||
{
|
||||
#ifdef HAVE_PTHREAD
|
||||
struct ars_priv_data *priv = (struct ars_priv_data *)rot->state.priv;
|
||||
|
||||
/* will be picked by handle_set_position() next polling tick */
|
||||
priv->target_az = az;
|
||||
priv->target_el = el;
|
||||
|
||||
return RIG_OK;
|
||||
#else
|
||||
return ars_set_position_sync(rot, az, el);
|
||||
#endif
|
||||
}
|
||||
|
||||
static int comparunsigned(const void *a, const void *b)
|
||||
{
|
||||
|
|
@ -465,7 +563,7 @@ ars_get_position(ROT *rot, azimuth_t *az, elevation_t *el)
|
|||
CHKPPRET(ars_clear_ctrl_pin(rot, CTL_PIN_CS));
|
||||
usleep (PP_IO_PERIOD);
|
||||
|
||||
for (i = 0; i < priv->dac_res; i++) {
|
||||
for (i = 0; i < priv->adc_res; i++) {
|
||||
CHKPPRET(ars_set_ctrl_pin(rot, CTL_PIN_CLK));
|
||||
usleep (PP_IO_PERIOD);
|
||||
|
||||
|
|
@ -493,7 +591,7 @@ ars_get_position(ROT *rot, azimuth_t *az, elevation_t *el)
|
|||
az_samples[num_sample] = 0;
|
||||
el_samples[num_sample] = 0;
|
||||
|
||||
for (i = 0; i < priv->dac_res; i++) {
|
||||
for (i = 0; i < priv->adc_res; i++) {
|
||||
CHKPPRET(ars_set_ctrl_pin(rot, CTL_PIN_CLK));
|
||||
usleep (PP_IO_PERIOD);
|
||||
|
||||
|
|
@ -526,8 +624,8 @@ ars_get_position(ROT *rot, azimuth_t *az, elevation_t *el)
|
|||
az_value = az_samples[NUM_SAMPLES/2];
|
||||
el_value = el_samples[NUM_SAMPLES/2];
|
||||
|
||||
*az = rs->min_az + ((float)az_value * (rs->max_az - rs->min_az)) / ((1 << priv->dac_res)-1);
|
||||
*el = rs->min_el + ((float)el_value * (rs->max_el - rs->min_el)) / ((1 << priv->dac_res)-1);
|
||||
*az = rs->min_az + ((float)az_value * (rs->max_az - rs->min_az)) / ((1 << priv->adc_res)-1);
|
||||
*el = rs->min_el + ((float)el_value * (rs->max_el - rs->min_el)) / ((1 << priv->adc_res)-1);
|
||||
|
||||
rig_debug(RIG_DEBUG_TRACE, "%s: az=%.1f el=%.1f\n", __func__, *az, *el);
|
||||
|
||||
|
|
|
|||
|
|
@ -22,12 +22,19 @@
|
|||
#ifndef _ROT_ARS_H
|
||||
#define _ROT_ARS_H 1
|
||||
|
||||
#include "hamlib/rig.h"
|
||||
|
||||
struct ars_priv_data {
|
||||
unsigned dac_res;
|
||||
unsigned adc_res;
|
||||
int brake_off;
|
||||
int curr_move;
|
||||
unsigned char pp_control;
|
||||
unsigned char pp_data;
|
||||
#ifdef HAVE_PTHREAD
|
||||
pthread_t thread;
|
||||
azimuth_t target_az;
|
||||
elevation_t target_el;
|
||||
#endif
|
||||
};
|
||||
|
||||
extern const struct rot_caps rci_se8_rot_caps;
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue