parent
0e4346d525
commit
47d9be6e34
@ -0,0 +1,253 @@
|
||||
/************************************************************************************
|
||||
* 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 <ctype.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <sys/stat.h>
|
||||
#include <math.h>
|
||||
|
||||
|
||||
#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<std::string> *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);
|
||||
|
||||
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;
|
||||
|
||||
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() {
|
||||
posX = 900.0;
|
||||
posY = 500.0;
|
||||
running = 0;
|
||||
|
||||
time_t t = time(NULL);
|
||||
srandom (t);
|
||||
dAngle = 2.0 * M_PI * (double)random() / (double) RAND_MAX;
|
||||
dLen = 5.0 + 10.0 * (double)random() / (double) RAND_MAX;
|
||||
printf ("%s:%d %s dAngle:%f dLen:%f\n", __FILE__, __LINE__, __FUNCTION__, dAngle, dLen);
|
||||
};
|
||||
|
||||
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
|
||||
|
||||
// add random error to values
|
||||
|
||||
// calculate movement
|
||||
dx = sin(dAngle) * dLen * ms;
|
||||
dy = cos(dAngle) * dLen * ms;
|
||||
posX += dx;
|
||||
posY += dy;
|
||||
|
||||
//
|
||||
// simulate motor axis movement
|
||||
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,77 @@
|
||||
|
||||
#ifndef _H_VIDEODEV_SIMULATION_H_
|
||||
#define _H_VIDEODEV_SIMULATION_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <getopt.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/mman.h>
|
||||
#include <linux/videodev2.h>
|
||||
|
||||
#include <list>
|
||||
#include <string>
|
||||
|
||||
|
||||
#include "convert.h"
|
||||
#include "gui.h"
|
||||
#include "videodev.h"
|
||||
|
||||
|
||||
class Simulation {
|
||||
private:
|
||||
float posX;
|
||||
float posY;
|
||||
float dAngle;
|
||||
float dLen;
|
||||
int running;
|
||||
GMutex mutex;
|
||||
|
||||
public:
|
||||
Simulation();
|
||||
~Simulation();
|
||||
void GetPos (int *nx, int *ny);
|
||||
void ThreadProcess();
|
||||
|
||||
void Start();
|
||||
void Stop();
|
||||
|
||||
void LockMutex() { g_mutex_lock(&mutex); };
|
||||
void UnLockMutex() { g_mutex_unlock(&mutex); };
|
||||
};
|
||||
|
||||
|
||||
class VideoDev_Simulation: public VideoDev {
|
||||
private:
|
||||
uint32_t w;
|
||||
uint32_t h;
|
||||
struct timeval starttv;
|
||||
unsigned char *inframe;
|
||||
ConvertData cdata;
|
||||
GThread *simulation_thread;
|
||||
|
||||
int Grab(VideoFrame *vf);
|
||||
int Open();
|
||||
int Close();
|
||||
int CaptureStart();
|
||||
int CaptureStop();
|
||||
int SetDevCtrl(unsigned int id, int value);
|
||||
int GetDevCtrl(unsigned int id, int *value);
|
||||
public:
|
||||
VideoDev_Simulation();
|
||||
~VideoDev_Simulation();
|
||||
int GetDeviceList(std::list<std::string> *list);
|
||||
int GetDeviceFormats(string device, std::list<string> *formats) { return VDEV_STATUS_OK; };
|
||||
int GetDeviceResolutions(string device, std::list<string> *formats) { return VDEV_STATUS_OK; };
|
||||
};
|
||||
|
||||
#endif
|
||||
Loading…
Reference in new issue