/************************************************************************************ * 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__); conf_width = 1920; conf_height = 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.SetResolution(conf_width, conf_height); simulation_thread = g_thread_new("Simulation", simulation_threadprocess_wrapper, NULL); inframe = (unsigned char *) malloc (conf_width * conf_height * 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, conf_width*conf_height*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 && x < conf_width && y >= 0 && y < conf_height) { inframe[3*(x+y*conf_width)+0] = 200; inframe[3*(x+y*conf_width)+1] = 200; inframe[3*(x+y*conf_width)+2] = 200; } } LockMutex(); Convert(&cdata, vf, inframe, (conf_width*conf_height*3), V4L2_PIX_FMT_RGB24, conf_width, conf_height); 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; w = 1920; h = 1080; posX = w/2; posY = h/2; running = 0; time_t t = time(NULL); srandom (t); // // object movement dAngle = 360.0 * (double)random() / (double) RAND_MAX; dLen = 1.0 + 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; printf ("%s:%d %s Axis1: %f° Len:%f Axis2: %f° Len:%f\n", __FILE__, __LINE__, __FUNCTION__, axis[0].defAngle,axis[0].defLen, axis[1].defAngle,axis[1].defLen); }; 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 (20000); ms = get_cycletime(&tv); LockMutex(); // // simulate rotation movement // calculate movement dx = sindeg(dAngle) * dLen * ms; dy = -cosdeg(dAngle) * dLen * ms; posX += dx; posY += dy; // // simulate motor axis movement for (int i = 0; i < 2; i++) if (axis[i].active) { dx = sindeg(axis[i].defAngle) * axis[i].defLen * (1.0 + axis[i].v) * ms; dy = -cosdeg(axis[i].defAngle) * axis[i].defLen * (1.0 + axis[i].v) * ms; posX -= dx; posY -= dy; } if (posX < 0) posX = w - 1.0; if (posY < 0) posY = h - 1.0; if (posX > w) posX = 0.0; if (posY > h) posY = 0.0; r = running; UnLockMutex(); } while (r); } void Simulation::Start() { } void Simulation::Stop() { running = 0; } void Simulation::SetResolution (int w, int h) { LockMutex(); this->w = w; this->h = h; posX = w / 2; posY = h / 2; UnLockMutex(); } 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 = 0; axis[1].active = 0; }