kopia lustrzana https://github.com/Hamlib/Hamlib
625 wiersze
15 KiB
C
625 wiersze
15 KiB
C
/*
|
|
* Hamlib Rotator backend - LinuxCNC no hardware port
|
|
* Copyright (c) 2015 by Robert Freeman
|
|
* Adapted from AMSAT code by Stephane Fillod
|
|
*
|
|
* 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 <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h> /* String function definitions */
|
|
#include <unistd.h> /* UNIX standard function definitions */
|
|
|
|
#ifdef HAVE_SYS_IOCTL_H
|
|
#include <sys/ioctl.h>
|
|
#endif
|
|
|
|
#include "hamlib/rotator.h"
|
|
#include "hamlib/rig.h"
|
|
|
|
#include "serial.h"
|
|
#include "token.h"
|
|
|
|
#include "register.h"
|
|
|
|
#define RSIZE (1024)
|
|
|
|
#define XDEGREE2MM(d) ((d)/9.0)
|
|
|
|
// 42-gear-22
|
|
#define YDEGREE2MM(d) ((d)/9.0)
|
|
|
|
//#define YDEGREE2MM(d) ((d) * ((1710) / (77*9)))
|
|
//#define YDEGREE2MM(d) ((d) * ((190) / (77)))
|
|
|
|
/*
|
|
|
|
$0=10 (step pulse, usec)
|
|
$1=25 (step idle delay, msec)
|
|
$2=0 (step port invert mask:00000000)
|
|
$3=0 (dir port invert mask:00000000)
|
|
$4=0 (step enable invert, bool)
|
|
$5=0 (limit pins invert, bool)
|
|
$6=0 (probe pin invert, bool)
|
|
$10=3 (status report mask:00000011)
|
|
$11=0.010 (junction deviation, mm)
|
|
$12=0.002 (arc tolerance, mm)
|
|
$13=0 (report inches, bool)
|
|
$20=0 (soft limits, bool)
|
|
$21=0 (hard limits, bool)
|
|
$22=0 (homing cycle, bool)
|
|
$23=0 (homing dir invert mask:00000000)
|
|
$24=25.000 (homing feed, mm/min)
|
|
$25=500.000 (homing seek, mm/min)
|
|
$26=250 (homing debounce, msec)
|
|
$27=1.000 (homing pull-off, mm)
|
|
$30=1000. (rpm max)
|
|
$31=0. (rpm min)
|
|
$32=0 (motor lock bool)
|
|
$33=7 (motor mode mask:00000111)
|
|
$100=80.000 (x, step/mm)
|
|
$101=80.000 (y, step/mm)
|
|
$102=80.000 (z, step/mm)
|
|
$110=10000.000 (x max rate, mm/min)
|
|
$111=10000.000 (y max rate, mm/min)
|
|
$112=10000.000 (z max rate, mm/min)
|
|
$120=250.000 (x accel, mm/sec^2)
|
|
$121=250.000 (y accel, mm/sec^2)
|
|
$122=250.000 (z accel, mm/sec^2)
|
|
$130=500.000 (x max travel, mm)
|
|
$131=500.000 (y max travel, mm)
|
|
$132=500.000 (z max travel, mm)
|
|
|
|
*/
|
|
|
|
char *grbl_get_config = "$$\r\n";
|
|
char *grbl_get_pos = "?\r\n";
|
|
|
|
char *grbl_init_list[] =
|
|
{
|
|
"$1=255\r\n", /* lock motors */
|
|
"$3=3\r\n", /* invert X and Y direction */
|
|
//"$100=80\r\n", /* axis-x a4988 16 microstep */
|
|
"$100=1776\r\n", /* axis-x a4988 16 microstep 42-gear-motor-22-(1710/77) (80*1710)/77 */
|
|
//"$101=160\n", /* axis-y drv8825 32 microstep */
|
|
//"$101=80\n", /* axis-y a4988 16 microstep */
|
|
"$101=1776\r\n", /* axis-y a4988 16 microstep 42-gear-motor-22-(1710/77) (80*1710)/77 */
|
|
"$110=50\r\n",
|
|
"$111=50\r\n",
|
|
"$120=25\r\n",
|
|
"$121=25\r\n",
|
|
"G90\r\n",
|
|
"G0 X0 Y0\r\n",
|
|
};
|
|
|
|
static int
|
|
grbl_request(ROT *rot, char *request, uint32_t req_size, char *response,
|
|
uint32_t *resp_size)
|
|
{
|
|
int retval;
|
|
static int fail_count = 0;
|
|
|
|
rot_debug(RIG_DEBUG_ERR, "req: [%s][%d]\n", request, fail_count);
|
|
|
|
if (rot->caps->rot_model == ROT_MODEL_GRBLTRK_SER
|
|
|| rot->caps->rot_model == ROT_MODEL_GRBLTRK_NET)
|
|
{
|
|
//fprintf(stderr, "ctrl by serial/network\n");
|
|
|
|
if ((retval = write_block(&rot->state.rotport, (unsigned char *)request,
|
|
req_size)) != RIG_OK)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s write_block fail!\n", __func__);
|
|
//exit(-1);
|
|
fail_count++;
|
|
//return RIG_EIO;
|
|
}
|
|
else
|
|
{
|
|
fail_count = 0;
|
|
}
|
|
|
|
rig_flush(&rot->state.rotport);
|
|
|
|
usleep(300000);
|
|
|
|
if ((retval = read_string(&rot->state.rotport, (unsigned char *)response, 1024,
|
|
"\n", 1, 0, 1)) < 0)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s read_string fail! (%d) \n", __func__, retval);
|
|
//exit(-1);
|
|
fail_count++;
|
|
//return RIG_EIO;
|
|
}
|
|
else
|
|
{
|
|
fail_count = 0;
|
|
}
|
|
|
|
if (fail_count >= 10)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s too much xfer fail! exit\n", __func__);
|
|
exit(-1);
|
|
}
|
|
|
|
rig_flush(&rot->state.rotport);
|
|
|
|
rot_debug(RIG_DEBUG_ERR, "rsp: [%s]\n", response);
|
|
//fprintf(stderr, "rsp: [%s]\n", response);
|
|
|
|
*resp_size = retval;
|
|
|
|
}
|
|
|
|
return RIG_OK;
|
|
}
|
|
|
|
static int
|
|
grbl_init(ROT *rot)
|
|
{
|
|
int i, retval;
|
|
uint32_t init_count;
|
|
char rsp[RSIZE];
|
|
uint32_t resp_size;
|
|
|
|
/* get total config */
|
|
grbl_request(rot, grbl_get_config, strlen(grbl_get_config), rsp, &resp_size);
|
|
|
|
if (strstr(rsp, grbl_init_list[0]) != NULL)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s: grbl already configured\n", __func__);
|
|
return RIG_OK;
|
|
}
|
|
|
|
init_count = sizeof(grbl_init_list) / sizeof(grbl_init_list[0]);
|
|
|
|
for (i = 0; i < init_count; i++)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "grbl_request [%s] ", grbl_init_list[i]);
|
|
retval = grbl_request(rot, grbl_init_list[i], strlen(grbl_init_list[i]), rsp,
|
|
&resp_size);
|
|
|
|
//fprintf(stderr, "done\n");
|
|
if (retval != RIG_OK)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "grbl_request [%s] fail\n", grbl_init_list[i]);
|
|
return RIG_EIO;
|
|
}
|
|
}
|
|
|
|
return RIG_OK;
|
|
}
|
|
|
|
static int
|
|
grbltrk_rot_set_position(ROT *rot, azimuth_t curr_az, elevation_t curr_el)
|
|
{
|
|
int i;
|
|
int retval;
|
|
static float prev_az, prev_el;
|
|
|
|
static float prev_x, curr_x;
|
|
float x[3], delta[3];
|
|
|
|
float y;
|
|
|
|
char req[RSIZE] = {0};
|
|
char rsp[RSIZE] = {0};
|
|
uint32_t rsp_size;
|
|
|
|
float min_value;
|
|
int min_index;
|
|
|
|
/* az:x: 0 - 360 */
|
|
/* el:y: 0 - 90 */
|
|
rot_debug(RIG_DEBUG_ERR,
|
|
"%s: (prev_x) = (%.3f); (prev_az) = (%.3f); (prev_el) = (%.3f); (curr_az, curr_el) = (%.3f, %.3f)\n",
|
|
__func__,
|
|
prev_x, prev_az, prev_el, curr_az, curr_el);
|
|
|
|
/* convert degree to mm, 360 degree = 40mm, 1 degree = 0.111mm */
|
|
//x = az * 0.111;
|
|
//y = el * 0.111;
|
|
|
|
/* 360 -> 0 */
|
|
if ((prev_az > 270 && prev_az < 360) &&
|
|
(curr_az > 0 && curr_az < 90))
|
|
{
|
|
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d\n", __func__, __LINE__);
|
|
|
|
if (prev_x >= XDEGREE2MM(270))
|
|
{
|
|
curr_x = XDEGREE2MM(curr_az) + XDEGREE2MM(360);
|
|
}
|
|
else
|
|
{
|
|
curr_x = XDEGREE2MM(curr_az);
|
|
}
|
|
|
|
/* 0 -> 360 */
|
|
}
|
|
else if ((prev_az > 0 && prev_az < 90) &&
|
|
(curr_az > 270 && curr_az < 360))
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d\n", __func__, __LINE__);
|
|
|
|
if (prev_x >= XDEGREE2MM(360))
|
|
{
|
|
curr_x = XDEGREE2MM(curr_az);
|
|
}
|
|
else
|
|
{
|
|
curr_x = XDEGREE2MM(curr_az) - XDEGREE2MM(360);
|
|
}
|
|
|
|
/* reset */
|
|
}
|
|
else if (curr_az == 0 && curr_el == 0)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s: reset\n", __func__);
|
|
curr_x = 0;
|
|
}
|
|
else
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d prev_x: %.3f\n", __func__, __LINE__, prev_x);
|
|
|
|
x[0] = XDEGREE2MM(curr_az) - XDEGREE2MM(360);
|
|
x[1] = XDEGREE2MM(curr_az);
|
|
x[2] = XDEGREE2MM(curr_az) + XDEGREE2MM(360);
|
|
|
|
delta[0] = prev_x - x[0];
|
|
delta[1] = prev_x - x[1];
|
|
delta[2] = prev_x - x[2];
|
|
|
|
if (delta[0] < 0) { delta[0] = -1 * delta[0]; }
|
|
|
|
if (delta[1] < 0) { delta[1] = -1 * delta[1]; }
|
|
|
|
if (delta[2] < 0) { delta[2] = -1 * delta[2]; }
|
|
|
|
min_value = delta[0];
|
|
min_index = 0;
|
|
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
if (delta[i] <= min_value)
|
|
{
|
|
min_value = delta[i];
|
|
min_index = i;
|
|
}
|
|
}
|
|
|
|
curr_x = x[min_index];
|
|
rot_debug(RIG_DEBUG_ERR, "min_index: %d; curr_x: %.3f\n", min_index, curr_x);
|
|
}
|
|
|
|
y = YDEGREE2MM(curr_el);
|
|
|
|
/**/
|
|
|
|
snprintf(req, sizeof(req), "G0 X%.3f Y%.3f\n", curr_x, y);
|
|
|
|
retval = grbl_request(rot, req, strlen(req), rsp, &rsp_size);
|
|
|
|
if (retval != RIG_OK)
|
|
{
|
|
return retval;
|
|
}
|
|
|
|
prev_az = curr_az;
|
|
prev_el = curr_el;
|
|
|
|
prev_x = curr_x;
|
|
return RIG_OK;
|
|
|
|
}
|
|
|
|
static int
|
|
grbltrk_rot_get_position(ROT *rot, azimuth_t *az, elevation_t *el)
|
|
{
|
|
|
|
//static char req[RSIZE];
|
|
static char rsp[RSIZE];
|
|
uint32_t rsp_size;
|
|
|
|
float mpos[3];
|
|
char dummy0[256];
|
|
char dummy1[256];
|
|
|
|
int retval;
|
|
int i;
|
|
|
|
rot_debug(RIG_DEBUG_ERR, "%s called\n", __func__);
|
|
|
|
//snprintf(req, sizeof(req), "?\r\n");
|
|
|
|
for (i = 0; i < 5; i++)
|
|
{
|
|
retval = grbl_request(rot, grbl_get_pos, strlen(grbl_get_pos), rsp, &rsp_size);
|
|
|
|
/*FIXME: X Y safe check */
|
|
|
|
if (retval != RIG_OK)
|
|
{
|
|
return retval;
|
|
}
|
|
|
|
if (strstr(rsp, "MPos") == NULL)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s no MPos found, continue\n", __func__);
|
|
continue;
|
|
}
|
|
|
|
/* grbl 0.9 mega328p */
|
|
/* <Idle,MPos:0.000,0.000,0.000,WPos:0.000,0.000,0.000,SC:0> */
|
|
//sscanf(rsp, "%[^','],MPos:%f,%f,%f,WPos:%f,%f,%f,%s", &dummy[0], &mpos[0], &mpos[1], &mpos[2], &wpos[0], &wpos[1], &wpos[2], &dummy[1]);
|
|
|
|
/* grbl 1.3a esp32 */
|
|
//<Idle|MPos:0.000,0.000,0.000|FS:0,0|Pn:P|WCO:5.000,0.000,0.000>
|
|
sscanf(rsp, "%[^'|']|MPos:%f,%f,%s", dummy0, &mpos[0], &mpos[1], dummy1);
|
|
|
|
//rot_debug(RIG_DEBUG_ERR, "%s: (%.3f, %.3f) (%.3f, %.3f)\n", __func__, mpos[0], mpos[1], wpos[0], wpos[1]);
|
|
|
|
//*az = (azimuth_t) mpos[0] / 0.111;
|
|
//*el = (elevation_t) mpos[1] / 0.111;
|
|
*az = (azimuth_t) mpos[0] * 9;
|
|
*el = (elevation_t) mpos[1] * 9;
|
|
|
|
if ((*az) < 0)
|
|
{
|
|
(*az) = (*az) + 360;
|
|
}
|
|
|
|
//rot_debug(RIG_DEBUG_ERR, "%s: (az, el) = (%.3f, %.3f)\n", __func__, *az, *el);
|
|
|
|
rot_debug(RIG_DEBUG_ERR, "%s: (az, el) = (%.3f, %.3f)\n", __func__, *az, *el);
|
|
|
|
return RIG_OK;
|
|
|
|
}
|
|
|
|
*az = (azimuth_t) 0;
|
|
*el = (elevation_t) 0;
|
|
return RIG_OK;
|
|
}
|
|
|
|
static int
|
|
grbltrk_rot_set_conf(ROT *rot, token_t token, const char *val)
|
|
{
|
|
int i, retval;
|
|
char req[RSIZE] = {0};
|
|
char rsp[RSIZE];
|
|
uint32_t resp_size, len;
|
|
|
|
rot_debug(RIG_DEBUG_ERR, "token: %ld; value: [%s]\n", token, val);
|
|
|
|
len = strlen(val);
|
|
|
|
if ((len != 0) && (val[0] == 'G'))
|
|
{
|
|
|
|
for (i = 0; i < len; i++)
|
|
{
|
|
|
|
if (val[i] == '@')
|
|
{
|
|
req[i] = ' ';
|
|
}
|
|
else
|
|
{
|
|
req[i] = val[i];
|
|
}
|
|
}
|
|
|
|
req[i] = '\n';
|
|
len = strlen(req);
|
|
|
|
rot_debug(RIG_DEBUG_ERR, "send gcode [%s]\n", req);
|
|
retval = grbl_request(rot, req, len, rsp, &resp_size);
|
|
|
|
if (retval < 0)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "grbl_request [%s] fail\n", val);
|
|
return RIG_EIO;
|
|
}
|
|
}
|
|
|
|
return RIG_OK;
|
|
}
|
|
|
|
static int
|
|
grbltrk_rot_init(ROT *rot)
|
|
{
|
|
int r = RIG_OK;
|
|
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d rot->caps->rot_model: %d\n", __func__, __LINE__,
|
|
rot->caps->rot_model);
|
|
|
|
return r;
|
|
}
|
|
|
|
static int
|
|
grbl_net_open(ROT *rot, int port)
|
|
{
|
|
//network_open(&rot->state.rotport, port);
|
|
|
|
//rot_debug(RIG_DEBUG_ERR, "%s:%d network_fd: %d\n", __func__, __LINE__, (&rot->state.rotport)->fd);
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d \n", __func__, __LINE__);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int
|
|
grbltrk_rot_open(ROT *rot)
|
|
{
|
|
int r = RIG_OK;
|
|
char host[128] = {0};
|
|
//char ip[32];
|
|
//int port;
|
|
|
|
//rot_debug(RIG_DEBUG_ERR, "%s:%d rot->caps->rot_model: %d\n", __func__, __LINE__, rot->caps->rot_model);
|
|
if (rot->caps->rot_model == ROT_MODEL_GRBLTRK_SER)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d ctrl via serial\n", __func__, __LINE__);
|
|
}
|
|
else if (rot->caps->rot_model == ROT_MODEL_GRBLTRK_NET)
|
|
{
|
|
rot_get_conf(rot, TOK_PATHNAME, host);
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d ctrl via net, host [%s]\n", __func__, __LINE__,
|
|
host);
|
|
grbl_net_open(rot, 23);
|
|
|
|
#if 0
|
|
|
|
if (sscanf(host, "%[^:]:%d", ip, &port) == 2)
|
|
{
|
|
grbl_net_open(rot, ip, port);
|
|
}
|
|
else
|
|
{
|
|
grbl_net_open(rot, NULL, 0); /* use default ip & port */
|
|
}
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
grbl_init(rot);
|
|
|
|
//rot_debug(RIG_DEBUG_ERR, "%s:%d\n", __func__, __LINE__);
|
|
|
|
return r;
|
|
}
|
|
|
|
static void
|
|
grbl_net_close(ROT *rot)
|
|
{
|
|
port_close(&rot->state.rotport, RIG_PORT_NETWORK);
|
|
}
|
|
|
|
static int
|
|
grbltrk_rot_close(ROT *rot)
|
|
{
|
|
int r = RIG_OK;
|
|
|
|
if (rot->caps->rot_model == ROT_MODEL_GRBLTRK_SER)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d\n", __func__, __LINE__);
|
|
}
|
|
else if (rot->caps->rot_model == ROT_MODEL_GRBLTRK_NET)
|
|
{
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d\n", __func__, __LINE__);
|
|
grbl_net_close(rot);
|
|
}
|
|
|
|
rot_debug(RIG_DEBUG_ERR, "%s:%d\n", __func__, __LINE__);
|
|
|
|
return r;
|
|
}
|
|
|
|
|
|
/** CNCTRK implements essentially only the set position function.
|
|
it assumes there is a LinuxCNC running with the Axis GUI */
|
|
const struct rot_caps grbltrk_serial_rot_caps =
|
|
{
|
|
ROT_MODEL(ROT_MODEL_GRBLTRK_SER),
|
|
.model_name = "GRBLTRK via Serial",
|
|
.mfg_name = "BG5DIW",
|
|
.version = "20220515.0",
|
|
.copyright = "LGPL",
|
|
.status = RIG_STATUS_BETA,
|
|
.rot_type = ROT_TYPE_OTHER,
|
|
.port_type = RIG_PORT_SERIAL,
|
|
.serial_rate_min = 9600,
|
|
.serial_rate_max = 115200,
|
|
.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 = 0,
|
|
|
|
|
|
.min_az = 0,
|
|
.max_az = 360,
|
|
.min_el = 0,
|
|
.max_el = 90,
|
|
|
|
.rot_init = grbltrk_rot_init,
|
|
.rot_open = grbltrk_rot_open,
|
|
|
|
.set_position = grbltrk_rot_set_position,
|
|
.get_position = grbltrk_rot_get_position,
|
|
.set_conf = grbltrk_rot_set_conf,
|
|
};
|
|
|
|
const struct rot_caps grbltrk_net_rot_caps =
|
|
{
|
|
ROT_MODEL(ROT_MODEL_GRBLTRK_NET),
|
|
.model_name = "GRBLTRK via Net",
|
|
.mfg_name = "BG5DIW",
|
|
.version = "20220515.0",
|
|
.copyright = "LGPL",
|
|
.status = RIG_STATUS_BETA,
|
|
.rot_type = ROT_TYPE_OTHER,
|
|
.port_type = RIG_PORT_NETWORK, /* RIG_PORT_NONE */
|
|
//.port_type = RIG_PORT_NONE, /* RIG_PORT_NONE */
|
|
|
|
.write_delay = 0,
|
|
.post_write_delay = 0,
|
|
.timeout = 300,
|
|
.retry = 0,
|
|
//.retry = 3,
|
|
|
|
|
|
.min_az = 0,
|
|
.max_az = 360,
|
|
.min_el = 0,
|
|
.max_el = 90,
|
|
|
|
.rot_init = grbltrk_rot_init,
|
|
.rot_open = grbltrk_rot_open,
|
|
.rot_close = grbltrk_rot_close,
|
|
|
|
.set_position = grbltrk_rot_set_position,
|
|
.get_position = grbltrk_rot_get_position,
|
|
.set_conf = grbltrk_rot_set_conf,
|
|
};
|
|
|
|
|
|
/* ************************************************************************* */
|
|
|
|
DECLARE_INITROT_BACKEND(grbltrk)
|
|
{
|
|
rig_debug(RIG_DEBUG_VERBOSE, "%s: _init called\n", __func__);
|
|
|
|
//rot_debug(RIG_DEBUG_ERR, "%s: _init called\n", __func__);
|
|
|
|
rot_register(&grbltrk_serial_rot_caps);
|
|
|
|
rot_register(&grbltrk_net_rot_caps);
|
|
|
|
return RIG_OK;
|
|
}
|