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.
321 lines
10 KiB
321 lines
10 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 "filter.h"
|
|
#include "detect.h"
|
|
#include "configuration.h"
|
|
#include "video.h"
|
|
#include "videodev.h"
|
|
#include "histogram.h"
|
|
|
|
extern PosCtl posctl;
|
|
|
|
extern GtkBuilder *_builder_; // work around for threads
|
|
GtkWidget *posctl_rot_da = NULL;
|
|
// GtkWidget *posctl_axis1_da = NULL;
|
|
// GtkWidget *posctl_axis1_da = NULL;
|
|
double linedash1[] = { 1.0, 4.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_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) {
|
|
int clientw, clienth;
|
|
|
|
position_f_2d lpos[5];
|
|
float lmax;
|
|
position_2d center;
|
|
|
|
//
|
|
// 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_a1anlge"));
|
|
GtkWidget *entry_axis1_lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenmi"));
|
|
GtkWidget *entry_axis1_lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenma"));
|
|
GtkWidget *entry_axis2_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2anlge"));
|
|
GtkWidget *entry_axis2_lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenmi"));
|
|
GtkWidget *entry_axis2_lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenma"));
|
|
|
|
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 a1_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_angle))) * (M_PI / 180);
|
|
float a1_lenmi = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_lenmi)));
|
|
float a1_lenma = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_lenma)));
|
|
float a2_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_angle))) * (M_PI / 180);
|
|
float a2_lenmi = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_lenmi)));
|
|
float a2_lenma = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_lenma)));
|
|
// float lenmax = max(max(max(a1_lenmi, a1_lenma),max(a2_lenmi, a2_lenma)), rotlen);
|
|
|
|
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);
|
|
|
|
//
|
|
// calculate angles
|
|
lpos[0].x = sin(rotangle) * rotlen;
|
|
lpos[0].y = cos(rotangle) * rotlen;
|
|
|
|
lpos[1].x = lpos[0].x + sin(a1_angle) * a1_lenmi;
|
|
lpos[1].y = lpos[0].y + cos(a1_angle) * a1_lenmi;
|
|
lpos[2].x = lpos[0].x + sin(a1_angle) * a1_lenma;
|
|
lpos[2].y = lpos[0].y + cos(a1_angle) * a1_lenma;
|
|
|
|
lpos[3].x = lpos[0].x + sin(a2_angle) * a2_lenmi;
|
|
lpos[3].y = lpos[0].y + cos(a2_angle) * a2_lenmi;
|
|
lpos[4].x = lpos[0].x + sin(a2_angle) * a2_lenma;
|
|
lpos[4].y = lpos[0].y + cos(a2_angle) * a2_lenma;
|
|
|
|
// find maximum
|
|
for (int i = 0; i < 5; i++) {
|
|
if (i == 0) {
|
|
lmax = fabs (lpos[0].x);
|
|
if (fabs(lpos[i].y) > lmax) lmax = fabs(lpos[i].y);
|
|
} else {
|
|
if (fabs(lpos[i].x) > lmax) lmax = fabs(lpos[i].x);
|
|
if (fabs(lpos[i].y) > lmax) lmax = fabs(lpos[i].y);
|
|
}
|
|
}
|
|
|
|
//
|
|
// draw
|
|
cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
|
|
cairo_set_font_size (cr, 11);
|
|
cairo_set_line_width(cr, 4.0);
|
|
|
|
// rot
|
|
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
|
|
cairo_move_to(cr, center.x, center.y);
|
|
cairo_line_to(cr, center.x + float (lpos[0].x * center.x * 0.8 / lmax),
|
|
center.y - float (lpos[0].y * center.y * 0.8 / lmax));
|
|
cairo_stroke(cr);
|
|
cairo_move_to(cr, center.x + float (lpos[0].x * center.x * 0.9 / lmax),
|
|
center.y - float (lpos[0].y * center.y * 0.9 / lmax));
|
|
cairo_show_text(cr, (char *)"E");
|
|
|
|
// axis1
|
|
cairo_set_source_rgb(cr, 0.0, 1.0, 0.5);
|
|
cairo_move_to(cr, center.x + float (lpos[1].x * center.x * 0.8 / lmax),
|
|
center.y - float (lpos[1].y * center.y * 0.8 / lmax));
|
|
cairo_line_to(cr, center.x + float (lpos[2].x * center.x * 0.8 / lmax),
|
|
center.y - float (lpos[2].y * center.y * 0.8 / lmax));
|
|
cairo_stroke(cr);
|
|
cairo_move_to(cr, center.x + float (lpos[2].x * center.x * 0.9 / lmax),
|
|
center.y - float (lpos[2].y * center.y * 0.9 / lmax));
|
|
cairo_show_text(cr, (char *)"1");
|
|
|
|
// axis2
|
|
cairo_set_source_rgb(cr, 0.0, 0.5, 1.0);
|
|
cairo_move_to(cr, center.x + float (lpos[3].x * center.x * 0.8 / lmax),
|
|
center.y - float (lpos[3].y * center.y * 0.8 / lmax));
|
|
cairo_line_to(cr, center.x + float (lpos[4].x * center.x * 0.8 / lmax),
|
|
center.y - float (lpos[4].y * center.y * 0.8 / lmax));
|
|
cairo_stroke(cr);
|
|
cairo_move_to(cr, center.x + float (lpos[4].x * center.x * 0.9 / lmax),
|
|
center.y - float (lpos[4].y * center.y * 0.9 / lmax));
|
|
cairo_show_text(cr, (char *)"2");
|
|
};
|
|
|
|
|
|
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);
|
|
};
|
|
|
|
|
|
void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) {
|
|
GtkWidget *da1 = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_axis1"));
|
|
GtkWidget *da2 = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_axis2"));
|
|
position_2d center;
|
|
|
|
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);
|
|
|
|
cairo_set_source_rgb(cr, 0.5, 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, 30, 10);
|
|
cairo_show_text(cr, (char *)"offset");
|
|
if (da1 != area && da2 != area) {
|
|
cairo_move_to(cr, 30, 0);
|
|
cairo_show_text(cr, (char *)"unknown");
|
|
return;
|
|
}
|
|
};
|
|
|
|
|
|
/*
|
|
* posctl gui update
|
|
*/
|
|
void posctl_gui_update() {
|
|
GtkWidget *btnclib = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib"));
|
|
GtkWidget *cbenable = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_enablectl"));
|
|
|
|
posctl.LockMutex();
|
|
int m = posctl.GetMode();
|
|
|
|
if (m == POSCTL_MODE_OFF) {
|
|
gtk_widget_set_sensitive(btnclib, true);
|
|
gtk_widget_set_sensitive(cbenable, true);
|
|
}
|
|
else {
|
|
gtk_widget_set_sensitive(btnclib, false);
|
|
gtk_widget_set_sensitive(cbenable, false);
|
|
}
|
|
|
|
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 (gpointer data) {
|
|
posctl_gui_update();
|
|
return false;
|
|
};
|
|
|
|
|
|
PosCtl::PosCtl() {
|
|
mode = POSCTL_MODE_OFF;
|
|
test_t1 = 0;
|
|
};
|
|
|
|
|
|
/*
|
|
* stop the control or the calibration
|
|
*/
|
|
void PosCtl::Stop() {
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
mode = POSCTL_MODE_OFF;
|
|
gdk_threads_add_idle(cb_thread_posctl, NULL);
|
|
}
|
|
|
|
|
|
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;
|
|
test_t1 = time (NULL);
|
|
gdk_threads_add_idle(cb_thread_posctl, NULL);
|
|
}
|
|
|
|
|
|
void PosCtl::StartControl() {
|
|
if (mode != POSCTL_MODE_OFF) {
|
|
printf ("%s:%d mode is not off, can't start control.\n", __FILE__, __LINE__);
|
|
return;
|
|
}
|
|
pid_axis[0].Start();
|
|
pid_axis[1].Start();
|
|
mode = POSCTL_MODE_CONTROL;
|
|
gdk_threads_add_idle(cb_thread_posctl, NULL);
|
|
}
|
|
|
|
|
|
/*
|
|
* 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);
|
|
};
|
|
|
|
|
|
/*
|
|
* Loop, if new data is aviable
|
|
*/
|
|
void PosCtl::Loop (int posx, int posy) {
|
|
time_t t = time(NULL);
|
|
|
|
// FIXME:: only for testing gui function
|
|
if ((t - test_t1) > 5) {
|
|
mode = POSCTL_MODE_OFF;
|
|
gdk_threads_add_idle(cb_thread_posctl, NULL);
|
|
}
|
|
};
|
|
|