You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1461 lines
48 KiB
1461 lines
48 KiB
/***************************************************************************************
|
|
*
|
|
* posctl.cc is part of SimpleSkyCam.
|
|
*
|
|
*****************************************************************************************/
|
|
|
|
#include <list>
|
|
#include <string>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <math.h>
|
|
#include "gui.h"
|
|
#include "pid.h"
|
|
#include "detect.h"
|
|
#include "configuration.h"
|
|
#include "video.h"
|
|
#include "videodev.h"
|
|
#include "histogram.h"
|
|
#include "error.h"
|
|
|
|
#ifdef BUILD_WINDOWS
|
|
#include "windows.h"
|
|
#endif
|
|
|
|
extern int video_enterdata;
|
|
extern PosCtl posctl;
|
|
extern Simulation simulation;
|
|
|
|
|
|
extern GtkBuilder *_builder_; // work around for threads
|
|
GtkWidget *posctl_rot_da = NULL;
|
|
GtkWidget *posctl_axis1_da = NULL;
|
|
GtkWidget *posctl_axis2_da = NULL;
|
|
double linedash1[] = { 1.0, 4.0 };
|
|
|
|
|
|
/*
|
|
* axis history chart.
|
|
*/
|
|
#define AXIS_HISTORY_CNT 400
|
|
struct AxisHistory {
|
|
double diff;
|
|
double out;
|
|
struct timeval ts;
|
|
};
|
|
struct AxisHistory axis_history[2][AXIS_HISTORY_CNT];
|
|
int axis_history_pos[2] = { 0, 0 };
|
|
|
|
struct PosCtl_ThreadData posctl_threaddata;
|
|
|
|
void axis_history_add(int axis, double diff, double out) {
|
|
if (axis > 1 || axis < 0) return;
|
|
|
|
axis_history_pos[axis]++;
|
|
if (axis_history_pos[axis] < 0) axis_history_pos[axis] = 0;
|
|
if (axis_history_pos[axis] >= AXIS_HISTORY_CNT) axis_history_pos[axis] = 0;
|
|
|
|
gettimeofday(&axis_history[axis][axis_history_pos[axis]].ts, NULL);
|
|
axis_history[axis][axis_history_pos[axis]].diff = diff;
|
|
axis_history[axis][axis_history_pos[axis]].out = out;
|
|
}
|
|
|
|
#define CALIB_DURATION_DELTA 120.0 // max time in sec for the detection of rotation movement
|
|
#define CALIB_DURATION_AXIS 120.0 // max time in sec for the detection of axis movement
|
|
#define CALIB_DURATION_DIST 20.0 // max distance to detect movement
|
|
#define CALIB_STARTSTOP_DELAY 2.5 // time to wait for start and stop
|
|
|
|
// axis definitions
|
|
#define AXIS_RA 1
|
|
#define AXIS_DEC 0
|
|
|
|
void posctl_gui_update();
|
|
|
|
void cb_posctl_show_window (GtkWidget *widget, gpointer data) {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
GtkWidget *wnd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "window-posctl"));
|
|
gtk_widget_show(wnd);
|
|
}
|
|
|
|
|
|
void cb_posctl_btncalib (GtkWidget *widget, gpointer data) {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
// GtkWidget *btn = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib"));
|
|
// gtk_widget_set_sensitive(btn, false);
|
|
|
|
posctl.StartCalibration();
|
|
posctl_gui_update();
|
|
}
|
|
|
|
|
|
void cb_posctl_btnstop (GtkWidget *widget, gpointer data) {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
posctl.Stop();
|
|
posctl_gui_update();
|
|
}
|
|
|
|
|
|
void cb_posctl_btncontrol (GtkWidget *widget, gpointer data) {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
posctl.StartControl();
|
|
posctl_gui_update();
|
|
}
|
|
|
|
|
|
void cb_menu_set_displaycalibdata(GtkCheckMenuItem *checkmenuitem, gpointer user_data) {
|
|
if (gtk_check_menu_item_get_active(checkmenuitem) == TRUE) config.SetCalibrationShowData(true);
|
|
else config.SetCalibrationShowData(false);
|
|
}
|
|
|
|
|
|
void cb_posctl_show (GtkWidget *widget, gpointer data) {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
posctl_threaddata.calib_mode = -1;
|
|
posctl_threaddata.target_pos.x = -1;
|
|
posctl_threaddata.target_pos.y = -1;
|
|
posctl_gui_update();
|
|
}
|
|
|
|
|
|
void cb_posctl_btnsetdest (GtkWidget *widget, gpointer data) {
|
|
video_enterdata = VID_ENTERDATA_SETDEST;
|
|
};
|
|
|
|
|
|
/*
|
|
* calibrate adjust control output..
|
|
*/
|
|
void cb_posctl_btn2steprun (GtkWidget *widget, gpointer data) {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
posctl.Run2Step();
|
|
};
|
|
|
|
|
|
void cb_posctl_btn2stepreset (GtkWidget *widget, gpointer data) {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
posctl.Reset2Step();
|
|
|
|
}
|
|
|
|
|
|
|
|
void cb_posctl_btn_axismove (GtkWidget *widget, gpointer data) {
|
|
GtkWidget *btn_ramin = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_ra_min"));
|
|
GtkWidget *btn_racenter = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_ra_center"));
|
|
GtkWidget *btn_ramax = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_ra_max"));
|
|
GtkWidget *btn_decmin = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_dec_min"));
|
|
GtkWidget *btn_deccenter = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_dec_center"));
|
|
GtkWidget *btn_decmax = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_dec_max"));
|
|
double ramin, decmin, ramax, decmax;
|
|
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
posctl.GetAxisParam(AXIS_RA, &ramin, &ramax, NULL, NULL, NULL);
|
|
posctl.GetAxisParam(AXIS_DEC, &decmin, &decmax, NULL, NULL, NULL);
|
|
|
|
if (widget == btn_ramin) posctl.OutputWriteValue(AXIS_RA, ramin);
|
|
else if (widget == btn_racenter) posctl.OutputWriteValue(AXIS_RA, (ramax-ramin)/2.0+ramin);
|
|
else if (widget == btn_ramax) posctl.OutputWriteValue(AXIS_RA, ramax);
|
|
else if (widget == btn_decmin) posctl.OutputWriteValue(AXIS_DEC, decmin);
|
|
else if (widget == btn_deccenter) posctl.OutputWriteValue(AXIS_DEC, (decmax-decmin)/2.0+decmin);
|
|
else if (widget == btn_decmax) posctl.OutputWriteValue(AXIS_DEC, decmax);
|
|
|
|
posctl_gui_update();
|
|
}
|
|
|
|
|
|
void cb_posctl_change_entry (GtkWidget *widget, gpointer data) {
|
|
double ramin, ramax, rap, rai, rad;
|
|
double decmin, decmax, decp, deci, decd;
|
|
|
|
GtkWidget *e_posdevice = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_device"));
|
|
GtkWidget *ra_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_min"));
|
|
GtkWidget *ra_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_max"));
|
|
GtkWidget *ra_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_kp"));
|
|
GtkWidget *ra_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_ki"));
|
|
GtkWidget *ra_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_kd"));
|
|
GtkWidget *dec_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_min"));
|
|
GtkWidget *dec_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_max"));
|
|
GtkWidget *dec_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_kp"));
|
|
GtkWidget *dec_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_ki"));
|
|
GtkWidget *dec_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_kd"));
|
|
GtkWidget *resetsim = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_resetsim"));
|
|
GtkWidget *inputfilter = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_inputfilter"));
|
|
|
|
const char *s;
|
|
|
|
posctl.GetAxisParam(AXIS_RA, &ramin, &ramax, &rap, &rai, &rad);
|
|
posctl.GetAxisParam(AXIS_DEC, &decmin, &decmax, &decp, &deci, &decd);
|
|
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
s = gtk_entry_get_text(GTK_ENTRY(widget));
|
|
if (strncmp(s, "SIMULATION", 11) == 0) gtk_widget_show (resetsim);
|
|
else gtk_widget_hide (resetsim);
|
|
|
|
posctl.LockMutex();
|
|
if (e_posdevice == widget) posctl.SetDevice(s);
|
|
else if (ra_min == widget) ramin = atof(s);
|
|
else if (ra_max == widget) ramax = atof(s);
|
|
else if (ra_kp == widget) rap = atof(s);
|
|
else if (ra_ki == widget) rai = atof(s);
|
|
else if (ra_kd == widget) rad = atof(s);
|
|
else if (dec_min == widget) decmin = atof(s);
|
|
else if (dec_max == widget) decmax = atof(s);
|
|
else if (dec_kp == widget) decp = atof(s);
|
|
else if (dec_ki == widget) deci = atof(s);
|
|
else if (dec_kd == widget) decd = atof(s);
|
|
else if (inputfilter == widget) posctl.SetFilter(atof(s));
|
|
posctl.SetAxisParam(AXIS_RA, ramin, ramax, rap, rai, rad);
|
|
posctl.SetAxisParam(AXIS_DEC, decmin, decmax, decp, deci, decd);
|
|
|
|
posctl.UnLockMutex();
|
|
}
|
|
|
|
|
|
/*
|
|
* draw position relative to target position.
|
|
* draw the axis vectors form the center of the screen.
|
|
* draw the earch movement to the center of the screen.
|
|
*/
|
|
void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) {
|
|
int clientw, clienth;
|
|
position_f_2d lpos, p;
|
|
position_2d center;
|
|
double pscale = 2.0;
|
|
|
|
//
|
|
// rotation da
|
|
GtkWidget *entry_rot_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotangle"));
|
|
GtkWidget *entry_rot_len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotlen"));
|
|
GtkWidget *entry_axis1_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_angle"));
|
|
GtkWidget *entry_axis1_len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_len"));
|
|
GtkWidget *entry_axis2_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_angle"));
|
|
GtkWidget *entry_axis2_len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_len"));
|
|
|
|
float rotangle = atof(gtk_entry_get_text(GTK_ENTRY(entry_rot_angle))) * (M_PI / 180);
|
|
float rotlen = atof(gtk_entry_get_text(GTK_ENTRY(entry_rot_len)));
|
|
float ra_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_angle))) * (M_PI / 180);
|
|
float ra_len = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_len)));
|
|
float dec_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_angle))) * (M_PI / 180);
|
|
float dec_len = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_len)));
|
|
|
|
if (posctl_rot_da == NULL) // should only be called once
|
|
posctl_rot_da = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_rotation"));
|
|
clienth = gtk_widget_get_allocated_height(posctl_rot_da);
|
|
clientw = gtk_widget_get_allocated_width(posctl_rot_da);
|
|
center.x = clientw >> 1;
|
|
center.y = clienth >> 1;
|
|
|
|
//
|
|
// draw the background
|
|
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
|
|
cairo_paint(cr);
|
|
cairo_fill (cr);
|
|
cairo_set_source_rgb(cr, 0.8, 0.8, 0.8);
|
|
cairo_rectangle (cr, 1, 1, clientw - 2, clienth - 2);
|
|
cairo_fill (cr);
|
|
|
|
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
|
|
cairo_set_dash(cr, linedash1, 2, 0);
|
|
cairo_move_to(cr, center.x, center.y - center.y * 0.8);
|
|
cairo_line_to(cr, center.x, center.y + center.y * 0.8);
|
|
cairo_stroke(cr);
|
|
|
|
cairo_move_to(cr, center.x - center.x * 0.8, center.y);
|
|
cairo_line_to(cr, center.x + center.x * 0.8, center.y);
|
|
cairo_stroke(cr);
|
|
cairo_set_dash(cr, NULL, 0, 0);
|
|
|
|
//
|
|
// draw
|
|
cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
|
|
cairo_set_font_size (cr, 11);
|
|
|
|
// rot
|
|
if (!isnan(rotangle) && !isnan(rotlen)) {
|
|
lpos.x = -sin(rotangle) * rotlen * 4.0;
|
|
lpos.y = -cos(rotangle) * rotlen * 4.0;
|
|
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
|
|
cairo_set_line_width(cr, 4.0);
|
|
cairo_move_to(cr, center.x, center.y);
|
|
cairo_line_to(cr, center.x + lpos.x, center.y + lpos.y);
|
|
cairo_stroke(cr);
|
|
cairo_set_line_width(cr, 0.5);
|
|
cairo_move_to(cr, center.x - lpos.x * 100.0, center.y - lpos.y * 100.0);
|
|
cairo_line_to(cr, center.x + lpos.x * 100.0, center.y + lpos.y * 100.0);
|
|
cairo_stroke(cr);
|
|
}
|
|
|
|
// axis1
|
|
if (!isnan(ra_angle) && !isnan(ra_len)) {
|
|
lpos.x = sin(ra_angle) * ra_len * 4.0;
|
|
lpos.y = cos(ra_angle) * ra_len * 4.0;
|
|
cairo_set_source_rgb(cr, 0.0, 0.75, 0.3);
|
|
cairo_set_line_width(cr, 4.0);
|
|
cairo_move_to(cr, center.x, center.y);
|
|
cairo_line_to(cr, center.x + lpos.x, center.y + lpos.y);
|
|
cairo_stroke(cr);
|
|
cairo_set_line_width(cr, 0.5);
|
|
cairo_move_to(cr, center.x - lpos.x * 100.0, center.y - lpos.y * 100.0);
|
|
cairo_line_to(cr, center.x + lpos.x * 100.0, center.y + lpos.y * 100.0);
|
|
cairo_stroke(cr);
|
|
}
|
|
|
|
// axis2
|
|
if (!isnan(dec_angle) && !isnan(dec_len)) {
|
|
lpos.x = sin(dec_angle) * dec_len * 4.0;
|
|
lpos.y = cos(dec_angle) * dec_len * 4.0;
|
|
cairo_set_source_rgb(cr, 0.0, 0.3, 0.75);
|
|
cairo_set_line_width(cr, 4.0);
|
|
cairo_move_to(cr, center.x, center.y);
|
|
cairo_line_to(cr, center.x + lpos.x, center.y + lpos.y);
|
|
cairo_stroke(cr);
|
|
cairo_set_line_width(cr, 0.5);
|
|
cairo_move_to(cr, center.x - lpos.x * 100.0, center.y - lpos.y * 100.0);
|
|
cairo_line_to(cr, center.x + lpos.x * 100.0, center.y + lpos.y * 100.0);
|
|
cairo_stroke(cr);
|
|
}
|
|
|
|
//
|
|
// draw position relative to the target
|
|
p = posctl.current_pos - posctl.target_pos;
|
|
p.x *= pscale;
|
|
p.y *= pscale;
|
|
cairo_set_source_rgb(cr, 0.8, 0.1, 0.1);
|
|
cairo_set_line_width(cr, 1.0);
|
|
cairo_move_to(cr, center.x-4 + p.x, center.y + p.y);
|
|
cairo_line_to(cr, center.x+4 + p.x, center.y + p.y);
|
|
cairo_stroke(cr);
|
|
cairo_move_to(cr, center.x + p.x, center.y-4 + p.y);
|
|
cairo_line_to(cr, center.x + p.x, center.y+4 + p.y);
|
|
cairo_stroke(cr);
|
|
cairo_move_to(cr, center.x+4 + p.x, center.y-4 + p.y);
|
|
cairo_line_to(cr, center.x-4 + p.x, center.y+4 + p.y);
|
|
cairo_stroke(cr);
|
|
cairo_move_to(cr, center.x+4 + p.x, center.y+4 + p.y);
|
|
cairo_line_to(cr, center.x-4 + p.x, center.y-4 + p.y);
|
|
cairo_stroke(cr);
|
|
|
|
// draw distance to axis
|
|
if (!isnan(posctl.calib_axis1.a) && !isnan(posctl.axis_pv[AXIS_DEC])) {
|
|
lpos.x = sindeg(posctl.calib_axis1.a+90.0) * posctl.axis_pv[AXIS_DEC] * pscale;
|
|
lpos.y = cosdeg(posctl.calib_axis1.a+90.0) * posctl.axis_pv[AXIS_DEC] * pscale;
|
|
cairo_set_source_rgb(cr, 0.5, 1.0, 0.5);
|
|
cairo_move_to(cr, center.x + p.x, center.y + p.y);
|
|
cairo_line_to(cr, center.x + p.x + lpos.x, center.y + p.y + lpos.y);
|
|
cairo_stroke(cr);
|
|
}
|
|
|
|
// draw distance to axis
|
|
if (!isnan(posctl.calib_axis2.a) && !isnan(posctl.axis_pv[AXIS_RA])) {
|
|
lpos.x = sindeg(posctl.calib_axis2.a+90.0) * posctl.axis_pv[AXIS_RA] * pscale;
|
|
lpos.y = cosdeg(posctl.calib_axis2.a+90.0) * posctl.axis_pv[AXIS_RA] * pscale;
|
|
cairo_set_source_rgb(cr, 0.5, 0.5, 1.0);
|
|
cairo_move_to(cr, center.x + p.x, center.y + p.y);
|
|
cairo_line_to(cr, center.x + p.x + lpos.x, center.y + p.y + lpos.y);
|
|
cairo_stroke(cr);
|
|
}
|
|
};
|
|
|
|
|
|
void cb_posctl_entryanglelen (GtkWidget *widget, gpointer data) {
|
|
if (posctl_rot_da == NULL) // should only be called once
|
|
posctl_rot_da = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_rotation"));
|
|
gdk_window_invalidate_rect(gtk_widget_get_window(posctl_rot_da), NULL, true);
|
|
};
|
|
|
|
|
|
#define AXIS_DIFF_MIN -20.0
|
|
#define AXIS_DIFF_MAX 20.0
|
|
|
|
void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) {
|
|
GtkWidget *dra = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_ra"));
|
|
GtkWidget *ddec = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_dec"));
|
|
|
|
position_2d center;
|
|
int axis = 0 , i, cnt;
|
|
double dx, dy;
|
|
double aoutmin[2];
|
|
double aoutmax[2];
|
|
char txt[255];
|
|
int clienth = gtk_widget_get_allocated_height(area);
|
|
int clientw = gtk_widget_get_allocated_width(area);
|
|
|
|
center.x = clientw >> 1;
|
|
center.y = clienth >> 1;
|
|
|
|
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
|
|
cairo_paint(cr);
|
|
cairo_fill (cr);
|
|
cairo_set_source_rgb(cr, 0.8, 0.8, 0.8);
|
|
cairo_rectangle (cr, 1, 1, clientw - 2, clienth - 2);
|
|
cairo_fill (cr);
|
|
|
|
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
|
|
cairo_set_dash(cr, linedash1, 2, 0);
|
|
cairo_move_to(cr, center.x, center.y - center.y * 0.8);
|
|
cairo_line_to(cr, center.x, clienth);
|
|
cairo_stroke(cr);
|
|
cairo_set_dash(cr, NULL, 0, 0);
|
|
|
|
if (dra == area) axis = 1;
|
|
else if (ddec == area) axis = 0;
|
|
else {
|
|
cairo_move_to(cr, 30, 0);
|
|
cairo_show_text(cr, (char *)"unknown");
|
|
return;
|
|
}
|
|
posctl.GetAxisParam(axis, &aoutmin[axis], &aoutmax[axis], NULL, NULL, NULL);
|
|
|
|
// draw diff
|
|
cairo_set_source_rgb(cr, 1.0, 0.5, 0.5);
|
|
cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
|
|
cairo_set_font_size (cr, 10);
|
|
cairo_move_to(cr, 10, 10);
|
|
cairo_show_text(cr, (char *)"diff");
|
|
// draw elements diff
|
|
i = axis_history_pos[axis];
|
|
cnt = 0;
|
|
do {
|
|
dx = ((double) clientw) * (axis_history[axis][i].diff - AXIS_DIFF_MIN) /
|
|
(AXIS_DIFF_MAX-AXIS_DIFF_MIN);
|
|
dy = ((double) clienth) * ((double)cnt) / ((double) AXIS_HISTORY_CNT);
|
|
|
|
if (dx >= clientw) dx = clientw-1;
|
|
if (dy >= clienth) dy = clienth-1;
|
|
if (dx < 0) dx = 0;
|
|
if (dy < 0) dy = 0;
|
|
|
|
if (cnt == 0) cairo_move_to(cr, (int)dx, int(dy));
|
|
else cairo_line_to(cr, (int)dx, int(dy));
|
|
|
|
cnt++;
|
|
i--;
|
|
if (i < 0) i = AXIS_HISTORY_CNT-1;
|
|
} while (i != axis_history_pos[axis]);
|
|
cairo_stroke(cr);
|
|
|
|
if (axis == 0) cairo_set_source_rgb(cr, 0.0, 0.75, 0.3);
|
|
else cairo_set_source_rgb(cr, 0.0, 0.3, 0.75);
|
|
cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
|
|
cairo_set_font_size (cr, 10);
|
|
cairo_move_to(cr, 60, 10);
|
|
cairo_show_text(cr, (char *)"out");
|
|
|
|
// draw elements diff
|
|
i = axis_history_pos[axis];
|
|
cnt = 0;
|
|
do {
|
|
dx = ((double) clientw) * (axis_history[axis][i].out - aoutmin[axis]) /
|
|
(aoutmax[axis]-aoutmin[axis]);
|
|
dy = ((double) clienth) * ((double)cnt) / ((double) AXIS_HISTORY_CNT);
|
|
|
|
if (dx >= clientw) dx = clientw-1;
|
|
if (dy >= clienth) dy = clienth-1;
|
|
if (dx < 0) dx = 1;
|
|
if (dy < 0) dy = 1;
|
|
|
|
if (cnt == 0) cairo_move_to(cr, (int)dx, int(dy));
|
|
else cairo_line_to(cr, (int)dx, int(dy));
|
|
|
|
cnt++;
|
|
i--;
|
|
if (i < 0) i = AXIS_HISTORY_CNT-1;
|
|
} while (i != axis_history_pos[axis]);
|
|
cairo_stroke(cr);
|
|
|
|
// draw min and max
|
|
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
|
|
cairo_move_to(cr, 0, clienth-7);
|
|
snprintf (txt, 255, "%2.2f", aoutmin[axis]);
|
|
cairo_show_text(cr, txt);
|
|
cairo_move_to(cr, clientw-25, clienth-7);
|
|
snprintf (txt, 255, "%2.2f", aoutmax[axis]);
|
|
cairo_show_text(cr, txt);
|
|
cairo_stroke(cr);
|
|
};
|
|
|
|
|
|
/*
|
|
* add result to the table
|
|
*/
|
|
void posctl_2step_gui_update(struct PosCtl_2Step_Data *d) {
|
|
|
|
GtkWidget *lramin = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_ramin"));
|
|
GtkWidget *lramax = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_ramax"));
|
|
GtkWidget *ldecmin = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_decmin"));
|
|
GtkWidget *ldecmax = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_decmax"));
|
|
GtkWidget *lraout = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_raout"));
|
|
GtkWidget *ldecout = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_decout"));
|
|
GtkWidget *btn = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_2steprun"));
|
|
|
|
if (d->mode == POSCTL_MODE_2STEPWAIT) gtk_widget_set_sensitive(btn, FALSE);
|
|
else gtk_widget_set_sensitive(btn, TRUE);
|
|
|
|
char txt[255];
|
|
// min
|
|
snprintf (txt, 255, "%2.5f [%2.5f]", d->npv[AXIS_RA], d->nop[AXIS_RA]);
|
|
gtk_label_set_text(GTK_LABEL(lramin), txt);
|
|
snprintf (txt, 255, "%2.5f [%2.5f]", d->npv[AXIS_DEC], d->nop[AXIS_DEC]);
|
|
gtk_label_set_text(GTK_LABEL(ldecmin), txt);
|
|
|
|
// max
|
|
snprintf (txt, 255, "%2.5f [%2.5f]", d->ppv[AXIS_RA], d->pop[AXIS_RA]);
|
|
gtk_label_set_text(GTK_LABEL(lramax), txt);
|
|
snprintf (txt, 255, "%2.5f [%2.5f]", d->ppv[AXIS_DEC], d->pop[AXIS_DEC]);
|
|
gtk_label_set_text(GTK_LABEL(ldecmax), txt);
|
|
|
|
// output
|
|
snprintf (txt, 255, "%2.5f [%2.5f]", d->pv[AXIS_RA], d->op[AXIS_RA]);
|
|
gtk_label_set_text(GTK_LABEL(lraout), txt);
|
|
snprintf (txt, 255, "%2.5f [%2.5f]", d->pv[AXIS_DEC], d->op[AXIS_DEC]);
|
|
gtk_label_set_text(GTK_LABEL(ldecout), txt);
|
|
|
|
cb_posctl_entryanglelen(NULL, NULL);
|
|
}
|
|
|
|
|
|
/*
|
|
* posctl gui update
|
|
*/
|
|
void posctl_gui_update() {
|
|
char txt[255];
|
|
double kp, ki, kd, mi, ma;
|
|
|
|
GtkWidget *caliblabel = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_calib_label"));
|
|
|
|
GtkWidget *btnclib = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib"));
|
|
GtkWidget *btnstop = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_stop"));
|
|
GtkWidget *btncontrol = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_control"));
|
|
GtkWidget *btnsimreset = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_resetsim"));
|
|
|
|
GtkWidget *e_cal_rotangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotangle"));
|
|
GtkWidget *e_cal_rotlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotlen"));
|
|
|
|
GtkWidget *e_cal_raangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_angle"));
|
|
GtkWidget *e_cal_ralen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_len"));
|
|
|
|
GtkWidget *e_cal_decangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_angle"));
|
|
GtkWidget *e_cal_declen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_len"));
|
|
GtkWidget *lb_earthaxis = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_earthaxis"));
|
|
GtkWidget *lb_axisaxis = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_axisaxis"));
|
|
|
|
GtkWidget *e_posdevice = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_device"));
|
|
GtkWidget *btn_ramin = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_ra_min"));
|
|
GtkWidget *btn_racenter = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_ra_center"));
|
|
GtkWidget *btn_ramax = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_ra_max"));
|
|
GtkWidget *btn_decmin = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_dec_min"));
|
|
GtkWidget *btn_deccenter = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_dec_center"));
|
|
GtkWidget *btn_decmax = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_dec_max"));
|
|
|
|
|
|
GtkWidget *ra_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_min"));
|
|
GtkWidget *ra_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_max"));
|
|
GtkWidget *ra_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_kp"));
|
|
GtkWidget *ra_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_ki"));
|
|
GtkWidget *ra_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_ra_kd"));
|
|
GtkWidget *ra_out = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_out_ra"));
|
|
GtkWidget *ra_pv = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_d_ra"));
|
|
GtkWidget *dec_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_min"));
|
|
GtkWidget *dec_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_max"));
|
|
GtkWidget *dec_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_kp"));
|
|
GtkWidget *dec_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_ki"));
|
|
GtkWidget *dec_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_dec_kd"));
|
|
GtkWidget *dec_out = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_out_dec"));
|
|
GtkWidget *dec_pv = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_d_dec"));
|
|
|
|
GtkWidget *pos_filter = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_inputfilter"));
|
|
GtkWidget *btn = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_2steprun"));
|
|
|
|
posctl.LockMutex();
|
|
int m = posctl.GetMode();
|
|
|
|
if (m == POSCTL_MODE_2STEPWAIT) gtk_widget_set_sensitive(btn, FALSE);
|
|
else gtk_widget_set_sensitive(btn, TRUE);
|
|
|
|
if (m == POSCTL_MODE_OFF) {
|
|
gtk_widget_set_sensitive(btnclib, true);
|
|
gtk_widget_set_sensitive(btncontrol, true);
|
|
gtk_widget_set_sensitive(btnstop, false);
|
|
gtk_widget_set_sensitive(btn_ramin, true);
|
|
gtk_widget_set_sensitive(btn_racenter, true);
|
|
gtk_widget_set_sensitive(btn_ramax, true);
|
|
gtk_widget_set_sensitive(btn_decmin, true);
|
|
gtk_widget_set_sensitive(btn_deccenter, true);
|
|
gtk_widget_set_sensitive(btn_decmax, true);
|
|
gtk_widget_set_sensitive(e_posdevice, true);
|
|
gtk_label_set_label(GTK_LABEL(caliblabel), (char *) "");
|
|
}
|
|
else {
|
|
if (m == POSCTL_MODE_CALIB) {
|
|
gtk_label_set_label(GTK_LABEL(caliblabel), (char *) "running");
|
|
}
|
|
gtk_widget_set_sensitive(btnclib, false);
|
|
gtk_widget_set_sensitive(btncontrol, false);
|
|
gtk_widget_set_sensitive(btnstop, true);
|
|
gtk_widget_set_sensitive(btn_ramin, false);
|
|
gtk_widget_set_sensitive(btn_racenter, false);
|
|
gtk_widget_set_sensitive(btn_ramax, false);
|
|
gtk_widget_set_sensitive(btn_decmin, false);
|
|
gtk_widget_set_sensitive(btn_deccenter, false);
|
|
gtk_widget_set_sensitive(btn_decmax, false);
|
|
gtk_widget_set_sensitive(e_posdevice, false);
|
|
}
|
|
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_rot.a);
|
|
gtk_entry_set_text (GTK_ENTRY(e_cal_rotangle), txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_rot.l);
|
|
gtk_entry_set_text (GTK_ENTRY(e_cal_rotlen), txt);
|
|
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis1.a);
|
|
gtk_entry_set_text (GTK_ENTRY(e_cal_raangle), txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis1.l);
|
|
gtk_entry_set_text (GTK_ENTRY(e_cal_ralen), txt);
|
|
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis2.a);
|
|
gtk_entry_set_text (GTK_ENTRY(e_cal_decangle), txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis2.l);
|
|
gtk_entry_set_text (GTK_ENTRY(e_cal_declen), txt);
|
|
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_rot.a - posctl.calib_axis1.a);
|
|
gtk_label_set_text (GTK_LABEL(lb_earthaxis), txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis1.a - posctl.calib_axis2.a);
|
|
gtk_label_set_text (GTK_LABEL(lb_axisaxis), txt);
|
|
|
|
gtk_entry_set_text (GTK_ENTRY(e_posdevice), posctl.GetDevice().c_str());
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.axis_op[AXIS_RA]);
|
|
gtk_label_set_text(GTK_LABEL(ra_out), txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.axis_op[AXIS_DEC]);
|
|
gtk_label_set_text(GTK_LABEL(dec_out), txt);
|
|
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.axis_pv[AXIS_RA]);
|
|
gtk_label_set_text(GTK_LABEL(ra_pv), txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.axis_pv[AXIS_DEC]);
|
|
gtk_label_set_text(GTK_LABEL(dec_pv), txt);
|
|
|
|
posctl.GetAxisParam(AXIS_RA, &mi, &ma, &kp, &ki, &kd);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", mi);
|
|
gtk_entry_set_text_nofocus (ra_min, txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", ma);
|
|
gtk_entry_set_text_nofocus (ra_max, txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", kp);
|
|
gtk_entry_set_text_nofocus (ra_kp, txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", ki);
|
|
gtk_entry_set_text_nofocus (ra_ki, txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", kd);
|
|
gtk_entry_set_text_nofocus (ra_kd, txt);
|
|
|
|
posctl.GetAxisParam(AXIS_DEC, &mi, &ma, &kp, &ki, &kd);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", mi);
|
|
gtk_entry_set_text_nofocus (dec_min, txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", ma);
|
|
gtk_entry_set_text_nofocus (dec_max, txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", kp);
|
|
gtk_entry_set_text_nofocus (dec_kp, txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", ki);
|
|
gtk_entry_set_text_nofocus (dec_ki, txt);
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", kd);
|
|
gtk_entry_set_text_nofocus (dec_kd, txt);
|
|
|
|
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.GetFilter());
|
|
gtk_entry_set_text (GTK_ENTRY(pos_filter), txt);
|
|
|
|
const gchar *txtptr = gtk_entry_get_text(GTK_ENTRY(e_posdevice));
|
|
if (strncmp(txtptr, "SIMULATION", 11) == 0) gtk_widget_show (btnsimreset);
|
|
else gtk_widget_hide (btnsimreset);
|
|
posctl.UnLockMutex();
|
|
}
|
|
|
|
/*
|
|
* callback from the detect thread.
|
|
* the gtk/gui updates must and will be processed in a separate gui thread
|
|
*/
|
|
gboolean cb_thread_posctl_2step (gpointer data) {
|
|
struct PosCtl_2Step_Data *d = (struct PosCtl_2Step_Data *) data;
|
|
posctl_gui_update();
|
|
|
|
if (data) {
|
|
posctl_2step_gui_update(d);
|
|
free (data);
|
|
}
|
|
|
|
return false;
|
|
};
|
|
|
|
|
|
|
|
/*
|
|
* callback from the detect thread.
|
|
* the gtk/gui updates must and will be processed in a separate gui thread
|
|
*/
|
|
gboolean cb_thread_posctl (gpointer data) {
|
|
GtkWidget *dra = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_ra"));
|
|
GtkWidget *ddec = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_dec"));
|
|
GtkWidget *tg = NULL;
|
|
|
|
posctl_gui_update();
|
|
|
|
gtk_widget_queue_draw(dra);
|
|
gtk_widget_queue_draw(ddec);
|
|
|
|
if (posctl_rot_da == NULL) // should only be called once
|
|
posctl_rot_da = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_rotation"));
|
|
gdk_window_invalidate_rect(gtk_widget_get_window(posctl_rot_da), NULL, true);
|
|
|
|
if (data != NULL) {
|
|
posctl_threaddata = *(struct PosCtl_ThreadData*) data;
|
|
free (data);
|
|
|
|
switch (posctl_threaddata.calib_mode) {
|
|
case (POSCTL_CALIB_MODE_STOP1):
|
|
tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_rs"));
|
|
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_DELTAM):
|
|
tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_r"));
|
|
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_AXIS1START):
|
|
tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_ra_s"));
|
|
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_AXIS1M):
|
|
tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_ra"));
|
|
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_AXIS2START):
|
|
tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_dec_s"));
|
|
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_AXIS2M):
|
|
tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_dec"));
|
|
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_FINISH):
|
|
tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_f"));
|
|
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
|
|
break;
|
|
default:
|
|
tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_off"));
|
|
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
|
|
break;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
};
|
|
|
|
|
|
PosCtl::PosCtl() {
|
|
mode = POSCTL_MODE_OFF;
|
|
|
|
#ifdef DEBUG_POSCTL
|
|
debug_tofile((char*)"posctl.log", 1, (char*)"mode , calEV,calEL , posX,posY,targetX,targetY,dx,dy , cal1V,cal1L , axis1.pv,axis1.op,axis1.kp,axis1.ki,axis1.kd , cal2V,cal2L , axis2.pv,axis2.op,axis2.kp,axis2.ki,axis2.kd\n");
|
|
#endif
|
|
|
|
calib_mode = POSCTL_CALIB_MODE_OFF;
|
|
#if BUILD_WINDOWS
|
|
device_fd = INVALID_HANDLE_VALUE;
|
|
#else
|
|
device_fd = -1;
|
|
#endif
|
|
device = "";
|
|
device_type = POSCTL_DEVTYPE_TTY;
|
|
posfilter = 0.0;
|
|
};
|
|
|
|
/*
|
|
* stop the control or the calibration
|
|
*/
|
|
void PosCtl::Stop() {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
LockMutex();
|
|
mode = POSCTL_MODE_OFF;
|
|
NotifyGtk();
|
|
for (int i = 0; i < 2; i++) {
|
|
npv[i] = NAN;
|
|
ppv[i] = NAN;
|
|
nop[i] = NAN;
|
|
pop[i] = NAN;
|
|
}
|
|
UnLockMutex();
|
|
}
|
|
|
|
|
|
void PosCtl::NotifyGtk2Step() {
|
|
struct PosCtl_2Step_Data *data = (struct PosCtl_2Step_Data*) malloc (sizeof(struct PosCtl_2Step_Data));
|
|
|
|
data->mode = mode;
|
|
for (int i = 0; i < 2; i++) {
|
|
data->pv[i] = axis_pv[i];
|
|
data->op[i] = axis_op[i];
|
|
data->npv[i] = npv[i];
|
|
data->ppv[i] = ppv[i];
|
|
data->nop[i] = nop[i];
|
|
data->pop[i] = pop[i];
|
|
}
|
|
|
|
gdk_threads_add_idle(cb_thread_posctl_2step, (gpointer) data);
|
|
}
|
|
|
|
|
|
void PosCtl::NotifyGtk() {
|
|
int i;
|
|
struct PosCtl_ThreadData *data = (struct PosCtl_ThreadData*) malloc(sizeof(struct PosCtl_ThreadData));
|
|
threaddata.calib_mode = calib_mode;
|
|
threaddata.target_pos.x = target_pos.x;
|
|
threaddata.target_pos.y = target_pos.y;
|
|
|
|
switch (calib_mode) {
|
|
case(POSCTL_CALIB_MODE_STOP1):
|
|
for (i = 0; i < 2; i++) {
|
|
threaddata.c[i].x = threaddata.c[i].y = -1;
|
|
threaddata.a1[1].x = threaddata.a1[i].y = -1;
|
|
threaddata.a2[1].x = threaddata.a2[i].y = -1;
|
|
}
|
|
break;
|
|
case(POSCTL_CALIB_MODE_DELTAM):
|
|
threaddata.c[0].x = calib_pos.x;
|
|
threaddata.c[0].y = calib_pos.y;
|
|
break;
|
|
case(POSCTL_CALIB_MODE_AXIS1START):
|
|
threaddata.c[1].x = calib_pos.x;
|
|
threaddata.c[1].y = calib_pos.y;
|
|
break;
|
|
case(POSCTL_CALIB_MODE_AXIS1M):
|
|
threaddata.a1[0].x = calib_pos.x;
|
|
threaddata.a1[0].y = calib_pos.y;
|
|
break;
|
|
case(POSCTL_CALIB_MODE_AXIS2START):
|
|
threaddata.a1[1].x = calib_pos.x;
|
|
threaddata.a1[1].y = calib_pos.y;
|
|
break;
|
|
case(POSCTL_CALIB_MODE_AXIS2M):
|
|
threaddata.a2[0].x = calib_pos.x;
|
|
threaddata.a2[0].y = calib_pos.y;
|
|
break;
|
|
case(POSCTL_CALIB_MODE_FINISH):
|
|
threaddata.a2[1].x = calib_pos.x;
|
|
threaddata.a2[1].y = calib_pos.y;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
*(struct PosCtl_ThreadData*)data = threaddata;
|
|
gdk_threads_add_idle(cb_thread_posctl, data);
|
|
}
|
|
|
|
|
|
/*
|
|
* Start Calibration with the max axis output (a1, a2)
|
|
*/
|
|
void PosCtl::StartCalibration() {
|
|
printf ("%s:%d\n", __FILE__, __LINE__);
|
|
if (mode != POSCTL_MODE_OFF) {
|
|
printf ("%s:%d mode is not off, can't start calibration.\n", __FILE__, __LINE__);
|
|
return;
|
|
}
|
|
mode = POSCTL_MODE_CALIB;
|
|
calib_mode = POSCTL_CALIB_MODE_START;
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
NotifyGtk();
|
|
}
|
|
|
|
void PosCtl::StartControl() {
|
|
if (mode != POSCTL_MODE_OFF) {
|
|
printf ("%s:%d mode is not off, can't start control.\n", __FILE__, __LINE__);
|
|
return;
|
|
}
|
|
printf ("%s:%d %s start controlling Target:%f , %f\n", __FILE__, __LINE__, __FUNCTION__,
|
|
target_pos.x, target_pos.y);
|
|
|
|
pid_axis[AXIS_RA].Start();
|
|
pid_axis[AXIS_DEC].Start();
|
|
mode = POSCTL_MODE_CONTROL;
|
|
NotifyGtk();
|
|
}
|
|
|
|
|
|
/*
|
|
* get and set PID parameter, no mutex lock
|
|
* the access to the data should be already locked by the curren tthread
|
|
*/
|
|
void PosCtl::SetAxisParam ( int axis, double min, double max,
|
|
double k, double i, double d) {
|
|
|
|
if (axis < 0 || axis > 1) return;
|
|
pid_axis[axis].SetParam(min, max, k, i, d);
|
|
};
|
|
|
|
|
|
void PosCtl::GetAxisParam ( int axis, double *min, double *max,
|
|
double *k, double *i, double *d) {
|
|
|
|
if (axis < 0 || axis > 1) return;
|
|
pid_axis[axis].GetParam(min, max, k, i, d);
|
|
};
|
|
|
|
|
|
/*
|
|
* calibration functionsdouble a1, double a2
|
|
*/
|
|
void PosCtl::CalibModeStart(int x, int y) {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
calib_pos.x = x;
|
|
calib_pos.y = y;
|
|
|
|
calib_mode = POSCTL_CALIB_MODE_STOP1;
|
|
if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
NotifyGtk();
|
|
};
|
|
|
|
|
|
void PosCtl::CalibModeWait(int x, int y) {
|
|
struct timeval tv;
|
|
float timediff;
|
|
|
|
gettimeofday (&tv, NULL);
|
|
timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
|
|
|
|
if (timediff > CALIB_STARTSTOP_DELAY) {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
calib_mode++;
|
|
calib_pos.x = x;
|
|
calib_pos.y = y;
|
|
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
NotifyGtk();
|
|
}
|
|
}
|
|
|
|
|
|
void PosCtl::CalibModeDelta(int x, int y) {
|
|
struct timeval tv;
|
|
float timediff;
|
|
float dist = hypot ((float)(x - calib_pos.x), (float)(y - calib_pos.y));
|
|
|
|
gettimeofday (&tv, NULL);
|
|
timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
|
|
|
|
if (timediff > CALIB_DURATION_DELTA || dist > CALIB_DURATION_DIST) {
|
|
position_f_2d fp;
|
|
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
fp.x = (x - calib_pos.x) / (float)timediff;
|
|
fp.y = (y - calib_pos.y) / (float)timediff;
|
|
|
|
LockMutex();
|
|
calib_rot_v = fp;
|
|
calc_vec2anglelen(&fp, &calib_rot);
|
|
printf ("%s:%d %s calib_rot_v.x:%f calib_rot_v.y:%f\n", __FILE__, __LINE__, __FUNCTION__, calib_rot_v.x, calib_rot_v.y);
|
|
UnLockMutex();
|
|
|
|
calib_mode = POSCTL_CALIB_MODE_AXIS1START;
|
|
|
|
if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
|
|
if (OutputWriteValue(AXIS_RA, pid_axis[AXIS_RA].GetMax())) mode = POSCTL_MODE_OFF;
|
|
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
calib_pos.x = x;
|
|
calib_pos.y = y;
|
|
NotifyGtk();
|
|
}
|
|
};
|
|
|
|
|
|
void PosCtl::CalibModeAxis(int x, int y) {
|
|
struct timeval tv;
|
|
float timediff;
|
|
position_f_2d fp;
|
|
float dist = hypot ((float)(x - calib_pos.x), (float)(y - calib_pos.y));
|
|
|
|
gettimeofday (&tv, NULL);
|
|
timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
|
|
|
|
|
|
if (timediff > CALIB_DURATION_AXIS || dist > CALIB_DURATION_DIST) {
|
|
printf ("%s:%d %s calib_mode: %d\n", __FILE__, __LINE__, __FUNCTION__, calib_mode);
|
|
printf ("%s:%d x: %d y: %d cpos.x:%d cpos.y:%d rot.x:%f rot.y:%f\n", __FILE__, __LINE__, x, y, calib_pos.x, calib_pos.y, calib_rot_v.x, calib_rot_v.y);
|
|
|
|
fp.x = (x - calib_pos.x) / timediff;
|
|
fp.y = (y - calib_pos.y) / timediff;
|
|
|
|
if (calib_mode == POSCTL_CALIB_MODE_AXIS1M) {
|
|
calib_axis1_v = fp - calib_rot_v;
|
|
calc_vec2anglelen(&calib_axis1_v, &calib_axis1);
|
|
if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
|
|
else if (OutputWriteValue(AXIS_DEC, pid_axis[AXIS_DEC].GetMax())) mode = POSCTL_MODE_OFF;
|
|
}
|
|
else if (calib_mode == POSCTL_CALIB_MODE_AXIS2M) {
|
|
calib_axis2_v = fp - calib_rot_v;
|
|
calc_vec2anglelen(&calib_axis2_v, &calib_axis2);
|
|
}
|
|
|
|
if (calib_mode != POSCTL_CALIB_MODE_OFF) calib_mode++;
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
|
|
calib_pos.x = x;
|
|
calib_pos.y = y;
|
|
NotifyGtk();
|
|
}
|
|
};
|
|
|
|
|
|
void PosCtl::CalibModeFinish() {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
mode = POSCTL_MODE_OFF;
|
|
calib_mode = POSCTL_CALIB_MODE_OFF;
|
|
if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
|
|
if (OutputWriteValue(AXIS_RA, 0.5 * (pid_axis[AXIS_RA].GetMin() + pid_axis[AXIS_RA].GetMax()))) mode = POSCTL_MODE_OFF;
|
|
if (OutputWriteValue(AXIS_DEC, 0.5 * (pid_axis[AXIS_DEC].GetMin() + pid_axis[AXIS_DEC].GetMax()))) mode = POSCTL_MODE_OFF;
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
NotifyGtk();
|
|
};
|
|
|
|
|
|
double PosCtl::filteredvalue(double old_value, double new_value, double dt, double filter) {
|
|
double res = NAN;
|
|
|
|
if (isnan(old_value) || filter == 0.0) res = new_value;
|
|
else {
|
|
double factor = dt/filter;
|
|
if (factor > 1.0) factor = 1.0;
|
|
res = old_value - (old_value - new_value) * factor;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
/*
|
|
* Loop, if new data is aviable{
|
|
*/
|
|
void PosCtl::Loop (int pos_x, int pos_y, double dt) {
|
|
position_f_2d p;
|
|
|
|
pos.x = PosCtl::filteredvalue(pos.x, pos_x, dt, posfilter);
|
|
pos.y = PosCtl::filteredvalue(pos.y, pos_y, dt, posfilter);
|
|
p.x = pos.x; p.y = pos.y;
|
|
axis_pv[AXIS_RA] = calib_axis2_v.perpendicular(p, target_pos);
|
|
axis_pv[AXIS_DEC] = calib_axis1_v.perpendicular(p, target_pos);
|
|
|
|
#ifdef DEBUG_POSCTL
|
|
static int lastmode = -1;
|
|
double kp0,ki0,kd0, kp1,ki1,kd1;
|
|
if (lastmode != mode && mode != POSCTL_MODE_CONTROL) {
|
|
debug_tofile((char*)"posctl.log", 0, (char*)"%d,,,,,,,,,,,,,,,,,,,,,,\n", mode);
|
|
}
|
|
#endif
|
|
//
|
|
// calibration mode?
|
|
if (mode == POSCTL_MODE_CALIB) {
|
|
switch (calib_mode) {
|
|
case (POSCTL_CALIB_MODE_START):
|
|
CalibModeStart(pos.x, pos.y);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_STOP1):
|
|
case (POSCTL_CALIB_MODE_AXIS1START):
|
|
case (POSCTL_CALIB_MODE_AXIS2START):
|
|
CalibModeWait(pos.x, pos.y);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_DELTAM):
|
|
CalibModeDelta(pos.x, pos.y);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_AXIS1M):
|
|
case (POSCTL_CALIB_MODE_AXIS2M):
|
|
CalibModeAxis(pos.x, pos.y);
|
|
break;
|
|
case (POSCTL_CALIB_MODE_FINISH):
|
|
CalibModeFinish();
|
|
break;
|
|
default:
|
|
calib_mode = POSCTL_CALIB_MODE_OFF;
|
|
mode = POSCTL_MODE_OFF;
|
|
NotifyGtk();
|
|
break;
|
|
}
|
|
NotifyGtk();
|
|
}
|
|
|
|
//
|
|
// dist_axis1+axis2 should contain the lot distance to the
|
|
// of the calibrated axis vector with the starting point at target_pos
|
|
// if the current position is below the axis line, the distance must be negative.
|
|
//
|
|
// this distance must be controlled by the PID axis controller.
|
|
// dist_axis1 --> will control pid_axis2
|
|
// dist_axis2 --> will control pis_axis1
|
|
else if (mode == POSCTL_MODE_CONTROL) {
|
|
LockMutex();
|
|
|
|
// calculate
|
|
// dist_axis < 0.0 pid should increase the output.
|
|
// > 0.0 pid should decrease the output
|
|
axis_op[AXIS_RA] = pid_axis[AXIS_RA].Update(0.0, axis_pv[AXIS_RA]);
|
|
axis_op[AXIS_DEC] = pid_axis[AXIS_DEC].Update(0.0, axis_pv[AXIS_DEC]);
|
|
|
|
#ifdef DEBUG_POSCTL
|
|
pid_axis[AXIS_RA].GetParam(NULL, NULL, &kp0, &ki0, &kd0);
|
|
pid_axis[AXIS_DEC].GetParam(NULL, NULL, &kp1, &ki1, &kd1);
|
|
debug_tofile((char*)"posctl.log", 0, (char*)"%d , %g,%g , %d,%d,%g,%g,%g,%g , %g,%g , %g,%g,%g,%g,%g , %g,%g , %g,%g,%g,%g,%g\n",
|
|
mode, calib_rot.a, calib_rot.l,
|
|
posx, posy, target_pos.x, target_pos.y, posx - target_pos.x, posy - target_pos.y,
|
|
calib_axis1.a, calib_axis1.l, axis_pv[0], axis_op[0],kp0,ki0,kd0,
|
|
calib_axis2.a, calib_axis2.l, axis_pv[1], axis_op[1],kp1,ki1,kd1);
|
|
#endif
|
|
|
|
if (OutputWriteValue(AXIS_RA, axis_op[AXIS_RA]) || OutputWriteValue(AXIS_DEC, axis_op[AXIS_DEC]))
|
|
mode = POSCTL_MODE_OFF;
|
|
|
|
axis_history_add(0, axis_pv[0], axis_op[0]);
|
|
axis_history_add(1, axis_pv[1], axis_op[1]);
|
|
|
|
UnLockMutex();
|
|
NotifyGtk();
|
|
}
|
|
|
|
else if (mode == POSCTL_MODE_2STEPWAIT) {
|
|
//
|
|
// wait time CALIB_STARTSTOP_DELAY sec over?
|
|
struct timeval tv;
|
|
float timediff;
|
|
|
|
gettimeofday (&tv, NULL);
|
|
timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
|
|
|
|
if (timediff > CALIB_STARTSTOP_DELAY) {
|
|
mode = POSCTL_MODE_2STEPRUN;
|
|
target_pos.x = pos.x;
|
|
target_pos.y = pos.y;
|
|
calib_timestamp = tv;
|
|
}
|
|
|
|
LockMutex();
|
|
NotifyGtk2Step();
|
|
UnLockMutex();
|
|
}
|
|
|
|
else if (mode == POSCTL_MODE_2STEPRUN) {
|
|
LockMutex();
|
|
NotifyGtk2Step();
|
|
UnLockMutex();
|
|
}
|
|
|
|
else {
|
|
LockMutex();
|
|
target_pos.x = pos.x;
|
|
target_pos.y = pos.y;
|
|
UnLockMutex();
|
|
}
|
|
|
|
current_pos.x = pos.x;
|
|
current_pos.y = pos.y;
|
|
#ifdef DEBUG_POSCTL
|
|
lastmode = mode;
|
|
#endif
|
|
|
|
};
|
|
|
|
|
|
/*
|
|
*
|
|
*/
|
|
|
|
void PosCtl::Reset2Step() {
|
|
LockMutex();
|
|
mode = POSCTL_MODE_OFF;
|
|
|
|
for (int i = 0; i < 2; i++) {
|
|
npv[i] = NAN;
|
|
ppv[i] = NAN;
|
|
nop[i] = NAN;
|
|
pop[i] = NAN;
|
|
}
|
|
NotifyGtk2Step();
|
|
UnLockMutex();
|
|
NotifyGtk();
|
|
};
|
|
|
|
|
|
/*
|
|
* set current pos as target
|
|
* if npv and ppv == NAN set min pos for both axes
|
|
* min and max values will be read from the PID objects
|
|
*/
|
|
void PosCtl::Run2Step() {
|
|
int allset = 0, i;
|
|
LockMutex();
|
|
|
|
for (i = 0, allset = 1; i < 2; i++) {
|
|
if (isnan(npv[i]) || isnan(ppv[i])) allset = 0;
|
|
}
|
|
|
|
if (mode != POSCTL_MODE_2STEPRUN && mode != POSCTL_MODE_2STEPWAIT) {
|
|
//
|
|
// just start from the beginning
|
|
|
|
for (int i = 0; i < 2; i++) {
|
|
npv[i] = NAN;
|
|
ppv[i] = NAN;
|
|
nop[i] = NAN;
|
|
pop[i] = NAN;
|
|
}
|
|
target_pos = pos;
|
|
mode = POSCTL_MODE_2STEPWAIT;
|
|
|
|
OutputWriteStart();
|
|
OutputWriteValue(AXIS_DEC, pid_axis[AXIS_DEC].GetMin());
|
|
OutputWriteValue(AXIS_RA, pid_axis[AXIS_RA].GetMin());
|
|
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
}
|
|
else if (mode == POSCTL_MODE_2STEPRUN) {
|
|
if (allset) {
|
|
//
|
|
// check if we got better values
|
|
// FIXME: need to finish what to do next
|
|
}
|
|
else {
|
|
//
|
|
// one value still missing: set nXX and pXX and go to maximum output
|
|
for (int i = 0; i < 2; i++) {
|
|
ppv[i] = npv[i] = axis_pv[i];
|
|
pop[i] = nop[i] = axis_op[i];
|
|
}
|
|
OutputWriteStart();
|
|
OutputWriteValue(AXIS_DEC, pid_axis[AXIS_DEC].GetMax());
|
|
OutputWriteValue(AXIS_RA, pid_axis[AXIS_RA].GetMax());
|
|
}
|
|
|
|
target_pos = pos;
|
|
mode = POSCTL_MODE_2STEPWAIT;
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
}
|
|
else {
|
|
printf ("%s:%d %s ERROR: something went wrong. Mode not correct.\n", __FILE__, __LINE__, __FUNCTION__);
|
|
}
|
|
|
|
printf ("%s:%d %s axis_op: (%f %f)\n", __FILE__, __LINE__, __FUNCTION__, axis_op[0], axis_op[1]);
|
|
NotifyGtk2Step();
|
|
|
|
UnLockMutex();
|
|
};
|
|
|
|
|
|
void PosCtl::SetDevice (std::string d) {
|
|
printf ("%s:%d %s new device:%s\n", __FILE__, __LINE__, __FUNCTION__, d.c_str());
|
|
|
|
OutputClose();
|
|
device = d;
|
|
|
|
if (d.compare ("SIMULATION") == 0) device_type = POSCTL_DEVTYPE_SIM;
|
|
else device_type = POSCTL_DEVTYPE_TTY;
|
|
};
|
|
|
|
|
|
int PosCtl::OutputClose() {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
#ifdef BUILD_WINDOWS
|
|
if (device_fd != INVALID_HANDLE_VALUE) CloseHandle(device_fd);
|
|
device_fd = INVALID_HANDLE_VALUE;
|
|
#else
|
|
if (device_fd > 0) close(device_fd);
|
|
device_fd = -1;
|
|
#endif
|
|
return 0;
|
|
};
|
|
|
|
|
|
int PosCtl::OutputOpen() {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
#ifdef BUILD_WINDOWS
|
|
if (device_fd != INVALID_HANDLE_VALUE) return 0;
|
|
#else
|
|
if (device_fd > 0) return 0;
|
|
#endif
|
|
|
|
#ifdef BUILD_WINDOWS
|
|
device_fd = CreateFile(device.c_str(), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
|
|
if (device_fd == INVALID_HANDLE_VALUE) {
|
|
printf ("%s:%d could not open device:%s Error:%ld\n", __FILE__, __LINE__,
|
|
device.c_str(), GetLastError());
|
|
errormessage_display ((char *)"window-posctl", (char *)"OutputOpen",
|
|
(char*)"%s:%d could not open device:%s Error:%ld\n", __FILE__, __LINE__, device.c_str(), GetLastError());
|
|
return -1;
|
|
}
|
|
COMMTIMEOUTS comTimeOut;
|
|
comTimeOut.ReadIntervalTimeout = 100;
|
|
comTimeOut.ReadTotalTimeoutMultiplier = 1;
|
|
comTimeOut.ReadTotalTimeoutConstant = 100;
|
|
comTimeOut.WriteTotalTimeoutMultiplier = 0;
|
|
comTimeOut.WriteTotalTimeoutConstant = 0;
|
|
SetCommTimeouts(device_fd, &comTimeOut);
|
|
#else
|
|
device_fd = open (device.c_str(), O_RDWR);
|
|
if (device_fd < 0) {
|
|
printf ("%s:%d could not open device:%s Error:%s\n", __FILE__, __LINE__,
|
|
device.c_str(), strerror(errno));
|
|
errormessage_display ((char *)"window-posctl", (char *)"OutputOpen",
|
|
(char*)"%s:%d could not open device:%s Error:%s\n", __FILE__, __LINE__, device.c_str(), strerror(errno));
|
|
return -1;
|
|
}
|
|
#endif
|
|
return 0;
|
|
};
|
|
|
|
int PosCtl::WriteTTY (char * outbuf) {
|
|
//printf ("%s:%d %s send: '%s'\n", __FILE__, __LINE__, __FUNCTION__, outbuf);
|
|
|
|
#ifdef BUILD_WINDOWS
|
|
DWORD len;
|
|
int res;
|
|
res = WriteFile(device_fd, outbuf, strlen(outbuf), &len, NULL);
|
|
if (res == 0 || len != strlen(outbuf)) {
|
|
printf ("%s:%d could not write data to device:%s Error:%ld\n", __FILE__, __LINE__,
|
|
device.c_str(), GetLastError());
|
|
errormessage_display ((char *)"window-posctl", (char *)"WriteTTY",
|
|
(char *) "%s:%d could not write data to device:%s Error:%ld\n", __FILE__, __LINE__,
|
|
device.c_str(), GetLastError());
|
|
}
|
|
#else
|
|
ssize_t len;
|
|
len = write (device_fd, outbuf, strlen(outbuf));
|
|
if ((size_t) len != strlen(outbuf) || len < 0) {
|
|
printf ("%s:%d could not write data to device:%s Error:%s\n", __FILE__, __LINE__,
|
|
device.c_str(), strerror(errno));
|
|
errormessage_display ((char *)"window-posctl", (char *)"WriteTTY",
|
|
(char *) "%s:%d could not write data to device:%s Error:%s\n", __FILE__, __LINE__,
|
|
device.c_str(), strerror(errno));
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
int PosCtl::ReadTTY (char * inbuf, int length) {
|
|
#ifdef BUILD_WINDOWS
|
|
DWORD len;
|
|
int res;
|
|
res = ReadFile(device_fd, inbuf, length, &len, NULL);
|
|
|
|
if (res == 0) {
|
|
printf ("%s:%d could not read data from device:%s Error:%ld\n", __FILE__, __LINE__,
|
|
device.c_str(), GetLastError());
|
|
errormessage_display ((char *)"window-posctl", (char *)"ReadTTY",
|
|
(char *) "%s:%d could not read data from device:%s Error:%ld\n", __FILE__, __LINE__,
|
|
device.c_str(), GetLastError());
|
|
inbuf[0] = 0;
|
|
}
|
|
else {
|
|
inbuf[len] = 0;
|
|
}
|
|
#else
|
|
ssize_t len;
|
|
// make device non-blocking
|
|
fcntl(device_fd, F_SETFL, fcntl(device_fd, F_GETFL) | O_NONBLOCK);
|
|
|
|
len = read (device_fd, inbuf, length);
|
|
|
|
// somehow the first read sometimes fails, no idea why
|
|
if (len < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) {
|
|
usleep(100000);
|
|
len = read (device_fd, inbuf, length);
|
|
}
|
|
|
|
if (len < 0) {
|
|
if (errno != EAGAIN) {
|
|
|
|
printf ("%s:%d could not read data from device:%s Error:(%d) %s\n", __FILE__, __LINE__,
|
|
device.c_str(), errno, strerror(errno));
|
|
errormessage_display ((char *)"window-posctl", (char *)"ReadTTY",
|
|
(char *) "%s:%d could not read data from device:%s Error:%s\n", __FILE__, __LINE__,
|
|
device.c_str(), strerror(errno));
|
|
}
|
|
inbuf[0] = 0;
|
|
}
|
|
else {
|
|
inbuf[len] = 0;
|
|
printf ("%s:%d %s receive: '%s'\n", __FILE__, __LINE__, __FUNCTION__, inbuf);
|
|
}
|
|
|
|
// make device blocking
|
|
fcntl(device_fd, F_SETFL, fcntl(device_fd, F_GETFL) & ~O_NONBLOCK);
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
|
|
/*
|
|
* write output, and save last set value in axis_op
|
|
*/
|
|
int PosCtl::OutputWriteValue (int axis, double value) {
|
|
char outbuf[255];
|
|
|
|
if (device_type == POSCTL_DEVTYPE_SIM) {
|
|
simulation.AxisSetValue(axis, value);
|
|
return 0;
|
|
}
|
|
|
|
#ifdef BUILD_WINDOWS
|
|
if (device_fd == INVALID_HANDLE_VALUE) if (OutputOpen() != 0) return -1;
|
|
#else
|
|
if (device_fd <= 0) if (OutputOpen() != 0) return -1;
|
|
#endif
|
|
//
|
|
// save language setting and set to plain C
|
|
std::string s = setlocale(LC_ALL, NULL);
|
|
setlocale (LC_ALL, "C");
|
|
|
|
snprintf(outbuf, 254, ":R%c%+09.4f#", (axis == AXIS_DEC ? 'D' : 'R'), value);
|
|
outbuf[254] = 0;
|
|
|
|
// reset language setting to default
|
|
setlocale (LC_ALL, s.c_str());
|
|
|
|
WriteTTY(outbuf);
|
|
ReadTTY(outbuf, sizeof(outbuf) - 1);
|
|
|
|
axis_op[axis] = value;
|
|
|
|
return 0;
|
|
};
|
|
|
|
int PosCtl::OutputWriteStop () {
|
|
char outbuf[255];
|
|
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
if (device_type == POSCTL_DEVTYPE_SIM) {
|
|
simulation.AxisStop();
|
|
return 0;
|
|
}
|
|
|
|
snprintf (outbuf, 255, ":Q#");
|
|
#ifdef BUILD_WINDOWS
|
|
if (device_fd == INVALID_HANDLE_VALUE) if (OutputOpen() != 0) return -1;
|
|
#else
|
|
if (device_fd <= 0) if (OutputOpen() != 0) return -1;
|
|
#endif
|
|
WriteTTY(outbuf);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int PosCtl::OutputWriteStart () {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
OutputWriteValue(AXIS_DEC, 0.0);
|
|
OutputWriteValue(AXIS_RA, 0.0);
|
|
|
|
return 0;
|
|
}
|