/*
XHC-HB04 Wireless MPG pendant LinuxCNC HAL module for LinuxCNC
Copyright (C) 2013 Frederic Rible (frible@teaser.fr)
Copyright (C) 2013 Rene Hopf (renehopf@mac.com)
Copyright (C) 2014 Marius Alksnys (marius.alksnys@gmail.com)
This program 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 of the License, or (at your option) any later version.
This program 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 the program; if not, write to the Free
Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
MA 02110-1301 USA.
*/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <signal.h>
#include <string.h>
#include <libusb.h>
#include <unistd.h>
#include <stdarg.h>
#include <hal.h>
#include <inifile.hh>
#include "config.h"
const char *modname = "xhc-hb04";
int hal_comp_id;
const char *section = "XHC-HB04";
bool simu_mode = true;
typedef struct {
char pin_name[256];
unsigned int code;
} xhc_button_t;
typedef enum {
axis_off = 0x00,
axis_x = 0x11,
axis_y = 0x12,
axis_z = 0x13,
axis_a = 0x14,
axis_b = 0x15,
axis_c = 0x16,
axis_spindle = 0x14,
axis_feed = 0x15
} xhc_axis_t;
#define NB_MAX_BUTTONS 32
#define STEPSIZE_BYTE 35
#define FLAGS_BYTE 36
// the defines below were found for an 18 button device
// bit 'or' patterns for STEPSIZE_BYTE
#define DISPLAY_HOME_ICON 0x10
#define DISPLAY_HEIGHT_SETTING_ICON 0x20
#define DISPLAY_HEIGHT_UNKNOWN_40 0x40
#define DISPLAY_HEIGHT_UNKNOWN_80 0x80
#define STEPSIZE_DISPLAY_0 0x00
#define STEPSIZE_DISPLAY_1 0x01
#define STEPSIZE_DISPLAY_5 0x02
#define STEPSIZE_DISPLAY_10 0x03
#define STEPSIZE_DISPLAY_20 0x04
#define STEPSIZE_DISPLAY_30 0x05
#define STEPSIZE_DISPLAY_40 0x06
#define STEPSIZE_DISPLAY_50 0x07
#define STEPSIZE_DISPLAY_100 0x08
#define STEPSIZE_DISPLAY_500 0x09
#define STEPSIZE_DISPLAY_1000 0x0A
#define STEPSIZE_DISPLAY_P6 0x0B
#define STEPSIZE_DISPLAY_UNKNOWN_0C 0x0C
#define STEPSIZE_DISPLAY_UNKNOWN_0D 0x0D
#define STEPSIZE_DISPLAY_UNKNOWN_0E 0x0E
#define STEPSIZE_DISPLAY_UNKNOWN_0F 0x0F
// guard for maximum number of items in a stepsize_sequence
#define MAX_STEPSIZE_SEQUENCE 10
// alternate stepsize sequences (use STEPSIZE_DISPLAY_*), terminate with 0:
static const int stepsize_sequence_1[MAX_STEPSIZE_SEQUENCE +1] = {1,10,100,1000, 0}; // default
static const int stepsize_sequence_2[MAX_STEPSIZE_SEQUENCE +1] = {1, 5, 10, 20, 0};
static const int stepsize_sequence_3[MAX_STEPSIZE_SEQUENCE +1] = {1,10,100, 0};
static const int stepsize_sequence_4[MAX_STEPSIZE_SEQUENCE +1] = {1, 5, 10, 20, 50,100,0};
static const int stepsize_sequence_5[MAX_STEPSIZE_SEQUENCE +1] = {1,10, 50,100,1000, 0};
static const int* stepsize_sequence = stepsize_sequence_1; // use the default
static int stepsize_idx = 0; // start at initial (zeroth) sequence
static int stepsize_last_idx = 0; //calculated`
typedef struct {
hal_float_t *x_wc, *y_wc, *z_wc, *a_wc, *b_wc, *c_wc;
hal_float_t *x_mc, *y_mc, *z_mc, *a_mc, *b_wc, *c_wc;
hal_float_t *feedrate_override, *feedrate;
hal_float_t *spindle_override, *spindle_rps;
hal_bit_t *button_pin[NB_MAX_BUTTONS];
hal_bit_t *jog_enable_off;
hal_bit_t *jog_enable_x;
hal_bit_t *jog_enable_y;
hal_bit_t *jog_enable_z;
hal_bit_t *jog_enable_a;
hal_bit_t *jog_enable_b;
hal_bit_t *jog_enable_c;
hal_bit_t *jog_enable_feedrate;
hal_bit_t *jog_enable_spindle;
hal_float_t *jog_scale;
hal_s32_t *jog_counts, *jog_counts_neg;
hal_float_t *jog_velocity;
hal_float_t *jog_max_velocity;
hal_float_t *jog_increment;
hal_bit_t *jog_plus_x, *jog_plus_y, *jog_plus_z, *jog_plus_a, *jog_plus_b, *jog_plus_c;
hal_bit_t *jog_minus_x, *jog_minus_y, *jog_minus_z, *jog_minus_a, *jog_minus_b, *jog_minus_c;
hal_bit_t *stepsize_up;
hal_bit_t *stepsize_down;
hal_s32_t *stepsize;
hal_bit_t *sleeping;
hal_bit_t *connected;
hal_bit_t *require_pendant;
hal_bit_t *inch_icon;
hal_bit_t *zero_x;
hal_bit_t *zero_y;
hal_bit_t *zero_z;
hal_bit_t *zero_a;
hal_bit_t *zero_b;
hal_bit_t *zero_c;
hal_bit_t *gotozero_x;
hal_bit_t *gotozero_y;
hal_bit_t *gotozero_z;
hal_bit_t *gotozero_a;
hal_bit_t *gotozero_b;
hal_bit_t *gotozero_c;
hal_bit_t *half_x;
hal_bit_t *half_y;
hal_bit_t *half_z;
hal_bit_t *half_a;
hal_bit_t *half_b;
hal_bit_t *half_c;
} xhc_hal_t;
#define STEP_UNDEFINED -1
#define STEP_NONE 0x0
#define STEP_UP 0x1
#define STEP_DOWN 0x2
typedef struct {
xhc_hal_t *hal;
xhc_axis_t axis;
xhc_button_t buttons[NB_MAX_BUTTONS];
unsigned char button_code;
char old_inc_step_status;
unsigned char button_step; // Used in simulation mode to handle the STEP increment
// Variables for velocity computation
hal_s32_t last_jog_counts;
struct timeval last_tv;
struct timeval last_wakeup;
} xhc_t;
static xhc_t xhc;
static int do_exit = 0;
static int do_reconnect = 0;
static bool wait_for_pendant_before_HAL = false;
struct libusb_transfer *transfer_in = NULL;
unsigned char in_buf[32];
void setup_asynch_transfer(libusb_device_handle *dev_handle);
extern "C" const char *
iniFind(FILE *fp, const char *tag, const char *section)
{
IniFile f(false, fp);
return(f.Find(tag, section));
}
void init_xhc(xhc_t *xhc)
{
memset(xhc, 0, sizeof(*xhc));
xhc->old_inc_step_status = STEP_UNDEFINED;
gettimeofday(&xhc->last_wakeup, NULL);
}
int xhc_encode_float(float v, unsigned char *buf)
{
unsigned int int_v = round(fabs(v) * 10000.0);
unsigned short int_part = int_v / 10000;
unsigned short fract_part = int_v % 10000;
if (v < 0) fract_part = fract_part | 0x8000;
*(short *)buf = int_part;
*((short *)buf+1) = fract_part;
return 4;
}
int xhc_encode_s16(int v, unsigned char *buf)
{
*(short *)buf = v;
return 2;
}
void xhc_display_encode(xhc_t *xhc, unsigned char *data, int len)
{
unsigned char buf[6*7];
unsigned char *p = buf;
int i;
int packet;
assert(len == 6*8);
memset(buf, 0, sizeof(buf));
*p++ = 0xFE;
*p++ = 0xFD;
*p++ = 0x0C;
if (xhc->axis == axis_a) p += xhc_encode_float(round(1000 * *(xhc->hal->a_wc)) / 1000, p);
else p += xhc_encode_float(round(1000 * *(xhc->hal->x_wc)) / 1000, p);
p += xhc_encode_float(round(1000 * *(xhc->hal->y_wc)) / 1000, p);
p += xhc_encode_float(round(1000 * *(xhc->hal->z_wc)) / 1000, p);
if (xhc->axis == axis_a) p += xhc_encode_float(round(1000 * *(xhc->hal->a_mc)) / 1000, p);
else p += xhc_encode_float(round(1000 * *(xhc->hal->x_mc)) / 1000, p);
p += xhc_encode_float(round(1000 * *(xhc->hal->y_mc)) / 1000, p);
p += xhc_encode_float(round(1000 * *(xhc->hal->z_mc)) / 1000, p);
p += xhc_encode_s16(round(100.0 * *(xhc->hal->feedrate_override)), p);
p += xhc_encode_s16(round(100.0 * *(xhc->hal->spindle_override)), p);
p += xhc_encode_s16(round(60.0 * *(xhc->hal->feedrate)), p);
p += xhc_encode_s16(round(60.0 * *(xhc->hal->spindle_rps)), p);
switch (*(xhc->hal->stepsize)) {
case 0: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_0; break;
case 1: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_1; break;
case 5: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_5; break;
case 10: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_10; break;
case 20: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_20; break;
case 30: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_30; break;
case 40: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_40; break;
case 50: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_50; break;
case 100: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_100; break;
case 500: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_500; break;
case 1000: buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_1000; break;
default: //stepsize not supported on the display:
buf[STEPSIZE_BYTE] = STEPSIZE_DISPLAY_0; break;
}
buf[FLAGS_BYTE] = 0;
if (*(xhc->hal->inch_icon)) {
buf[FLAGS_BYTE] |= 0x80;
}
// Multiplex to 6 USB transactions
p = buf;
for (packet=0; packet<6; packet++) {
for (i=0; i<8; i++) {
if (i == 0) data[i+8*packet] = 6;
else data[i+8*packet] = *p++;
}
}
}
void xhc_set_display(libusb_device_handle *dev_handle, xhc_t *xhc)
{
unsigned char data[6*8];
int packet;
xhc_display_encode(xhc, data, sizeof(data));
for (packet=0; packet<6; packet++) {
int r = libusb_control_transfer(dev_handle,
LIBUSB_DT_HID, //bmRequestType 0x21
LIBUSB_REQUEST_SET_CONFIGURATION, //bRequest 0x09
0x0306, //wValue
0x00, //wIndex
data+8*packet, //*data
8, //wLength
0); //timeout
if (r < 0) {
do_reconnect = 1;
}
}
}
void hexdump(unsigned char *data, int len)
{
int i;
for (i=0; i<len; i++) printf("%02X ", data[i]);
}
void linuxcnc_simu(xhc_t *xhc)
{
static int last_jog_counts;
xhc_hal_t *hal = xhc->hal;
// for simu, always step up
*(hal->stepsize_up) = (xhc->button_step && xhc->button_code == xhc->button_step);
if (*(hal->jog_counts) != last_jog_counts) {
int delta_int = *(hal->jog_counts) - last_jog_counts;
float delta = delta_int * *(hal->jog_scale);
if (*(hal->jog_enable_x)) {
*(hal->x_mc) += delta;
*(hal->x_wc) += delta;
}
if (*(hal->jog_enable_y)) {
*(hal->y_mc) += delta;
*(hal->y_wc) += delta;
}
if (*(hal->jog_enable_z)) {
*(hal->z_mc) += delta;
*(hal->z_wc) += delta;
}
if (*(hal->jog_enable_a)) {
*(hal->a_mc) += delta;
*(hal->a_wc) += delta;
}
if (*(hal->jog_enable_b)) {
*(hal->b_mc) += delta;
*(hal->b_wc) += delta;
}
if (*(hal->jog_enable_a)) {
*(hal->c_mc) += delta;
*(hal->c_wc) += delta;
}
if (*(hal->jog_enable_spindle)) {
*(hal->spindle_override) += delta_int * 0.01;
if (*(hal->spindle_override) > 1) *(hal->spindle_override) = 1;
if (*(hal->spindle_override) < 0) *(hal->spindle_override) = 0;
*(hal->spindle_rps) = 25000.0/60.0 * *(hal->spindle_override);
}
if (*(hal->jog_enable_feedrate)) {
*(hal->feedrate_override) += delta_int * 0.01;
if (*(hal->feedrate_override) > 1) *(hal->feedrate_override) = 1;
if (*(hal->feedrate_override) < 0) *(hal->feedrate_override) = 0;
*(hal->feedrate) = 3000.0/60.0 * *(hal->feedrate_override);
}
last_jog_counts = *(hal->jog_counts);
}
}
void compute_velocity(xhc_t *xhc)
{
timeval now, delta_tv;
gettimeofday(&now, NULL);
if (xhc->last_tv.tv_sec == 0) xhc->last_tv = now;
timersub(&now, &xhc->last_tv, &delta_tv);
float elapsed = delta_tv.tv_sec + 1e-6f*delta_tv.tv_usec;
if (elapsed <= 0) return;
float delta_pos = (*(xhc->hal->jog_counts) - xhc->last_jog_counts) * *(xhc->hal->jog_scale);
float velocity = *(xhc->hal->jog_max_velocity) * 60.0f * *(xhc->hal->jog_scale);
float k = 0.05f;
if (delta_pos) {
*(xhc->hal->jog_velocity) = (1 - k) * *(xhc->hal->jog_velocity) + k * velocity;
*(xhc->hal->jog_increment) = fabs(delta_pos);
*(xhc->hal->jog_plus_x) = (delta_pos > 0) && *(xhc->hal->jog_enable_x);
*(xhc->hal->jog_minus_x) = (delta_pos < 0) && *(xhc->hal->jog_enable_x);
*(xhc->hal->jog_plus_y) = (delta_pos > 0) && *(xhc->hal->jog_enable_y);
*(xhc->hal->jog_minus_y) = (delta_pos < 0) && *(xhc->hal->jog_enable_y);
*(xhc->hal->jog_plus_z) = (delta_pos > 0) && *(xhc->hal->jog_enable_z);
*(xhc->hal->jog_minus_z) = (delta_pos < 0) && *(xhc->hal->jog_enable_z);
*(xhc->hal->jog_plus_a) = (delta_pos > 0) && *(xhc->hal->jog_enable_a);
*(xhc->hal->jog_minus_a) = (delta_pos < 0) && *(xhc->hal->jog_enable_a);
*(xhc->hal->jog_plus_b) = (delta_pos > 0) && *(xhc->hal->jog_enable_b);
*(xhc->hal->jog_minus_b) = (delta_pos < 0) && *(xhc->hal->jog_enable_b);
*(xhc->hal->jog_plus_c) = (delta_pos > 0) && *(xhc->hal->jog_enable_c);
*(xhc->hal->jog_minus_c) = (delta_pos < 0) && *(xhc->hal->jog_enable_c);
xhc->last_jog_counts = *(xhc->hal->jog_counts);
xhc->last_tv = now;
}
else {
*(xhc->hal->jog_velocity) = (1 - k) * *(xhc->hal->jog_velocity);
if (elapsed > 0.25) {
*(xhc->hal->jog_velocity) = 0;
*(xhc->hal->jog_plus_x) = 0;
*(xhc->hal->jog_minus_x) = 0;
*(xhc->hal->jog_plus_y) = 0;
*(xhc->hal->jog_minus_y) = 0;
*(xhc->hal->jog_plus_z) = 0;
*(xhc->hal->jog_minus_z) = 0;
*(xhc->hal->jog_plus_a) = 0;
*(xhc->hal->jog_minus_a) = 0;
*(xhc->hal->jog_plus_b) = 0;
*(xhc->hal->jog_minus_b) = 0;
*(xhc->hal->jog_plus_c) = 0;
*(xhc->hal->jog_minus_c) = 0;
}
}
}
void handle_step(xhc_t *xhc)
{
int _inc_step_status = STEP_NONE;
int _stepsize = *(xhc->hal->stepsize); // Use a local variable to avoid STEP display as 0 on pendant during transitions
if (*(xhc->hal->stepsize_up)) {
_inc_step_status = STEP_UP;
if (*(xhc->hal->stepsize_down)) {
_inc_step_status = STEP_NONE; // none if both pressed
}
} else if (*(xhc->hal->stepsize_down)) {
_inc_step_status = STEP_DOWN;
} else {
_inc_step_status = STEP_NONE;
}
if (_inc_step_status != xhc->old_inc_step_status) {
if (_inc_step_status == STEP_UP) {
stepsize_idx++;
// restart idx when 0 terminator reached:
if (stepsize_sequence[stepsize_idx] == 0) stepsize_idx = 0;
}
if (_inc_step_status == STEP_DOWN) {
stepsize_idx--;
// restart at stepsize_last_idx when stepsize_idx < 0
if (stepsize_idx < 0) stepsize_idx = stepsize_last_idx;
}
_stepsize = stepsize_sequence[stepsize_idx];
}
xhc->old_inc_step_status = _inc_step_status;
*(xhc->hal->stepsize) = _stepsize;
*(xhc->hal->jog_scale) = *(xhc->hal->stepsize) * 0.001f;
}
void cb_response_in(struct libusb_transfer *transfer)
{
int i;
if (!*(xhc.hal->connected)) return;
if (transfer->actual_length > 0) {
if (simu_mode) hexdump(in_buf, transfer->actual_length);
xhc.button_code = in_buf[1];
xhc.axis = (xhc_axis_t)in_buf[3];
*(xhc.hal->jog_counts) += ((signed char)in_buf[4]);
*(xhc.hal->jog_counts_neg) = - *(xhc.hal->jog_counts);
*(xhc.hal->jog_enable_off) = (xhc.axis == axis_off);
*(xhc.hal->jog_enable_x) = (xhc.axis == axis_x);
*(xhc.hal->jog_enable_y) = (xhc.axis == axis_y);
*(xhc.hal->jog_enable_z) = (xhc.axis == axis_z);
*(xhc.hal->jog_enable_a) = (xhc.axis == axis_a);
*(xhc.hal->jog_enable_b) = (xhc.axis == axis_b);
*(xhc.hal->jog_enable_c) = (xhc.axis == axis_c);
*(xhc.hal->jog_enable_feedrate) = (xhc.axis == axis_feed);
*(xhc.hal->jog_enable_spindle) = (xhc.axis == axis_spindle);
for (i=0; i<NB_MAX_BUTTONS; i++) {
if (!xhc.hal->button_pin[i]) continue;
*(xhc.hal->button_pin[i]) = (xhc.button_code == xhc.buttons[i].code);
if (strcmp("button-zero", xhc.buttons[i].pin_name) == 0) {
*(xhc.hal->zero_x) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_x);
*(xhc.hal->zero_y) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_y);
*(xhc.hal->zero_z) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_z);
*(xhc.hal->zero_a) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_a);
*(xhc.hal->zero_b) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_b);
*(xhc.hal->zero_c) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_c);
}
if (strcmp("button-goto-zero", xhc.buttons[i].pin_name) == 0) {
*(xhc.hal->gotozero_x) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_x);
*(xhc.hal->gotozero_y) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_y);
*(xhc.hal->gotozero_z) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_z);
*(xhc.hal->gotozero_a) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_a);
*(xhc.hal->gotozero_b) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_b);
*(xhc.hal->gotozero_c) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_c);
}
if (strcmp("button-half", xhc.buttons[i].pin_name) == 0) {
*(xhc.hal->half_x) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_x);
*(xhc.hal->half_y) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_y);
*(xhc.hal->half_z) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_z);
*(xhc.hal->half_a) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_a);
*(xhc.hal->half_b) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_b);
*(xhc.hal->half_c) = (xhc.button_code == xhc.buttons[i].code) && (xhc.axis == axis_c);
}
if (simu_mode && *(xhc.hal->button_pin[i])) {
printf("%s pressed", xhc.buttons[i].pin_name);
}
}
if (simu_mode) {
if ((signed char)in_buf[4] != 0) printf(" delta %+3d",(signed char)in_buf[4]);
printf("\n");
}
//detect pendant going to sleep (occurs for 18 button pendant)
if ( in_buf[0]==0x04
&& in_buf[1]==0
&& in_buf[2]==0
&& in_buf[3]==0
&& in_buf[4]==0
&& in_buf[5]==0) {
*(xhc.hal->sleeping) = 1;
if (simu_mode) {
struct timeval now;
gettimeofday(&now, NULL);
fprintf(stderr,"Sleep, idle for %ld seconds\n",
now.tv_sec - xhc.last_wakeup.tv_sec);
}
} else {
gettimeofday(&xhc.last_wakeup, NULL);
if (*(xhc.hal->sleeping)) {
if (simu_mode) {
fprintf(stderr,"Wake\n");
}
}
*(xhc.hal->sleeping) = 0;
}
}
libusb_submit_transfer(transfer);
}
void setup_asynch_transfer(libusb_device_handle *dev_handle)
{
libusb_fill_bulk_transfer( transfer_in, dev_handle, (0x1 | LIBUSB_ENDPOINT_IN),
in_buf, sizeof(in_buf),
cb_response_in, NULL, 0); // no user data
libusb_submit_transfer(transfer_in);
}
static void quit(int sig)
{
do_exit = 1;
}
static int hal_pin_simu(char *pin_name, void **ptr, int s)
{
printf("Creating pin: %s\n", pin_name);
*ptr = calloc(s, 1);
return 0;
}
int _hal_pin_float_newf(hal_pin_dir_t dir, hal_float_t ** data_ptr_addr, int comp_id, const char *fmt, ...)
{
char pin_name[256];
va_list args;
va_start(args,fmt);
vsprintf(pin_name, fmt, args);
va_end(args);
if (simu_mode) {
return hal_pin_simu(pin_name, ( void**)data_ptr_addr, sizeof(*data_ptr_addr));
}
else {
return hal_pin_float_new(pin_name, dir, data_ptr_addr, comp_id);
}
}
int _hal_pin_s32_newf(hal_pin_dir_t dir, hal_s32_t ** data_ptr_addr, int comp_id, const char *fmt, ...)
{
char pin_name[256];
va_list args;
va_start(args,fmt);
vsprintf(pin_name, fmt, args);
va_end(args);
if (simu_mode) {
return hal_pin_simu(pin_name, ( void**)data_ptr_addr, sizeof(*data_ptr_addr));
}
else {
return hal_pin_s32_new(pin_name, dir, data_ptr_addr, comp_id);
}
}
int _hal_pin_bit_newf(hal_pin_dir_t dir, hal_bit_t ** data_ptr_addr, int comp_id, const char *fmt, ...)
{
char pin_name[256];
va_list args;
va_start(args,fmt);
vsprintf(pin_name, fmt, args);
va_end(args);
if (simu_mode) {
return hal_pin_simu(pin_name, ( void**)data_ptr_addr, sizeof(*data_ptr_addr));
}
else {
return hal_pin_bit_new(pin_name, dir, data_ptr_addr, comp_id);
}
}
static void hal_setup()
{
int r, i;
if (!simu_mode) {
hal_comp_id = hal_init(modname);
if (hal_comp_id < 1) {
fprintf(stderr, "%s: ERROR: hal_init failed\n", modname);
exit(1);
}
xhc.hal = (xhc_hal_t *)hal_malloc(sizeof(xhc_hal_t));
if (xhc.hal == NULL) {
fprintf(stderr, "%s: ERROR: unable to allocate HAL shared memory\n", modname);
exit(1);
}
}
else {
xhc.hal = (xhc_hal_t *)calloc(sizeof(xhc_hal_t), 1);
}
r = 0;
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->x_mc), hal_comp_id, "%s.x.pos-absolute", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->y_mc), hal_comp_id, "%s.y.pos-absolute", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->z_mc), hal_comp_id, "%s.z.pos-absolute", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->a_mc), hal_comp_id, "%s.a.pos-absolute", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->b_mc), hal_comp_id, "%s.b.pos-absolute", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->c_mc), hal_comp_id, "%s.c.pos-absolute", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->x_wc), hal_comp_id, "%s.x.pos-relative", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->y_wc), hal_comp_id, "%s.y.pos-relative", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->z_wc), hal_comp_id, "%s.z.pos-relative", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->a_wc), hal_comp_id, "%s.a.pos-relative", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->b_wc), hal_comp_id, "%s.b.pos-relative", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->c_wc), hal_comp_id, "%s.c.pos-relative", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->feedrate), hal_comp_id, "%s.feed-value", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->feedrate_override), hal_comp_id, "%s.feed-override", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->spindle_rps), hal_comp_id, "%s.spindle-rps", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->spindle_override), hal_comp_id, "%s.spindle-override", modname);
for (i=0; i<NB_MAX_BUTTONS; i++) {
if (!xhc.buttons[i].pin_name[0]) continue;
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->button_pin[i]), hal_comp_id, "%s.%s", modname, xhc.buttons[i].pin_name);
if (strcmp("button-zero", xhc.buttons[i].pin_name) == 0) {
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->zero_x), hal_comp_id, "%s.%s-x", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->zero_y), hal_comp_id, "%s.%s-y", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->zero_z), hal_comp_id, "%s.%s-z", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->zero_a), hal_comp_id, "%s.%s-a", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->zero_b), hal_comp_id, "%s.%s-b", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->zero_c), hal_comp_id, "%s.%s-c", modname, xhc.buttons[i].pin_name);
}
if (strcmp("button-goto-zero", xhc.buttons[i].pin_name) == 0) {
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->gotozero_x), hal_comp_id, "%s.%s-x", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->gotozero_y), hal_comp_id, "%s.%s-y", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->gotozero_z), hal_comp_id, "%s.%s-z", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->gotozero_a), hal_comp_id, "%s.%s-a", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->gotozero_b), hal_comp_id, "%s.%s-b", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->gotozero_c), hal_comp_id, "%s.%s-c", modname, xhc.buttons[i].pin_name);
}
if (strcmp("button-half", xhc.buttons[i].pin_name) == 0) {
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->half_x), hal_comp_id, "%s.%s-x", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->half_y), hal_comp_id, "%s.%s-y", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->half_z), hal_comp_id, "%s.%s-z", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->half_a), hal_comp_id, "%s.%s-a", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->half_b), hal_comp_id, "%s.%s-b", modname, xhc.buttons[i].pin_name);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->half_c), hal_comp_id, "%s.%s-c", modname, xhc.buttons[i].pin_name);
}
if (strcmp("button-step", xhc.buttons[i].pin_name) == 0) xhc.button_step = xhc.buttons[i].code;
}
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->sleeping), hal_comp_id, "%s.sleeping", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->connected), hal_comp_id, "%s.connected", modname);
r |= _hal_pin_bit_newf(HAL_IN, &(xhc.hal->stepsize_up), hal_comp_id, "%s.stepsize-up", modname);
r |= _hal_pin_bit_newf(HAL_IN, &(xhc.hal->stepsize_down), hal_comp_id, "%s.stepsize-down", modname);
r |= _hal_pin_s32_newf(HAL_OUT, &(xhc.hal->stepsize), hal_comp_id, "%s.stepsize", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->require_pendant), hal_comp_id, "%s.require_pendant", modname);
r |= _hal_pin_bit_newf(HAL_IN, &(xhc.hal->inch_icon), hal_comp_id, "%s.inch-icon", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_enable_off), hal_comp_id, "%s.jog.enable-off", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_enable_x), hal_comp_id, "%s.jog.enable-x", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_enable_y), hal_comp_id, "%s.jog.enable-y", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_enable_z), hal_comp_id, "%s.jog.enable-z", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_enable_a), hal_comp_id, "%s.jog.enable-a", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_enable_b), hal_comp_id, "%s.jog.enable-b", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_enable_c), hal_comp_id, "%s.jog.enable-c", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_enable_feedrate), hal_comp_id, "%s.jog.enable-feed-override", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_enable_spindle), hal_comp_id, "%s.jog.enable-spindle-override", modname);
r |= _hal_pin_float_newf(HAL_OUT, &(xhc.hal->jog_scale), hal_comp_id, "%s.jog.scale", modname);
r |= _hal_pin_s32_newf(HAL_OUT, &(xhc.hal->jog_counts), hal_comp_id, "%s.jog.counts", modname);
r |= _hal_pin_s32_newf(HAL_OUT, &(xhc.hal->jog_counts_neg), hal_comp_id, "%s.jog.counts-neg", modname);
r |= _hal_pin_float_newf(HAL_OUT, &(xhc.hal->jog_velocity), hal_comp_id, "%s.jog.velocity", modname);
r |= _hal_pin_float_newf(HAL_IN, &(xhc.hal->jog_max_velocity), hal_comp_id, "%s.jog.max-velocity", modname);
r |= _hal_pin_float_newf(HAL_OUT, &(xhc.hal->jog_increment), hal_comp_id, "%s.jog.increment", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_plus_x), hal_comp_id, "%s.jog.plus-x", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_minus_x), hal_comp_id, "%s.jog.minus-x", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_plus_y), hal_comp_id, "%s.jog.plus-y", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_minus_y), hal_comp_id, "%s.jog.minus-y", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_plus_z), hal_comp_id, "%s.jog.plus-z", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_minus_z), hal_comp_id, "%s.jog.minus-z", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_plus_a), hal_comp_id, "%s.jog.plus-a", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_minus_a), hal_comp_id, "%s.jog.minus-a", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_plus_b), hal_comp_id, "%s.jog.plus-b", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_minus_b), hal_comp_id, "%s.jog.minus-b", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_plus_c), hal_comp_id, "%s.jog.plus-c", modname);
r |= _hal_pin_bit_newf(HAL_OUT, &(xhc.hal->jog_minus_c), hal_comp_id, "%s.jog.minus-c", modname);
return;
}
int read_ini_file(char *filename)
{
FILE *fd = fopen(filename, "r");
const char *bt;
int nb_buttons = 0;
if (!fd) {
perror(filename);
return -1;
}
IniFile f(false, fd);
while ( (bt = f.Find("BUTTON", section, nb_buttons+1)) && nb_buttons < NB_MAX_BUTTONS) {
if (sscanf(bt, "%x:%s", &xhc.buttons[nb_buttons].code, xhc.buttons[nb_buttons].pin_name) !=2 ) {
fprintf(stderr, "%s: syntax error\n", bt);
return -1;
}
nb_buttons++;
}
return 0;
}
#define STRINGIFY_IMPL(S) #S
#define STRINGIFY(s) STRINGIFY_IMPL(s)
static void Usage(char *name)
{
fprintf(stderr, "%s version %s by Frederic RIBLE (frible@teaser.fr)\n", name, PACKAGE_VERSION);
fprintf(stderr, "\n");
fprintf(stderr, "Usage: %s [-I button-cfg-file] [-h] [-H] [-s n]\n", name);
fprintf(stderr, " -I button-cfg-file: configuration file defining the MPG keyboard layout\n");
fprintf(stderr, " -h: usage (this)\n");
fprintf(stderr, " -H: run in real-time HAL mode (run in simulation mode by default)\n");
fprintf(stderr, " -x: wait for pendant detection before creating HAL pins\n");
fprintf(stderr, " -s: step sequence (multiplied by 0.001 unit):\n");
fprintf(stderr, " 1: 1,10,100,1000 (default)\n");
fprintf(stderr, " 2: 1,5,10,20\n");
fprintf(stderr, " 3: 1,10,100\n");
fprintf(stderr, " 4: 1,5,10,20,50,100\n");
fprintf(stderr, " 5: 1,10,50,100,1000\n");
fprintf(stderr, "\n");
fprintf(stderr, "Configuration file section format:\n");
fprintf(stderr, "[XHC-HB04]\n");
fprintf(stderr, "BUTTON=XN:button-thenameN\n");
fprintf(stderr, "...\n");
fprintf(stderr, " where XN=hexcode, button-thenameN=nameforbutton\n");
}
int main (int argc,char **argv)
{
libusb_device **devs;
libusb_device_handle *dev_handle;
libusb_context *ctx = NULL;
int r,idx;
ssize_t cnt;
#define MAX_WAIT_SECS 10
int wait_secs = 0;
int opt;
bool hal_ready_done = false;
init_xhc(&xhc);
while ((opt = getopt(argc, argv, "HhI:xs:")) != -1) {
switch (opt) {
case 'I':
if (read_ini_file(optarg)) {
printf("Problem reading ini file: %s\n\n",optarg);
Usage(argv[0]);
exit(EXIT_FAILURE);
}
break;
case 'H':
simu_mode = false;
break;
case 's':
switch (optarg[0]) {
case '1': stepsize_sequence = stepsize_sequence_1;break;
case '2': stepsize_sequence = stepsize_sequence_2;break;
case '3': stepsize_sequence = stepsize_sequence_3;break;
case '4': stepsize_sequence = stepsize_sequence_4;break;
case '5': stepsize_sequence = stepsize_sequence_5;break;
default:
printf("Unknown sequence: %s\n\n",optarg);
Usage(argv[0]);
exit(EXIT_FAILURE);
break;
}
break;
case 'x':
wait_for_pendant_before_HAL = true;
break;
default:
Usage(argv[0]);
exit(EXIT_FAILURE);
}
}
// compute the last valid idx for use with stepsize-down
for (idx=0; idx < MAX_STEPSIZE_SEQUENCE; idx++) {
if (stepsize_sequence[idx] == 0) break;
}
stepsize_last_idx = idx - 1;
hal_setup();
signal(SIGINT, quit);
signal(SIGTERM, quit);
if (!wait_for_pendant_before_HAL && !simu_mode) {
hal_ready(hal_comp_id);
hal_ready_done = true;
}
while (!do_exit) {
//on reconnect wait for device to be gone
if (do_reconnect == 1) {
sleep(5);
do_reconnect = 0;
}
r = libusb_init(&ctx);
if(r < 0) {
perror("libusb_init");
return 1;
}
#if LIBUSB_API_VERSION >= 0x01000106
libusb_set_option(ctx, LIBUSB_OPTION_LOG_LEVEL, 2);
#else
libusb_set_debug(ctx, 2);
#endif
// use environmental variable LIBUSB_DEBUG if needed
printf("%s: waiting for XHC-HB04 device\n",modname);
*(xhc.hal->connected) = 0;
wait_secs = 0;
*(xhc.hal->require_pendant) = wait_for_pendant_before_HAL;
*(xhc.hal->stepsize) = stepsize_sequence[0];
do {
cnt = libusb_get_device_list(ctx, &devs);
if (cnt < 0) {
perror("libusb_get_device_list");
return 1;
}
dev_handle = libusb_open_device_with_vid_pid(ctx, 0x10CE, 0xEB70);
libusb_free_device_list(devs, 1);
if (dev_handle == NULL) {
if (wait_for_pendant_before_HAL) {
wait_secs++;
if (wait_secs >= MAX_WAIT_SECS/2) {
printf("%s: waiting for XHC-HB04 device (%d)\n",modname,wait_secs);
}
if (wait_secs > MAX_WAIT_SECS) {
printf("%s: MAX_WAIT_SECS exceeded, exiting\n",modname);
exit(1);
}
}
sleep(1);
}
} while(dev_handle == NULL && !do_exit);
if (dev_handle) printf("%s: found XHC-HB04 device\n",modname);
if (dev_handle) {
if (libusb_kernel_driver_active(dev_handle, 0) == 1) {
libusb_detach_kernel_driver(dev_handle, 0);
}
r = libusb_claim_interface(dev_handle, 0);
if (r < 0) {
perror("libusb_claim_interface");
return 1;
}
//allocate the transfer struct here only once after successful connection
transfer_in = libusb_alloc_transfer(0);
}
*(xhc.hal->connected) = 1;
if (!hal_ready_done && !simu_mode) {
hal_ready(hal_comp_id);
hal_ready_done = true;
}
if (dev_handle) {
setup_asynch_transfer(dev_handle);
xhc_set_display(dev_handle, &xhc);
}
if (dev_handle) {
while (!do_exit && !do_reconnect) {
struct timeval tv;
tv.tv_sec = 0;
tv.tv_usec = 30000;
r = libusb_handle_events_timeout(ctx, &tv);
compute_velocity(&xhc);
if (simu_mode) linuxcnc_simu(&xhc);
handle_step(&xhc);
xhc_set_display(dev_handle, &xhc);
}
*(xhc.hal->connected) = 0;
printf("%s: connection lost, cleaning up\n",modname);
if (*(xhc.hal->require_pendant)) {
do_exit = 1;
}
libusb_cancel_transfer(transfer_in); // ignore result
assert (0 == libusb_handle_events_completed(ctx, nullptr));
libusb_free_transfer(transfer_in);
r = libusb_release_interface(dev_handle, 0);
assert (r == 0 || r == LIBUSB_ERROR_NO_DEVICE);
libusb_close(dev_handle);
} else {
while (!do_exit) usleep(70000);
}
libusb_exit(ctx);
} // while (!do_exit)
}