diff --git a/ChangeLog b/ChangeLog index 97d1bc5..9d63661 100644 --- a/ChangeLog +++ b/ChangeLog @@ -1,3 +1,8 @@ +2023-02-16: +- fixed positioncontrol. made some misstakes in calculating the angle + of the axsis vectors. +- changed: convert function is now only presend in videodef class. + 2023-02-07: - position control is now working. has still some glitches with negative Kp, Ki and Kd values. But in general it is working. diff --git a/error.h b/error.h index 11343e3..5054759 100644 --- a/error.h +++ b/error.h @@ -15,6 +15,7 @@ struct ErrorMessage { char message[ERRORMSG_LEN]; }; +void errorexit (char *fmt,...); void errormessage_display (char *windowname, char *title, char *fmt,...); #endif // _ERROR_H_ diff --git a/videodev-dummy.h b/videodev-dummy.h index f222ffe..b696250 100644 --- a/videodev-dummy.h +++ b/videodev-dummy.h @@ -32,7 +32,7 @@ class VideoDev_Dummy: public VideoDev { private: ConvertData cdata; - int Grab(VideoFrame *vf); + int Grab(VideoFrameRaw *vf); int Open(); int Close(); int CaptureStart(); diff --git a/videodev-dumpfile.cc b/videodev-dumpfile.cc index fc8aa17..02b0e8c 100644 --- a/videodev-dumpfile.cc +++ b/videodev-dumpfile.cc @@ -25,8 +25,6 @@ #endif #include - -#include "convert.h" #include "configuration.h" #include "videodev-dumpfile.h" @@ -182,8 +180,7 @@ int VideoDev_Dumpfile::CaptureStart() { // read timestamp and first header and frame gettimeofday(&starttv, NULL); - - ConvertStart(&cdata, pixformat); + pixelformat = pixformat; ReadFrame(); return VDEV_STATUS_OK; @@ -203,9 +200,6 @@ int VideoDev_Dumpfile::CaptureStop() { inframe_size = 0; inframe = NULL; } - - ConvertStop(&cdata, pixformat); - return VDEV_STATUS_OK; }; @@ -215,7 +209,7 @@ int VideoDev_Dumpfile::CaptureStop() { * If something goes wrong return an error code. * Return code VDEV_STATUS_AGAIN is not an error. There was no video image ready to read. */ -int VideoDev_Dumpfile::Grab(VideoFrame *vf) { +int VideoDev_Dumpfile::Grab(VideoFrameRaw *vf) { struct timeval curtv; unsigned int diff = 0; @@ -237,7 +231,7 @@ int VideoDev_Dumpfile::Grab(VideoFrame *vf) { __FILE__, __LINE__, (diff - inframe_nexttime)); LockMutex(); - Convert(&cdata, vf, inframe, inframe_size, pixformat, w, h); + vf->CopyFrom(pixformat, w, h, inframe_size, inframe); UnLockMutex(); // diff --git a/videodev-dumpfile.h b/videodev-dumpfile.h index 1dbca4e..b099047 100644 --- a/videodev-dumpfile.h +++ b/videodev-dumpfile.h @@ -21,8 +21,6 @@ #include #include - -#include "convert.h" #include "gui.h" #include "videodev.h" @@ -42,7 +40,7 @@ private: int inframe_maxsize; int inframe_size; - int Grab(VideoFrame *vf); + int Grab(VideoFrameRaw *vf); int Open(); int Close(); int CaptureStart(); diff --git a/videodev-simulation.cc b/videodev-simulation.cc index 678fc6c..9703846 100644 --- a/videodev-simulation.cc +++ b/videodev-simulation.cc @@ -107,7 +107,7 @@ int VideoDev_Simulation::CaptureStart() { if (inframe != NULL) Close(); if (Open() != VDEV_STATUS_OK) return VDEV_STATUS_ERROR; - ConvertStart(&cdata, V4L2_PIX_FMT_RGB24); + pixelformat = V4L2_PIX_FMT_RGB24; get_cycletime(&lastframetv); return VDEV_STATUS_OK; @@ -118,7 +118,6 @@ int VideoDev_Simulation::CaptureStop() { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); if (inframe != NULL) Close(); - ConvertStop(&cdata, V4L2_PIX_FMT_RGB24); return VDEV_STATUS_OK; }; @@ -129,7 +128,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 -int VideoDev_Simulation::Grab(VideoFrame *vf) { +int VideoDev_Simulation::Grab(VideoFrameRaw *vf) { int posx, posy, x ,y, r, radius; double a, dsec; @@ -152,8 +151,7 @@ int VideoDev_Simulation::Grab(VideoFrame *vf) { } LockMutex(); - Convert(&cdata, vf, inframe, (conf_width*conf_height*3), V4L2_PIX_FMT_RGB24, - conf_width, conf_height); + vf->CopyFrom(V4L2_PIX_FMT_RGB24, conf_width, conf_height, (conf_width*conf_height*3), inframe); UnLockMutex(); return VDEV_STATUS_OK; diff --git a/videodev-simulation.h b/videodev-simulation.h index 9051636..e85fce5 100644 --- a/videodev-simulation.h +++ b/videodev-simulation.h @@ -72,7 +72,7 @@ private: ConvertData cdata; GThread *simulation_thread; - int Grab(VideoFrame *vf); + int Grab(VideoFrameRaw *vf); int Open(); int Close(); int CaptureStart(); diff --git a/videodev-svbcam.cc b/videodev-svbcam.cc index c8b069c..d9b3f7e 100644 --- a/videodev-svbcam.cc +++ b/videodev-svbcam.cc @@ -289,8 +289,7 @@ int VideoDev_SVBCam::CaptureStart() { if (inframe != NULL) free (inframe); inframe_size = 4 * inframe_w * inframe_h; inframe = (unsigned char*) malloc(inframe_size); - - ConvertStart(&cdata, inframe_pixfmt); + pixelformat = inframe_pixfmt; return VDEV_STATUS_OK; }; @@ -315,8 +314,6 @@ int VideoDev_SVBCam::CaptureStop() { inframe_size = 0; } - ConvertStop(&cdata, inframe_pixfmt); - return VDEV_STATUS_OK; }; @@ -327,7 +324,7 @@ int VideoDev_SVBCam::CaptureStop() { * Return code VDEV_STATUS_AGAIN is not an error. There was no video image ready to read. */ // FIXME: SVBGetVideoData needs to be outside of Lock/UnLockMutex - using inside thread inbuffer -int VideoDev_SVBCam::Grab(VideoFrame *vf) { +int VideoDev_SVBCam::Grab(VideoFrameRaw *vf) { int err; if (inframe == NULL) return VDEV_STATUS_ERROR; @@ -339,7 +336,7 @@ int VideoDev_SVBCam::Grab(VideoFrame *vf) { } } LockMutex(); - Convert(&cdata, vf, inframe, inframe_size, inframe_pixfmt, inframe_w, inframe_h); + vf->CopyFrom(inframe_pixfmt, inframe_w, inframe_h, inframe_size, inframe); UnLockMutex(); return VDEV_STATUS_OK; diff --git a/videodev-svbcam.h b/videodev-svbcam.h index a5e51da..7a1ce44 100644 --- a/videodev-svbcam.h +++ b/videodev-svbcam.h @@ -44,7 +44,7 @@ private: ConvertData cdata; int camid; - int Grab(VideoFrame *vf); + int Grab(VideoFrameRaw *vf); int Open(); int Close(); int CaptureStart(); diff --git a/videodev-v4l2.cc b/videodev-v4l2.cc index 16c366f..bf1f684 100644 --- a/videodev-v4l2.cc +++ b/videodev-v4l2.cc @@ -395,7 +395,7 @@ int VideoDev_V4L2::CaptureStart() { return VDEV_STATUS_ERROR; } - ConvertStart(&cdata, fmt.fmt.pix.pixelformat); + pixelformat = fmt.fmt.pix.pixelformat; return VDEV_STATUS_OK; }; @@ -405,8 +405,6 @@ int VideoDev_V4L2::CaptureStop() { enum v4l2_buf_type type; printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); - ConvertStop(&cdata, fmt.fmt.pix.pixelformat); - switch (io) { case IOMODE_READ: /* Nothing to do. */ @@ -431,7 +429,7 @@ int VideoDev_V4L2::CaptureStop() { * If something goes wrong return an error code. * Return code VDEV_STATUS_AGAIN is not an error. There was no video image ready to read. */ -int VideoDev_V4L2::Grab(VideoFrame *vf) { +int VideoDev_V4L2::Grab(VideoFrameRaw *vf) { struct v4l2_buffer buf; int len; @@ -451,7 +449,7 @@ int VideoDev_V4L2::Grab(VideoFrame *vf) { } else { LockMutex(); - Convert(&cdata, vf, inbuffer[0].data, len, fmt.fmt.pix.pixelformat, fmt.fmt.pix.width, fmt.fmt.pix.height); + UnLockMutex(); } break; @@ -477,7 +475,7 @@ int VideoDev_V4L2::Grab(VideoFrame *vf) { if (buf.index >= 0 && buf.index < VDEV_INBUFFERS) { LockMutex(); - Convert(&cdata, vf, inbuffer[buf.index].data, buf.bytesused, fmt.fmt.pix.pixelformat, fmt.fmt.pix.width, fmt.fmt.pix.height); + vf->CopyFrom(fmt.fmt.pix.pixelformat, fmt.fmt.pix.width, fmt.fmt.pix.height, buf.bytesused, inbuffer[buf.index].data); UnLockMutex(); } diff --git a/videodev-v4l2.h b/videodev-v4l2.h index 2b0cd3d..858e1c1 100644 --- a/videodev-v4l2.h +++ b/videodev-v4l2.h @@ -46,9 +46,7 @@ private: struct v4l2_crop crop; struct v4l2_format fmt; - ConvertData cdata; - - int Grab(VideoFrame *vf); + int Grab(VideoFrameRaw *vf); int Open(); int Close(); int CaptureStart(); diff --git a/videodev-vfw.cc b/videodev-vfw.cc index 7d53f4f..b4df1c6 100644 --- a/videodev-vfw.cc +++ b/videodev-vfw.cc @@ -19,6 +19,11 @@ VideoDev_VFW::VideoDev_VFW() { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); cap = NULL; camid = -1; + inframe_pixfmt = 0x0; + inframe_w = -1; + inframe_h = -1; + inframe = NULL; + inframe_size = 0; }; @@ -176,7 +181,7 @@ int VideoDev_VFW::CaptureStart() { printf ("%s:%d %s Could not set frame callback to VFW id %d\n", __FILE__, __LINE__, __FUNCTION__, camid); } - ConvertStart(&cdata, inframe_pixfmt); + pixelformat = inframe_pixfmt; return VDEV_STATUS_OK; }; @@ -192,7 +197,6 @@ int VideoDev_VFW::CaptureStop() { capCaptureAbort(cap); capSetCallbackOnError(cap, NULL); capSetCallbackOnFrame(cap, NULL); - ConvertStop(&cdata, inframe_pixfmt); return VDEV_STATUS_OK; }; @@ -202,12 +206,12 @@ int VideoDev_VFW::CaptureStop() { * Return code VDEV_STATUS_AGAIN is not an error. There was no video image ready to read. */ // FIXME: SVBGetVideoData needs to be outside of Lock/UnLockMutex - using inside thread inbuffer -int VideoDev_VFW::Grab(VideoFrame *vf) { +int VideoDev_VFW::Grab(VideoFrameRaw *vf) { //if (inframe == NULL) return VDEV_STATUS_ERROR; LockMutex(); - Convert(&cdata, vf, VFW_frame_buffer, VFW_frame_size, inframe_pixfmt, conf_width, conf_height); + vf->CopyFrom(inframe_pixfmt, conf_width, conf_height, VFW_frame_size, VFW_frame_buffer) UnLockMutex(); return VDEV_STATUS_OK; diff --git a/videodev-vfw.h b/videodev-vfw.h index 561247d..995dee7 100644 --- a/videodev-vfw.h +++ b/videodev-vfw.h @@ -13,9 +13,9 @@ private: ConvertData cdata; int camid; - HWND cap; + HWND cap; - int Grab(VideoFrame *vf); + int Grab(VideoFrameRaw *vf); int Open(); int Close(); int CaptureStart(); diff --git a/videodev.cc b/videodev.cc index e9317ec..1988abe 100644 --- a/videodev.cc +++ b/videodev.cc @@ -26,6 +26,7 @@ #include "video.h" #include "videoframe.h" #include "videodev.h" +#include "convert.h" VideoDev::VideoDev() { @@ -38,6 +39,7 @@ VideoDev::VideoDev() { conf_width = 600; running = 0; callback = NULL; + pixelformat = 0x0; g_mutex_init (&mutex); }; @@ -162,14 +164,18 @@ void VideoDev::ThreadProcess() { running = 0; } + if (running) { + ConvertStart(&cdata, pixelformat); + } + threaddata.running = 1; // prevent reading all controlls every time. // will be set to 2 in callback function, after the first frame // is read and all controls a loaded. if (callback) gdk_threads_add_idle(callback, &threaddata); while (running) { - i = Grab(&threaddata.vf); - + i = Grab(&threaddata.vfr); + Convert(&cdata, &threaddata.vf, threaddata.vfr.data, threaddata.vfr.size, threaddata.vfr.pixfmt, threaddata.vfr.w, threaddata.vfr.h); switch (i) { case VDEV_STATUS_OK: if (callback) gdk_threads_add_idle(callback, &threaddata); @@ -196,6 +202,7 @@ void VideoDev::ThreadProcess() { // // stop capturing if (callback) gdk_threads_add_idle(callback, NULL); + ConvertStop(&cdata, pixelformat); CaptureStop(); Close(); diff --git a/videodev.h b/videodev.h index 21c4528..03271e0 100644 --- a/videodev.h +++ b/videodev.h @@ -66,6 +66,7 @@ struct { struct { int running; VideoFrame vf; + VideoFrameRaw vfr; } typedef VideoDevThreadData; #ifndef CLEAR @@ -85,13 +86,14 @@ private: std::string conf_devicename; // human friendly name of the device std::string conf_format; // video or image format (should every device have) std::string conf_parameter; // can hold additional parameters + uint32_t pixelformat; // pixelformat int conf_height; int conf_width; int running; // 0 ... not running // 1 ... initialized (init, first frame) // 2 ... running - + ConvertData cdata; GMutex mutex; gboolean (*callback)(gpointer data); @@ -101,7 +103,7 @@ private: std::list vidctrls; /* grabs a single frame, writes the RGB24 converted frame to the VideoFrame pointer */ - virtual int Grab(VideoFrame *vf) { return VDEV_STATUS_AGAIN; }; + virtual int Grab(VideoFrameRaw *vf) { return VDEV_STATUS_AGAIN; }; /* opens the device, will need to fill the controls as well */ virtual int Open() { return VDEV_STATUS_OK; }; diff --git a/videoframe.cc b/videoframe.cc index 26fc97e..88c7a9e 100644 --- a/videoframe.cc +++ b/videoframe.cc @@ -1,11 +1,74 @@ +#include #include #include "video.h" #include "config.h" #include "gui.h" +#include "error.h" -#define VIDEOFRAME_DEPTH_BYTES 3 +VideoFrameRaw::VideoFrameRaw() { + size = 0; + data = NULL; + w = 0; + h = 0; + pixfmt = 0x0; +} + +VideoFrameRaw::~VideoFrameRaw() { + if (data != NULL) { + free (data); + data = NULL; + } +} + + +int VideoFrameRaw::CopyFrom(int spixfmt, int sw, int sh, int ssize, unsigned char *sdata) { + if (size != ssize) { + if (ReAlloc(ssize) != 0) { + errorexit((char*) "%s:%d %s could not reallocate memory. Error:%s\n", + __FILE__, __LINE__, __FUNCTION__, strerror(errno)); + } + } + + if (data == NULL) { + errorexit((char*) "%s:%d %s invalid pointer of data. Error:%s\n", + __FILE__, __LINE__, __FUNCTION__, strerror(errno)); + } + w = sw; + h = sh; + pixfmt = spixfmt; + memcpy (data, sdata, size); + + return 0; +} + + +int VideoFrameRaw::ReAlloc(int newsize) { + unsigned char *newdata = NULL; + if (newsize == 0) { + free (data); + data = NULL; + size = newsize; + return 0; + } + if (data == NULL) { + if ((data = (unsigned char*) malloc (newsize)) == NULL) return -1; + } + else if (size < newsize || size > newsize) { + if ((newdata = (unsigned char*) realloc (data, newsize)) == NULL) { + free (data); + size = 0; + return -1; + } + data = newdata; + } + size = newsize; + return 0; +} + + +#define VIDEOFRAME_DEPTH_BYTES 3 VideoFrame::VideoFrame() { data = NULL; w = 0; diff --git a/videoframe.h b/videoframe.h index 7e46673..f87a3dc 100644 --- a/videoframe.h +++ b/videoframe.h @@ -4,6 +4,21 @@ #include "gui.h" #include "config.h" +class VideoFrameRaw { +private: +public: + unsigned char *data; + int size; + int w; + int h; + uint32_t pixfmt; + VideoFrameRaw(); + ~VideoFrameRaw(); + int ReAlloc(int newsize); + int CopyFrom(int spixfmt, int sw, int sh, int ssize, unsigned char *sdata); +}; + + class FloatImage { public: