diff --git a/detect.h b/detect.h
index 2a538c7..eb1e7ff 100644
--- a/detect.h
+++ b/detect.h
@@ -33,18 +33,49 @@ enum {
POSCTL_MODE_CONTROL
};
+enum {
+ POSCTL_CALIB_MODE_OFF = 0,
+ POSCTL_CALIB_MODE_START,
+ POSCTL_CALIB_MODE_DELTA,
+ POSCTL_CALIB_MODE_AXIS1_MI,
+ POSCTL_CALIB_MODE_AXIS1_MA,
+ POSCTL_CALIB_MODE_AXIS2_MI,
+ POSCTL_CALIB_MODE_AXIS2_MA,
+ POSCTL_CALIB_MODE_FINISH
+};
+
+
+
class PosCtl {
private:
GMutex mutex;
int mode;
PID pid_axis[2];
+ struct timeval calib_timestamp;
+ int calib_mode;
+ position_2d calib_pos;
+
+ void CalibModeStart(int x, int y);
+ void CalibModeDelta(int x, int y);
+ void CalibModeAxis(int x, int y);
+ void CalibModeFinish();
- time_t test_t1; // FIXME: only for testing gui functions
public:
PosCtl();
~PosCtl() {};
+ double calib_rot_angle;
+ double calib_rot_len;
+
+ double calib_axis1_angle;
+ double calib_axis1_lenmi;
+ double calib_axis1_lenma;
+
+ double calib_axis2_angle;
+ double calib_axis2_lenmi;
+ double calib_axis2_lenma;
+
void LockMutex() { g_mutex_lock(&mutex); };
void UnLockMutex() { g_mutex_unlock(&mutex); };
diff --git a/posctl.cc b/posctl.cc
index 0d55190..3064408 100644
--- a/posctl.cc
+++ b/posctl.cc
@@ -59,10 +59,10 @@ void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer
// 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_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1angle"));
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_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle"));
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"));
@@ -217,9 +217,22 @@ void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer da
* posctl gui update
*/
void posctl_gui_update() {
+ char txt[255];
+
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"));
+ 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_a1angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1angle"));
+ GtkWidget *e_cal_a1lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenmi"));
+ GtkWidget *e_cal_a1lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenma"));
+
+ GtkWidget *e_cal_a2angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle"));
+ GtkWidget *e_cal_a2lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenmi"));
+ GtkWidget *e_cal_a2lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenma"));
+
posctl.LockMutex();
int m = posctl.GetMode();
@@ -232,6 +245,25 @@ void posctl_gui_update() {
gtk_widget_set_sensitive(cbenable, false);
}
+ strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_rot_angle);
+ gtk_entry_set_text (GTK_ENTRY(e_cal_rotangle), txt);
+ strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_rot_len);
+ gtk_entry_set_text (GTK_ENTRY(e_cal_rotlen), txt);
+
+ strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1_angle);
+ gtk_entry_set_text (GTK_ENTRY(e_cal_a1angle), txt);
+ strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1_lenmi);
+ gtk_entry_set_text (GTK_ENTRY(e_cal_a1lenmi), txt);
+ strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1_lenma);
+ gtk_entry_set_text (GTK_ENTRY(e_cal_a1lenma), txt);
+
+ strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2_angle);
+ gtk_entry_set_text (GTK_ENTRY(e_cal_a2angle), txt);
+ strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2_lenmi);
+ gtk_entry_set_text (GTK_ENTRY(e_cal_a2lenmi), txt);
+ strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2_lenma);
+ gtk_entry_set_text (GTK_ENTRY(e_cal_a2lenma), txt);
+
posctl.UnLockMutex();
}
@@ -248,7 +280,16 @@ gboolean cb_thread_posctl (gpointer data) {
PosCtl::PosCtl() {
mode = POSCTL_MODE_OFF;
- test_t1 = 0;
+
+ calib_mode = POSCTL_CALIB_MODE_OFF;
+ calib_rot_len = 0.0;
+ calib_rot_angle = 0.0;
+ calib_axis1_angle = 0.0;
+ calib_axis1_lenmi = 0.0;
+ calib_axis1_lenma = 0.0;
+ calib_axis2_angle = 0.0;
+ calib_axis2_lenmi = 0.0;
+ calib_axis2_lenma = 0.0;
};
@@ -269,7 +310,8 @@ void PosCtl::StartCalibration() {
return;
}
mode = POSCTL_MODE_CALIB;
- test_t1 = time (NULL);
+ calib_mode = POSCTL_CALIB_MODE_START;
+ gettimeofday (&calib_timestamp, NULL);
gdk_threads_add_idle(cb_thread_posctl, NULL);
}
@@ -306,15 +348,105 @@ void PosCtl::GetAxisParam ( int axis, double *min, double *max,
/*
- * Loop, if new data is aviable
+ * calibration functions
*/
-void PosCtl::Loop (int posx, int posy) {
- time_t t = time(NULL);
+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_DELTA;
+ gettimeofday (&calib_timestamp, NULL);
+ gdk_threads_add_idle(cb_thread_posctl, NULL);
+};
+
+
+void PosCtl::CalibModeDelta(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 > 10.0) {
+ position_f_2d fp;
+
+ printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
+
+ fp.x = x - calib_pos.x;
+ fp.x /= (float)timediff;
+ fp.y = y - calib_pos.y;
+ fp.y /= (float)timediff;
+
+ LockMutex();
+ calib_rot_angle = atan2(fp.x, fp.y) * 180.0 / M_PI;
+ calib_rot_len = sqrt(fp.x * fp.x + fp.y * fp.y);
+ UnLockMutex();
+
+ calib_mode = POSCTL_CALIB_MODE_AXIS1_MA;
+ gettimeofday (&calib_timestamp, NULL);
+ gdk_threads_add_idle(cb_thread_posctl, NULL);
+
+ }
+};
+
+
+void PosCtl::CalibModeAxis(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 > 10.0) {
+ printf ("%s:%d %s calib_mode: %d\n", __FILE__, __LINE__, __FUNCTION__, calib_mode);
- // FIXME:: only for testing gui function
- if ((t - test_t1) > 5) {
- mode = POSCTL_MODE_OFF;
+ calib_mode++;
+ gettimeofday (&calib_timestamp, NULL);
gdk_threads_add_idle(cb_thread_posctl, NULL);
}
};
+
+void PosCtl::CalibModeFinish() {
+ printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
+
+ mode = POSCTL_MODE_OFF;
+ calib_mode = POSCTL_CALIB_MODE_OFF;
+ gettimeofday (&calib_timestamp, NULL);
+ gdk_threads_add_idle(cb_thread_posctl, NULL);
+};
+
+
+/*
+ * Loop, if new data is aviable
+ */
+void PosCtl::Loop (int posx, int posy) {
+ //
+ // calibration mode?
+ if (mode == POSCTL_MODE_CALIB) {
+ switch (calib_mode) {
+ case (POSCTL_CALIB_MODE_START):
+ CalibModeStart(posx, posy);
+ break;
+ case (POSCTL_CALIB_MODE_DELTA):
+ CalibModeDelta(posx, posy);
+ break;
+ case (POSCTL_CALIB_MODE_AXIS1_MI):
+ case (POSCTL_CALIB_MODE_AXIS1_MA):
+ case (POSCTL_CALIB_MODE_AXIS2_MI):
+ case (POSCTL_CALIB_MODE_AXIS2_MA):
+ CalibModeAxis(posx, posy);
+ break;
+ case (POSCTL_CALIB_MODE_FINISH):
+ CalibModeFinish();
+ break;
+ default:
+ calib_mode = POSCTL_CALIB_MODE_OFF;
+ mode = POSCTL_MODE_OFF;
+ gdk_threads_add_idle(cb_thread_posctl, NULL);
+ }
+ }
+};
+
diff --git a/simpleskycam.ui b/simpleskycam.ui
index d1954fa..31f155f 100644
--- a/simpleskycam.ui
+++ b/simpleskycam.ui
@@ -758,7 +758,7 @@
4
4
-
-