From 4a9d471ca13c3b5b1bf2dc0f716679c7aa5d4688 Mon Sep 17 00:00:00 2001 From: Steffen Pohle Date: Fri, 20 Jan 2023 00:42:52 +0100 Subject: [PATCH] rotation is now calculated on calibration. --- detect.h | 33 ++++++++++- posctl.cc | 152 ++++++++++++++++++++++++++++++++++++++++++++---- simpleskycam.ui | 4 +- 3 files changed, 176 insertions(+), 13 deletions(-) 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 - + True False True @@ -788,7 +788,7 @@ - + True False True