SimpleSkyCam/videodev-simulation.cc

/************************************************************************************
 * 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;
}