/************************************************************************************ * videodev-simulation: * creates an black screen with an small white circle at a simulated movement * needed for testing the PID functionality * * This is needed only for debugging. ************************************************************************************/ #include #include #include #include #include "convert.h" #include "configuration.h" #include "videodev-simulation.h" Simulation simulation; gpointer simulation_threadprocess_wrapper (gpointer data); VideoDev_Simulation::VideoDev_Simulation() { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); w = 1920; h = 1080; inframe = NULL; simulation_thread = NULL; }; VideoDev_Simulation::~VideoDev_Simulation() { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); if (inframe != NULL) Close(); inframe = NULL; } /* * searchs in the given path for any videodump files and present them as * video device. */ int VideoDev_Simulation::GetDeviceList(std::list *list) { list->push_back("SIMULATION"); return 1; } /* * Open Device * prepare the buffer, InitMMAP and read all controls */ int VideoDev_Simulation::Open() { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); if (inframe != NULL) Close(); conf_width = w; conf_height = h; conf_format = convert_from_pixelformat (V4L2_PIX_FMT_RGB24); simulation_thread = g_thread_new("Simulation", simulation_threadprocess_wrapper, NULL); inframe = (unsigned char *) malloc (w * h * 3); return VDEV_STATUS_OK; }; /* * Close Device * Free videobuffer */ int VideoDev_Simulation::Close() { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); if (inframe != NULL) { free (inframe); inframe = NULL; } return VDEV_STATUS_OK; }; /***************************************************************************************************** * VideoGrabbing */ /* * send the start capture signal to the cam */ int VideoDev_Simulation::CaptureStart() { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); if (inframe != NULL) Close(); if (Open() != VDEV_STATUS_OK) return VDEV_STATUS_ERROR; ConvertStart(&cdata, V4L2_PIX_FMT_RGB24); get_cycletime(&lastframetv); return VDEV_STATUS_OK; }; 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; }; /* * try to grab one frame and convert it into RGB32. * 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. */ #define SIMULATION_SIZE 15 int VideoDev_Simulation::Grab(VideoFrame *vf) { int posx, posy, x ,y; double r, a, dsec; // try to match a speed of 20Hz dsec = get_cycletime(&lastframetv); if (dsec < 0.040) usleep (50000 - dsec*1000.0); memset (inframe, 0x0, w*h*3); simulation.GetPos(&posx, &posy); for (r = 1; r < SIMULATION_SIZE; r++) for (a = 0; a < M_PI * 2.0; a += 0.1) { x = posx + (sin(a) * r); y = posy + (cos(a) * r); if (x >= 0 && (unsigned int)x < w && y >= 0 && (unsigned int)y < h) { inframe[3*(x+y*w)+0] = 200; inframe[3*(x+y*w)+1] = 200; inframe[3*(x+y*w)+2] = 200; } } LockMutex(); Convert(&cdata, vf, inframe, (w*h*3), V4L2_PIX_FMT_RGB24, w, h); UnLockMutex(); return VDEV_STATUS_OK; } /***************************************************************************************************** * Controls */ /* * set video control identified by id */ int VideoDev_Simulation::SetDevCtrl(unsigned int id, int value) { return VDEV_STATUS_OK; }; /* * get video control identified by id */ int VideoDev_Simulation::GetDevCtrl(unsigned int id, int *value) { return VDEV_STATUS_OK; }; /********************************************************************************************************* * Simulation */ gpointer simulation_threadprocess_wrapper (gpointer data) { simulation.ThreadProcess(); return NULL; } Simulation::Simulation() { int i; posX = 900.0; posY = 500.0; running = 0; time_t t = time(NULL); srandom (t); // // object movement dAngle = 2.0 * M_PI * (double)random() / (double) RAND_MAX; dLen = 2.5 + 5.0 * (double)random() / (double) RAND_MAX; printf ("%s:%d %s dAngle:%f dLen:%f\n", __FILE__, __LINE__, __FUNCTION__, dAngle, dLen); // // axis movement i = random() % 2; axis[i % 2].defAngle = dAngle + (10.0 * (double)random() / ((double) RAND_MAX)) - 5.0; axis[i % 2].defLen = dLen + (1.0 * (double)random() / ((double) RAND_MAX) - 0.5); axis[i % 2].v = 1.0; axis[i % 2].active = 1; i++; axis[i % 2].defAngle = dAngle + (10.0 * (double)random() / ((double) RAND_MAX)) + 85.0; axis[i % 2].defLen = dLen + (1.0 * (double)random() / ((double) RAND_MAX) - 0.5); axis[i % 2].v = 1.0; axis[i % 2].active = 1; }; Simulation::~Simulation() { LockMutex (); running = 0; UnLockMutex (); }; void Simulation::GetPos (int *nx, int *ny) { LockMutex(); if (nx != NULL) *nx = (int)posX; if (ny != NULL) *ny = (int)posY; UnLockMutex(); } void Simulation::ThreadProcess() { int r = 1; struct timeval tv; double ms; double dx, dy; get_cycletime (&tv); running = 1; do { usleep (10000); ms = get_cycletime(&tv); LockMutex(); // // simulate rotation movement // calculate movement dx = sin(dAngle) * dLen * ms; dy = -cos(dAngle) * dLen * ms; posX += dx; posY += dy; // // simulate motor axis movement for (int i = 0; i < 2; i++) if (axis[i].active) { dx = sin(axis[i].defAngle) * axis[i].defLen * axis[i].v * ms; dy = -cos(axis[i].defAngle) * axis[i].defLen * axis[i].v * ms; posX -= dx; posY -= dy; } if (posX < 0) posX = 1920.0 - 1.0; if (posY < 0) posY = 1080.0 - 1.0; if (posX > 1920) posX = 0.0; if (posY > 1080) posY = 0.0; r = running; UnLockMutex(); } while (r); } void Simulation::Start() { } void Simulation::Stop() { running = 0; } void Simulation::AxisSetValue (int a, double v) { printf ("%s:%d %s Axis:%d Value:%f\n", __FILE__, __LINE__, __FUNCTION__, a, v); if (a < 0 || a > 1) return; axis[a].v = v; axis[a].active = 1; } void Simulation::AxisStop() { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); axis[0].active = 1; axis[1].active = 1; }