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.
373 lines
8.5 KiB
373 lines
8.5 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>
|
|
#ifdef BUILD_WINDOWS
|
|
#include "windows.h"
|
|
#else
|
|
#include <arpa/inet.h>
|
|
#endif
|
|
#include <sys/stat.h>
|
|
#include <math.h>
|
|
|
|
#include "debug.h"
|
|
#include "convert.h"
|
|
#include "configuration.h"
|
|
#include "videodev-simulation.h"
|
|
|
|
#define SIMULATION_MOTORSPEEDUP 1.0
|
|
|
|
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;
|
|
pixelformat = 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();
|
|
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
|
|
#define SIMULATION_NOISE 15
|
|
int VideoDev_Simulation::Grab(VideoFrameRaw *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) + (SIMULATION_NOISE/2 - SIMULATION_NOISE *(float) rand()/ (float)RAND_MAX);
|
|
y = posy + (cos(a) * r) + (SIMULATION_NOISE/2 - SIMULATION_NOISE *(float) rand()/ (float)RAND_MAX);
|
|
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();
|
|
vf->CopyFrom(V4L2_PIX_FMT_RGB24, conf_width, conf_height, (conf_width*conf_height*3), inframe);
|
|
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;
|
|
|
|
#ifdef DEBUG_POSCTL
|
|
debug_tofile((char*)"simulation.log", 1, (char*)"dAngle,dLen,x,y,dx,dy,timedelay,a1.defAngle,a1.defLen,a1.v,a1.dx,a1.dy,a2.defAngle,a2.defLen,a2.v,a2.dx,a2.dy,finalX,finalY\n");
|
|
#endif
|
|
|
|
Reset();
|
|
};
|
|
|
|
|
|
void Simulation::Reset() {
|
|
int i;
|
|
static int first = 1;
|
|
|
|
LockMutex();
|
|
|
|
if(first) {
|
|
time_t t = time(NULL);
|
|
srand (t);
|
|
first = 0;
|
|
}
|
|
|
|
//
|
|
// object movement
|
|
dAngle = 360.0 * (double)rand() / (double) RAND_MAX;
|
|
dLen = 1.0 + 5.0 * (double)rand() / (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 = 180.0 + dAngle + (10.0 * (double)rand() / ((double) RAND_MAX)) - 5.0;
|
|
axis[i % 2].defLen = dLen + ((dLen/2.0) * (double)rand() / ((double) RAND_MAX) - (dLen/4.0));
|
|
axis[i % 2].v = 1.0;
|
|
axis[i % 2].vdest = 1.0;
|
|
axis[i % 2].active = 1;
|
|
|
|
i++;
|
|
axis[i % 2].defAngle = 180.0 + dAngle + (10.0 * (double)rand() / ((double) RAND_MAX)) + 85.0;
|
|
axis[i % 2].defLen = dLen + ((dLen/2.0) * (double)rand() / ((double) RAND_MAX) - (dLen/4.0));
|
|
axis[i % 2].v = 1.0;
|
|
axis[i % 2].vdest = 1.0;
|
|
axis[i % 2].active = 1;
|
|
|
|
// normalize angle between 0..360°
|
|
for (i = 0; i < 2; i++) {
|
|
while (axis[i].defAngle < 0.0) axis[i].defAngle += 360.0;
|
|
while (axis[i].defAngle > 360.0) axis[i].defAngle -= 360.0;
|
|
}
|
|
|
|
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[3], dy[3];
|
|
#ifdef DEBUG_POSCTL
|
|
double x, y;
|
|
#endif
|
|
|
|
get_cycletime (&tv);
|
|
if (running) return;
|
|
running = 1;
|
|
|
|
do {
|
|
usleep (20000);
|
|
ms = get_cycletime(&tv);
|
|
LockMutex();
|
|
|
|
//
|
|
// simulate rotation movement
|
|
|
|
// calculate movement
|
|
dx[0] = sindeg(dAngle) * dLen * ms;
|
|
dy[0] = cosdeg(dAngle) * dLen * ms;
|
|
|
|
//
|
|
// simulate motor axis movement
|
|
for (int i = 0; i < 2; i++)
|
|
if (axis[i].active) {
|
|
dx[i+1] = sindeg(axis[i].defAngle) * axis[i].defLen * (1.0 + axis[i].v) * ms;
|
|
dy[i+1] = cosdeg(axis[i].defAngle) * axis[i].defLen * (1.0 + axis[i].v) * ms;
|
|
}
|
|
else {
|
|
dx[i+1] = 0.0;
|
|
dy[i+1] = 0.0;
|
|
}
|
|
|
|
// this is needed for debugging
|
|
#ifdef DEBUG_POSCTL
|
|
x = posX; y = posY;
|
|
#endif
|
|
posX = posX + dx[0] + dx[1] + dx[2];
|
|
posY = posY + dy[0] + dy[1] + dy[2];
|
|
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;
|
|
|
|
for (int i = 0; i < 2; i++) {
|
|
axis[i].v = axis[i].v * (1.0-SIMULATION_MOTORSPEEDUP) + axis[i].vdest * SIMULATION_MOTORSPEEDUP;
|
|
}
|
|
// printf ("%s:%d axis %f (%f) %f (%f)\n", __FILE__, __LINE__, axis[0].v, axis[0].vdest, axis[1].v, axis[1].vdest);
|
|
|
|
|
|
#ifdef DEBUG_POSCTL
|
|
debug_tofile((char*)"simulation.log", 0, (char*)"%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n",
|
|
dAngle, dLen, x, y, dx[0], dy[0], ms,
|
|
axis[0].defAngle,axis[0].defLen,axis[0].v,dx[1],dy[1],
|
|
axis[1].defAngle,axis[1].defLen,axis[1].v,dx[2],dy[2],
|
|
posX, posY
|
|
);
|
|
#endif
|
|
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].vdest = v;
|
|
axis[a].active = 1;
|
|
}
|
|
|
|
|
|
void Simulation::AxisStop() {
|
|
// printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
axis[0].vdest = axis[0].v = 0.0;
|
|
axis[1].vdest = axis[1].v = 0.0;
|
|
axis[0].active = 0;
|
|
axis[1].active = 0;
|
|
}
|