You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
325 lines
6.9 KiB
325 lines
6.9 KiB
/************************************************************************************
|
|
* 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);
|
|
|
|
void cb_posctl_btnsimreset (GtkWidget *widget, gpointer data) {
|
|
simulation.Reset();
|
|
}
|
|
|
|
|
|
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<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_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;
|
|
}
|
|
simulation.Stop();
|
|
|
|
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 16
|
|
int VideoDev_Simulation::Grab(VideoFrame *vf) {
|
|
int posx, posy, x ,y, r, radius;
|
|
double 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);
|
|
|
|
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);
|
|
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() {
|
|
w = 1920;
|
|
h = 1080;
|
|
posX = w/2;
|
|
posY = h/2;
|
|
running = 0;
|
|
|
|
Reset();
|
|
};
|
|
|
|
|
|
void Simulation::Reset() {
|
|
int i;
|
|
|
|
LockMutex();
|
|
|
|
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 = 0;
|
|
|
|
axis[i % 2].defAngle = dAngle + (10.0 * (double)random() / ((double) RAND_MAX)) - 5.0;
|
|
axis[i % 2].defLen = dLen + (3.0 * (double)random() / ((double) RAND_MAX) - 1.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 + (3.0 * (double)random() / ((double) RAND_MAX) - 1.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);
|
|
UnLockMutex();
|
|
}
|
|
|
|
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);
|
|
if (running) return;
|
|
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);
|
|
|
|
running = 0;
|
|
}
|
|
|
|
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;
|
|
}
|