diff --git a/detect.cc b/detect.cc index 121de5d..2d94879 100644 --- a/detect.cc +++ b/detect.cc @@ -5,6 +5,7 @@ #include #include #include +#include #include "config.h" #include "gui.h" #include "detect.h" @@ -75,14 +76,18 @@ int Detect::NewFrame(VideoFrame *newframe) { // Thread: newFrame |------> Find Object --- not found ---> send gui information void Detect::Thread() { DetectOutput doutput; + struct timeval timestamp; + double dt; int oldX, oldY; objectX = -1; objectY = -1; doutput.detmatrix = (uint32_t*)malloc (sizeof(uint32_t) * DET_MAXSHIFT * DET_MAXSHIFT); + gettimeofday (×tamp, NULL); while (running) { // check for new frame + dt = get_cycletime(×tamp); LockInMutex(); if (inFrameNew == 1) { inFrameNew = 0; @@ -144,7 +149,7 @@ void Detect::Thread() { if (detmatrix != NULL) memcpy (doutput.detmatrix, detmatrix, sizeof(uint32_t) * DET_MAXSHIFT * DET_MAXSHIFT); - posctl.Loop (objectX, objectY); + posctl.Loop (objectX, objectY, dt); UnLockMutex(); // unlock detect dataset diff --git a/detect.h b/detect.h index 7f39848..2127d41 100644 --- a/detect.h +++ b/detect.h @@ -95,6 +95,8 @@ class PosCtl { struct timeval calib_timestamp; int calib_mode; position_2d calib_pos; + position_f_2d pos; + double posfilter; struct PosCtl_ThreadData threaddata; // will be copied to gtk thread void NotifyGtk(); @@ -109,6 +111,9 @@ class PosCtl { int OutputOpen(); int WriteTTY(char *); int ReadTTY(char *, int); + + static double filteredvalue(double old_value, double new_value, double dt, double filter); + public: PosCtl(); ~PosCtl() {}; @@ -134,9 +139,10 @@ class PosCtl { void SetAxisParam (int axis, double min, double max, double k, double i, double d); void GetAxisParam (int axis, double *min, double *max, double *k, double *i, double *d); void SetDevice(std::string d); + void SetFilter(double f) { posfilter = f; }; std::string GetDevice() { return device; }; - void Loop (int posx, int posy); + void Loop (int posx, int posy, double dt); int OutputWriteValue (int axis, double value); int OutputWriteStop (); int OutputWriteStart (); diff --git a/pid.cc b/pid.cc index d4db5c2..c89cfe7 100644 --- a/pid.cc +++ b/pid.cc @@ -7,6 +7,7 @@ #include #include #include +#include #include "pid.h" /* @@ -80,7 +81,6 @@ double PID::Update(double target, double value, int64_t time) { double P, I, D; double Derivative; - // error error = target - value; // proportional part diff --git a/posctl.cc b/posctl.cc index f3e6953..311d00d 100644 --- a/posctl.cc +++ b/posctl.cc @@ -169,6 +169,7 @@ void cb_posctl_change_entry (GtkWidget *widget, gpointer data) { 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; @@ -192,7 +193,7 @@ void cb_posctl_change_entry (GtkWidget *widget, gpointer data) { 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); @@ -672,6 +673,7 @@ PosCtl::PosCtl() { #endif device = ""; device_type = POSCTL_DEVTYPE_TTY; + posfilter = 0.0; }; /* @@ -903,10 +905,28 @@ void PosCtl::CalibModeFinish() { }; +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 posx, int posy) { +void PosCtl::Loop (int pos_x, int pos_y, double dt) { + + pos.x = PosCtl::filteredvalue(pos.x, pos_x, dt, posfilter); + pos.y = PosCtl::filteredvalue(pos.y, pos_y, dt, posfilter); + #ifdef DEBUG_POSCTL static int lastmode = -1; double kp0,ki0,kd0, kp1,ki1,kd1; @@ -919,19 +939,19 @@ void PosCtl::Loop (int posx, int posy) { if (mode == POSCTL_MODE_CALIB) { switch (calib_mode) { case (POSCTL_CALIB_MODE_START): - CalibModeStart(posx, posy); + CalibModeStart(pos.x, pos.y); break; case (POSCTL_CALIB_MODE_STOP1): case (POSCTL_CALIB_MODE_AXIS1START): case (POSCTL_CALIB_MODE_AXIS2START): - CalibModeWait(posx, posy); + CalibModeWait(pos.x, pos.y); break; case (POSCTL_CALIB_MODE_DELTAM): - CalibModeDelta(posx, posy); + CalibModeDelta(pos.x, pos.y); break; case (POSCTL_CALIB_MODE_AXIS1M): case (POSCTL_CALIB_MODE_AXIS2M): - CalibModeAxis(posx, posy); + CalibModeAxis(pos.x, pos.y); break; case (POSCTL_CALIB_MODE_FINISH): CalibModeFinish(); @@ -962,7 +982,7 @@ void PosCtl::Loop (int posx, int posy) { // dist_axis < 0.0 pid should increase the output. // > 0.0 pid should decrease the output target_p.x = target_pos.x; target_p.y = target_pos.y; - p.x = posx; p.y = posy; + 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); axis_op[AXIS_RA] = pid_axis[AXIS_RA].Update(0.0, axis_pv[AXIS_RA]); @@ -990,13 +1010,13 @@ void PosCtl::Loop (int posx, int posy) { else { LockMutex(); - target_pos.x = posx; - target_pos.y = posy; + target_pos.x = pos.x; + target_pos.y = pos.y; UnLockMutex(); } - current_pos.x = posx; - current_pos.y = posy; + current_pos.x = pos.x; + current_pos.y = pos.y; #ifdef DEBUG_POSCTL lastmode = mode; #endif diff --git a/simpleskycam.ui b/simpleskycam.ui index 034d27d..f6594e7 100644 --- a/simpleskycam.ui +++ b/simpleskycam.ui @@ -1209,7 +1209,7 @@ True True - 4 + 6 @@ -1221,7 +1221,7 @@ True True - 4 + 6 @@ -1233,7 +1233,7 @@ True True - 4 + 6 @@ -1245,7 +1245,7 @@ True True - 4 + 6 @@ -1257,7 +1257,7 @@ True True - 4 + 6 @@ -1468,7 +1468,7 @@ True True - 4 + 6 @@ -1480,7 +1480,7 @@ True True - 4 + 6 @@ -1492,7 +1492,7 @@ True True - 4 + 6 @@ -1504,7 +1504,7 @@ True True - 4 + 6 @@ -1516,7 +1516,7 @@ True True - 4 + 6 @@ -1870,7 +1870,40 @@ - + + True + False + + + True + False + Input Filter: + + + False + True + 0 + + + + + True + True + 6 + + + + False + True + 1 + + + + + False + True + 2 + diff --git a/videodev-simulation.cc b/videodev-simulation.cc index c9a514b..032da1b 100644 --- a/videodev-simulation.cc +++ b/videodev-simulation.cc @@ -20,6 +20,8 @@ #include "configuration.h" #include "videodev-simulation.h" +#define SIMULATION_MOTORSPEEDUP 1.0 + Simulation simulation; gpointer simulation_threadprocess_wrapper (gpointer data); @@ -128,6 +130,7 @@ int VideoDev_Simulation::CaptureStop() { * Return code VDEV_STATUS_AGAIN is not an error. There was no video image ready to read. */ #define SIMULATION_SIZE 16 +#define SIMULATION_NOISE 15 int VideoDev_Simulation::Grab(VideoFrameRaw *vf) { int posx, posy, x ,y, r, radius; double a, dsec; @@ -141,8 +144,8 @@ int VideoDev_Simulation::Grab(VideoFrameRaw *vf) { radius = conf_width * SIMULATION_SIZE / 1920; for (r = 1; r < radius; r++) for (a = 0; a < M_PI * 2.0; a += 0.1) { - x = posx + (sin(a) * r); - y = posy + (cos(a) * r); + x = posx + (sin(a) * r) + (SIMULATION_NOISE/2 - SIMULATION_NOISE *(float) rand()/ (float)RAND_MAX); + y = posy + (cos(a) * r) + (SIMULATION_NOISE/2 - SIMULATION_NOISE *(float) rand()/ (float)RAND_MAX); if (x >= 0 && x < conf_width && y >= 0 && y < conf_height) { inframe[3*(x+y*conf_width)+0] = 200; inframe[3*(x+y*conf_width)+1] = 200; diff --git a/videodev-simulation.h b/videodev-simulation.h index 9792f5f..6f78051 100644 --- a/videodev-simulation.h +++ b/videodev-simulation.h @@ -26,7 +26,6 @@ #include "gui.h" #include "videodev.h" -#define SIMULATION_MOTORSPEEDUP 0.01 struct SimAxisCtl { int active; double vdest;